pycram.datastructures.pose

pycram.datastructures.pose#

Attributes#

Point

Classes#

`Vector3`	A 3D vector with x, y and z coordinates.
`Quaternion`	A quaternion with x, y, z and w components.
`Pose`	A pose in 3D space.
`Header`	A header with a timestamp.
`Vector3Stamped`	A Vector3 with an attached ROS Header (timestamp and frame).
`PoseStamped`	A pose in 3D space with a timestamp.
`Transform`	A pose in 3D space.
`TransformStamped`	A pose in 3D space with a timestamp.
`GraspPose`	A pose from which a grasp can be performed along with the respective arm and grasp description.

Module Contents#

class pycram.datastructures.pose.Vector3#

Bases: pycram.has_parameters.HasParameters

A 3D vector with x, y and z coordinates.

x: float = 0#

y: float = 0#

z: float = 0#

euclidean_distance(other: typing_extensions.Self) → float#

The euclidian distance between this vector and another vector.

Parameters:: other – The other vector to calculate the distance to.
Returns:: The euclidian distance

ros_message()#

Convert the vector to a ROS message of type Vector3.

Returns:: The ROS message.

to_list() → typing_extensions.List[float]#

Convert the vector to a list.

Returns:: A list containing the x, y and z coordinates.

to_spatial_type(reference_frame: semantic_digital_twin.world_description.world_entity.Body = None) → semantic_digital_twin.spatial_types.spatial_types.Vector3#

round(decimals: int = 4)#

Rounds the coordinates of the vector to the specified number of decimal places.

Parameters:: decimals – Number of decimal places to round to.

almost_equal(other: typing_extensions.Self, tolerance: float = 1e-06) → bool#

Check if two vectors are almost equal within a given tolerance.

Parameters:

other – The other vector to compare to.
tolerance – The tolerance for the comparison as number of decimal places.

Returns:

True if the vectors are almost equal, False otherwise.

vector_to_position(other: typing_extensions.Self) → Vector3#

Calculates a vector from this vector to another vector.

Parameters:: other – The vector to calculate the vector to.
Returns:: A new vector between this vector and the other vector.

to_numpy() → numpy.ndarray#

Convert the vector to a numpy array.

Returns:: A numpy array containing the x, y and z coordinates.

__add__(other: typing_extensions.Self) → Vector3#

Adds two vectors together.

Parameters:: other – The other vector to add.
Returns:: A new vector that is the sum of this vector and the other vector.

__sub__(other: typing_extensions.Self) → Vector3#

Subtracts two vectors.

Parameters:: other – The other vector to subtract.
Returns:: A new vector that is the difference of this vector and the other vector.

__mul__(other: float) → Vector3#

Multiplies the vector by a scalar.

Parameters:: other – The scalar to multiply by.
Returns:: A new vector that is the product of this vector and the scalar.

__rmul__(other: float) → Vector3#

Multiplies the vector by a scalar (right multiplication).

Parameters:: other – The scalar to multiply by.
Returns:: A new vector that is the product of this vector and the scalar.

classmethod from_list(vector: typing_extensions.List[float]) → typing_extensions.Self#

Factory to create a Vector3 from a list of coordinates.

Parameters:: vector – The list of coordinates.
Returns:: A new Vector3 object.

class pycram.datastructures.pose.Quaternion#

Bases: pycram.has_parameters.HasParameters

A quaternion with x, y, z and w components.

x: float = 0#

y: float = 0#

z: float = 0#

w: float = 1#

__post_init__()#

normalize()#: Normalize the quaternion in-place.

ros_message()#

to_list() → typing_extensions.List[float]#

Convert the quaternion to a list.

Returns:: A list containing the x, y, z and w components.

to_numpy() → numpy.ndarray#

Convert the quaternion to a numpy array.

Returns:: A numpy array containing the x, y, z and w components.

to_spatial_type() → semantic_digital_twin.spatial_types.spatial_types.Quaternion#

Convert the quaternion to a SpatialQuaternion.

Returns:: A SpatialQuaternion object containing the x, y, z and w components.

round(decimals: int = 4)#

Rounds the components of the quaternion to the specified number of decimal places.

Parameters:: decimals – The number of decimal places to round to.

almost_equal(other: typing_extensions.Self, tolerance: float = 1e-06) → bool#

Check if two quaternions are almost equal within a given tolerance.

Parameters:

other – The other quaternion to compare to.
tolerance – The tolerance for the comparison as number of decimal places.

Returns:

True if the quaternions are almost equal, False otherwise.

__mul__(other: typing_extensions.Self) → Quaternion#

Multiplies two quaternions together.

Parameters:: other – The other quaternion to multiply with.
Returns:: A new quaternion that is the product of this quaternion and the other quaternion.

classmethod from_list(quaternion: typing_extensions.List[float]) → typing_extensions.Self#

Factory to create a Quaternion from a list of components.

Parameters:: quaternion – A list of components [x, y, z, w].
Returns:: A new Quaternion object.

classmethod from_matrix(matrix: numpy.ndarray) → typing_extensions.Self#

Create a Quaternion from a 3x3 rotation matrix.

Parameters:: matrix – A 3x3 rotation matrix as numpy array.
Returns:: A Quaternion object created from the matrix.

__eq__(other)#

class pycram.datastructures.pose.Pose#

Bases: pycram.has_parameters.HasParameters

A pose in 3D space.

position: Vector3#

orientation: Quaternion#

__repr__()#

ros_message()#

Convert the pose to a ROS message of type Pose.

Returns:: The ROS message.

to_list()#

Convert the pose to a list of [position, orientation].

Returns:: A list containing the position and orientation of this pose.

to_spatial_type() → semantic_digital_twin.spatial_types.spatial_types.TransformationMatrix#

round(decimals: int = 4)#

Rounds the components of the pose (position and orientation) to the specified number of decimal places.

Parameters:: decimals – The number of decimal places to round to.

almost_equal(other: Pose, position_tolerance: float = 1e-06, orientation_tolerance: float = 1e-05) → bool#

Check if two poses are almost equal within given tolerances for position and orientation.

Parameters:

other – The other pose to compare to.
position_tolerance – Tolerance for position comparison as number of decimal places.
orientation_tolerance – Tolerance for orientation comparison as number of decimal places.

Returns:

True if the poses are almost equal, False otherwise.

__eq__(other: typing_extensions.Self) → bool#

Check if two poses are equal. Uses almost_equal with a tolerance of 1e-4 for both position and orientation.

Parameters:: other – The other pose to compare to.
Returns:: True if the poses are equal, False otherwise.

classmethod from_matrix(matrix: numpy.ndarray) → typing_extensions.Self#

Create a Pose from a 4x4 transformation matrix.

Parameters:: matrix – A 4x4 transformation matrix as numpy array.
Returns:: A pose object created from the matrix.

classmethod from_list(position: typing_extensions.List[float], orientation: typing_extensions.List[float]) → typing_extensions.Self#

Factory to create a Pose from a list of position and orientation.

Parameters:

position – List of position [x, y, z].
orientation – List of orientation [x, y, z, w].

Returns:

A new Pose object.

class pycram.datastructures.pose.Header#

A header with a timestamp.

frame_id: semantic_digital_twin.world_description.world_entity.Body = None#

stamp: datetime.datetime#

sequence: int = 0#

ros_message()#

Convert the header to a ROS message of type Header.

Returns:: The ROS message.

__deepcopy__(memo)#

class pycram.datastructures.pose.Vector3Stamped#

Bases: Vector3

A Vector3 with an attached ROS Header (timestamp and frame). Inherits all vector operations and adds frame/time metadata.

header: Header#

property frame_id#

__repr__()#

ros_message()#

Convert to a ROS Vector3Stamped message.

Returns:: The ROS message.

classmethod from_ros_message(message)#

Create a Vector3Stamped from a ROS message.

Parameters:: message – The Vector3Stamped ROS message.
Returns:: A new Vector3Stamped object.

to_spatial_type() → semantic_digital_twin.spatial_types.spatial_types.Vector3#

class pycram.datastructures.pose.PoseStamped#

Bases: pycram.has_parameters.HasParameters

A pose in 3D space with a timestamp.

pose: Pose#

header: Header#

property position#

property orientation#

property frame_id#

__repr__()#

ros_message()#

Convert the pose to a ROS message of type PoseStamped.

Returns:: The ROS message.

classmethod from_ros_message(message: ROSPoseStamped) → typing_extensions.Self#

Create a PoseStamped from a ROS message.

Parameters:: message – The PoseStamped ROS message.
Returns:: A new PoseStamped object created from the ROS message.

classmethod from_list(position: typing_extensions.Optional[typing_extensions.List[float]] = None, orientation: typing_extensions.Optional[typing_extensions.List[float]] = None, frame: typing_extensions.Optional[semantic_digital_twin.world_description.world_entity.Body] = None) → typing_extensions.Self#

Factory to create a PoseStamped from a list of position and orientation.

Parameters:

position – Position as a list of [x, y, z].
orientation – Orientation as a list of [x, y, z, w].
frame – Frame in which the pose is defined.

Returns:

A new PoseStamped object.

classmethod from_matrix(matrix: numpy.ndarray, frame: semantic_digital_twin.world_description.world_entity.Body) → typing_extensions.Self#

Create a PoseStamped from a 4x4 transformation matrix and a frame.

Parameters:

matrix – A 4x4 transformation matrix as numpy array.
frame – The frame in which the pose is defined.

Returns:

A PoseStamped object created from the matrix and frame.

classmethod from_spatial_type(spatial_type: semantic_digital_twin.spatial_types.spatial_types.TransformationMatrix) → typing_extensions.Self#

Create a PoseStamped from a SpatialTransformationMatrix and a frame.

Parameters:: spatial_type – A SpatialTransformationMatrix object.
Returns:: A PoseStamped object created from the spatial type and frame.

to_transform_stamped(child_link_id: semantic_digital_twin.world_description.world_entity.Body) → TransformStamped#

Converts the PoseStamped to a TransformStamped given a frame to which the transform is pointing.

Parameters:: child_link_id – Frame to which the transform is pointing.
Returns:: A TransformStamped object.

to_spatial_type() → semantic_digital_twin.spatial_types.spatial_types.TransformationMatrix#

Converts the PoseStamped to a SpatialTransformationMatrix.

Returns:: A SpatialTransformationMatrix object representing the pose in 3D space.

round(decimals: int = 4)#

Rounds the components of the pose (position and orientation) to the specified number of decimal places.

Parameters:: decimals – Number of decimal places to round to.

to_list()#

Convert the pose to a list of [position, orientation, frame_id].

Returns:: A list of [pose, frame_id].

almost_equal(other: PoseStamped, position_tolerance: float = 1e-06, orientation_tolerance: float = 1e-05) → bool#

Check if two PoseStamped objects are almost equal within given tolerances for position and orientation and if the frame_id is the same.

Parameters:

other – The other PoseStamped object to compare to.
position_tolerance – Tolerance for position comparison as number of decimal places.
orientation_tolerance – Tolerance for orientation comparison as number of decimal places.

Returns:

True if the PoseStamped objects are almost equal, False otherwise.

rotate_by_quaternion(quaternion: typing_extensions.List[float])#

Rotates the orientation of the pose by a given quaternion.

Parameters:: quaternion – A list representing the quaternion [x, y, z, w].

is_facing_2d_axis(pose_b: PoseStamped, axis: typing_extensions.Optional[pycram.datastructures.enums.AxisIdentifier] = AxisIdentifier.X, threshold_deg=82) → typing_extensions.Tuple[bool, float]#

Check if this pose is facing another pose along a specific axis (X or Y) within a given angular threshold.

Parameters:

pose_b – The target pose to compare against.
axis – The axis to check alignment with (‘x’ or ‘y’). Defaults to ‘x’.
threshold_deg – The maximum angular difference in degrees to consider as ‘facing’. Defaults to 82 degrees.

Returns:

Tuple of (True/False if facing, signed angular difference in radians).

is_facing_x_or_y(pose_b: PoseStamped) → bool#

Check if this pose is facing another pose along either the X or Y axis within a default angular threshold.

Parameters:: pose_b – The target pose to compare against.
Returns:: True if this pose is facing the target along either X or Y axis, False otherwise.

class pycram.datastructures.pose.Transform#

Bases: Pose

A pose in 3D space.

property translation#

property rotation#

to_matrix() → numpy.ndarray#

Converts the transform to a 4x4 transformation matrix.

Returns:: A numpy array representing the transformation matrix.

__invert__() → Transform#

Inverts the transform, returning a new Transform object.

Returns:: The inverted Transform object.

__mul__(other: Transform) → Transform#

Multiplies two transforms together, returning a new Transform object.

Parameters:: other – The other Transform object to multiply with.
Returns:: A new Transform object that is the product of this transform and the other transform.

classmethod from_pose(pose: Pose) → typing_extensions.Self#

Create a Transform from a Pose object.

Parameters:: pose – The pose to convert to a Transform.
Returns:: A new Transform object created from the Pose.

ros_message()#

Convert the transform to a ROS message of type Transform.

Returns:: The ROS message.

class pycram.datastructures.pose.TransformStamped#

Bases: PoseStamped

A pose in 3D space with a timestamp.

child_frame_id: semantic_digital_twin.world_description.world_entity.Body = ''#: Target frame id of the transform.

pose: Transform#: The transform of the transform.

property transform: Transform#

property translation#

property rotation#

__invert__() → typing_extensions.Self#

Inverts the transform, returning a new TransformStamped object which points from child_frame_id to frame_id.

Returns:: A new TransformStamped object that is the inverse of this transform.

__mul__(other) → typing_extensions.Self#

Multiplies two TransformStamped objects together, returning a new TransformStamped object.

Parameters:: other – The other TransformStamped object to multiply with.
Returns:: A new TransformStamped object that is the product of this transform and the other transform.

__deepcopy__(memo)#

classmethod from_list(translation: typing_extensions.List[float] = None, rotation: typing_extensions.List[float] = None, frame: semantic_digital_twin.world_description.world_entity.Body = None, child_frame_id: semantic_digital_twin.world_description.world_entity.Body = None) → typing_extensions.Self#

Factory to create a TransformStamped from a list of position and orientation.

Parameters:

translation – Translation as a list of [x, y, z].
rotation – Rotation as a list of [x, y, z, w].
frame – Original frame in which the transform is defined.
child_frame_id – Target frame id of the transform.

Returns:

A new TransformStamped object.

ros_message()#

Convert the TransformStamped to a ROS message of type TransformStamped.

Returns:: The ROS message.

to_pose_stamped() → PoseStamped#

Converts the TransformStamped to a PoseStamped object.

Returns:: A PoseStamped object created from the TransformStamped.

to_spatial_type() → semantic_digital_twin.spatial_types.spatial_types.TransformationMatrix#

Converts the TransformStamped to a SpatialTransformationMatrix.

Returns:: A SpatialTransformationMatrix object representing the transform in 3D space.

inverse_times(other: TransformStamped) → typing_extensions.Self#

Multiply this TransformStamped by the inverse of another TransformStamped. Essentially, this is the same as “subtracting” another TransformStamped from this one.

Parameters:: other – The other TransformStamped to subtract.
Returns:: A new TransformStamped object that is the result of this transform minus the other transform.

class pycram.datastructures.pose.GraspPose#

Bases: PoseStamped

A pose from which a grasp can be performed along with the respective arm and grasp description.

arm: pycram.datastructures.enums.Arms = None#: Arm corresponding to the grasp pose.

grasp_description: pycram.datastructures.grasp.GraspDescription = None#: Grasp description corresponding to the grasp pose.

pycram.datastructures.pose.Point#