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: