def geometry_pose_component_set(pose, component_name, value): """ Set an XYZ-RPY component of a pose. This function does not modify in place. It returns a new pose. Parameters: * pose (Pose): The pose * component_name (str): The component to get. May be `X`, `Y`, `Z`, `R_R`, `R_P`, or `R_Y` * value (float): The new pose component value in meters or degrees Return (Pose): The new pose with updated value """ geom_util = GeometryUtil(node = PyriSandboxContext.node) xyz,rpy = geom_util.pose_to_xyz_rpy(_convert_to_pose(pose)) rpy = np.rad2deg(rpy) if component_name == "X": xyz[0] = value elif component_name == "Y": xyz[1] = value elif component_name == "Z": xyz[2] = value elif component_name == "R_R": rpy[0] = value elif component_name == "R_P": rpy[1] = value elif component_name == "R_Y": rpy[2] = value else: assert False, "Invalid pose component" rpy = np.deg2rad(rpy) return geom_util.xyz_rpy_to_pose(xyz,rpy)
def geometry_pose_component_get(pose, component_name): """ Get an XYZ-RPY component of a pose. Parameters: * pose (Pose): The pose * component_name (str): The component to get. May be `X`, `Y`, `Z`, `R_R`, `R_P`, or `R_Y` Return (float): The pose value """ geom_util = GeometryUtil(node = PyriSandboxContext.node) xyz,rpy = geom_util.pose_to_xyz_rpy(_convert_to_pose(pose)) rpy = np.rad2deg(rpy) if component_name == "X": return float(xyz[0]) if component_name == "Y": return float(xyz[1]) if component_name == "Z": return float(xyz[2]) if component_name == "R_R": return float(rpy[0]) if component_name == "R_P": return float(rpy[1]) if component_name == "R_Y": return float(rpy[2]) assert False, "Invalid pose component"
center = RRN.NewStructure("com.robotraconteur.geometry.NamedPose2D", c) pose2d_dtype = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Pose2D", c) size2d_dtype = RRN.GetNamedArrayDType("com.robotraconteur.geometry.Size2D", c) center.pose = np.zeros((1,),dtype=pose2d_dtype) center.pose[0]["position"]["x"] = 990 center.pose[0]["position"]["y"] = 64 center.pose[0]["orientation"] = np.deg2rad(10) b.center = center size = np.zeros((1,),dtype=size2d_dtype) size[0]["width"] = 100 size[0]["height"] = 100 b.size = size #res = c.detect_aruco_stored_image("test18", "camera_intrinsic_calibration", "camera_extrinsic_calibration0", "DICT_4X4_1000", 200, 0.0375, None) res = c.detect_aruco_camera_capture("camera", "camera_calibration_intrinsic", "camera_calibration_extrinsic", "DICT_4X4_1000", 200, 0.0375, None) m = res.detected_markers[0] print(m.marker_corners) print(res.detected_markers) xyz,rpy = geom_util.pose_to_xyz_rpy(m.marker_pose.pose) T = geom_util.pose_to_rox_transform(m.marker_pose.pose) img_util = ImageUtil(client_obj = c) img = img_util.compressed_image_to_array(res.display_image) cv2.imshow("",img) cv2.waitKey() cv2.destroyAllWindows()