962 lines
45 KiB
Python
962 lines
45 KiB
Python
import grpc
|
||
import numpy as np
|
||
import sys
|
||
import os
|
||
|
||
current_directory = os.path.dirname(os.path.abspath(__file__))
|
||
if current_directory not in sys.path:
|
||
sys.path.append(current_directory)
|
||
# 相机协议
|
||
from aimdk.protocol.hal.sensors import rs2_camera_pb2
|
||
from aimdk.protocol.hal.sensors import rs2_camera_pb2_grpc
|
||
from aimdk.protocol.sim import sim_camera_service_pb2
|
||
from aimdk.protocol.sim import sim_camera_service_pb2_grpc
|
||
# 臂协议
|
||
from aimdk.protocol.hal.jaka import jaka_pb2
|
||
from aimdk.protocol.hal.jaka import jaka_pb2_grpc
|
||
# 关节协议
|
||
from aimdk.protocol.hal.joint import joint_channel_pb2
|
||
from aimdk.protocol.hal.joint import joint_channel_pb2_grpc
|
||
# 夹爪协议
|
||
from aimdk.protocol.sim import sim_gripper_service_pb2
|
||
from aimdk.protocol.sim import sim_gripper_service_pb2_grpc
|
||
# 物体协议
|
||
from aimdk.protocol.sim import sim_object_service_pb2
|
||
from aimdk.protocol.sim import sim_object_service_pb2_grpc
|
||
# observation协议
|
||
from aimdk.protocol.sim import sim_observation_service_pb2
|
||
from aimdk.protocol.sim import sim_observation_service_pb2_grpc
|
||
import time
|
||
import json
|
||
import pinocchio
|
||
import os
|
||
|
||
|
||
# 目前代码中所有的旋转角度都以角度为单位
|
||
class Rpc_Client:
|
||
def __init__(self, client_host, robot_urdf="A2D_120s.urdf"):
|
||
for i in range(600):
|
||
try:
|
||
self.channel = grpc.insecure_channel(client_host, options=[
|
||
('grpc.max_receive_message_length', 16094304)
|
||
])
|
||
grpc.channel_ready_future(self.channel).result(timeout=5)
|
||
self.robot_urdf = robot_urdf
|
||
except grpc.FutureTimeoutError as e:
|
||
print(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}", flush=True)
|
||
time.sleep(3)
|
||
if i >= 599:
|
||
raise e
|
||
except grpc.RpcError as e:
|
||
print(f"Failed to connect to gRPC server[{i}] ({client_host}): {e}", flush=True)
|
||
time.sleep(3)
|
||
if i >= 599:
|
||
raise e
|
||
|
||
def set_frame_state(self, action: str, substage_id: int, active_id: str, passive_id: str, if_attached: bool,
|
||
set_gripper_open=False, set_gripper_close=False, arm="default", target_pose=None):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.SetFrameStateReq()
|
||
if target_pose is not None:
|
||
target_pose = target_pose.tolist()
|
||
frame_state = {
|
||
"action": action,
|
||
"substage_id": substage_id,
|
||
"if_attached": if_attached,
|
||
"set_gripper_open": set_gripper_open,
|
||
"set_gripper_close": set_gripper_close,
|
||
"active_id": active_id,
|
||
"passive_id": passive_id,
|
||
"arm": arm,
|
||
"target_pose": target_pose
|
||
}
|
||
_frame_state: str = json.dumps(frame_state)
|
||
print(_frame_state)
|
||
req.frame_state = _frame_state
|
||
response = stub.SetFrameState(req)
|
||
return response
|
||
|
||
def capture_frame(self, camera_prim_path):
|
||
stub = rs2_camera_pb2_grpc.CameraServiceStub(self.channel)
|
||
req = rs2_camera_pb2.GetCameraDataRequest()
|
||
req.serial_no = camera_prim_path
|
||
response = stub.GetCameraData(req)
|
||
# print(response.color_info)
|
||
# if response.color_image.data is not None:
|
||
# print(np.frombuffer(response.color_image.data, dtype=np.uint8))
|
||
# print(np.frombuffer(response.depth_image.data, dtype=np.float32))
|
||
return response
|
||
|
||
def capture_semantic_frame(self, camera_prim_path):
|
||
stub = sim_camera_service_pb2_grpc.SimCameraServiceStub(self.channel)
|
||
req = sim_camera_service_pb2.GetSemanticRequest()
|
||
req.serial_no = "/World/Raise_A2/base/collisions/camera"
|
||
req.serial_no = camera_prim_path
|
||
response = stub.GetSemanticData(req)
|
||
# if response.semantic_mask.data is not None:
|
||
# print(np.frombuffer(response.semantic_mask.data, dtype=np.int16))
|
||
|
||
return response
|
||
|
||
def moveto(self, target_position, target_quaternion, arm_name, is_backend=True, ee_interpolation=False,
|
||
distance_frame=0.0008):
|
||
stub = jaka_pb2_grpc.A2wArmControlServiceStub(self.channel)
|
||
req = jaka_pb2.LinearMoveReq()
|
||
req.robot_name = arm_name
|
||
req.pose.position.x, req.pose.position.y, req.pose.position.z = target_position
|
||
req.pose.rpy.rw, req.pose.rpy.rx, req.pose.rpy.ry, req.pose.rpy.rz = target_quaternion
|
||
req.is_block = is_backend
|
||
req.ee_interpolation = ee_interpolation
|
||
req.distance_frame = distance_frame
|
||
response = stub.LinearMove(req)
|
||
return response
|
||
|
||
def set_joint_positions(self, target_joint_position, is_trajectory):
|
||
stub = joint_channel_pb2_grpc.JointControlServiceStub(self.channel)
|
||
req = joint_channel_pb2.SetJointReq()
|
||
req.is_trajectory = is_trajectory
|
||
for pos in target_joint_position:
|
||
joint_position = joint_channel_pb2.JointCommand()
|
||
joint_position.position = pos
|
||
req.commands.append(joint_position)
|
||
response = stub.SetJointPosition(req)
|
||
return response
|
||
|
||
def get_joint_positions(self):
|
||
stub = joint_channel_pb2_grpc.JointControlServiceStub(self.channel)
|
||
req = joint_channel_pb2.GetJointReq()
|
||
req.serial_no = "robot"
|
||
response = stub.GetJointPosition(req)
|
||
return response
|
||
|
||
# usd_path目前值定在data下的目录 如genie3D/01.usd
|
||
# prim_path需要指定在"/World/Objects/xxx"内，方便reset
|
||
def add_object(self, usd_path, prim_path, label_name, target_position, target_quaternion, target_scale, color,
|
||
material, mass=0.01,
|
||
add_particle=True, particle_position=[0, 0, 0], particle_scale=[0.1, 0.1, 0.1],
|
||
particle_color=[1, 1, 1]):
|
||
stub = sim_object_service_pb2_grpc.SimObjectServiceStub(self.channel)
|
||
req = sim_object_service_pb2.AddObjectReq()
|
||
req.usd_path = usd_path
|
||
req.prim_path = prim_path
|
||
req.label_name = label_name
|
||
req.object_color.r, req.object_color.g, req.object_color.b = color
|
||
req.object_pose.position.x, req.object_pose.position.y, req.object_pose.position.z = target_position
|
||
req.object_pose.rpy.rw, req.object_pose.rpy.rx, req.object_pose.rpy.ry, req.object_pose.rpy.rz = target_quaternion
|
||
req.object_scale.x, req.object_scale.y, req.object_scale.z = target_scale
|
||
req.object_material = material
|
||
req.object_mass = mass
|
||
req.add_particle = add_particle
|
||
req.particle_position.x, req.particle_position.y, req.particle_position.z = particle_position
|
||
req.particle_scale.x, req.particle_scale.y, req.particle_scale.z = particle_scale
|
||
req.particle_color.r, req.particle_color.g, req.particle_color.b = particle_color
|
||
|
||
response = stub.AddObject(req)
|
||
return response
|
||
|
||
def get_object_pose(self, prim_path):
|
||
stub = sim_object_service_pb2_grpc.SimObjectServiceStub(self.channel)
|
||
req = sim_object_service_pb2.GetObjectPoseReq()
|
||
req.prim_path = prim_path
|
||
response = stub.GetObjectPose(req)
|
||
return response
|
||
|
||
def get_object_joint(self, prim_path):
|
||
stub = sim_object_service_pb2_grpc.SimObjectServiceStub(self.channel)
|
||
req = sim_object_service_pb2.GetObjectJointReq()
|
||
req.prim_path = prim_path
|
||
response = stub.GetObjectJoint(req)
|
||
return response
|
||
|
||
def set_gripper_state(self, gripper_command, is_right, opened_width):
|
||
stub = sim_gripper_service_pb2_grpc.SimGripperServiceStub(self.channel)
|
||
req = sim_gripper_service_pb2.SetGripperStateReq()
|
||
req.gripper_command = gripper_command
|
||
req.is_right = is_right
|
||
req.opened_width = opened_width
|
||
response = stub.SetGripperState(req)
|
||
return response
|
||
|
||
def get_gripper_state(self, is_right):
|
||
stub = sim_gripper_service_pb2_grpc.SimGripperServiceStub(self.channel)
|
||
req = sim_gripper_service_pb2.GetGripperStateReq()
|
||
req.is_right = is_right
|
||
response = stub.GetGripperState(req)
|
||
return response
|
||
|
||
# client端获取get某一帧的全部observation
|
||
def get_observation(self, data_keys):
|
||
observation = {}
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.GetObservationReq()
|
||
if 'camera' in data_keys:
|
||
req.isCam = True
|
||
req.CameraReq.render_depth = data_keys['camera']['render_depth']
|
||
req.CameraReq.render_semantic = data_keys['camera']['render_semantic']
|
||
for prim in data_keys['camera']['camera_prim_list']:
|
||
req.CameraReq.camera_prim_list.append(prim)
|
||
if 'pose' in data_keys:
|
||
req.isPose = True
|
||
for pose in data_keys['pose']:
|
||
req.objectPrims.append(pose)
|
||
req.isJoint = data_keys['joint_position']
|
||
req.isGripper = data_keys['gripper']
|
||
response = stub.GetObservation(req)
|
||
|
||
# protobuf转字典
|
||
camera_datas = {}
|
||
for camera_data, camera_prim in zip(response.camera, data_keys['camera']['camera_prim_list']):
|
||
cam_data = {
|
||
"rgb_camera": np.frombuffer(camera_data.rgb_camera.data, dtype=np.uint8),
|
||
"depth_camera": np.frombuffer(camera_data.depth_camera.data, dtype=np.float32),
|
||
"camera_info": {"width": camera_data.camera_info.width,
|
||
"height": camera_data.camera_info.height,
|
||
"ppx": camera_data.camera_info.ppx,
|
||
"ppy": camera_data.camera_info.ppy,
|
||
"fx": camera_data.camera_info.fx,
|
||
"fy": camera_data.camera_info.fy}
|
||
}
|
||
camera_datas[camera_prim] = cam_data
|
||
joint_datas = {}
|
||
for joint in response.joint.left_arm:
|
||
joint_datas[joint.name] = joint.position
|
||
object_datas = {}
|
||
if 'pose' in data_keys:
|
||
for object, obj_name in zip(response.pose, data_keys['pose']):
|
||
object_data = {
|
||
"position": np.array(
|
||
[object.object_pose.position.x, object.object_pose.position.y, object.object_pose.position.z]),
|
||
"rotation": np.array(
|
||
[object.object_pose.rpy.rw, object.object_pose.rpy.rx, object.object_pose.rpy.ry,
|
||
object.object_pose.rpy.rz])
|
||
}
|
||
object_datas[obj_name] = object_data
|
||
gripper_datas = {
|
||
"left": {
|
||
"position": np.array(
|
||
[response.gripper.left_gripper.position.x, response.gripper.left_gripper.position.y,
|
||
response.gripper.left_gripper.position.z]),
|
||
"rotation": np.array([response.gripper.left_gripper.rpy.rw, response.gripper.left_gripper.rpy.rx,
|
||
response.gripper.left_gripper.rpy.ry, response.gripper.left_gripper.rpy.rz])
|
||
|
||
},
|
||
"right": {
|
||
"position": np.array(
|
||
[response.gripper.right_gripper.position.x, response.gripper.right_gripper.position.y,
|
||
response.gripper.right_gripper.position.z]),
|
||
"rotation": np.array([response.gripper.right_gripper.rpy.rw, response.gripper.right_gripper.rpy.rx,
|
||
response.gripper.right_gripper.rpy.ry, response.gripper.right_gripper.rpy.rz])
|
||
}}
|
||
observation = {
|
||
"camera": camera_datas,
|
||
"joint": joint_datas,
|
||
"pose": object_datas,
|
||
"gripper": gripper_datas
|
||
}
|
||
return observation
|
||
|
||
# client端启动开始录制
|
||
def start_recording(self, data_keys, fps, task_name):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.GetObservationReq()
|
||
req.startRecording = True
|
||
req.fps = fps
|
||
req.task_name = task_name
|
||
if 'camera' in data_keys:
|
||
render_depth = data_keys['camera']['render_depth']
|
||
render_semantic = data_keys['camera']['render_semantic']
|
||
camera_prim_list = data_keys['camera']['camera_prim_list']
|
||
req.isCam = True
|
||
req.CameraReq.render_depth = data_keys['camera']['render_depth']
|
||
req.CameraReq.render_semantic = data_keys['camera']['render_semantic']
|
||
for prim in data_keys['camera']['camera_prim_list']:
|
||
req.CameraReq.camera_prim_list.append(prim)
|
||
if data_keys['pose']:
|
||
req.isPose = True
|
||
for pose in data_keys['pose']:
|
||
req.objectPrims.append(pose)
|
||
req.isJoint = data_keys['joint_position']
|
||
req.isGripper = data_keys['gripper']
|
||
response = stub.GetObservation(req)
|
||
return (response)
|
||
|
||
# client端启动结束录制
|
||
def stop_recording(self):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.GetObservationReq()
|
||
req.stopRecording = True
|
||
response = stub.GetObservation(req)
|
||
return (response)
|
||
|
||
def reset(self):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.ResetReq()
|
||
req.reset = True
|
||
response = stub.Reset(req)
|
||
return (response)
|
||
|
||
def AttachObj(self, prim_paths, is_right=True):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.AttachReq()
|
||
req.is_right = is_right
|
||
for prim in prim_paths:
|
||
req.obj_prims.append(prim)
|
||
response = stub.AttachObj(req)
|
||
return (response)
|
||
|
||
def DetachObj(self):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.DetachReq()
|
||
req.detach = True
|
||
response = stub.DetachObj(req)
|
||
return (response)
|
||
|
||
def MultiPlan(self, robot_name, target_poses):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.MultiMoveReq()
|
||
req.plan = True
|
||
req.robot_name = robot_name
|
||
req.plans_index = 0
|
||
for pose in target_poses:
|
||
_pose = sim_observation_service_pb2.SE3RpyPose()
|
||
_pose.position.x, _pose.position.y, _pose.position.z = pose[0]
|
||
_pose.rpy.rw, _pose.rpy.rx, _pose.rpy.ry, _pose.rpy.rz = pose[1]
|
||
req.poses.append(_pose)
|
||
response = stub.MultiMove(req)
|
||
return (response)
|
||
|
||
def MultiMove(self, robot_name, cmd_plan):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.MultiMoveReq()
|
||
req.plan = False
|
||
req.robot_name = robot_name
|
||
req.cmd_plan = cmd_plan
|
||
response = stub.MultiMove(req)
|
||
return (response)
|
||
|
||
def SendTaskStatus(self, isSuccess, fail_stage_step):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.TaskStatusReq()
|
||
for step in fail_stage_step:
|
||
req.failStep.append(step)
|
||
req.isSuccess = isSuccess
|
||
response = stub.TaskStatus(req)
|
||
return (response)
|
||
|
||
def Exit(self):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.ExitReq()
|
||
req.exit = True
|
||
response = stub.Exit(req)
|
||
return (response)
|
||
|
||
def GetEEPose(self, is_right):
|
||
stub = joint_channel_pb2_grpc.JointControlServiceStub(self.channel)
|
||
req = joint_channel_pb2.GetEEPoseReq()
|
||
req.is_right = is_right
|
||
response = stub.GetEEPose(req)
|
||
return response
|
||
|
||
def GetIKStatus(self, target_poses, is_right, ObsAvoid=False):
|
||
stub = joint_channel_pb2_grpc.JointControlServiceStub(self.channel)
|
||
req = joint_channel_pb2.GetIKStatusReq()
|
||
req.is_right = is_right
|
||
req.ObsAvoid = ObsAvoid
|
||
urdf = os.path.dirname(os.path.abspath(__file__)) + "/robot_urdf/" + self.robot_urdf
|
||
model = pinocchio.buildModelFromUrdf(urdf)
|
||
data = model.createData()
|
||
joint_names = []
|
||
for name in model.names:
|
||
joint_names.append(str(name))
|
||
|
||
for pose in target_poses:
|
||
_pose = sim_observation_service_pb2.SE3RpyPose()
|
||
_pose.position.x, _pose.position.y, _pose.position.z = pose["position"]
|
||
_pose.rpy.rw, _pose.rpy.rx, _pose.rpy.ry, _pose.rpy.rz = pose["rotation"]
|
||
req.target_pose.append(_pose)
|
||
response = stub.GetIKStatus(req)
|
||
result = []
|
||
for ik_status in response.IKStatus:
|
||
joint_datas = {}
|
||
for name, position in zip(ik_status.joint_names, ik_status.joint_positions):
|
||
joint_datas[name] = position
|
||
|
||
joint_positions = []
|
||
for name in model.names:
|
||
if name == 'universe':
|
||
continue
|
||
if name not in joint_datas:
|
||
joint_positions.append(0)
|
||
else:
|
||
joint_positions.append(joint_datas[name])
|
||
joint_positions = np.array(joint_positions)
|
||
|
||
pinocchio.forwardKinematics(model, data, joint_positions)
|
||
if ("A2D" in self.robot_urdf):
|
||
J = pinocchio.computeJointJacobian(model, data, joint_positions, 24)
|
||
else:
|
||
J = pinocchio.computeJointJacobian(model, data, joint_positions, 7)
|
||
manip = np.sqrt(np.linalg.det(np.dot(J, J.T)))
|
||
result.append({
|
||
"status": ik_status.isSuccess,
|
||
"Jacobian": manip,
|
||
"joint_positions": ik_status.joint_positions,
|
||
"joint_names": ik_status.joint_names
|
||
})
|
||
return result
|
||
|
||
def GetManipulability(self, joint_data):
|
||
urdf = os.path.dirname(os.path.abspath(__file__)) + "/robot_urdf/" + self.robot_urdf
|
||
model = pinocchio.buildModelFromUrdf(urdf)
|
||
data = model.createData()
|
||
joint_id = 24
|
||
joint_positions = []
|
||
for name in model.names:
|
||
if name == 'universe':
|
||
continue
|
||
# if name not in joint_data:
|
||
if name not in ['joint_lift_body', 'joint_body_pitch', 'Joint1_r', 'Joint2_r', 'Joint3_r', 'Joint4_r',
|
||
'Joint5_r', 'Joint6_r', 'Joint7_r', 'right_Left_1_Joint', 'right_Left_0_Joint',
|
||
'right_Left_Support_Joint', 'right_Right_1_Joint', 'right_Right_0_Joint',
|
||
'right_Right_Support_Joint']:
|
||
joint_positions.append(0)
|
||
else:
|
||
joint_positions.append(joint_data[name])
|
||
joint_positions = np.array(joint_positions)
|
||
print(joint_positions)
|
||
pinocchio.forwardKinematics(model, data, joint_positions)
|
||
J = pinocchio.computeFrameJacobian(model, data, joint_positions, joint_id)
|
||
manip = np.sqrt(np.linalg.det(np.dot(J, J.T)))
|
||
return manip
|
||
|
||
def GetObjectsOfType(self, obj_type):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.GetObjectsOfTypeReq()
|
||
req.obj_type = obj_type
|
||
response = stub.GetObjectsOfType(req)
|
||
return (response)
|
||
|
||
def AddCamera(self, camera_prim, camera_position, camera_rotation, width, height, focus_length, horizontal_aperture,
|
||
vertical_aperture, is_local):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.AddCameraReq()
|
||
req.camera_prim = camera_prim
|
||
req.camera_pose.position.x, req.camera_pose.position.y, req.camera_pose.position.z = camera_position
|
||
req.camera_pose.rpy.rw, req.camera_pose.rpy.rx, req.camera_pose.rpy.ry, req.camera_pose.rpy.rz = camera_rotation
|
||
req.focus_length = focus_length
|
||
req.horizontal_aperture = horizontal_aperture
|
||
req.vertical_aperture = vertical_aperture
|
||
req.width = width
|
||
req.height = height
|
||
req.is_local = is_local
|
||
response = stub.AddCamera(req)
|
||
return (response)
|
||
|
||
def InitRobot(self, robot_cfg, robot_usd, scene_usd, init_position=[0, 0, 0], init_rotation=[1, 0, 0, 0],
|
||
stand_type="cylinder", stand_size_x=0.1, stand_size_y=0.1):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.InitRobotReq()
|
||
req.robot_cfg_file, req.robot_usd_path, req.scene_usd_path = robot_cfg, robot_usd, scene_usd
|
||
req.robot_pose.position.x, req.robot_pose.position.y, req.robot_pose.position.z = init_position
|
||
req.robot_pose.rpy.rw, req.robot_pose.rpy.rx, req.robot_pose.rpy.ry, req.robot_pose.rpy.rz = init_rotation
|
||
req.stand_type = stand_type
|
||
req.stand_size_x = stand_size_x
|
||
req.stand_size_y = stand_size_y
|
||
response = stub.InitRobot(req)
|
||
return (response)
|
||
|
||
def DrawLine(self, point_list_1, point_list_2, colors, sizes, name):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.DrawLineReq()
|
||
for point in point_list_1:
|
||
_point = sim_observation_service_pb2.Vec3()
|
||
_point.x, _point.y, _point.z = point
|
||
req.point_list_1.append(_point)
|
||
for point in point_list_2:
|
||
_point = sim_observation_service_pb2.Vec3()
|
||
_point.x, _point.y, _point.z = point
|
||
req.point_list_2.append(_point)
|
||
for color in colors:
|
||
_color = sim_observation_service_pb2.Vec3()
|
||
_color.x, _color.y, _color.z = color
|
||
req.colors.append(_color)
|
||
for size in sizes:
|
||
req.sizes.append(size)
|
||
req.name = name
|
||
response = stub.DrawLine(req)
|
||
return (response)
|
||
|
||
def ClearLine(self, name):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.ClearLineReq()
|
||
req.name = name
|
||
response = stub.ClearLine(req)
|
||
return (response)
|
||
|
||
def SetObjectPose(self, object_info, joint_cmd, object_joint_info=None):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.SetObjectPoseReq()
|
||
for object in object_info:
|
||
object_pose = sim_observation_service_pb2.ObjectPose()
|
||
object_pose.prim_path = object["prim_path"]
|
||
object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z = object["position"]
|
||
object_pose.pose.rpy.rw, object_pose.pose.rpy.rx, object_pose.pose.rpy.ry, object_pose.pose.rpy.rz = object[
|
||
"rotation"]
|
||
req.object_pose.append(object_pose)
|
||
for pos in joint_cmd:
|
||
cmd = sim_observation_service_pb2.JointCommand()
|
||
cmd.position = pos
|
||
req.joint_cmd.append(cmd)
|
||
if object_joint_info is not None:
|
||
for joint in object_joint_info:
|
||
object_joint = sim_observation_service_pb2.ObjectJoint()
|
||
object_joint.prim_path = joint["prim_path"]
|
||
for pos in joint["joint_cmd"]:
|
||
cmd = sim_observation_service_pb2.JointCommand()
|
||
cmd.position = pos
|
||
object_joint.joint_cmd.append(cmd)
|
||
req.object_joint.append(object_joint)
|
||
response = stub.SetObjectPose(req)
|
||
return (response)
|
||
|
||
def SetCameraPose(self, camera_prim_name, camera_pose):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.SetCameraPoseReq()
|
||
req.camera_prim_name = camera_prim_name
|
||
req.camera_pose.position.x, req.camera_pose.position.y, req.camera_pose.position.z = camera_pose["position"]
|
||
req.camera_pose.rpy.rw, req.camera_pose.rpy.rx, req.camera_pose.rpy.ry, req.camera_pose.rpy.rz = camera_pose[
|
||
"rotation"]
|
||
response = stub.SetCameraPose(req)
|
||
return (response)
|
||
|
||
def SetTrajectoryList(self, trajectory_list, is_block=False):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.SetTrajectoryListReq()
|
||
req.is_block = is_block
|
||
for point in trajectory_list:
|
||
pose = sim_observation_service_pb2.SE3RpyPose()
|
||
pose.position.x, pose.position.y, pose.position.z = point[0]
|
||
pose.rpy.rw, pose.rpy.rx, pose.rpy.ry, pose.rpy.rz = point[1]
|
||
req.trajectory_point.append(pose)
|
||
response = stub.SetTrajectoryList(req)
|
||
return response
|
||
|
||
def SetTargetPoint(self, position):
|
||
stub = sim_object_service_pb2_grpc.SimObjectServiceStub(self.channel)
|
||
req = sim_object_service_pb2.SetTargetPointReq()
|
||
req.point_position.x, req.point_position.y, req.point_position.z = position
|
||
response = stub.SetTargetPoint(req)
|
||
return response
|
||
|
||
def SetMaterial(self, material_info):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.SetMaterailReq()
|
||
for mat in material_info:
|
||
print(mat)
|
||
mat_info = sim_observation_service_pb2.MaterialInfo()
|
||
mat_info.object_prim = mat["object_prim"]
|
||
mat_info.material_name = mat["material_name"]
|
||
mat_info.material_path = mat["material_path"]
|
||
if "label_name" in mat:
|
||
mat_info.label_name = mat["label_name"]
|
||
req.materials.append(mat_info)
|
||
response = stub.SetMaterial(req)
|
||
return (response)
|
||
|
||
def SetLight(self, light_info):
|
||
stub = sim_observation_service_pb2_grpc.SimObservationServiceStub(self.channel)
|
||
req = sim_observation_service_pb2.SetLightReq()
|
||
for light in light_info:
|
||
print(light)
|
||
light_cfg = sim_observation_service_pb2.LightCfg()
|
||
light_cfg.light_type = light["light_type"]
|
||
light_cfg.light_prim = light["light_prim"]
|
||
light_cfg.light_temperature = light["light_temperature"]
|
||
light_cfg.light_intensity = light["light_intensity"]
|
||
light_cfg.light_rotation.rw, light_cfg.light_rotation.rx, light_cfg.light_rotation.ry, light_cfg.light_rotation.rz = \
|
||
light["rotation"]
|
||
light_cfg.light_texture = light["texture"]
|
||
req.lights.append(light_cfg)
|
||
response = stub.SetLight(req)
|
||
return (response)
|
||
|
||
|
||
# 调用示例
|
||
def run():
|
||
rpc_client = Rpc_Client(client_host="localhost:50051")
|
||
try:
|
||
while True:
|
||
send_msg = input("input Command: ")
|
||
if send_msg == "30":
|
||
result = rpc_client.DrawLine(
|
||
point_list_1=[[np.random.uniform(-5, 5), np.random.uniform(-5, 5), np.random.uniform(-5, 5)]],
|
||
point_list_2=[[np.random.uniform(-5, 5), np.random.uniform(-5, 5), np.random.uniform(-5, 5)]],
|
||
colors=[[np.random.uniform(0, 1), np.random.uniform(0, 1), np.random.uniform(0, 1)]],
|
||
sizes=[np.random.uniform(0, 1)],
|
||
name="test"
|
||
)
|
||
if send_msg == "3111":
|
||
result = rpc_client.ClearLine(name="test")
|
||
if send_msg == "point":
|
||
rpc_client.SetTargetPoint([0, 1, 1])
|
||
if send_msg == "test_mat":
|
||
rpc_client.SetMaterial([{
|
||
"object_prim": "/World/huxing/a_keting/yangbanjian_A_016",
|
||
"material_name": "Ash",
|
||
"material_path": "materials/wood/Ash"
|
||
},
|
||
{
|
||
"object_prim": "/World/huxing/a_Base/Floor_003",
|
||
"material_name": "Ash",
|
||
"material_path": "materials/wood/Ash"
|
||
|
||
}])
|
||
rpc_client.SetLight([{
|
||
"light_type": "Distant",
|
||
"light_prim": "/World/huxing/DistantLight",
|
||
"light_temperature": 2000,
|
||
"light_intensity": 1000,
|
||
"rotation": [1, 0.5, 0.5, 0.5],
|
||
"texture": ""
|
||
},
|
||
{
|
||
"light_type": "Dome",
|
||
"light_prim": "/World/DomeLight",
|
||
"light_temperature": 6500,
|
||
"light_intensity": 1000,
|
||
"rotation": [1, 0, 0, 0],
|
||
"texture": "materials/hdri/abandoned_hall_01_4k.hdr"
|
||
},
|
||
{
|
||
"light_type": "Rect",
|
||
"light_prim": "/World/RectLight",
|
||
"light_temperature": 4500,
|
||
"light_intensity": 1000,
|
||
"rotation": [1, 0, 0, 0],
|
||
"texture": ""
|
||
},
|
||
{
|
||
"light_type": "Rect",
|
||
"light_prim": "/World/RectLight_01",
|
||
"light_temperature": 8500,
|
||
"light_intensity": 1000,
|
||
"rotation": [1, 0, 0, 0],
|
||
"texture": ""
|
||
}])
|
||
|
||
if send_msg == "908":
|
||
pose_1 = [1, 1, 1], [1, 0, 0, 0]
|
||
pose_2 = [1, 0, 1], [1, 0, 0, 0]
|
||
trajecory_list = [pose_1, pose_2] * 10
|
||
result = rpc_client.SetTrajectoryList(trajecory_list, True)
|
||
if send_msg == "31":
|
||
target_poses = []
|
||
for i in range(1):
|
||
# x = np.random.uniform(0.3, 0.7)
|
||
# y = np.random.uniform(-0.3, 0.3)
|
||
# z = np.random.uniform(0.96, 1.2)
|
||
# pose = {
|
||
# "position": [x,y,z],
|
||
# "rotation": [1,0,0,0]
|
||
# }
|
||
x = 0.57597359649470348
|
||
y = -0.45669529659303565
|
||
z = 1.0198275517174573
|
||
rw = 0.066194084385260935
|
||
rx = 0.70713063274436749
|
||
ry = 0.70071350186850612
|
||
rz = 0.0677141028599091
|
||
pose = {
|
||
"position": [x, y, z],
|
||
"rotation": [rw, rx, ry, rz]
|
||
}
|
||
target_poses.append(pose)
|
||
|
||
result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=False)
|
||
print(result)
|
||
if send_msg == "1":
|
||
# 相机
|
||
# A2W
|
||
rpc_client.capture_frame(camera_prim_path="/World/Raise_A2/base_link/Head_Camera")
|
||
result = rpc_client.capture_semantic_frame(camera_prim_path="/World/Raise_A2/base_link/Head_Camera")
|
||
print(result)
|
||
if send_msg == "2":
|
||
# 臂
|
||
x = np.random.uniform(0.3, 0.7)
|
||
y = np.random.uniform(-0.3, 0)
|
||
z = np.random.uniform(0.1, 0.5)
|
||
rpc_client.moveto(target_position=[x, y, z], target_quaternion=np.array([0.0, -0.0, -1.0, 0.0]),
|
||
arm_name="left", is_backend=True, ee_interpolation=True)
|
||
if send_msg == "20":
|
||
result = rpc_client.MultiPlan(robot_name="left", target_poses=[
|
||
[[0.5169683509992052, 0.1313259611510117, 1.1018942820728093],
|
||
[0.40020943, 0.57116637, 0.69704651, -0.16651593]],
|
||
[[0.5610938560120418, 0.048608636026916924, 1.0269891277236924],
|
||
[0.40020943, 0.57116637, 0.69704651, -0.16651593]],
|
||
[[0.5610938560120418, 0.048608636026916924, 1.2269891277236924],
|
||
[0.40020943, 0.57116637, 0.69704651, -0.16651593]]
|
||
])
|
||
print(result[0])
|
||
|
||
if send_msg == "44":
|
||
import time
|
||
# test cabinet
|
||
rpc_client.InitRobot("A2D_120s_horizon.json", "A2D.usd", "omnimanip_Simple_Room_01/simple_room.usd",
|
||
init_position=[-0.4, 0, -0.55])
|
||
rpc_client.reset()
|
||
time.sleep(1)
|
||
rpc_client.DetachObj()
|
||
rpc_client.reset()
|
||
time.sleep(1)
|
||
rpc_client.add_object(
|
||
usd_path="objects/guanglun/storagefurniture/storagefurniture011/Storagefurniture011.usd",
|
||
prim_path="/World/Objects/storagefurniture", label_name="test_storagefurniture",
|
||
target_position=np.array([
|
||
0.64,
|
||
-0.3,
|
||
0.40]), target_quaternion=np.array([0., 0., 0.70711, 0.70711]),
|
||
target_scale=np.array([1, 1, 1]), color=np.array([1, 0, 1]),
|
||
material="Plastic", mass=0.01)
|
||
time.sleep(1)
|
||
rpc_client.set_gripper_state(gripper_command="open", is_right=True, opened_width=1)
|
||
|
||
target_poses = []
|
||
start_time = time.time()
|
||
for i in range(50):
|
||
x = np.random.uniform(0, 0.2)
|
||
y = np.random.uniform(-0.3, 0.3)
|
||
z = np.random.uniform(0.96, 1.2)
|
||
pose = {
|
||
"position": [x, y, z],
|
||
"rotation": [1, x, y, z]
|
||
}
|
||
target_poses.append(pose)
|
||
result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=False)
|
||
result = rpc_client.GetIKStatus(target_poses=target_poses, is_right=True)
|
||
rpc_client.DetachObj()
|
||
print("open")
|
||
rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle1")
|
||
rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle2")
|
||
obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3")
|
||
obj_xyz = np.array(
|
||
[obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z])
|
||
ee_pose = rpc_client.GetEEPose(is_right=True)
|
||
ee_quat = np.array(
|
||
[ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz])
|
||
goal_xyz = obj_xyz.copy()
|
||
goal_xyz[0] = goal_xyz[0] + 0.3
|
||
goal_xyz[2] = goal_xyz[2] + 0.55
|
||
print(goal_xyz)
|
||
rpc_client.moveto(
|
||
target_position=goal_xyz,
|
||
target_quaternion=ee_quat,
|
||
arm_name="left",
|
||
is_backend=False,
|
||
ee_interpolation=False,
|
||
distance_frame=0.001)
|
||
obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3")
|
||
obj_xyz = np.array(
|
||
[obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z])
|
||
ee_pose = rpc_client.GetEEPose(is_right=True)
|
||
ee_quat = np.array(
|
||
[ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz])
|
||
goal_xyz = obj_xyz.copy()
|
||
rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle1")
|
||
rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle2")
|
||
rpc_client.moveto(
|
||
target_position=goal_xyz,
|
||
target_quaternion=ee_quat,
|
||
arm_name="left",
|
||
is_backend=True,
|
||
ee_interpolation=False,
|
||
distance_frame=0.001)
|
||
rpc_client.set_gripper_state(gripper_command="close", is_right=True, opened_width=0.02)
|
||
time.sleep(1)
|
||
obj_pose = rpc_client.get_object_pose("/World/Objects/storagefurniture/Storagefurniture011_Handle3")
|
||
obj_xyz = np.array(
|
||
[obj_pose.object_pose.position.x, obj_pose.object_pose.position.y, obj_pose.object_pose.position.z])
|
||
ee_pose = rpc_client.GetEEPose(is_right=True)
|
||
ee_quat = np.array(
|
||
[ee_pose.ee_pose.rpy.rw, ee_pose.ee_pose.rpy.rx, ee_pose.ee_pose.rpy.ry, ee_pose.ee_pose.rpy.rz])
|
||
goal_xyz = obj_xyz.copy()
|
||
goal_xyz[0] = goal_xyz[0] - 0.1
|
||
rpc_client.moveto(
|
||
target_position=goal_xyz,
|
||
target_quaternion=ee_quat,
|
||
arm_name="left",
|
||
is_backend=True,
|
||
ee_interpolation=False,
|
||
distance_frame=0.001)
|
||
if send_msg == "21":
|
||
rpc_client.MultiMove(robot_name="left", plan_index=0)
|
||
if send_msg == "22":
|
||
rpc_client.MultiMove(robot_name="left", plan_index=1)
|
||
if send_msg == "23":
|
||
rpc_client.MultiMove(robot_name="left", plan_index=2)
|
||
if send_msg == "24":
|
||
rpc_client.start_recording()
|
||
|
||
if send_msg == "3":
|
||
# 关节
|
||
joint_states = rpc_client.get_joint_positions().states
|
||
joint_datas = {}
|
||
for joint in joint_states:
|
||
joint_datas[joint.name] = joint.position
|
||
result = rpc_client.GetManipulability(joint_datas)
|
||
print(result)
|
||
if send_msg == "4":
|
||
# 手
|
||
rpc_client.set_gripper_state(gripper_command="open", is_right=True, opened_width=0.1)
|
||
if send_msg == "5":
|
||
# 物体
|
||
|
||
import time
|
||
# rpc_client.reset()
|
||
rpc_client.add_object(usd_path="Collected_cabinet_000/cabinet_000.usd",
|
||
prim_path="/World/Objects/cabinet", label_name="test_cabinet",
|
||
target_position=np.array([.8, 0., -0.1]),
|
||
target_quaternion=np.array([0., 0., 0.70711, 0.70711]),
|
||
target_scale=np.array([1, 1, 1]), color=np.array([1, 0, 1]),
|
||
material="Plastic", mass=0.01)
|
||
time.sleep(1)
|
||
result = rpc_client.get_object_joint("/World/Objects/cabinet")
|
||
print(result)
|
||
# x = np.random.uniform(0.2, 0.64)
|
||
# y = np.random.uniform(-0.3, 0.3)
|
||
# rpc_client.add_object(usd_path="genie3D/09.usd",
|
||
# prim_path="/World/Objects/object8", label_name="test7",
|
||
# target_position=np.array([np.random.uniform(0.2, 0.64),np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,1,0]), material="Brass", mass=1)
|
||
|
||
# rpc_client.add_object(usd_path="genie3D/02/02.usd",
|
||
# prim_path="/World/Objects/object2", label_name="test2",
|
||
# target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,1,1]))
|
||
# rpc_client.get_object_pose(prim_path="/World/Xform/box_place")
|
||
|
||
# rpc_client.add_object(usd_path="genie3D/06/06.usd",
|
||
# prim_path="/World/Objects/object6", label_name="test6",
|
||
# target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([1,0,0]))
|
||
# rpc_client.get_object_pose(prim_path="/World/Xform/box_pick_00")
|
||
|
||
# rpc_client.add_object(usd_path="genie3D/01/01.usd",
|
||
# prim_path="/World/Objects/object1", label_name="test1",
|
||
# target_position=np.array([np.random.uniform(0.2, 0.64), np.random.uniform(-0.3, 0.3),1.2]), target_quaternion=np.array([0.0,-0.0,-1.0, 0.0]), target_scale=np.array([0.01,0.01,0.01]), color=np.array([0,0,1]))
|
||
# result = rpc_client.get_object_pose(prim_path="/World/Raise_A2_W_T1/head_link/visuals")
|
||
if send_msg == "6":
|
||
# 获取全部信息
|
||
rpc_client.get_joint_positions()
|
||
if send_msg == "7":
|
||
rpc_client.set_gripper_state(gripper_command="close", is_right=True, opened_width=0.02)
|
||
if send_msg == "8":
|
||
result = rpc_client.get_observation(
|
||
data_keys={
|
||
'camera': {
|
||
'camera_prim_list': [
|
||
'/World/Raise_A2/base_link/Head_Camera'
|
||
],
|
||
'render_depth': True,
|
||
'render_semantic': True
|
||
},
|
||
'pose': [
|
||
'/World/Raise_A2/base_link/Head_Camera'
|
||
],
|
||
'joint_position': True,
|
||
'gripper': True
|
||
}
|
||
)
|
||
print(result["camera"])
|
||
if send_msg == "9":
|
||
rpc_client.start_recording(data_keys={
|
||
'camera': {
|
||
'camera_prim_list': [
|
||
'/World/Raise_A2/base_link/Head_Camera'
|
||
],
|
||
'render_depth': True,
|
||
'render_semantic': True
|
||
},
|
||
'pose': [
|
||
'/World/Raise_A2/base_link/Head_Camera'
|
||
],
|
||
'joint_position': True,
|
||
'gripper': True
|
||
}, fps=30, task_name="test"
|
||
|
||
)
|
||
time.sleep(1)
|
||
rpc_client.stop_recording()
|
||
rpc_client.SendTaskStatus(False)
|
||
if send_msg == "10":
|
||
rpc_client.stop_recording()
|
||
if send_msg == "11":
|
||
rpc_client.reset()
|
||
if send_msg == "13":
|
||
result = rpc_client.Exit()
|
||
print(result)
|
||
|
||
if send_msg == '112':
|
||
rpc_client.InitRobot(robot_cfg="Franka.json", robot_usd="Franka/franka.usd",
|
||
scene_usd="Pick_Place_Franka_Yellow_Table.usd")
|
||
|
||
if send_msg == '113':
|
||
position = [1.2, 0., 1.2]
|
||
rotation = [0.61237, 0.35355, 0.35355, 0.61237]
|
||
rotation = [0.65328, 0.2706, 0.2706, 0.65328]
|
||
width = 640
|
||
height = 480
|
||
rpc_client.AddCamera("/World/Sensors/Head_Camera", position, rotation, width, height, 18.14756, 20.955,
|
||
15.2908, False)
|
||
response = rpc_client.capture_frame(camera_prim_path="/World/Sensors/Head_Camera")
|
||
# cam_info
|
||
cam_info = {
|
||
"W": response.color_info.width,
|
||
"H": response.color_info.height,
|
||
"K": np.array(
|
||
[
|
||
[response.color_info.fx, 0, response.color_info.ppx],
|
||
[0, response.color_info.fy, response.color_info.ppy],
|
||
[0, 0, 1],
|
||
]
|
||
),
|
||
"scale": 1,
|
||
}
|
||
# rgb
|
||
# rgb = np.frombuffer(response.color_image.data, dtype=np.uint8).reshape(cam_info["H"], cam_info["W"], 4)[
|
||
# :, :, :3
|
||
# ]
|
||
# import cv2
|
||
# cv2.imwrite("head_img.png", rgb)
|
||
|
||
if send_msg == '114':
|
||
position = [0.05, 0., 0.]
|
||
rotation = [0.06163, 0.70442, 0.70442, 0.06163]
|
||
width = 640
|
||
height = 480
|
||
rpc_client.AddCamera("/panda/panda_hand/Hand_Camera_1", position, rotation, width, height, 18.14756,
|
||
20.955, 15.2908, True)
|
||
response = rpc_client.capture_frame(camera_prim_path="/panda/panda_hand/Hand_Camera_1")
|
||
# cam_info
|
||
cam_info = {
|
||
"W": response.color_info.width,
|
||
"H": response.color_info.height,
|
||
"K": np.array(
|
||
[
|
||
[response.color_info.fx, 0, response.color_info.ppx],
|
||
[0, response.color_info.fy, response.color_info.ppy],
|
||
[0, 0, 1],
|
||
]
|
||
),
|
||
"scale": 1,
|
||
}
|
||
# rgb
|
||
rgb = np.frombuffer(response.color_image.data, dtype=np.uint8).reshape(cam_info["H"], cam_info["W"], 4)[
|
||
:, :, :3
|
||
]
|
||
import cv2
|
||
cv2.imwrite("hand_img.png", rgb)
|
||
|
||
except Exception as e:
|
||
print("failed.{}".format(e))
|
||
return False
|
||
|
||
|
||
if __name__ == '__main__':
|
||
run() |