# 发送指令 # 001. 拍照 # 002. 左手/右手移动指定位姿 # 003. 全身关节移动到指定角度 # 004. 获取夹爪的位姿 # 005. 获取任意物体的位姿 # 006. 添加物体 import time import numpy as np from scipy.spatial.transform import Rotation from data_gen_dependencies.client import Rpc_Client from data_gen_dependencies.robot import Robot from data_gen_dependencies.utils import ( get_quaternion_wxyz_from_rotation_matrix, get_rotation_matrix_from_quaternion, matrix_to_euler_angles, get_quaternion_from_euler, get_quaternion_from_rotation_matrix ) def normalize(vec, eps=1e-12): norm = np.linalg.norm(vec, axis=-1) norm = np.maximum(norm, eps) out = (vec.T / norm).T return out def rot6d_to_mat(d6): a1, a2 = d6[..., :3], d6[..., 3:] b1 = normalize(a1) b2 = a2 - np.sum(b1 * a2, axis=-1, keepdims=True) * b1 b2 = normalize(b2) b3 = np.cross(b1, b2, axis=-1) out = np.stack((b1, b2, b3), axis=-2) return out def pose10d_to_mat(d10): pos = d10[..., :3] d6 = d10[..., 3:] rotmat = rot6d_to_mat(d6) out = np.zeros(d10.shape[:-1] + (4, 4), dtype=d10.dtype) out[..., :3, :3] = rotmat out[..., :3, 3] = pos out[..., 3, 3] = 1 return out class IsaacSimRpcRobot(Robot): def __init__(self, robot_cfg="Franka_120s.json", scene_usd="Pick_Place_Franka_Yellow_Table.usd", client_host="localhost:50051", position=[0, 0, 0], rotation=[1, 0, 0, 0], stand_type="cylinder", stand_size_x=0.1, stand_size_y=0.1): robot_urdf = robot_cfg.split(".")[0] + ".urdf" self.client = Rpc_Client(client_host, robot_urdf) self.client.InitRobot(robot_cfg=robot_cfg, robot_usd="", scene_usd=scene_usd, init_position=position, init_rotation=rotation, stand_type=stand_type, stand_size_x=stand_size_x, stand_size_y=stand_size_y) self.cam_info = None self.robot_gripper_2_grasp_gripper = np.array([ [0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0] ]) # TODO config file if "A2D" in robot_cfg: self.joint_names = { "left": ['Joint1_l', 'Joint2_l', 'Joint3_l', 'Joint4_l', 'Joint5_l', 'Joint6_l', 'Joint7_l'], "right": ['Joint1_r', 'Joint2_r', 'Joint3_r', 'Joint4_r', 'Joint5_r', 'Joint6_r', 'Joint7_r'] } else: self.joint_names = { "left": ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7'], "right": ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7'] } self.init_position = position self.init_rotation = rotation self.setup() def reset(self): self.target_object = None self.client.reset() time.sleep(0.5) # self.client.set_joint_positions([0, -0.569, 0, -2.809, 0, 3.04, 0.741, 1.0, 0.0, 1.0, -1.0, 0.0, 1.0, 0.0, 0.0], False) # self.close_gripper(id="right") time.sleep(0.5) pass def setup(self): self.target_object = None # set robot init state def get_observation(self, data_keys): """ Example data_keys = { 'camera': { 'camera_prim_list': [ '/World/Raise_A2_W_T1/head_link/D455_Solid/TestCameraDepth' ], 'render_depth': True, 'render_semantic': True }, 'pose': [ '/World/Raise_A2_W_T1/head_link/D455_Solid/TestCameraDepth' ], 'joint_position': True, 'gripper': True } """ observation = {} observation = self.client.get_observation(data_keys) if "camera" in data_keys: render_depth = data_keys["camera"]["render_depth"] if "render_depth" in data_keys["camera"] else False render_semantic = ( data_keys["camera"]["render_semantic"] if "render_semantic" in data_keys["camera"] else False ) cam_data = {} for cam_prim in data_keys["camera"]["camera_prim_list"]: cam_data[cam_prim] = {} response = self.client.capture_frame(camera_prim_path=cam_prim) # 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, } cam_data[cam_prim]["cam_info"] = cam_info # c2w c2w = self.get_prim_world_pose(cam_prim, camera=True) cam_data[cam_prim]['c2w'] = c2w # rgb rgb = np.frombuffer(response.color_image.data, dtype=np.uint8).reshape(cam_info["H"], cam_info["W"], 4)[ :, :, :3 ] cam_data[cam_prim]["image"] = rgb # depth if render_depth: depth = np.frombuffer(response.depth_image.data, dtype=np.float32).reshape( cam_info["H"], cam_info["W"] ) cam_data[cam_prim]["depth"] = depth # semantic if render_semantic: response = self.client.capture_semantic_frame(camera_prim_path=cam_prim) prim_id = {} for label in response.label_dict: name, id = label.label_name, label.label_id prim = '/World/Objects/' + name prim_id[prim] = id mask = np.frombuffer(response.semantic_mask.data, dtype=np.int32).reshape(cam_info['H'], cam_info['W']) cam_data[cam_prim]['semantic'] = { 'prim_id': prim_id, 'mask': mask } observation["camera"] = cam_data if "pose" in data_keys: pose_data = {} for obj_prim in data_keys["pose"]: pose_data[obj_prim] = self.get_prim_world_pose(prim_path=obj_prim) observation["pose"] = pose_data if "joint_position" in data_keys: # TODO 是否要区分双臂？ joint_position = self.client.get_joint_positions() observation["joint_position"] = joint_position if "gripper" in data_keys and data_keys["gripper"] == True: gripper_state = {} gripper_state["left"] = self.client.get_gripper_state(is_right=False) gripper_state["right"] = self.client.get_gripper_state(is_right=True) observation["gripper"] = gripper_state return observation def open_gripper(self, id="left", width=0.1): is_Right = True if id == "right" else False if width is None: width = 0.1 self.client.set_gripper_state(gripper_command="open", is_right=is_Right, opened_width=width) self.client.DetachObj() def close_gripper(self, id="left", force=50): is_Right = True if id == "right" else False self.client.set_gripper_state(gripper_command="close", is_right=is_Right, opened_width=0.00) # if self.target_object is not None and is_Right: # self.client.DetachObj() # self.client.AttachObj(prim_paths=['/World/Objects/'+self.target_object]) def move_pose(self, target_pose, type, arm="right", **kwargs): # import ipdb;ipdb.set_trace() if type.lower() == 'trajectory': content = { "type": 'trajectory_4x4_pose', "data": target_pose, } else: if type == 'AvoidObs': type = 'ObsAvoid' elif type == 'Normal': type = 'Simple' content = { "type": "matrix", "matrix": target_pose, 'trajectory_type': type, "arm": arm } return self.move(content) def set_gripper_action(self, action, arm="right"): # self.gripper_server.set_gripper_action(action) assert arm in ['left', 'right'] time.sleep(0.3) if action is None: return if action == 'open': self.open_gripper(id=arm, width=0.1) elif action == 'close': self.close_gripper(id=arm) def move(self, content): """ type: str, 'matrix' or 'joint' 'pose': np.array, 4x4 'joint': np.array, 1xN """ type = content["type"] arm_name = content.get('arm', "right") print(arm_name) if type == "matrix": if isinstance(content["matrix"], list): content["matrix"] = np.array(content["matrix"]) R, T = content["matrix"][:3, :3], content["matrix"][:3, 3] quat_wxyz = get_quaternion_from_euler(matrix_to_euler_angles(R), order="ZYX") ee_interpolation = False target_position = T if "trajectory_type" in content and content["trajectory_type"] == "Simple": is_backend = True target_rotation = quat_wxyz elif "trajectory_type" in content and content["trajectory_type"] == "Straight": is_backend = True target_rotation = quat_wxyz ee_interpolation = True else: is_backend = False init_rotation_matrix = get_rotation_matrix_from_quaternion(self.init_rotation) translation_matrix = np.zeros((4, 4)) translation_matrix[:3, :3] = init_rotation_matrix translation_matrix[:3, 3] = self.init_position translation_matrix[3, 3] = 1 target_matrix = np.linalg.inv(translation_matrix) @ content["matrix"] target_rotation_matrix, target_position = target_matrix[:3, :3], target_matrix[:3, 3] target_rotation = get_quaternion_from_euler(matrix_to_euler_angles(target_rotation_matrix), order="ZYX") print(f"target_position is{target_position}") print(f"target_rotation is{target_rotation}") # import ipdb;ipdb.set_trace() # inverse_local_orientation = [self.init_rotation[0], -1*self.init_rotation[1], -1*self.init_rotation[2], -1*self.init_rotation[3]] # local_position = quat_multiplication(inverse_local_orientation,T-self.init_position) # target_position = local_position state = ( self.client.moveto( target_position=target_position, target_quaternion=target_rotation, arm_name=arm_name, is_backend=is_backend, ee_interpolation=ee_interpolation ).errmsg == "True" ) print("move!", T, quat_wxyz, state) elif type == "quat": if "trajectory_type" in content and content["trajectory_type"] == "Simple": is_backend = True else: is_backend = False state = ( self.client.moveto( target_position=content["xyz"], target_quaternion=content["quat"], arm_name="left", is_backend=is_backend, ).errmsg == "True" ) elif type == "joint": state = self.client.set_joint_positions(content["position"]) elif type == "euler": is_backend = True T_curr = self.get_ee_pose() xyzrpy_input = content["xyzrpy"] xyz_curr = T_curr[:3, 3] rpy_curr = Rotation.from_matrix(T_curr[:3, :3]).as_euler("xyz") incr = content.get("incr", False) if incr: xyz_tgt = xyz_curr + np.array(xyzrpy_input[:3]) rpy_tgt = rpy_curr + np.array(xyzrpy_input[3:]) quat_tgt = get_quaternion_from_rotation_matrix( Rotation.from_euler("xyz", rpy_tgt).as_matrix() ) else: raise NotImplementedError state = ( self.client.moveto( target_position=xyz_tgt, target_quaternion=quat_tgt, arm_name="left", is_backend=is_backend, ).errmsg == "True" ) elif type == 'move_pose_list': action_list = content["data"] for action in action_list: T_curr = content["ee_transform"] # current ee2world gripper_curr = content["gripper_position"] action_pose10d = action[:-1] gripper_tgt = action[-1] xyz_curr = T_curr[:3, 3] rotation_curr = T_curr[:3, :3] euler_curr = Rotation.from_matrix(rotation_curr).as_euler("xyz") action_repr = content.get("action_repr", "rela") if action_repr == 'delta': mat_tgt = pose10d_to_mat(action_pose10d) xyz_delta = mat_tgt[:3, 3] rotation_delta = mat_tgt[:3, :3] xyz_tgt = xyz_curr + np.array(xyz_delta[:3]) rotation_tgt = np.dot(rotation_curr, rotation_delta) euler_tgt = Rotation.from_matrix(rotation_tgt).as_euler("xyz") quat_tgt = get_quaternion_from_rotation_matrix(rotation_tgt) else: raise RuntimeError() print(f"xyz_curr: {xyz_curr}, euler_curr: {euler_curr}, gripper_curr: {gripper_curr}") print(f"xyz_tgt: {xyz_tgt}, euler_tgt: {euler_tgt}, gripper_tgt: {gripper_tgt}") start_time = time.time() state = ( self.client.moveto( target_position=xyz_tgt, target_quaternion=quat_tgt, arm_name="right", is_backend=True, ).errmsg == "True" ) print(f"move state {state}, move executed in {(time.time() - start_time):.3f} s") xyz_curr = self.get_ee_pose()[:3, 3] euler_curr = Rotation.from_matrix(self.get_ee_pose()[:3, :3]).as_euler("xyz") print(f"xyz_curr: {xyz_curr}, euler_curr: {euler_curr}") print( f"xyz dist: {(np.linalg.norm(xyz_curr - xyz_tgt)):.4f}, rpy dist: {(np.linalg.norm(euler_curr - euler_tgt)):.4f}") start_time = time.time() if gripper_curr > 0.5 and gripper_tgt < 0.5: self.close_gripper(id="right", force=50) print(f"Close gripper executed in {(time.time() - start_time):.3f} s") elif gripper_curr < 0.5 and gripper_tgt > 0.5: self.open_gripper(id="right", width=0.1) print(f"Open gripper executed in {(time.time() - start_time):.3f} s") elif type.lower() == 'trajectory_4x4_pose': waypoint_list = content["data"] traj = [] for pose in waypoint_list: xyz = pose[:3, 3] quat_wxyz = get_quaternion_from_rotation_matrix(pose[:3, :3]) pose = list(xyz), list(quat_wxyz) traj.append(pose) print("Set Trajectory ! ") self.client.SetTrajectoryList(traj, is_block=True) # self.client.SetTrajectoryList(traj) # err: not block state = True else: raise NotImplementedError return state def get_prim_world_pose(self, prim_path, camera=False): rotation_x_180 = np.array([[1.0, 0.0, 0.0, 0], [0.0, -1.0, 0.0, 0], [0.0, 0.0, -1.0, 0], [0, 0, 0, 1]]) response = self.client.get_object_pose(prim_path=prim_path) x, y, z = ( response.object_pose.position.x, response.object_pose.position.y, response.object_pose.position.z, ) quat_wxyz = np.array( [ response.object_pose.rpy.rw, response.object_pose.rpy.rx, response.object_pose.rpy.ry, response.object_pose.rpy.rz, ] ) rot_mat = get_rotation_matrix_from_quaternion(quat_wxyz) pose = np.eye(4) pose[:3, :3] = rot_mat pose[:3, 3] = np.array([x, y, z]) if camera: pose = pose @ rotation_x_180 return pose def pose_from_cam_to_robot(self, pose2cam): """transform pose from cam-coordinate to robot-coordinate""" cam2world = self.get_prim_world_pose(prim_path=self.base_camera, camera=True) pose2world = cam2world @ pose2cam return pose2world def pose_from_world_to_cam(self, pose2world): """transform pose from world-coordinate to cam-coordinate""" cam2world = self.get_prim_world_pose(prim_path=self.base_camera, camera=True) pose2cam = np.linalg.inv(cam2world) @ pose2world return pose2cam def decode_gripper_pose(self, gripper_pose): """Decode gripper-pose at cam-coordinate to end-pose at robot-coordinate""" gripper_pose = self.pose_from_cam_to_robot(gripper_pose) angle = 0 # np.pi / 2 rot_z = np.array( [ [np.cos(angle), -np.sin(angle), 0], [np.sin(angle), np.cos(angle), 0], [0, 0, 1], ] ) flange2gripper = np.eye(4) flange2gripper[:3, :3] = rot_z flange2gripper[2, 3] = -0.01 return gripper_pose @ flange2gripper def get_ee_pose(self, id="right", **kwargs): state = self.client.GetEEPose(is_right=id == "right") xyz = np.array( [ state.ee_pose.position.x, state.ee_pose.position.y, state.ee_pose.position.z, ] ) quat = np.array( [ state.ee_pose.rpy.rw, state.ee_pose.rpy.rx, state.ee_pose.rpy.ry, state.ee_pose.rpy.rz, ] ) pose = np.eye(4) pose[:3, 3] = xyz pose[:3, :3] = get_rotation_matrix_from_quaternion(quat) return pose def check_ik(self, pose, id="right", **kwargs): xyz, quat = pose[:3, 3], get_quaternion_from_rotation_matrix(pose[:3, :3]) state = self.client.GetIKStatus(target_position=xyz, target_rotation=quat, is_right=id == "right") return state.isSuccess def solve_ik(self, pose, arm="right", type="Simple", **kwargs): single_mode = len(pose.shape) == 2 if single_mode: pose = pose[np.newaxis, ...] xyz, quat = pose[:, :3, 3], get_quaternion_wxyz_from_rotation_matrix(pose[:, :3, :3]) target_poses = [] for i in range(len(pose)): pose = { "position": xyz[i], "rotation": quat[i] } target_poses.append(pose) ObsAvoid = type == 'ObsAvoid' or type == 'AvoidObs' result = self.client.GetIKStatus(target_poses=target_poses, is_right=arm == 'right', ObsAvoid=ObsAvoid) # True: isaac, False: curobo ik_result = [] jacobian_score = [] joint_positions = [] joint_names = [] for state in result: ik_result.append(state["status"]) jacobian_score.append(state["Jacobian"]) joint_positions.append(state["joint_positions"]) joint_names.append(state["joint_names"]) # result = np.array(result) if single_mode: ik_result = ik_result[0] jacobian_score = jacobian_score[0] joint_positions = joint_positions[0] joint_names = joint_names[0] info = { "jacobian_score": np.array(jacobian_score), "joint_positions": np.array(joint_positions), "joint_names": np.array(joint_names) } return np.array(ik_result), info