easy_motion.easy_motion_utils module

easy_motion.easy_motion_utils.affine_to_transform(affine: numpy.ndarray, frame_id: str, child_frame_id: str) → geometry_msgs.msg._transform_stamped.TransformStamped

Convert a 4x4 affine transformation matrix into a ROS2 TransformStamped.

affinenp.ndarray

4x4 transformation matrix. The top-left 3x3 block is the rotation, the top-right 3x1 column is the translation.

frame_idstr

The name of the parent frame (TransformStamped.header.frame_id).

child_frame_idstr

The name of the child frame (TransformStamped.child_frame_id).

TransformStamped

ROS2 TransformStamped message containing the given transform.

easy_motion.easy_motion_utils.build_affine(translation: Optional[Iterable] = None, rotation: Optional[Iterable] = None) → numpy.ndarray

Build an affine matrix from a quaternion and a translation.

Parameters

• rotation – The quaternion as [w, x, y, z]

• translation – The translation as [x, y, z]

Returns

The quaternion and the translation array

easy_motion.easy_motion_utils.pose_stamped_to_affine(pose: geometry_msgs.msg._pose_stamped.PoseStamped) → numpy.ndarray

Convert a PoseStamped to a 4x4 affine transformation matrix.

posePoseStamped

The pose to convert.

np.ndarray

4x4 affine transformation matrix, with rotation in the top-left 3x3 and translation in the last column.

easy_motion.easy_motion_utils.quat_to_rot_xyzw(q)

Quaternion [x, y, z, w] -> 3x3 rotation matrix (SciPy order).

easy_motion.easy_motion_utils.transform_to_affine(transform: geometry_msgs.msg._transform_stamped.TransformStamped) → numpy.ndarray

Convert a ROS2 TransformStamped message into a 4x4 affine transformation matrix.

transformTransformStamped

The ROS2 TransformStamped message containing translation and rotation.

np.ndarray

4x4 affine transformation matrix where: - The top-left 3x3 block is the rotation matrix. - The top-right 3x1 column is the translation vector. - The last row is [0, 0, 0, 1].

easy_motion.easy_motion_utils.transform_to_pose_stamped(transform: geometry_msgs.msg._transform_stamped.TransformStamped) → geometry_msgs.msg._pose_stamped.PoseStamped

Convert a TransformStamped to a PoseStamped.

transformTransformStamped

The transform to convert.

PoseStamped

The converted pose.