rpy.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #include <pinocchio/math/rpy.hpp>
6 #include <pinocchio/math/quaternion.hpp>
7 #include <pinocchio/spatial/skew.hpp>
8 
9 #include <boost/variant.hpp> // to avoid C99 warnings
10 
11 #include <boost/test/unit_test.hpp>
12 #include <boost/utility/binary.hpp>
13 
14 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
15 
16 BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
17 {
18  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
19  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
20  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
21 
22  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);
23 
24  Eigen::Matrix3d Raa = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
25  * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
26  * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
27  ).toRotationMatrix();
28 
29  BOOST_CHECK(R.isApprox(Raa));
30 
31  Eigen::Vector3d v;
32  v << r, p, y;
33 
34  Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);
35 
36  BOOST_CHECK(Rv.isApprox(Raa));
37  BOOST_CHECK(Rv.isApprox(R));
38 }
39 
40 BOOST_AUTO_TEST_CASE(test_matrixToRpy)
41 {
42  #ifdef NDEBUG
43  const int n = 1e5;
44  #else
45  const int n = 1e2;
46  #endif
47  for(int k = 0; k < n ; ++k)
48  {
49  Eigen::Quaterniond quat;
50  pinocchio::quaternion::uniformRandom(quat);
51  const Eigen::Matrix3d R = quat.toRotationMatrix();
52 
53  const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
54  Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
55 
56  BOOST_CHECK(Rprime.isApprox(R));
57  BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
58  BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
59  BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
60  }
61 
62 #ifdef NDEBUG
63  const int n2 = 1e3;
64 #else
65  const int n2 = 1e2;
66 #endif
67 
68  // Test singular case theta = pi/2
69  for(int k = 0; k < n2 ; ++k)
70  {
71  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
72  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
73  Eigen::Matrix3d Rp;
74  Rp << 0.0, 0.0, 1.0,
75  0.0, 1.0, 0.0,
76  -1.0, 0.0, 0.0;
77  const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix()
78  * Rp
79  * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();
80 
81  const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
82  Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
83 
84  BOOST_CHECK(Rprime.isApprox(R));
85  BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
86  BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
87  BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
88  }
89 
90  // Test singular case theta = -pi/2
91  for(int k = 0; k < n2 ; ++k)
92  {
93  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
94  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
95  Eigen::Matrix3d Rp;
96  Rp << 0.0, 0.0, -1.0,
97  0.0, 1.0, 0.0,
98  1.0, 0.0, 0.0;
99  const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix()
100  * Rp
101  * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();
102 
103  const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
104  Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
105 
106  BOOST_CHECK(Rprime.isApprox(R));
107  BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
108  BOOST_CHECK(-M_PI/2 <= v[1] && v[1] <= M_PI/2);
109  BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
110  }
111 }
112 
113 
114 BOOST_AUTO_TEST_CASE(test_computeRpyJacobian)
115 {
116  // Check identity at zero
117  Eigen::Vector3d rpy(Eigen::Vector3d::Zero());
118  Eigen::Matrix3d j0 = pinocchio::rpy::computeRpyJacobian(rpy);
119  BOOST_CHECK(j0.isIdentity());
120  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
121  BOOST_CHECK(jL.isIdentity());
122  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
123  BOOST_CHECK(jW.isIdentity());
124  Eigen::Matrix3d jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
125  BOOST_CHECK(jA.isIdentity());
126 
127  // Check correct identities between different versions
128  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
129  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
130  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
131  rpy = Eigen::Vector3d(r, p, y);
132  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
133  j0 = pinocchio::rpy::computeRpyJacobian(rpy);
134  jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
135  jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
136  jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
137  BOOST_CHECK(j0 == jL);
138  BOOST_CHECK(jW == jA);
139  BOOST_CHECK(jW.isApprox(R*jL));
140 
141  // Check against analytical formulas
142  Eigen::Vector3d jL0Expected = Eigen::Vector3d::UnitX();
143  Eigen::Vector3d jL1Expected = Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix().transpose().col(1);
144  Eigen::Vector3d jL2Expected = (Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
145  * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX())
146  ).toRotationMatrix().transpose().col(2);
147  BOOST_CHECK(jL.col(0).isApprox(jL0Expected));
148  BOOST_CHECK(jL.col(1).isApprox(jL1Expected));
149  BOOST_CHECK(jL.col(2).isApprox(jL2Expected));
150 
151  Eigen::Vector3d jW0Expected = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
152  * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
153  ).toRotationMatrix().col(0);
154  Eigen::Vector3d jW1Expected = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix().col(1);
155  Eigen::Vector3d jW2Expected = Eigen::Vector3d::UnitZ();
156  BOOST_CHECK(jW.col(0).isApprox(jW0Expected));
157  BOOST_CHECK(jW.col(1).isApprox(jW1Expected));
158  BOOST_CHECK(jW.col(2).isApprox(jW2Expected));
159 
160  // Check against finite differences
161  Eigen::Vector3d rpydot = Eigen::Vector3d::Random();
162  double const eps = 1e-7;
163  double const tol = 1e-5;
164 
165  Eigen::Matrix3d dRdr = (pinocchio::rpy::rpyToMatrix(r + eps, p, y) - R) / eps;
166  Eigen::Matrix3d dRdp = (pinocchio::rpy::rpyToMatrix(r, p + eps, y) - R) / eps;
167  Eigen::Matrix3d dRdy = (pinocchio::rpy::rpyToMatrix(r, p, y + eps) - R) / eps;
168  Eigen::Matrix3d Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2];
169 
170  Eigen::Vector3d omegaL = jL * rpydot;
171  BOOST_CHECK(Rdot.isApprox(R * pinocchio::skew(omegaL), tol));
172 
173  Eigen::Vector3d omegaW = jW * rpydot;
174  BOOST_CHECK(Rdot.isApprox(pinocchio::skew(omegaW) * R, tol));
175 }
176 
177 
178 BOOST_AUTO_TEST_CASE(test_computeRpyJacobianInverse)
179 {
180  // Check correct identities between different versions
181  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
182  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
183  p *= 0.999; // ensure we are not too close to a singularity
184  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
185  Eigen::Vector3d rpy(r, p, y);
186 
187  Eigen::Matrix3d j0 = pinocchio::rpy::computeRpyJacobian(rpy);
188  Eigen::Matrix3d j0inv = pinocchio::rpy::computeRpyJacobianInverse(rpy);
189  BOOST_CHECK(j0inv.isApprox(j0.inverse()));
190 
191  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
192  Eigen::Matrix3d jLinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::LOCAL);
193  BOOST_CHECK(jLinv.isApprox(jL.inverse()));
194 
195  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
196  Eigen::Matrix3d jWinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::WORLD);
197  BOOST_CHECK(jWinv.isApprox(jW.inverse()));
198 
199  Eigen::Matrix3d jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
200  Eigen::Matrix3d jAinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
201  BOOST_CHECK(jAinv.isApprox(jA.inverse()));
202 }
203 
204 
205 BOOST_AUTO_TEST_CASE(test_computeRpyJacobianTimeDerivative)
206 {
207  // Check zero at zero velocity
208  double r = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
209  double p = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/M_PI)) - (M_PI/2);
210  double y = static_cast <double> (rand()) / (static_cast <double> (RAND_MAX/(2*M_PI))) - M_PI;
211  Eigen::Vector3d rpy(r, p, y);
212  Eigen::Vector3d rpydot(Eigen::Vector3d::Zero());
213  Eigen::Matrix3d dj0 = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot);
214  BOOST_CHECK(dj0.isZero());
215  Eigen::Matrix3d djL = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL);
216  BOOST_CHECK(djL.isZero());
217  Eigen::Matrix3d djW = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::WORLD);
218  BOOST_CHECK(djW.isZero());
219  Eigen::Matrix3d djA = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
220  BOOST_CHECK(djA.isZero());
221 
222  // Check correct identities between different versions
223  rpydot = Eigen::Vector3d::Random();
224  dj0 = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot);
225  djL = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL);
226  djW = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::WORLD);
227  djA = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
228  BOOST_CHECK(dj0 == djL);
229  BOOST_CHECK(djW == djA);
230 
231  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
232  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
233  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
234  Eigen::Vector3d omegaL = jL * rpydot;
235  Eigen::Vector3d omegaW = jW * rpydot;
236  BOOST_CHECK(omegaW.isApprox(R*omegaL));
237  BOOST_CHECK(djW.isApprox(pinocchio::skew(omegaW)*R*jL + R*djL));
238  BOOST_CHECK(djW.isApprox(R*pinocchio::skew(omegaL)*jL + R*djL));
239 
240  // Check against finite differences
241  double const eps = 1e-7;
242  double const tol = 1e-5;
243  Eigen::Vector3d rpyEps = rpy;
244 
245  rpyEps[0] += eps;
246  Eigen::Matrix3d djLdr = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
247  rpyEps[0] = rpy[0];
248  rpyEps[1] += eps;
249  Eigen::Matrix3d djLdp = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
250  rpyEps[1] = rpy[1];
251  rpyEps[2] += eps;
252  Eigen::Matrix3d djLdy = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
253  rpyEps[2] = rpy[2];
254  Eigen::Matrix3d djLf = djLdr * rpydot[0] + djLdp * rpydot[1] + djLdy * rpydot[2];
255  BOOST_CHECK(djL.isApprox(djLf, tol));
256 
257  rpyEps[0] += eps;
258  Eigen::Matrix3d djWdr = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
259  rpyEps[0] = rpy[0];
260  rpyEps[1] += eps;
261  Eigen::Matrix3d djWdp = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
262  rpyEps[1] = rpy[1];
263  rpyEps[2] += eps;
264  Eigen::Matrix3d djWdy = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
265  rpyEps[2] = rpy[2];
266  Eigen::Matrix3d djWf = djWdr * rpydot[0] + djWdp * rpydot[1] + djWdy * rpydot[2];
267  BOOST_CHECK(djW.isApprox(djWf, tol));
268 }
269 
270 BOOST_AUTO_TEST_SUITE_END()
pinocchio::WORLD
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
Definition: src/multibody/fwd.hpp:46
n
Vec3f n
y
y
pinocchio::rpy::computeRpyJacobian
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobian(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the Jacobian of the Roll-Pitch-Yaw conversion.
dcrba.eps
int eps
Definition: dcrba.py:384
r
FCL_REAL r
M_PI
#define M_PI
pinocchio::rpy::matrixToRpy
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > matrixToRpy(const Eigen::MatrixBase< Matrix3Like > &R)
Convert from Transformation Matrix to Roll, Pitch, Yaw.
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
R
R
quat
quat
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
Definition: rpy.cpp:16
pinocchio::quaternion::uniformRandom
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: math/quaternion.hpp:108
pinocchio::LOCAL_WORLD_ALIGNED
The LOCAL_WORLD_ALIGNED frame convention corresponds to the frame centered on the moving part (Joint...
Definition: src/multibody/fwd.hpp:48
pinocchio::rpy::computeRpyJacobianInverse
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobianInverse(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion.
pinocchio::toRotationMatrix
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition: rotation.hpp:24
pinocchio::rpy::rpyToMatrix
Eigen::Matrix< Scalar, 3, 3 > rpyToMatrix(const Scalar &r, const Scalar &p, const Scalar &y)
Convert from Roll, Pitch, Yaw to rotation Matrix.
pinocchio::skew
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21
rpy
Definition: rpy.py:1
pinocchio::rpy::computeRpyJacobianTimeDerivative
Eigen::Matrix< typename Vector3Like0::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options > computeRpyJacobianTimeDerivative(const Eigen::MatrixBase< Vector3Like0 > &rpy, const Eigen::MatrixBase< Vector3Like1 > &rpydot, const ReferenceFrame rf=LOCAL)
Compute the time derivative Jacobian of the Roll-Pitch-Yaw conversion.

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32