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 handle_create(self, *args): try: robot_local_device_name = self.vue["$data"].robot_selected intrinsic_calib = self.vue["$data"].camera_intrinsic_selected extrinsic_calib = self.vue["$data"].camera_extrinsic_selected image_sequence_global_name = self.vue[ "$data"].image_sequence_selected aruco_dict = self.vue["$data"].aruco_dict_selected aruco_tag_id = int(self.vue["$data"].aruco_tag_id) aruco_tag_size = float(self.vue["$data"].aruco_tag_size) xyz = np.zeros((3, ), dtype=np.float64) rpy = np.zeros((3, ), dtype=np.float64) xyz[0] = float(self.vue["$data"].marker_pose_x) xyz[1] = float(self.vue["$data"].marker_pose_y) xyz[2] = float(self.vue["$data"].marker_pose_z) rpy[0] = float(self.vue["$data"].marker_pose_r_r) rpy[1] = float(self.vue["$data"].marker_pose_r_p) rpy[2] = float(self.vue["$data"].marker_pose_r_y) rpy = np.deg2rad(rpy) robot_calib = self.core.device_manager.get_device_subscription( "vision_robot_calibration").GetDefaultClient() geom_util = GeometryUtil(client_obj=robot_calib) marker_pose = geom_util.xyz_rpy_to_pose(xyz, rpy) self.core.create_task(do_calibration(robot_local_device_name,intrinsic_calib,extrinsic_calib,\ image_sequence_global_name,aruco_dict,aruco_tag_id, aruco_tag_size, marker_pose, self.new_name,self.core)) except: traceback.print_exc()
def geometry_pose_new(x,y,z,r_r,r_p,r_y): """ Create a new pose using XYZ-RPY format. Units are meters and degrees Parameters: * x (float): X position in meters * y (float): Y position in meters * z (float): Z position in meters * r_r (float): Roll in degrees * r_p (float): Pitch in degrees * r_y (float): Yaw in degrees Return (Pose): Pose named array """ xyz = np.array([x,y,z],dtype=np.float64) rpy = np.deg2rad(np.array([r_r,r_p,r_y],dtype=np.float64)) geom_util = GeometryUtil(node = PyriSandboxContext.node) return geom_util.xyz_rpy_to_pose(xyz,rpy)
from RobotRaconteurCompanion.Util.GeometryUtil import GeometryUtil import numpy as np import cv2 d = DeviceManagerClient('rr+tcp://localhost:59902?service=device_manager', autoconnect=False) d.refresh_devices(1) d.connect_device("vision_robot_calibration") calibration_service = d.get_device_client("vision_robot_calibration", 1) geom_util = GeometryUtil(client_obj=calibration_service) marker_pose = geom_util.xyz_rpy_to_pose( np.array([0.0393, -0.0091, 0.055]), np.array(np.deg2rad([90.0, 0.0, 180.0]))) ret = calibration_service.calibrate_robot_origin( "robot", "camera_intrinsic_calibration", "camera_extrinsic_calibration", "robot_calib0", "DICT_6X6_250", 150, 0.0316, marker_pose, "") # "robot_origin_calibration0" image_util = ImageUtil(client_obj=calibration_service) geom_util = GeometryUtil(client_obj=calibration_service) T = geom_util.named_pose_to_rox_transform(ret.robot_pose.pose) print(T) print(ret) for img1 in ret.display_images: