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