def goto_pose(self, pose: Pose, linear: bool = True, relative: bool = False) -> None: """Commands the YuMi to goto the given pose. Parameters ---------- pose : RigidTransform linear : bool, optional If True, will use MoveL in RAPID to ensure linear path. Otherwise use MoveJ in RAPID, which does not ensure linear path. Defaults to True relative : bool, optional If True, will use goto_pose_relative by computing the delta pose from current pose to target pose. Defaults to False """ if relative: cur_pose = self.get_pose() delta_pose = Pose.from_tr_matrix(cur_pose.inversed().as_tr_matrix() * pose.as_tr_matrix()) rot = np.rad2deg(quaternion.as_euler_angles(delta_pose.orientation.as_quaternion())) self.goto_pose_delta(delta_pose.position, rot) else: body = self._get_pose_body(pose) if linear: cmd = CmdCodes.goto_pose_linear else: cmd = CmdCodes.goto_pose req = self._construct_req(cmd, body) self._request(req, timeout=self._motion_timeout)
def calibrate_robot( robot_joints: list[Joint], robot_pose: Pose, camera_pose: Pose, camera_parameters: CameraParameters, robot: URDF, depth_image: Image, draw_results: bool = False, ) -> Pose: logger.info("Creating robot model...") fk = robot.visual_trimesh_fk(cfg={joint.name: joint.value for joint in robot_joints}) robot_tr_matrix = robot_pose.as_tr_matrix() res_mesh = o3d.geometry.TriangleMesh() for tm in fk: pose = fk[tm] mesh = o3d.geometry.TriangleMesh( vertices=o3d.utility.Vector3dVector(tm.vertices), triangles=o3d.utility.Vector3iVector(tm.faces) ) mesh.transform(np.dot(robot_tr_matrix, pose)) mesh.compute_vertex_normals() res_mesh += mesh mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1) sim_pcd = res_mesh.sample_points_uniformly(int(1e5)) camera_tr_matrix = camera_pose.as_tr_matrix() camera_matrix = camera_parameters.as_camera_matrix() depth = depth_image_to_np(depth_image) wh = (depth_image.width, depth_image.height) dist = np.array(camera_parameters.dist_coefs) newcameramtx, _ = cv2.getOptimalNewCameraMatrix(camera_matrix, dist, wh, 1, wh) depth = cv2.undistort(depth, camera_matrix, dist, None, newcameramtx) real_pcd = o3d.geometry.PointCloud.create_from_depth_image( o3d.geometry.Image(depth), o3d.camera.PinholeCameraIntrinsic( depth_image.width, depth_image.height, camera_parameters.fx, camera_parameters.fy, camera_parameters.cx, camera_parameters.cy, ), ) real_pcd.transform(camera_tr_matrix) bb = sim_pcd.get_axis_aligned_bounding_box() real_pcd = real_pcd.crop(bb.scale(1.25, bb.get_center())) # logger.info("Outlier removal...") # real_pcd = real_pcd.remove_radius_outlier(nb_points=50, radius=0.01)[0] sim_pcd = sim_pcd.select_by_index(sim_pcd.hidden_point_removal(np.array(list(camera_pose.position)), 500)[1]) print("Estimating normals...") real_pcd.estimate_normals() if draw_results: o3d.visualization.draw_geometries([sim_pcd, real_pcd, mesh_frame]) threshold = 1.0 trans_init = np.identity(4) """ evaluation = o3d.pipelines.registration.evaluate_registration( sim_pcd, real_pcd, threshold, trans_init, ) logger.info(evaluation) """ logger.info("Applying point-to-plane robust ICP...") loss = o3d.pipelines.registration.TukeyLoss(k=0.25) p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane(loss) reg_p2p = o3d.pipelines.registration.registration_icp( sim_pcd, real_pcd, threshold, trans_init, p2l, o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=100), ) logger.info(reg_p2p) logger.debug(reg_p2p.transformation) # TODO somehow check if calibration is ok if draw_results: draw_registration_result(sim_pcd, real_pcd, trans_init, reg_p2p.transformation) robot_tr_matrix = np.dot(reg_p2p.transformation, robot_tr_matrix) pose = Pose.from_tr_matrix(robot_tr_matrix) logger.info("Done") return pose