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;
39  pinocchio::SINCOS(t, &st, &ct);
40  alpha = st / t;
41  beta = (1 - ct) / t2;
42  zeta = (1 - alpha) / (t2);
43  }
44 
45  Matrix3 V = alpha * Matrix3::Identity() + beta * pinocchio::skew(v3) + zeta * v3 * v3.transpose();
46 
47  return V;
48 }
49 
50 template<typename Vector3Like>
51 Eigen::Matrix<typename Vector3Like::Scalar, 3, 3, 0>
52 computeVinv(const Eigen::MatrixBase<Vector3Like> & v3)
53 {
54  typedef typename Vector3Like::Scalar Scalar;
55  typedef Eigen::Matrix<Scalar, 3, 3, 0> ReturnType;
56  typedef ReturnType Matrix3;
57 
58  Scalar t2 = v3.squaredNorm();
59  const Scalar t = pinocchio::math::sqrt(t2);
60 
61  Scalar alpha, beta;
62  if (t < 1e-4)
63  {
64  alpha = Scalar(1) - t2 / Scalar(12) - t2 * t2 / Scalar(720);
65  beta = Scalar(1) / Scalar(12) + t2 / Scalar(720);
66  }
67  else
68  {
69  Scalar st, ct;
70  pinocchio::SINCOS(t, &st, &ct);
71  alpha = t * st / (Scalar(2) * (Scalar(1) - ct));
72  beta = Scalar(1) / t2 - st / (Scalar(2) * t * (Scalar(1) - ct));
73  }
74 
75  Matrix3 Vinv =
76  alpha * Matrix3::Identity() - 0.5 * pinocchio::skew(v3) + beta * v3 * v3.transpose();
77 
78  return Vinv;
79 }
80 
81 BOOST_AUTO_TEST_CASE(test_log3)
82 {
83  using CppAD::AD;
84  using CppAD::NearEqual;
85 
86  typedef double Scalar;
87  typedef AD<Scalar> ADScalar;
88 
89  typedef pinocchio::SE3Tpl<Scalar> SE3;
90  typedef pinocchio::MotionTpl<Scalar> Motion;
91  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
92  typedef pinocchio::MotionTpl<ADScalar> ADMotion;
93 
94  Motion v(Motion::Zero());
95  SE3 M(SE3::Random());
96  M.translation().setZero();
97 
98  SE3::Matrix3 rot_next = M.rotation() * pinocchio::exp3(v.angular());
99 
100  SE3::Matrix3 Jlog3;
101  pinocchio::Jlog3(M.rotation(), Jlog3);
102 
103  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
104  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ADVector;
105 
106  ADMotion ad_v(v.cast<ADScalar>());
107  ADSE3 ad_M(M.cast<ADScalar>());
108  ADSE3::Matrix3 rot = ad_M.rotation();
109 
110  ADVector X(3);
111 
112  X = ad_v.angular();
113 
114  CppAD::Independent(X);
115  ADMotion::Vector3 X_ = X;
116 
117  ADSE3::Matrix3 ad_rot_next = rot * pinocchio::exp3(X_);
118  ADMotion::Vector3 log_R_next = pinocchio::log3(ad_rot_next);
119 
120  ADVector Y(3);
121  Y = log_R_next;
122 
123  CppAD::ADFun<Scalar> map(X, Y);
124 
125  CPPAD_TESTVECTOR(Scalar) x(3);
126  Eigen::Map<Motion::Vector3>(x.data()).setZero();
127 
128  CPPAD_TESTVECTOR(Scalar) nu_next_vec = map.Forward(0, x);
129  Motion::Vector3 nu_next(Eigen::Map<Motion::Vector3>(nu_next_vec.data()));
130 
131  SE3::Matrix3 rot_next_from_map = pinocchio::exp3(nu_next);
132 
133  CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x);
134 
135  Matrix jacobian = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(), 3, 3);
136 
137  BOOST_CHECK(rot_next_from_map.isApprox(rot_next));
138  BOOST_CHECK(jacobian.isApprox(Jlog3));
139 }
140 
141 BOOST_AUTO_TEST_CASE(test_explog_translation)
142 {
143  using CppAD::AD;
144  using CppAD::NearEqual;
145 
146  typedef double Scalar;
147  typedef AD<Scalar> ADScalar;
148 
149  typedef pinocchio::SE3Tpl<Scalar> SE3;
150  typedef pinocchio::MotionTpl<Scalar> Motion;
151  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
152  typedef pinocchio::MotionTpl<ADScalar> ADMotion;
153 
154  Motion v(Motion::Zero());
155  SE3 M(SE3::Random()); // M.rotation().setIdentity();
156 
157  {
158  Motion::Vector3 v3_test;
159  v3_test.setRandom();
160  SE3::Matrix3 V = computeV(v3_test);
161  SE3::Matrix3 Vinv = computeVinv(v3_test);
162 
163  BOOST_CHECK((V * Vinv).isIdentity());
164  }
165 
166  SE3 M_next = M * pinocchio::exp6(v);
167  // BOOST_CHECK(M_next.rotation().isIdentity());
168 
169  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
170  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ADVector;
171 
172  ADMotion ad_v(v.cast<ADScalar>());
173  ADSE3 ad_M(M.cast<ADScalar>());
174 
175  ADVector X(6);
176 
177  X = ad_v.toVector();
178 
179  CppAD::Independent(X);
180  ADMotion::Vector6 X_ = X;
181 
182  pinocchio::MotionRef<ADMotion::Vector6> ad_v_ref(X_);
183  ADSE3 ad_M_next = ad_M * pinocchio::exp6(ad_v_ref);
184 
185  ADVector Y(6);
186  Y.head<3>() = ad_M_next.translation();
187  Y.tail<3>() =
188  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 =
204  Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(), Y.size(), X.size());
205 
206  BOOST_CHECK(jacobian.topLeftCorner(3, 3).isApprox(M.rotation()));
207 }
208 
209 BOOST_AUTO_TEST_CASE(test_explog)
210 {
211  using CppAD::AD;
212  using CppAD::NearEqual;
213 
214  typedef double Scalar;
215  typedef AD<Scalar> ADScalar;
216 
217  typedef pinocchio::SE3Tpl<Scalar> SE3;
218  typedef pinocchio::MotionTpl<Scalar> Motion;
219  typedef pinocchio::SE3Tpl<ADScalar> ADSE3;
220  typedef pinocchio::MotionTpl<ADScalar> ADMotion;
221 
222  Motion v(Motion::Zero());
223  SE3 M(SE3::Random()); // M.translation().setZero();
224 
225  SE3::Matrix6 Jlog6;
226  pinocchio::Jlog6(M, Jlog6);
227 
228  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;
229  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ADVector;
230 
231  ADMotion ad_v(v.cast<ADScalar>());
232  ADSE3 ad_M(M.cast<ADScalar>());
233 
234  ADVector X(6);
235 
236  X.segment<3>(Motion::LINEAR) = ad_v.linear();
237  X.segment<3>(Motion::ANGULAR) = ad_v.angular();
238 
239  CppAD::Independent(X);
240  ADMotion::Vector6 X_ = X;
241  pinocchio::MotionRef<ADMotion::Vector6> v_X(X_);
242 
243  ADSE3 ad_M_next = ad_M * pinocchio::exp6(v_X);
244  ADMotion ad_log_M_next = pinocchio::log6(ad_M_next);
245 
246  ADVector Y(6);
247  Y.segment<3>(Motion::LINEAR) = ad_log_M_next.linear();
248  Y.segment<3>(Motion::ANGULAR) = ad_log_M_next.angular();
249 
250  CppAD::ADFun<Scalar> map(X, Y);
251 
252  CPPAD_TESTVECTOR(Scalar) x(6);
253  Eigen::Map<Motion::Vector6>(x.data()).setZero();
254 
255  CPPAD_TESTVECTOR(Scalar) nu_next_vec = map.Forward(0, x);
256  Motion nu_next(Eigen::Map<Motion::Vector6>(nu_next_vec.data()));
257 
258  CPPAD_TESTVECTOR(Scalar) jac = map.Jacobian(x);
259 
260  Matrix jacobian = Eigen::Map<PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(Matrix)>(jac.data(), 6, 6);
261 
262  // Check using finite differencies
263  Motion dv(Motion::Zero());
264  typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
265  Matrix6 Jlog6_fd(Matrix6::Zero());
266  Motion v_plus, v0(log6(M));
267 
268  const Scalar eps = 1e-8;
269  for (int k = 0; k < 6; ++k)
270  {
271  dv.toVector()[k] = eps;
272  SE3 M_plus = M * exp6(dv);
273  v_plus = log6(M_plus);
274  Jlog6_fd.col(k) = (v_plus - v0).toVector() / eps;
275  dv.toVector()[k] = 0;
276  }
277 
278  SE3::Matrix6 Jlog6_analytic;
279  pinocchio::Jlog6(M, Jlog6_analytic);
280 
281  BOOST_CHECK(Jlog6.isApprox(Jlog6_analytic));
282  BOOST_CHECK(Jlog6_fd.isApprox(Jlog6, pinocchio::math::sqrt(eps)));
283 }
284 
285 BOOST_AUTO_TEST_SUITE_END()
computeV
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, 0 > computeV(const Eigen::MatrixBase< Vector3Like > &v3)
Definition: cppad/spatial.cpp:20
V
V
meshcat-viewer.v0
int v0
Definition: meshcat-viewer.py:88
autodiff-rnea.dv
dv
Definition: autodiff-rnea.py:27
computeVinv
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, 0 > computeVinv(const Eigen::MatrixBase< Vector3Like > &v3)
Definition: cppad/spatial.cpp:52
codegen-rnea.jac
jac
Definition: codegen-rnea.py:41
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
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: spatial/explog.hpp:36
explog.hpp
pinocchio::python::context::Motion
MotionTpl< Scalar, Options > Motion
Definition: bindings/python/context/generic.hpp:54
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: spatial/explog.hpp:668
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
motion.hpp
pinocchio::Jlog3
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
Definition: spatial/explog.hpp:240
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_log3)
Definition: cppad/spatial.cpp:81
t2
dictionary t2
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:22
collision-with-point-clouds.X
X
Definition: collision-with-point-clouds.py:36
se3.hpp
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: spatial/explog.hpp:347
cartpole.v
v
Definition: cartpole.py:153
x
x
M
M
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
pinocchio::jacobian
void jacobian(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel, ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > &cdata, const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< JacobianMatrix > &jacobian_matrix)
Definition: constraint-model-visitor.hpp:199
cppad.hpp
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:332
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: spatial/explog.hpp:435
pinocchio::quaternion::exp6
void exp6(const MotionDense< MotionDerived > &motion, Eigen::MatrixBase< Config_t > &qout)
The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
Definition: explog-quaternion.hpp:92
dcrba.eps
int eps
Definition: dcrba.py:458
Y
Y
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: spatial/explog.hpp:83
t
Transform3f t
pinocchio::python::context::SE3
SE3Tpl< Scalar, Options > SE3
Definition: bindings/python/context/generic.hpp:53
pinocchio::MotionTpl
Definition: context/casadi.hpp:27
pinocchio::MotionRef
Definition: context/casadi.hpp:38
X

pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:12