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()