RobotRaconteurCompanion.Util.GeometryUtil

Utility class for working with Robot Raconteur geometry types. The utility functions convert between Robot Raconteur geometry types and Python types. numpy and general_robotics_toolbox types are used where appropriate.

A simple example:

from RobotRaconteur.Client import *
from RobotRaconteur.Companion.Util import GeometryUtil

c = RRN.ConnectService('rr+tcp://localhost:22222?service=MyRobot')
geom_util = GeometryUtil(client_obj=c)

# Create GeometryUtil using the connected object

# Get a position vector
v = c.getf_position()

# Convert to numpy array
v2 = vector3_to_xyz(v)

# Convert a rpy xyz to a Robot Raconteur com.robotraconteur.geometry.Transform
t = rpy_xyz_to_transform(np.deg2rad([10,20,30],[0.1,0.2,0.3]))

The GeometryUtil class works with com.robotraconteur.geometry, com.robotraconteur.geometryf, and com.robotraconteur.geometryi. These standard service types use float64, float32, and int32 respectively. Use the dtype argument to specify the type to use for numpy arrays. The default is float64.

GeometryUtil

class RobotRaconteurCompanion.Util.GeometryUtil.GeometryUtil(node=None, client_obj=None)

R_to_quaternion(R, dtype=<class 'numpy.float64'>)

Converts a 3x3 rotation matrix to a Robot Raconteur Quaternion

Parameters:

R (numpy.ndarray) – The 3x3 rotation matrix
dtype (com.robotraconteur.geometry.Quaternion) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Quaternion

Return type:

com.robotraconteur.geometry.Quaternion

abgxyz_to_vector6(abgxyz, dtype=<class 'numpy.float64'>)

Converts a 6 element vector to a Robot Raconteur Vector6

Parameters:

abgxyz (numpy.ndarray) – The 6D vector
dtype (com.robotraconteur.geometry.Vector6) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Vector6

Return type:

com.robotraconteur.geometry.Vector6

array_to_spatial_acceleration(spatial_acceleration, dtype=<class 'numpy.float64'>)

Converts a 6 element spatial acceleration vector to a Robot Raconteur SpatialAcceleration

Parameters:

spatial_acceleration (numpy.ndarray) – The 6 element spatial acceleration vector
dtype (com.robotraconteur.geometry.SpatialAcceleration) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur SpatialAcceleration

Return type:

com.robotraconteur.geometry.SpatialAcceleration

array_to_spatial_velocity(spatial_velocity, dtype=<class 'numpy.float64'>)

Converts a 6 element spatial velocity vector to a Robot Raconteur SpatialVelocity

Parameters:

spatial_velocity (numpy.ndarray) – The 6 element spatial velocity vector
dtype (com.robotraconteur.geometry.SpatialVelocity) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur SpatialVelocity

Return type:

com.robotraconteur.geometry.SpatialVelocity

array_to_wrench(wrench, dtype=<class 'numpy.float64'>)

Converts a 6 element wrench vector to a Robot Raconteur Wrench

Parameters:

wrench (numpy.ndarray) – The 6 element wrench vector
dtype (com.robotraconteur.geometry.Wrench) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Wrench

Return type:

com.robotraconteur.geometry.Wrench

named_pose_to_rox_transform(rr_named_pose)

Convert a Robot Raconteur NamedPose to a general_robotics_toolbox Transform

Parameters:: rr_named_pose (com.robotraconteur.geometry.NamedPose) – The Robot Raconteur NamedPose
Returns:: The general_robotics_toolbox Transform
Return type:: general_robotics_toolbox.Transform

named_pose_to_xyz_rpy(transform)

Convert a Robot Raconteur NamedPose to a 3 element position vector and 3 element roll-pitch-yaw vector in radians

Parameters:: transform (com.robotraconteur.geometry.NamedPose) – The Robot Raconteur NamedPose
Returns:: The 3 element position vector and 3 element roll-pitch-yaw vector
Return type:: Tuple[numpy.ndarray, numpy.ndarray]

named_transform_to_rox_transform(rr_named_transform)

Convert a Robot Raconteur NamedTransform to a general_robotics_toolbox Transform

Parameters:: rr_named_transform (com.robotraconteur.geometry.NamedTransform) – The Robot Raconteur NamedTransform
Returns:: The general_robotics_toolbox Transform
Return type:: general_robotics_toolbox.Transform

named_transform_to_xyz_rpy(transform)

Converts a Robot Raconteur NamedTransform to a 3 element position vector and 3 element roll-pitch-yaw vector in radians.

Parameters:: transform (com.robotraconteur.geometry.NamedTransform) – The Robot Raconteur NamedTransform
Returns:: The 3 element position vector and 3 element roll-pitch-yaw vector in radians
Return type:: Tuple[numpy.ndarray, numpy.ndarray]

point2d_to_xy(rr_point2d)

Converts a Robot Raconteur Point2D to a 2 element vector

Parameters:: rr_point2d (com.robotraconteur.geometry.Point2D) – The Robot Raconteur Point2D
Returns:: The 2D vector
Return type:: numpy.ndarray

point_to_xyz(rr_point)

Converts a Robot Raconteur Point to a 3 element vector

Parameters:: rr_point (com.robotraconteur.geometry.Point) – The Robot Raconteur Point
Returns:: The 3 element vector
Return type:: numpy.ndarray

pose_to_rox_transform(rr_pose)

Convert a Robot Raconteur Pose to a general_robotics_toolbox Transform

Parameters:: rr_pose (com.robotraconteur.geometry.Pose) – The Robot Raconteur Pose
Returns:: The general_robotics_toolbox Transform
Return type:: general_robotics_toolbox.Transform

pose_to_xyz_rpy(transform)

Converts a Robot Raconteur Pose to a 3 element position vector and 3 element roll-pitch-yaw vector

Parameters:: transform (com.robotraconteur.geometry.Pose) – The Robot Raconteur Pose
Returns:: The 3 element position vector and 3 element roll-pitch-yaw vector
Return type:: Tuple[numpy.ndarray, numpy.ndarray]

q_to_quaternion(q, dtype=<class 'numpy.float64'>)

Converts a 4 element vector to a Robot Raconteur Quaternion. The order of the elements is [w,x,y,z]

Parameters:

q (numpy.ndarray) – The 4 element vector
dtype (com.robotraconteur.geometry.Quaternion) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Quaternion

Return type:

com.robotraconteur.geometry.Quaternion

quaternion_to_R(rr_quaternion)

Converts a Robot Raconteur Quaternion to a 3x3 rotation matrix

Parameters:: rr_quaternion (com.robotraconteur.geometry.Quaternion) – The Robot Raconteur Quaternion
Returns:: The 3x3 rotation matrix
Return type:: numpy.ndarray

quaternion_to_q(rr_quaternion)

Converts a Robot Raconteur Quaternion to a 4 element vector. The order of the elements is [w,x,y,z]

Parameters:: rr_quaternion (com.robotraconteur.geometry.Quaternion) – The Robot Raconteur Quaternion
Returns:: The 4 element vector
Return type:: numpy.ndarray

quaternion_to_rpy(rr_quaternion)

Convert a Robot Raconteur Quaternion to a roll-pitch-yaw vector in radians

Parameters:: rr_quaternion (com.robotraconteur.geometry.Quaternion) – The Robot Raconteur Quaternion
Returns:: The roll-pitch-yaw vector
Return type:: numpy.ndarray

rox_transform_to_named_pose(rox_transform, dtype=<class 'numpy.float64'>)

Converts a general_robotics_toolbox Transform to a Robot Raconteur NamedPose. The client_frame_id and parent_frame_id must be set in the general_robotics_toolbox Transform.

Parameters:

rox_transform (general_robotics_toolbox.Transform) – The general_robotics_toolbox Transform
dtype (com.robotraconteur.geometry.NamedPose) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur NamedPose

Return type:

com.robotraconteur.geometry.NamedPose

rox_transform_to_named_transform(rox_transform, dtype=<class 'numpy.float64'>)

Converts a general_robotics_toolbox Transform to a Robot Raconteur NamedTransform. The rox_transform must have parent_frame_id and child_frame_id set.

Parameters:

rox_transform (general_robotics_toolbox.Transform) – The general_robotics_toolbox Transform
dtype (com.robotraconteur.geometry.NamedTransform) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur NamedTransform

Return type:

com.robotraconteur.geometry.NamedTransform

rox_transform_to_pose(rox_transform, dtype=<class 'numpy.float64'>)

Converts a general_robotics_toolbox Transform to a Robot Raconteur Pose

Parameters:

rox_transform (general_robotics_toolbox.Transform) – The general_robotics_toolbox Transform
dtype (com.robotraconteur.geometry.Pose) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Pose

Return type:

com.robotraconteur.geometry.Pose

rox_transform_to_transform(rox_transform, dtype=<class 'numpy.float64'>)

Converts a general_robotics_toolbox Transform to a Robot Raconteur Transform

Parameters:

rox_transform (general_robotics_toolbox.Transform) – The general_robotics_toolbox Transform
dtype (com.robotraconteur.geometry.Transform) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Transform

rpy_to_quaternion(rpy, dtype=<class 'numpy.float64'>)

Convert a roll-pitch-yaw vector in radians to a Robot Raconteur Quaternion

Parameters:

rpy (numpy.ndarray) – The roll-pitch-yaw vector
dtype (com.robotraconteur.geometry.Quaternion) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Quaternion

Return type:

com.robotraconteur.geometry.Quaternion

size2d_to_wh(rr_size2d)

Converts a Robot Raconteur Size2D to a 2 element vector

Parameters:: rr_size2d (com.robotraconteur.geometry.Size2D) – The Robot Raconteur Size2D
Returns:: The 2 element vector
Return type:: numpy.ndarray

size_to_whd(rr_size)

Converts a Robot Raconteur Size to a 3 element vector

Parameters:: rr_size (com.robotraconteur.geometry.Size) – The Robot Raconteur Size
Returns:: The 3 element vector
Return type:: numpy.ndarray

spatial_acceleration_to_array(rr_spatial_acceleration)

Converts a Robot Raconteur SpatialAcceleration to a 6 element spatial acceleration vector

Parameters:: rr_spatial_acceleration (com.robotraconteur.geometry.SpatialAcceleration) – The Robot Raconteur SpatialAcceleration
Returns:: The 6 element spatial acceleration vector
Return type:: numpy.ndarray

spatial_velocity_to_array(rr_spatial_velocity)

Converts a Robot Raconteur SpatialVelocity to a 6 element spatial velocity vector

Parameters:: rr_spatial_velocity (com.robotraconteur.geometry.SpatialVelocity) – The Robot Raconteur SpatialVelocity
Returns:: The 6 element spatial velocity vector
Return type:: numpy.ndarray

transform_to_rox_transform(rr_transform)

Converts a Robot Raconteur Transform to a general_robotics_toolbox Transform

Parameters:: rr_transform (com.robotraconteur.geometry.Transform) – The Robot Raconteur Transform
Returns:: The general_robotics_toolbox Transform
Return type:: general_robotics_toolbox.Transform

transform_to_xyz_rpy(transform)

Converts a Robot Raconteur Transform to a 3 element position vector and 3 element roll-pitch-yaw vector in radians.

Parameters:: transform (com.robotraconteur.geometry.Transform) – The Robot Raconteur Transform
Returns:: The 3 element position vector and 3 element roll-pitch-yaw vector in radians
Return type:: Tuple[numpy.ndarray, numpy.ndarray]

vector2_to_xy(rr_vector2)

Converts a Robot Raconteur Vector2 to a 2 element vector

Parameters:: rr_vector2 (com.robotraconteur.geometry.Vector2) – The Robot Raconteur Vector2
Returns:: The 2D vector
Return type:: numpy.ndarray

vector3_to_xyz(rr_vector3)

” Converts a Robot Raconteur Vector3 to a 3 element vector

Parameters:: rr_vector3 (com.robotraconteur.geometry.Vector3) – The Robot Raconteur Vector3
Returns:: The 3D vector
Return type:: numpy.ndarray

vector6_to_abgxyz(rr_vector6)

Converts a Robot Raconteur Vector6 to a 6 element vector

Parameters:: rr_vector6 (com.robotraconteur.geometry.Vector6) – The Robot Raconteur Vector6
Returns:: The 6D vector
Return type:: numpy.ndarray

wh_to_size2d(wh, dtype=<class 'numpy.float64'>)

Converts a 2 element vector to a Robot Raconteur Size2D

Parameters:

wh (numpy.ndarray) – The 2 element vector
dtype (com.robotraconteur.geometry.Size2D) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Size2D

Return type:

com.robotraconteur.geometry.Size2D

whd_to_size(whd, dtype=<class 'numpy.float64'>)

Converts a 3 element vector to a Robot Raconteur Size

Parameters:

whd (numpy.ndarray) – The 3 element vector
dtype (com.robotraconteur.geometry.Size) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Size

Return type:

com.robotraconteur.geometry.Size

wrench_to_array(rr_wrench)

Converts a Robot Raconteur Wrench to a 6 element wrench vector

Parameters:: rr_wrench (com.robotraconteur.geometry.Wrench) – The Robot Raconteur Wrench
Returns:: The 6 element wrench vector
Return type:: numpy.ndarray

xy_to_point2d(xy, dtype=<class 'numpy.float64'>)

Converts a 2 element vector to a Robot Raconteur Point2D

Parameters:

xy (numpy.ndarray) – The 2 element vector
dtype (com.robotraconteur.geometry.Point2D) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

xy_to_vector2(xy, dtype=<class 'numpy.float64'>)

Converts a 2 element vector to a Robot Raconteur Vector2 type

Parameters:

xy (numpy.ndarray) – The 2D vector
dtype (com.robotraconteur.geometry.Vector2) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

xyz_rpy_to_named_pose(xyz, rpy, parent_frame_id, child_frame_id, dtype=<class 'numpy.float64'>)

Converts a 3 element position vector and 3 element roll-pitch-yaw vector in radians to a Robot Raconteur NamedPose

Parameters:

xyz (numpy.ndarray) – The 3 element position vector
rpy (numpy.ndarray) – The 3 element roll-pitch-yaw vector in radians
parent_frame_id (str) – The parent frame identifier
child_frame_id (str) – The child frame identifier

xyz_rpy_to_named_transform(xyz, rpy, parent_frame_id, child_frame_id, dtype=<class 'numpy.float64'>)

Converts a 3 element position vector and 3 element roll-pitch-yaw vector in radians to a Robot Raconteur NamedTransform

Parameters:

xyz (numpy.ndarray) – The 3 element position vector
rpy (numpy.ndarray) – The 3 element roll-pitch-yaw vector in radians
parent_frame_id (str) – The parent frame name
child_frame_id (str) – The child frame name
dtype (com.robotraconteur.geometry.NamedTransform) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur NamedTransform

Return type:

com.robotraconteur.geometry.NamedTransform

xyz_rpy_to_pose(xyz, rpy, dtype=<class 'numpy.float64'>)

Converts a 3 element position vector and 3 element roll-pitch-yaw vector in radians to a Robot Raconteur Pose

Parameters:

xyz (numpy.ndarray) – The 3 element position vector
rpy (numpy.ndarray) – The 3 element roll-pitch-yaw vector in radians
dtype (com.robotraconteur.geometry.Pose) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Pose

Return type:

com.robotraconteur.geometry.Pose

xyz_rpy_to_transform(xyz, rpy, dtype=<class 'numpy.float64'>)

Converts a 3 element position vector and 3 element roll-pitch-yaw vector in radians to a Robot Raconteur Transform

Parameters:

xyz (numpy.ndarray) – The 3 element position vector
rpy (numpy.ndarray) – The 3 element roll-pitch-yaw vector in radians
dtype (com.robotraconteur.geometry.Transform) – The numpy dtype of the transform. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Transform

Return type:

com.robotraconteur.geometry.Transform

xyz_to_point(xyz, dtype=<class 'numpy.float64'>)

Converts a 3 element vector to a Robot Raconteur Point

Parameters:

xyz (numpy.ndarray) – The 3 element vector
dtype (com.robotraconteur.geometry.Point) – The numpy dtype of the vector. Must be float64, float32, or int32. Defaults to float64

Returns:

The Robot Raconteur Point

Return type:

com.robotraconteur.geometry.Point