GCC Code Coverage Report

Directory: ./
File: include/pinocchio/math/rpy.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 78 0.0%
Branches: 0 195 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_math_rpy_hxx__
6 #define __pinocchio_math_rpy_hxx__
7
8 #include <Eigen/Geometry>
9
10 #include "pinocchio/math/sincos.hpp"
11
12 namespace pinocchio
13 {
14 namespace rpy
15 {
16 template<typename Scalar>
17 ✗ Eigen::Matrix<Scalar, 3, 3> rpyToMatrix(const Scalar & r, const Scalar & p, const Scalar & y)
18 {
19 typedef Eigen::AngleAxis<Scalar> AngleAxis;
20 typedef Eigen::Matrix<Scalar, 3, 1> Vector3s;
21 ✗ return (AngleAxis(y, Vector3s::UnitZ()) * AngleAxis(p, Vector3s::UnitY())
22 ✗ * AngleAxis(r, Vector3s::UnitX()))
23 ✗ .toRotationMatrix();
24 }
25
26 template<typename Vector3Like>
27 Eigen::
28 Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
29 ✗ rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
30 {
31 ✗ PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
32 ✗ return rpyToMatrix(rpy[0], rpy[1], rpy[2]);
33 }
34
35 template<typename Matrix3Like>
36 Eigen::
37 Matrix<typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
38 ✗ matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
39 {
40 ✗ PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
41 ✗ assert(R.isUnitary() && "R is not a unitary matrix");
42
43 typedef typename Matrix3Like::Scalar Scalar;
44 typedef Eigen::Matrix<Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
45 ReturnType;
46 ✗ static const Scalar pi = PI<Scalar>();
47
48 ✗ ReturnType res = R.eulerAngles(2, 1, 0).reverse();
49
50 ✗ if (res[1] < -pi / 2)
51 ✗ res[1] += 2 * pi;
52
53 ✗ if (res[1] > pi / 2)
54 {
55 ✗ res[1] = pi - res[1];
56 ✗ if (res[0] < Scalar(0))
57 ✗ res[0] += pi;
58 else
59 ✗ res[0] -= pi;
60 // res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign
61 ✗ res[2] -= pi;
62 }
63
64 ✗ return res;
65 }
66
67 template<typename Vector3Like>
68 Eigen::
69 Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
70 ✗ computeRpyJacobian(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
71 {
72 typedef typename Vector3Like::Scalar Scalar;
73 typedef Eigen::Matrix<
74 typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
75 ReturnType;
76 ✗ ReturnType J;
77 ✗ const Scalar p = rpy[1];
78 Scalar sp, cp;
79 ✗ SINCOS(p, &sp, &cp);
80 ✗ switch (rf)
81 {
82 ✗ case LOCAL: {
83 ✗ const Scalar r = rpy[0];
84 Scalar sr, cr;
85 ✗ SINCOS(r, &sr, &cr);
86 ✗ J << Scalar(1.0), Scalar(0.0), -sp, Scalar(0.0), cr, sr * cp, Scalar(0.0), -sr, cr * cp;
87 ✗ return J;
88 }
89 ✗ case WORLD:
90 case LOCAL_WORLD_ALIGNED: {
91 ✗ const Scalar y = rpy[2];
92 Scalar sy, cy;
93 ✗ SINCOS(y, &sy, &cy);
94 ✗ J << cp * cy, -sy, Scalar(0.0), cp * sy, cy, Scalar(0.0), -sp, Scalar(0.0), Scalar(1.0);
95 ✗ return J;
96 }
97 ✗ default: {
98 ✗ throw std::invalid_argument("Bad reference frame.");
99 }
100 }
101 }
102
103 template<typename Vector3Like>
104 Eigen::
105 Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
106 ✗ computeRpyJacobianInverse(const Eigen::MatrixBase<Vector3Like> & rpy, const ReferenceFrame rf)
107 {
108 typedef typename Vector3Like::Scalar Scalar;
109 typedef Eigen::Matrix<
110 typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
111 ReturnType;
112 ✗ ReturnType J;
113 ✗ const Scalar p = rpy[1];
114 Scalar sp, cp;
115 ✗ SINCOS(p, &sp, &cp);
116 ✗ Scalar tp = sp / cp;
117 ✗ switch (rf)
118 {
119 ✗ case LOCAL: {
120 ✗ const Scalar r = rpy[0];
121 Scalar sr, cr;
122 ✗ SINCOS(r, &sr, &cr);
123 ✗ J << Scalar(1.0), sr * tp, cr * tp, Scalar(0.0), cr, -sr, Scalar(0.0), sr / cp, cr / cp;
124 ✗ return J;
125 }
126 ✗ case WORLD:
127 case LOCAL_WORLD_ALIGNED: {
128 ✗ const Scalar y = rpy[2];
129 Scalar sy, cy;
130 ✗ SINCOS(y, &sy, &cy);
131 ✗ J << cy / cp, sy / cp, Scalar(0.0), -sy, cy, Scalar(0.0), cy * tp, sy * tp, Scalar(1.0);
132 ✗ return J;
133 }
134 ✗ default: {
135 ✗ throw std::invalid_argument("Bad reference frame.");
136 }
137 }
138 }
139
140 template<typename Vector3Like0, typename Vector3Like1>
141 Eigen::
142 Matrix<typename Vector3Like0::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
143 ✗ computeRpyJacobianTimeDerivative(
144 const Eigen::MatrixBase<Vector3Like0> & rpy,
145 const Eigen::MatrixBase<Vector3Like1> & rpydot,
146 const ReferenceFrame rf)
147 {
148 typedef typename Vector3Like0::Scalar Scalar;
149 typedef Eigen::Matrix<
150 typename Vector3Like0::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options>
151 ReturnType;
152 ✗ ReturnType J;
153 ✗ const Scalar p = rpy[1];
154 ✗ const Scalar dp = rpydot[1];
155 Scalar sp, cp;
156 ✗ SINCOS(p, &sp, &cp);
157 ✗ switch (rf)
158 {
159 ✗ case LOCAL: {
160 ✗ const Scalar r = rpy[0];
161 ✗ const Scalar dr = rpydot[0];
162 Scalar sr, cr;
163 ✗ SINCOS(r, &sr, &cr);
164 ✗ J << Scalar(0.0), Scalar(0.0), -cp * dp, Scalar(0.0), -sr * dr, cr * cp * dr - sr * sp * dp,
165 ✗ Scalar(0.0), -cr * dr, -sr * cp * dr - cr * sp * dp;
166 ✗ return J;
167 }
168 ✗ case WORLD:
169 case LOCAL_WORLD_ALIGNED: {
170 ✗ const Scalar y = rpy[2];
171 ✗ const Scalar dy = rpydot[2];
172 Scalar sy, cy;
173 ✗ SINCOS(y, &sy, &cy);
174 ✗ J << -sp * cy * dp - cp * sy * dy, -cy * dy, Scalar(0.0), cp * cy * dy - sp * sy * dp,
175 ✗ -sy * dy, Scalar(0.0), -cp * dp, Scalar(0.0), Scalar(0.0);
176 ✗ return J;
177 }
178 ✗ default: {
179 ✗ throw std::invalid_argument("Bad reference frame.");
180 }
181 }
182 }
183
184 } // namespace rpy
185 } // namespace pinocchio
186 #endif // #ifndef __pinocchio_math_rpy_hxx__
187