GCC Code Coverage Report

Directory: ./
File: include/pinocchio/math/rpy.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 72 87 82.8%
Branches: 100 423 23.6%
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 618 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
4/8
✓ Branch 1 taken 618 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 618 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 618 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 618 times.
✗ Branch 11 not taken.
 618 return (AngleAxis(y, Vector3s::UnitZ()) * AngleAxis(p, Vector3s::UnitY())
22
1/2
✓ Branch 2 taken 618 times.
✗ Branch 3 not taken.
 618 * AngleAxis(r, Vector3s::UnitX()))
23
3/6
✓ Branch 1 taken 618 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 618 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 618 times.
✗ Branch 8 not taken.
 618 .toRotationMatrix();
24 }
25
26 template<typename Vector3Like>
27 Eigen::
28 Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
29 609 rpyToMatrix(const Eigen::MatrixBase<Vector3Like> & rpy)
30 {
31
2/4
✓ Branch 1 taken 609 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 609 times.
✗ Branch 5 not taken.
 609 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, rpy, 3, 1);
32 609 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 602 matrixToRpy(const Eigen::MatrixBase<Matrix3Like> & R)
39 {
40
2/4
✓ Branch 1 taken 602 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 602 times.
✗ Branch 5 not taken.
 602 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
41
2/4
✓ Branch 2 taken 602 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 602 times.
✗ Branch 5 not taken.
 602 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
3/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 600 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 602 static const Scalar pi = PI<Scalar>();
47
48
2/4
✓ Branch 2 taken 602 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 602 times.
✗ Branch 6 not taken.
 602 ReturnType res = R.eulerAngles(2, 1, 0).reverse();
49
50
2/2
✓ Branch 1 taken 32 times.
✓ Branch 2 taken 570 times.
 602 if (res[1] < -pi / 2)
51 32 res[1] += 2 * pi;
52
53
2/2
✓ Branch 1 taken 52 times.
✓ Branch 2 taken 550 times.
 602 if (res[1] > pi / 2)
54 {
55 52 res[1] = pi - res[1];
56
2/2
✓ Branch 1 taken 28 times.
✓ Branch 2 taken 24 times.
 52 if (res[0] < Scalar(0))
57 28 res[0] += pi;
58 else
59 24 res[0] -= pi;
60 // res[2] > 0 according to Eigen's eulerAngles doc, no need to check its sign
61 52 res[2] -= pi;
62 }
63
64 602 return res;
65 }
66
67 template<typename Vector3Like>
68 Eigen::
69 Matrix<typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
70 34 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
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 ReturnType J;
77
1/4
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 34 const Scalar p = rpy[1];
78 ✗ Scalar sp, cp;
79
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 34 SINCOS(p, &sp, &cp);
80
2/3
✓ Branch 0 taken 17 times.
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 switch (rf)
81 {
82 17 case LOCAL: {
83
1/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 17 const Scalar r = rpy[0];
84 ✗ Scalar sr, cr;
85
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 17 SINCOS(r, &sr, &cr);
86
9/34
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 17 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 17 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 17 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 17 times.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
 17 J << Scalar(1.0), Scalar(0.0), -sp, Scalar(0.0), cr, sr * cp, Scalar(0.0), -sr, cr * cp;
87 17 return J;
88 }
89 17 case WORLD:
90 case LOCAL_WORLD_ALIGNED: {
91
1/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 17 const Scalar y = rpy[2];
92 ✗ Scalar sy, cy;
93
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 17 SINCOS(y, &sy, &cy);
94
9/34
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 17 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 17 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 17 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 17 times.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
 17 J << cp * cy, -sy, Scalar(0.0), cp * sy, cy, Scalar(0.0), -sp, Scalar(0.0), Scalar(1.0);
95 17 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 8 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
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 ReturnType J;
113
1/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 8 const Scalar p = rpy[1];
114 ✗ Scalar sp, cp;
115
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 8 SINCOS(p, &sp, &cp);
116
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 8 Scalar tp = sp / cp;
117
2/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 switch (rf)
118 {
119 4 case LOCAL: {
120
1/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 4 const Scalar r = rpy[0];
121 ✗ Scalar sr, cr;
122
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 4 SINCOS(r, &sr, &cr);
123
9/34
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
 4 J << Scalar(1.0), sr * tp, cr * tp, Scalar(0.0), cr, -sr, Scalar(0.0), sr / cp, cr / cp;
124 4 return J;
125 }
126 4 case WORLD:
127 case LOCAL_WORLD_ALIGNED: {
128
1/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 4 const Scalar y = rpy[2];
129 ✗ Scalar sy, cy;
130
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 4 SINCOS(y, &sy, &cy);
131
9/34
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
 4 J << cy / cp, sy / cp, Scalar(0.0), -sy, cy, Scalar(0.0), cy * tp, sy * tp, Scalar(1.0);
132 4 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 16 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
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 ReturnType J;
153
1/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 16 const Scalar p = rpy[1];
154
1/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 16 const Scalar dp = rpydot[1];
155 ✗ Scalar sp, cp;
156
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 16 SINCOS(p, &sp, &cp);
157
2/3
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 switch (rf)
158 {
159 8 case LOCAL: {
160
1/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 8 const Scalar r = rpy[0];
161
1/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 8 const Scalar dr = rpydot[0];
162 ✗ Scalar sr, cr;
163
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 8 SINCOS(r, &sr, &cr);
164
6/38
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
 8 J << Scalar(0.0), Scalar(0.0), -cp * dp, Scalar(0.0), -sr * dr, cr * cp * dr - sr * sp * dp,
165
3/22
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
 8 Scalar(0.0), -cr * dr, -sr * cp * dr - cr * sp * dp;
166 8 return J;
167 }
168 8 case WORLD:
169 case LOCAL_WORLD_ALIGNED: {
170
1/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 8 const Scalar y = rpy[2];
171
1/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 8 const Scalar dy = rpydot[2];
172 ✗ Scalar sy, cy;
173
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 8 SINCOS(y, &sy, &cy);
174
4/42
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 58 not taken.
✗ Branch 59 not taken.
✗ Branch 61 not taken.
✗ Branch 62 not taken.
 8 J << -sp * cy * dp - cp * sy * dy, -cy * dy, Scalar(0.0), cp * cy * dy - sp * sy * dp,
175
5/18
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 8 -sy * dy, Scalar(0.0), -cp * dp, Scalar(0.0), Scalar(0.0);
176 8 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