hofee/gen_data_agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

solve dependencies problem

Browse Source
This commit is contained in:
hofee
2025-09-05 15:49:00 +08:00
parent 12a6b47969
commit 21fbd5a323
114 changed files with 11337 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

962
data_gen_dependencies/client.py Normal file
View File

@@ -0,0 +1,962 @@
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()