cppad-spatial.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2019 CNRS INRIA
3 //
4 
5 #include "pinocchio/autodiff/cppad.hpp"
6 
7 #include "pinocchio/spatial/se3.hpp"
8 #include "pinocchio/spatial/motion.hpp"
9 #include "pinocchio/spatial/explog.hpp"
10 
11 #include <iostream>
12 
13 #include <boost/test/unit_test.hpp>
14 #include <boost/utility/binary.hpp>
15 
16 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
17 
18 template<typename Vector3Like>
19 Eigen::Matrix<typename Vector3Like::Scalar,3,3,0>
20 computeV(const Eigen::MatrixBase<Vector3Like> & v3)
21 {
22  typedef typename Vector3Like::Scalar Scalar;
23  typedef Eigen::Matrix<Scalar,3,3,0> ReturnType;
24  typedef ReturnType Matrix3;
25 
26  Scalar t2 = v3.squaredNorm();
27  const Scalar t = pinocchio::math::sqrt(t2);
28  Scalar alpha, beta, zeta;
29 
30  if (t < 1e-4)
31  {
32  alpha = Scalar(1) + t2/Scalar(6) - t2*t2/Scalar(120);
33  beta = Scalar(1)/Scalar(2) - t2/Scalar(24);
34  zeta = Scalar(1)/Scalar(6) - t2/Scalar(120);
35  }
36  else
37  {
38  Scalar st,ct; pinocchio::SINCOS(t,&st,&ct);
39  alpha = st/t;
40  beta = (1-ct)/t2;
41  zeta = (1 - alpha)/(t2);
42  }
43 
44  Matrix3 V
45  = alpha * Matrix3::Identity()
46  + beta * pinocchio::skew(v3)
47  + zeta * v3 * v3.transpose();
48 
49  return V;
50 }
51 
52 template<typename Vector3Like>
53 Eigen::Matrix<typename Vector3Like::Scalar,3,3,0>
54 computeVinv(const Eigen::MatrixBase<Vector3Like> & v3)
55 {
56  typedef typename Vector3Like::Scalar Scalar;
57  typedef Eigen::Matrix<Scalar,3,3,0> ReturnType;
58  typedef ReturnType Matrix3;
59 
60  Scalar t2 = v3.squaredNorm();
61  const Scalar t = pinocchio::math::sqrt(t2);
62 
63  Scalar alpha, beta;
64  if (t < 1e-4)
65  {
66  alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
67  beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
68  }
69  else
70  {
71  Scalar st,ct; pinocchio::SINCOS(t,&st,&ct);
72  alpha = t*st/(Scalar(2)*(Scalar(1)-ct));
73  beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct));
74  }
75 
76  Matrix3 Vinv
77  = alpha * Matrix3::Identity()
78  - 0.5 * pinocchio::skew(v3)
79  + beta * v3 * v3.transpose();
80 
81  return Vinv;
82 }
83 
84 BOOST_AUTO_TEST_CASE(test_log3)
85 {
86  using CppAD::AD;
87  using CppAD::NearEqual;
88 
89  typedef double Scalar;
90  typedef AD<Scalar> ADScalar;
91 
92  typedef pinocchio::SE3Tpl<Scalar> SE3;
93  typedef pinocchio::MotionTpl<Scalar> Motion;
94  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
95  typedef pinocchio::MotionTpl<ADScalar> ADMotion;
96 
97  Motion v(Motion::Zero());
98  SE3 M(SE3::Random()); M.translation().setZero();
99 
100  SE3::Matrix3 rot_next = M.rotation() * pinocchio::exp3(v.angular());
101 
102  SE3::Matrix3 Jlog3;
103  pinocchio::Jlog3(M.rotation(), Jlog3);
104 
105  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> Matrix;
106  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector;
107 
108  ADMotion ad_v(v.cast<ADScalar>());
109  ADSE3 ad_M(M.cast<ADScalar>());
110  ADSE3::Matrix3 rot = ad_M.rotation();
111 
112  ADVector X(3);
113 
114  X = ad_v.angular();
115 
116  CppAD::Independent(X);
117  ADMotion::Vector3 X_ = X;
118 
119  ADSE3::Matrix3 ad_rot_next = rot * pinocchio::exp3(X_);
120  ADMotion::Vector3 log_R_next = pinocchio::log3(ad_rot_next);
121 
122  ADVector Y(3);
123  Y = log_R_next;
124 
125  CppAD::ADFun<Scalar> map(X,Y);
126 
127  CPPAD_TESTVECTOR(Scalar) x(3);
128  Eigen::Map<Motion::Vector3>(x.data()).setZero();
129 
130  CPPAD_TESTVECTOR(Scalar) nu_next_vec = map.Forward(0,x);
131  Motion::Vector3 nu_next(Eigen::Map<Motion::Vector3>(nu_next_vec.data()));
132 
133  SE3::Matrix3 rot_next_from_map = pinocchio::exp3(nu_next);
134 
135  CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x);
136 
137  Matrix jacobian = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),3,3);
138 
139  BOOST_CHECK(rot_next_from_map.isApprox(rot_next));
140  BOOST_CHECK(jacobian.isApprox(Jlog3));
141 }
142 
143 BOOST_AUTO_TEST_CASE(test_explog_translation)
144 {
145  using CppAD::AD;
146  using CppAD::NearEqual;
147 
148  typedef double Scalar;
149  typedef AD<Scalar> ADScalar;
150 
151  typedef pinocchio::SE3Tpl<Scalar> SE3;
152  typedef pinocchio::MotionTpl<Scalar> Motion;
153  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
154  typedef pinocchio::MotionTpl<ADScalar> ADMotion;
155 
156  Motion v(Motion::Zero());
157  SE3 M(SE3::Random()); //M.rotation().setIdentity();
158 
159  {
160  Motion::Vector3 v3_test; v3_test.setRandom();
161  SE3::Matrix3 V = computeV(v3_test);
162  SE3::Matrix3 Vinv = computeVinv(v3_test);
163 
164  BOOST_CHECK((V*Vinv).isIdentity());
165  }
166 
167  SE3 M_next = M * pinocchio::exp6(v);
168 // BOOST_CHECK(M_next.rotation().isIdentity());
169 
170  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> Matrix;
171  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector;
172 
173  ADMotion ad_v(v.cast<ADScalar>());
174  ADSE3 ad_M(M.cast<ADScalar>());
175 
176  ADVector X(6);
177 
178  X = ad_v.toVector();
179 
180  CppAD::Independent(X);
181  ADMotion::Vector6 X_ = X;
182 
183  pinocchio::MotionRef<ADMotion::Vector6> ad_v_ref(X_);
184  ADSE3 ad_M_next = ad_M * pinocchio::exp6(ad_v_ref);
185 
186  ADVector Y(6);
187  Y.head<3>() = ad_M_next.translation();
188  Y.tail<3>() = ad_M.translation() + ad_M.rotation() * computeV(ad_v_ref.angular()) * ad_v_ref.linear();
189 
190  CppAD::ADFun<Scalar> map(X,Y);
191 
192  CPPAD_TESTVECTOR(Scalar) x((size_t)X.size());
193  Eigen::Map<Motion::Vector6>(x.data()).setZero();
194 
195  CPPAD_TESTVECTOR(Scalar) translation_vec = map.Forward(0,x);
196  Motion::Vector3 translation1(Eigen::Map<Motion::Vector3>(translation_vec.data()));
197  Motion::Vector3 translation2(Eigen::Map<Motion::Vector3>(translation_vec.data()+3));
198  BOOST_CHECK(translation1.isApprox(M_next.translation()));
199  BOOST_CHECK(translation2.isApprox(M_next.translation()));
200 
201  CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x);
202 
203  Matrix jacobian = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),Y.size(),X.size());
204 
205  BOOST_CHECK(jacobian.topLeftCorner(3,3).isApprox(M.rotation()));
206 
207 }
208 
209 
210 BOOST_AUTO_TEST_CASE(test_explog)
211 {
212  using CppAD::AD;
213  using CppAD::NearEqual;
214 
215  typedef double Scalar;
216  typedef AD<Scalar> ADScalar;
217 
218  typedef pinocchio::SE3Tpl<Scalar> SE3;
219  typedef pinocchio::MotionTpl<Scalar> Motion;
220  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
221  typedef pinocchio::MotionTpl<ADScalar> ADMotion;
222 
223  Motion v(Motion::Zero());
224  SE3 M(SE3::Random()); //M.translation().setZero();
225 
226  SE3::Matrix6 Jlog6;
227  pinocchio::Jlog6(M, Jlog6);
228 
229  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> Matrix;
230  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1> ADVector;
231 
232  ADMotion ad_v(v.cast<ADScalar>());
233  ADSE3 ad_M(M.cast<ADScalar>());
234 
235  ADVector X(6);
236 
237  X.segment<3>(Motion::LINEAR) = ad_v.linear();
238  X.segment<3>(Motion::ANGULAR) = ad_v.angular();
239 
240  CppAD::Independent(X);
241  ADMotion::Vector6 X_ = X;
242  pinocchio::MotionRef< ADMotion::Vector6> v_X(X_);
243 
244  ADSE3 ad_M_next = ad_M * pinocchio::exp6(v_X);
245  ADMotion ad_log_M_next = pinocchio::log6(ad_M_next);
246 
247  ADVector Y(6);
248  Y.segment<3>(Motion::LINEAR) = ad_log_M_next.linear();
249  Y.segment<3>(Motion::ANGULAR) = ad_log_M_next.angular();
250 
251  CppAD::ADFun<Scalar> map(X,Y);
252 
253  CPPAD_TESTVECTOR(Scalar) x(6);
254  Eigen::Map<Motion::Vector6>(x.data()).setZero();
255 
256  CPPAD_TESTVECTOR(Scalar) nu_next_vec = map.Forward(0,x);
257  Motion nu_next(Eigen::Map<Motion::Vector6>(nu_next_vec.data()));
258 
259  CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x);
260 
261  Matrix jacobian = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(),6,6);
262 
263  // Check using finite differencies
264  Motion dv(Motion::Zero());
265  typedef Eigen::Matrix<Scalar,6,6> Matrix6;
266  Matrix6 Jlog6_fd(Matrix6::Zero());
267  Motion v_plus, v0(log6(M));
268 
269  const Scalar eps = 1e-8;
270  for(int k = 0; k < 6; ++k)
271  {
272  dv.toVector()[k] = eps;
273  SE3 M_plus = M * exp6(dv);
274  v_plus = log6(M_plus);
275  Jlog6_fd.col(k) = (v_plus-v0).toVector()/eps;
276  dv.toVector()[k] = 0;
277  }
278 
279  SE3::Matrix6 Jlog6_analytic;
280  pinocchio::Jlog6(M, Jlog6_analytic);
281 
282  BOOST_CHECK(Jlog6.isApprox(Jlog6_analytic));
283  BOOST_CHECK(Jlog6_fd.isApprox(Jlog6,pinocchio::math::sqrt(eps)));
284 }
285 
286 BOOST_AUTO_TEST_SUITE_END()
pinocchio::log3
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
Definition: src/spatial/explog.hpp:75
V
V
computeVinv
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, 0 > computeVinv(const Eigen::MatrixBase< Vector3Like > &v3)
Definition: cppad-spatial.cpp:54
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: src/spatial/explog.hpp:223
dcrba.eps
int eps
Definition: dcrba.py:384
computeV
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, 0 > computeV(const Eigen::MatrixBase< Vector3Like > &v3)
Definition: cppad-spatial.cpp:20
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: src/spatial/explog.hpp:594
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: src/spatial/explog.hpp:403
pinocchio::quaternion::Jlog3
void Jlog3(const Eigen::QuaternionBase< QuaternionLike > &quat, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Computes the Jacobian of log3 operator for a unit quaternion.
Definition: explog-quaternion.hpp:195
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
pinocchio::exp3
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
Definition: src/spatial/explog.hpp:34
x
x
pinocchio::Motion
MotionTpl< double, 0 > Motion
Definition: src/spatial/fwd.hpp:57
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
setZero
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_log3)
Definition: cppad-spatial.cpp:84
t
Transform3f t
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
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: src/spatial/explog.hpp:326
M
M
pinocchio::SE3
SE3Tpl< double, 0 > SE3
Definition: src/spatial/fwd.hpp:53

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