Line data Source code
1 : #ifndef CONTROLS_UTILS_TRANSFORMS_H
2 : #define CONTROLS_UTILS_TRANSFORMS_H
3 :
4 : #include <utils/NodeUtils.h>
5 : #include <utils/Types.h>
6 :
7 : /**
8 : * @brief Wraps an angle to the range (-pi, pi]
9 : *
10 : * @param r [in] Angle in radians
11 : *
12 : * @return Wrapped angle in radians
13 : */
14 1 : double wrap(double r);
15 :
16 : /**
17 : * @brief Converts a Pose to a 6-element XYZRPY vector
18 : *
19 : * @param pose [in] Pose containing position and quaternion orientation
20 : *
21 : * @return 6-element vector [x, y, z, roll, pitch, yaw]
22 : */
23 1 : Vector6d pose2n(const Pose& pose);
24 :
25 : /**
26 : * @brief Converts a 6-element XYZRPY vector to a Pose
27 : *
28 : * @param pose [in] 6-element vector [x, y, z, roll, pitch, yaw]
29 : *
30 : * @return Pose with position and quaternion orientation
31 : */
32 1 : Pose n2pose(const Vector6d& pose);
33 :
34 : /**
35 : * @brief Converts a 3-element RPY vector from radians to degrees
36 : *
37 : * @param rpy [in] Angles in radians
38 : *
39 : * @return Angles in degrees
40 : */
41 1 : Vector3d rad2deg(const Vector3d& rpy);
42 :
43 : /**
44 : * @brief Converts a 3-element RPY vector from degrees to radians
45 : *
46 : * @param rpy [in] Angles in degrees
47 : *
48 : * @return Angles in radians
49 : */
50 1 : Vector3d deg2rad(const Vector3d& rpy);
51 :
52 : /**
53 : * @brief Builds a 3x3 rotation matrix from roll-pitch-yaw angles
54 : *
55 : * @param rpy [in] Roll, pitch, yaw in radians
56 : *
57 : * @return ZYX rotation matrix
58 : */
59 1 : Matrix3d rpy2rot(const Vector3d& rpy);
60 :
61 : /**
62 : * @brief Builds a 3x3 rotation matrix from individual roll-pitch-yaw angles
63 : *
64 : * @param roll [in] Roll angle in radians
65 : * @param pitch [in] Pitch angle in radians
66 : * @param yaw [in] Yaw angle in radians
67 : *
68 : * @return ZYX rotation matrix
69 : */
70 1 : Matrix3d rpy2rot(const double roll, const double pitch, const double yaw);
71 :
72 : /**
73 : * @brief Builds a 2x2 rotation matrix from a single planar angle
74 : *
75 : * @param angle [in] Rotation angle in radians
76 : *
77 : * @return 2D rotation matrix
78 : */
79 1 : Matrix2d ang2rot(const double angle);
80 :
81 : /**
82 : * @brief Extracts the rotation angle from a 2x2 rotation matrix
83 : *
84 : * @param m [in] 2D rotation matrix
85 : *
86 : * @return Rotation angle in radians
87 : */
88 1 : double rot2ang(const Matrix2d& m);
89 :
90 : /**
91 : * @brief Extracts the 2D translation vector from a 3x3 homogeneous transform
92 : *
93 : * @param r [in] 3x3 homogeneous transformation matrix
94 : *
95 : * @return 2-element translation vector
96 : */
97 1 : Vector2d getT(const Matrix3d& r);
98 :
99 : /**
100 : * @brief Extracts the 2x2 rotation block from a 3x3 homogeneous transform
101 : *
102 : * @param r [in] 3x3 homogeneous transformation matrix
103 : *
104 : * @return 2x2 rotation matrix
105 : */
106 1 : Matrix2d getR(const Matrix3d& r);
107 :
108 : /**
109 : * @brief Converts a Pose to a 3x3 homogeneous 2D transformation matrix
110 : *
111 : * @param p [in] Pose (only XY position and yaw are used)
112 : *
113 : * @return 3x3 2D homogeneous transform
114 : */
115 1 : Matrix3d pose2tf_2d(const Pose& p);
116 :
117 : /**
118 : * @brief Converts a 3x3 homogeneous 2D transformation matrix to a Pose
119 : *
120 : * @param m [in] 3x3 2D homogeneous transform
121 : *
122 : * @return Pose with XY position and yaw orientation
123 : */
124 1 : Pose tf2pose_2d(const Matrix3d& m);
125 :
126 : /**
127 : * @brief Converts a Pose to a 4x4 homogeneous 3D transformation matrix
128 : *
129 : * @param p [in] Pose with 3D position and quaternion orientation
130 : *
131 : * @return 4x4 homogeneous transform
132 : */
133 1 : Matrix4d pose2tf(const Pose& p);
134 :
135 : /**
136 : * @brief Converts a 4x4 homogeneous 3D transformation matrix to a Pose
137 : *
138 : * @param m [in] 4x4 homogeneous transform
139 : *
140 : * @return Pose with 3D position and quaternion orientation
141 : */
142 1 : Pose tf2pose(const Matrix4d& m);
143 :
144 : /**
145 : * @brief Extracts the 3D translation vector from a 4x4 homogeneous transform
146 : *
147 : * @param r [in] 4x4 homogeneous transformation matrix
148 : *
149 : * @return 3-element translation vector
150 : */
151 1 : Vector3d getT(const Matrix4d& r);
152 :
153 : /**
154 : * @brief Extracts the 3x3 rotation block from a 4x4 homogeneous transform
155 : *
156 : * @param r [in] 4x4 homogeneous transformation matrix
157 : *
158 : * @return 3x3 rotation matrix
159 : */
160 1 : Matrix3d getR(const Matrix4d& r);
161 :
162 : /**
163 : * @brief Projects a 4x4 3D homogeneous transform down to a 3x3 2D transform
164 : *
165 : * @param m [in] 4x4 3D homogeneous transform
166 : *
167 : * @return 3x3 2D homogeneous transform using only the XY plane
168 : */
169 1 : Matrix3d tf_3dto2d(const Matrix4d& m);
170 :
171 : /**
172 : * @brief Embeds a 3x3 2D homogeneous transform into a 4x4 3D transform
173 : *
174 : * @param m [in] 3x3 2D homogeneous transform
175 : *
176 : * @return 4x4 3D homogeneous transform with identity Z and rotation blocks
177 : */
178 1 : Matrix4d tf_2dto3d(const Matrix3d& m);
179 :
180 : /**
181 : * @brief Extracts roll-pitch-yaw angles from a rotation matrix
182 : *
183 : * @param r [in] 3x3 rotation matrix
184 : *
185 : * @return 3-element vector [roll, pitch, yaw] in radians
186 : */
187 1 : Vector3d rot2rpy(const Matrix3d& r);
188 :
189 : /**
190 : * @brief Extracts the yaw angle from a rotation matrix
191 : *
192 : * @param r [in] 3x3 rotation matrix
193 : *
194 : * @return Yaw angle in radians
195 : */
196 1 : double rot2yaw(const Matrix3d& r);
197 :
198 : /**
199 : * @brief Converts a quaternion to a 3x3 rotation matrix
200 : *
201 : * @param q [in] Unit quaternion
202 : *
203 : * @return Equivalent 3x3 rotation matrix
204 : */
205 1 : Matrix3d quat2rot(const Quaterniond& q);
206 :
207 : /**
208 : * @brief Converts a 3x3 rotation matrix to a quaternion
209 : *
210 : * @param r [in] 3x3 rotation matrix
211 : *
212 : * @return Equivalent unit quaternion
213 : */
214 1 : Quaterniond rot2quat(const Matrix3d& r);
215 :
216 : /**
217 : * @brief Converts a quaternion to roll-pitch-yaw angles
218 : *
219 : * @param q [in] Unit quaternion
220 : *
221 : * @return 3-element vector [roll, pitch, yaw] in radians
222 : */
223 1 : Vector3d quat2rpy(const Quaterniond& q);
224 :
225 : /**
226 : * @brief Extracts the yaw angle from a quaternion
227 : *
228 : * @param q [in] Unit quaternion
229 : *
230 : * @return Yaw angle in radians
231 : */
232 1 : double quat2yaw(const Quaterniond& q);
233 :
234 : /**
235 : * @brief Converts a roll-pitch-yaw vector to a quaternion
236 : *
237 : * @param rpy [in] Roll, pitch, yaw in radians
238 : *
239 : * @return Equivalent unit quaternion
240 : */
241 1 : Quaterniond rpy2quat(const Vector3d& rpy);
242 :
243 : /**
244 : * @brief Converts individual roll-pitch-yaw angles to a quaternion
245 : *
246 : * @param r [in] Roll angle in radians
247 : * @param p [in] Pitch angle in radians
248 : * @param y [in] Yaw angle in radians
249 : *
250 : * @return Equivalent unit quaternion
251 : */
252 1 : Quaterniond rpy2quat(const double r, const double p, const double y);
253 :
254 : /**
255 : * @brief Strips roll and pitch from a quaternion, retaining only yaw
256 : *
257 : * @param q [in] Input quaternion
258 : *
259 : * @return Quaternion representing only the yaw component of q
260 : */
261 1 : Quaterniond extractYaw(const Quaterniond& q);
262 :
263 : /**
264 : * @brief Extracts the X, Y, and yaw components from a 6-element state vector
265 : *
266 : * @param v [in] 6-element vector [x, y, z, roll, pitch, yaw]
267 : *
268 : * @return 3-element vector [x, y, yaw]
269 : */
270 1 : Vector3d extract2d(const Vector6d& v);
271 :
272 : /**
273 : * @brief Extracts the X, Y, and yaw components from a vehicle State
274 : *
275 : * @param state [in] Full 6DOF vehicle state
276 : *
277 : * @return 3-element vector [x, y, yaw]
278 : */
279 1 : Vector3d extract2d(const State& state);
280 :
281 : /**
282 : * @brief Builds the 3x3 Jacobian mapping body angular rates to Euler-angle rates
283 : *
284 : * @param rpy [in] Current roll, pitch, yaw in radians
285 : *
286 : * @return 3x3 Jacobian matrix J such that ṅ_ang = J * ω
287 : */
288 1 : Matrix3d jacobian(const Vector3d& rpy);
289 :
290 : /**
291 : * @brief Builds the 3x3 inverse Jacobian mapping Euler-angle rates to body angular rates
292 : *
293 : * @param rpy [in] Current roll, pitch, yaw in radians
294 : *
295 : * @return 3x3 inverse Jacobian matrix J⁻¹
296 : */
297 1 : Matrix3d jacobian_inv(const Vector3d& rpy);
298 :
299 : /**
300 : * @brief Computes the time derivative of the 6x6 transformation Jacobian
301 : *
302 : * @param rpy [in] Current roll, pitch, yaw in radians
303 : * @param n2dot [in] World-frame angular velocity [ṙoll, ṗitch, ẏaw] in rad/s
304 : *
305 : * @return 6x6 Jacobian time derivative matrix J̇
306 : */
307 1 : Matrix6d getdJ(const Vector3d& rpy, const Vector3d& n2dot);
308 :
309 : /**
310 : * @brief Builds the 6x6 world-to-body transformation matrix from RPY angles
311 : *
312 : * @param rpy [in] Roll, pitch, yaw in radians
313 : *
314 : * @return 6x6 block-diagonal transformation [R, 0; 0, J]
315 : */
316 1 : Matrix6d transformation(const Vector3d& rpy);
317 :
318 : /**
319 : * @brief Builds the 6x6 world-to-body transformation matrix from a quaternion
320 : *
321 : * @param q [in] Unit quaternion
322 : *
323 : * @return 6x6 block-diagonal transformation [R, 0; 0, J]
324 : */
325 1 : Matrix6d transformation(const Quaterniond& q);
326 :
327 : /**
328 : * @brief Builds the 6x6 world-to-body transformation matrix from a rotation matrix
329 : *
330 : * @param r [in] 3x3 rotation matrix
331 : *
332 : * @return 6x6 block-diagonal transformation [R, 0; 0, J]
333 : */
334 1 : Matrix6d transformation(const Matrix3d& r);
335 :
336 : /**
337 : * @brief Builds the 6x6 body-to-world inverse transformation matrix from RPY angles
338 : *
339 : * @param rpy [in] Roll, pitch, yaw in radians
340 : *
341 : * @return 6x6 block-diagonal inverse transformation [Rᵀ, 0; 0, J⁻¹]
342 : */
343 1 : Matrix6d transformation_inv(const Vector3d& rpy);
344 :
345 : /**
346 : * @brief Builds the 6x6 body-to-world inverse transformation matrix from a quaternion
347 : *
348 : * @param q [in] Unit quaternion
349 : *
350 : * @return 6x6 block-diagonal inverse transformation [Rᵀ, 0; 0, J⁻¹]
351 : */
352 1 : Matrix6d transformation_inv(const Quaterniond& q);
353 :
354 : /**
355 : * @brief Builds the 6x6 body-to-world inverse transformation matrix from a rotation matrix
356 : *
357 : * @param r [in] 3x3 rotation matrix
358 : *
359 : * @return 6x6 block-diagonal inverse transformation [Rᵀ, 0; 0, J⁻¹]
360 : */
361 1 : Matrix6d transformation_inv(const Matrix3d& r);
362 :
363 : /**
364 : * @brief Builds a 3x3 skew-symmetric matrix from a 3-vector
365 : *
366 : * @param v [in] 3-element vector
367 : *
368 : * @return Skew-symmetric matrix [v]×
369 : */
370 1 : Matrix3d skew(const Vector3d& v);
371 :
372 : /**
373 : * @brief Extracts the axial vector from a 3x3 skew-symmetric matrix
374 : *
375 : * @param v [in] 3x3 skew-symmetric matrix
376 : *
377 : * @return 3-element axial vector
378 : */
379 1 : Vector3d skew(const Matrix3d& v);
380 :
381 : /**
382 : * @brief Computes the signed error between two vectors as (y - d)
383 : *
384 : * @param d [in] Desired (goal) vector
385 : * @param y [in] Current (actual) vector
386 : *
387 : * @return Element-wise error (y - d)
388 : */
389 1 : VectorXd error(const VectorXd& d, const VectorXd& y);
390 :
391 : /**
392 : * @brief Computes the orientation error between two quaternions as (y - d)
393 : *
394 : * @param qd [in] Desired quaternion
395 : * @param q [in] Current quaternion
396 : *
397 : * @return 3-element rotation-error vector (imaginary part of qd⁻¹ * q)
398 : */
399 1 : Vector3d error(const Quaterniond& qd, const Quaterniond& q);
400 :
401 : /**
402 : * @brief Computes the 6DOF pose error between two Poses as (y - d)
403 : *
404 : * @param d [in] Desired pose
405 : * @param y [in] Current pose
406 : *
407 : * @return 6-element pose error [Δx, Δy, Δz, Δroll, Δpitch, Δyaw]
408 : */
409 1 : Vector6d error(const Pose& d, const Pose& y);
410 :
411 : /**
412 : * @brief Computes the planar (X-Y-yaw) pose error between two Poses as (y - d)
413 : *
414 : * @param d [in] Desired pose
415 : * @param y [in] Current pose
416 : *
417 : * @return 3-element error [Δx, Δy, Δyaw]
418 : */
419 1 : Vector3d error2d(const Pose& d, const Pose& y);
420 :
421 : /**
422 : * @brief Computes the full 18DOF state error (pose + twist + acceleration) as (y - d)
423 : *
424 : * @param d [in] Desired state
425 : * @param y [in] Current state
426 : *
427 : * @return 18-element error vector [pose_err(6), twist_err(6), accel_err(6)]
428 : */
429 1 : Vector18d error(const State& d, const State& y);
430 :
431 : /**
432 : * @brief Computes the RPY orientation error between two RPY angle vectors
433 : *
434 : * @param rpyd [in] Desired roll-pitch-yaw in radians
435 : * @param rpy [in] Current roll-pitch-yaw in radians
436 : *
437 : * @return 3-element RPY error vector
438 : */
439 1 : Vector3d errorrpy(const Vector3d& rpyd, const Vector3d& rpy);
440 :
441 : /**
442 : * @brief Converts a ROS Vector3 shared-pointer message to an Eigen Vector3d
443 : *
444 : * @param v [in] Shared pointer to a geometry_msgs::msg::Vector3
445 : *
446 : * @return Equivalent Eigen 3-vector
447 : */
448 1 : Vector3d convert(const geometry_msgs::msg::Vector3::ConstSharedPtr& v);
449 :
450 : /**
451 : * @brief Converts a ROS Vector3 message to an Eigen Vector3d
452 : *
453 : * @param v [in] geometry_msgs::msg::Vector3 message
454 : *
455 : * @return Equivalent Eigen 3-vector
456 : */
457 1 : Vector3d convert(const geometry_msgs::msg::Vector3& v);
458 :
459 : /**
460 : * @brief Converts a ROS Point shared-pointer message to an Eigen Vector3d
461 : *
462 : * @param v [in] Shared pointer to a geometry_msgs::msg::Point
463 : *
464 : * @return Equivalent Eigen 3-vector
465 : */
466 1 : Vector3d convert(const geometry_msgs::msg::Point::ConstSharedPtr& v);
467 :
468 : /**
469 : * @brief Converts a ROS Point message to an Eigen Vector3d
470 : *
471 : * @param v [in] geometry_msgs::msg::Point message
472 : *
473 : * @return Equivalent Eigen 3-vector
474 : */
475 1 : Vector3d convert(const geometry_msgs::msg::Point& v);
476 :
477 : /**
478 : * @brief Converts a ROS Twist shared-pointer message to a 6-element Eigen vector
479 : *
480 : * @param v [in] Shared pointer to a geometry_msgs::msg::Twist
481 : *
482 : * @return 6-element vector [vx, vy, vz, wx, wy, wz]
483 : */
484 1 : Vector6d convert(const geometry_msgs::msg::Twist::ConstSharedPtr& v);
485 :
486 : /**
487 : * @brief Converts a ROS Twist message to a 6-element Eigen vector
488 : *
489 : * @param v [in] geometry_msgs::msg::Twist message
490 : *
491 : * @return 6-element vector [vx, vy, vz, wx, wy, wz]
492 : */
493 1 : Vector6d convert(const geometry_msgs::msg::Twist& v);
494 :
495 : /**
496 : * @brief Converts a ROS TwistStamped message to a 6-element Eigen vector
497 : *
498 : * @param v [in] geometry_msgs::msg::TwistStamped message
499 : *
500 : * @return 6-element vector [vx, vy, vz, wx, wy, wz]
501 : */
502 1 : Vector6d convert(const geometry_msgs::msg::TwistStamped& v);
503 :
504 : /**
505 : * @brief Converts a ROS TwistStamped shared-pointer message to a 6-element Eigen vector
506 : *
507 : * @param v [in] Shared pointer to a geometry_msgs::msg::TwistStamped
508 : *
509 : * @return 6-element vector [vx, vy, vz, wx, wy, wz]
510 : */
511 1 : Vector6d convert(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& v);
512 :
513 : /**
514 : * @brief Converts a ROS Quaternion shared-pointer message to an Eigen Quaterniond
515 : *
516 : * @param v [in] Shared pointer to a geometry_msgs::msg::Quaternion
517 : *
518 : * @return Normalized Eigen quaternion
519 : */
520 1 : Quaterniond convert(const geometry_msgs::msg::Quaternion::ConstSharedPtr& v);
521 :
522 : /**
523 : * @brief Converts a ROS Quaternion message to an Eigen Quaterniond
524 : *
525 : * @param v [in] geometry_msgs::msg::Quaternion message
526 : *
527 : * @return Normalized Eigen quaternion
528 : */
529 1 : Quaterniond convert(const geometry_msgs::msg::Quaternion& v);
530 :
531 : /**
532 : * @brief Converts a ROS Pose shared-pointer message to a Pose struct
533 : *
534 : * @param p [in] Shared pointer to a geometry_msgs::msg::Pose
535 : *
536 : * @return Equivalent Pose struct
537 : */
538 1 : Pose convert(const geometry_msgs::msg::Pose::ConstSharedPtr& p);
539 :
540 : /**
541 : * @brief Converts a ROS Pose message to a Pose struct
542 : *
543 : * @param p [in] geometry_msgs::msg::Pose message
544 : *
545 : * @return Equivalent Pose struct
546 : */
547 1 : Pose convert(const geometry_msgs::msg::Pose& p);
548 :
549 : /**
550 : * @brief Converts a ROS Odometry shared-pointer message to a State struct
551 : *
552 : * @param odom [in] Shared pointer to a nav_msgs::msg::Odometry message
553 : *
554 : * @return Equivalent State struct with pose and twist populated
555 : */
556 1 : State convert(const nav_msgs::msg::Odometry::ConstSharedPtr& odom);
557 :
558 : /**
559 : * @brief Converts a ROS Odometry message to a State struct
560 : *
561 : * @param odom [in] nav_msgs::msg::Odometry message
562 : *
563 : * @return Equivalent State struct with pose and twist populated
564 : */
565 1 : State convert(const nav_msgs::msg::Odometry& odom);
566 :
567 : /**
568 : * @brief Converts a Pose struct to a ROS Pose message
569 : *
570 : * @param p [in] Pose struct
571 : *
572 : * @return Equivalent geometry_msgs::msg::Pose message
573 : */
574 1 : geometry_msgs::msg::Pose convert(const Pose& p);
575 :
576 : /**
577 : * @brief Converts a State struct to a ROS Odometry message
578 : *
579 : * @param state [in] Vehicle state
580 : *
581 : * @return Equivalent nav_msgs::msg::Odometry message
582 : */
583 1 : nav_msgs::msg::Odometry convert(const State& state);
584 :
585 : /**
586 : * @brief Converts a 6-element Eigen vector to a ROS Twist message
587 : *
588 : * @param v [in] 6-element vector [vx, vy, vz, wx, wy, wz]
589 : *
590 : * @return Equivalent geometry_msgs::msg::Twist message
591 : */
592 1 : geometry_msgs::msg::Twist convert(const Vector6d& v);
593 :
594 : /**
595 : * @brief Converts a 6-element force/torque vector to a ROS WrenchStamped message
596 : *
597 : * @param v [in] 6-element wrench [Fx, Fy, Fz, Tx, Ty, Tz] in N and Nm
598 : * @param frame_id [in] TF frame ID to embed in the message header
599 : * @param t [in] Timestamp in seconds; uses current ROS time if <= 0
600 : *
601 : * @return Equivalent geometry_msgs::msg::WrenchStamped message
602 : */
603 1 : geometry_msgs::msg::WrenchStamped convert(const Vector6d& v, const std::string& frame_id, double t);
604 :
605 : /**
606 : * @brief Computes the Moore-Penrose pseudo-inverse of a matrix
607 : *
608 : * @param m [in] Input matrix
609 : *
610 : * @return Right pseudo-inverse mᵀ (m mᵀ)⁻¹
611 : */
612 1 : MatrixXd pseudoInverse(const MatrixXd& m);
613 :
614 : #endif // CONTROLS_UTILS_TRANSFORMS_H
|
