Conversion from geometric ROS messages into SE(3) @param msg: Message to transform. Acceptable types - C{geometry_msgs/Pose}, C{geometry_msgs/PoseStamped}, C{geometry_msgs/Transform}, or C{geometry_msgs/TransformStamped} @return: a 4x4 SE(3) matrix as a numpy array @note: Throws Typ
(msg)
| 61 | |
| 62 | |
| 63 | def msg_to_se3(msg): |
| 64 | """Conversion from geometric ROS messages into SE(3) |
| 65 | |
| 66 | @param msg: Message to transform. Acceptable types - C{geometry_msgs/Pose}, C{geometry_msgs/PoseStamped}, |
| 67 | C{geometry_msgs/Transform}, or C{geometry_msgs/TransformStamped} |
| 68 | @return: a 4x4 SE(3) matrix as a numpy array |
| 69 | @note: Throws TypeError if we receive an incorrect type. |
| 70 | """ |
| 71 | if isinstance(msg, Pose): |
| 72 | p, q = pose_to_pq(msg) |
| 73 | elif isinstance(msg, PoseStamped): |
| 74 | p, q = pose_stamped_to_pq(msg) |
| 75 | elif isinstance(msg, Transform): |
| 76 | p, q = transform_to_pq(msg) |
| 77 | elif isinstance(msg, TransformStamped): |
| 78 | p, q = transform_stamped_to_pq(msg) |
| 79 | else: |
| 80 | raise TypeError("Invalid type for conversion to SE(3)") |
| 81 | norm = np.linalg.norm(q) |
| 82 | if np.abs(norm - 1.0) > 1e-3: |
| 83 | raise ValueError( |
| 84 | "Received un-normalized quaternion (q = {0:s} ||q|| = {1:3.6f})".format( |
| 85 | str(q), np.linalg.norm(q))) |
| 86 | elif np.abs(norm - 1.0) > 1e-6: |
| 87 | q = q / norm |
| 88 | g = tr.quaternion_matrix(q) |
| 89 | g[0:3, -1] = p |
| 90 | return g |
no test coverage detected