drims2_motion_server.drims2_utils module
- drims2_motion_server.drims2_utils.affine_to_transform(affine: ndarray, frame_id: str, child_frame_id: str) TransformStamped
Convert a 4x4 affine transformation matrix into a ROS2 TransformStamped.
Parameters
- 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).
Returns
- TransformStamped
ROS2 TransformStamped message containing the given transform.
- drims2_motion_server.drims2_utils.build_affine(translation: Iterable | None = None, rotation: Iterable | None = None) 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
- drims2_motion_server.drims2_utils.pose_stamped_to_affine(pose: PoseStamped) ndarray
Convert a PoseStamped to a 4x4 affine transformation matrix.
Parameters
- posePoseStamped
The pose to convert.
Returns
- np.ndarray
4x4 affine transformation matrix, with rotation in the top-left 3x3 and translation in the last column.
- drims2_motion_server.drims2_utils.quat_to_rot_xyzw(q)
Quaternion [x, y, z, w] -> 3x3 rotation matrix (SciPy order).
- drims2_motion_server.drims2_utils.transform_to_affine(transform: TransformStamped) ndarray
Convert a ROS2 TransformStamped message into a 4x4 affine transformation matrix.
Parameters
- transformTransformStamped
The ROS2 TransformStamped message containing translation and rotation.
Returns
- 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].