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.
`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.

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.

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.

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.

copy() → typing_extensions.Self#

Create a deep copy of the pose.

Returns:: A new Pose object that is a copy of this pose.

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: str = 'map'#

stamp: datetime.datetime#

sequence: int = 0#

ros_message()#

Convert the header to a ROS message of type Header.

Returns:: The ROS message.

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[str] = 'map') → 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.

to_transform_stamped(child_link_id: str) → 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.

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.

copy() → typing_extensions.Self#

Create a deep copy of the PoseStamped object.

Returns:: A new PoseStamped object that is a copy of this object.

to_list()#

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

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

static calculate_closest_faces(pose_to_robot_vector: Vector3, specified_grasp_axis: typing_extensions.Optional[pycram.datastructures.enums.AxisIdentifier] = None) → typing_extensions.Tuple[pycram.datastructures.grasp.GraspDescription, pycram.datastructures.grasp.GraspDescription]#

Determines the faces of the object based on the input vector.

If specified_grasp_axis is None, it calculates the primary and secondary faces based on the vector’s magnitude determining which sides of the object are most aligned with the robot. This will either be the x, y plane for side faces or the z axis for top/bottom faces. If specified_grasp_axis is provided, it only considers the specified axis and calculates the faces aligned with that axis.

Parameters:

pose_to_robot_vector – A 3D vector representing one of the robot’s axes in the pose’s frame, with irrelevant components set to np.nan.
specified_grasp_axis – Specifies a specific axis (e.g., X, Y, Z) to focus on.

Returns:

A tuple of two Grasp enums representing the primary and secondary faces.

calculate_grasp_descriptions(robot: pycram.world_concepts.world_object.Object, grasp_alignment: typing_extensions.Optional[pycram.datastructures.grasp.PreferredGraspAlignment] = None) → typing_extensions.List[pycram.datastructures.grasp.GraspDescription]#

This method determines the possible grasp configurations (approach axis and vertical alignment) of the self, taking into account the self’s orientation, position, and whether the gripper should be rotated by 90°.

Parameters:: robot – The robot for which the grasp configurations are being calculated.
Returns:: A sorted list of GraspDescription instances representing all grasp permutations.

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: str = ''#: 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.

classmethod from_list(translation: typing_extensions.List[float] = None, rotation: typing_extensions.List[float] = None, frame: str = 'map', child_frame_id='') → 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.

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#