casadi-joints.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #include "pinocchio/autodiff/casadi.hpp"
6 
7 #include "pinocchio/multibody/joint/joint-generic.hpp"
8 #include "pinocchio/multibody/liegroup/liegroup.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
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_jointRX_motion_space)
17 {
18  typedef casadi::SX AD_double;
19  typedef pinocchio::JointCollectionDefaultTpl<AD_double> JointCollectionAD;
20  typedef pinocchio::JointCollectionDefaultTpl<double> JointCollection;
21 
22  typedef pinocchio::SE3Tpl<AD_double> SE3AD;
23  typedef pinocchio::MotionTpl<AD_double> MotionAD;
27 
28  typedef Eigen::Matrix<AD_double,Eigen::Dynamic,1> VectorXAD;
29  typedef Eigen::Matrix<AD_double,6,1> Vector6AD;
30 
31  typedef JointCollectionAD::JointModelRX JointModelRXAD;
32  typedef JointModelRXAD::ConfigVector_t ConfigVectorAD;
33 // typedef JointModelRXAD::TangentVector_t TangentVectorAD;
34  typedef JointCollectionAD::JointDataRX JointDataRXAD;
35 
37  typedef JointModelRX::ConfigVector_t ConfigVector;
38  typedef JointModelRX::TangentVector_t TangentVector;
40 
41  JointModelRX jmodel; jmodel.setIndexes(0,0,0);
42  JointDataRX jdata(jmodel.createData());
43 
44  JointModelRXAD jmodel_ad = jmodel.cast<AD_double>();
45  JointDataRXAD jdata_ad(jmodel_ad.createData());
46 
47  typedef pinocchio::LieGroup<JointModelRX>::type JointOperation;
48  ConfigVector q(jmodel.nq()); JointOperation().random(q);
49 
50  casadi::SX cs_q = casadi::SX::sym("q", jmodel.nq());
51  ConfigVectorAD q_ad(jmodel.nq());
52  for(Eigen::DenseIndex k = 0; k < jmodel.nq(); ++k)
53  {
54  q_ad[k] = cs_q(k);
55  }
56 
57  // Zero order
58  jmodel_ad.calc(jdata_ad,q_ad);
59  jmodel.calc(jdata,q);
60 
61  SE3 M1(jdata.M);
62  SE3AD M2(jdata_ad.M);
63 
64  casadi::SX cs_trans(3,1);
65  for(Eigen::DenseIndex k = 0; k < 3; ++k)
66  {
67  cs_trans(k) = M2.translation()[k];
68  }
69  casadi::SX cs_rot(3,3);
70  for(Eigen::DenseIndex i = 0; i < 3; ++i)
71  {
72  for(Eigen::DenseIndex j = 0; j < 3; ++j)
73  {
74  cs_rot(i,j) = M2.rotation()(i,j);
75  }
76  }
77 
78  casadi::Function eval_placement("eval_placement", casadi::SXVector {cs_q}, casadi::SXVector {cs_trans,cs_rot});
79  std::cout << "Joint Placement = " << eval_placement << std::endl;
80 
81  std::vector<double> q_vec((size_t)jmodel.nq());
82  Eigen::Map<ConfigVector>(q_vec.data(),jmodel.nq(),1) = q;
83  casadi::DMVector res = eval_placement(casadi::DMVector {q_vec});
84  std::cout << "M(q)=" << res << std::endl;
85 
86  BOOST_CHECK(M1.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->data())));
87  BOOST_CHECK(M1.rotation().isApprox(Eigen::Map<SE3::Matrix3>(res[1]->data())));
88 
89  // First order
90  casadi::SX cs_v = casadi::SX::sym("v", jmodel.nv());
91  TangentVector v(TangentVector::Random(jmodel.nv()));
92  VectorXAD v_ad(jmodel_ad.nv());
93 
94  std::vector<double> v_vec((size_t)jmodel.nv());
95  Eigen::Map<TangentVector>(v_vec.data(),jmodel.nv(),1) = v;
96 
97  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
98  {
99  v_ad[k] = cs_v(k);
100  }
101 
102  jmodel.calc(jdata,q,v);
103  Motion m(jdata.v);
104  ConstraintXd Sref(jdata.S.matrix());
105 
106  jmodel_ad.calc(jdata_ad,q_ad,v_ad);
107  Vector6AD Y;
108  MotionAD m_ad(jdata_ad.v);
109 
110  casadi::SX cs_vel(6,1);
111  for(Eigen::DenseIndex k = 0; k < 6; ++k)
112  {
113  cs_vel(k) = m_ad.toVector()[k];
114  }
115  casadi::Function eval_velocity("eval_velocity", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {cs_vel});
116  std::cout << "Joint Velocity = " << eval_velocity << std::endl;
117 
118  casadi::DMVector res_vel = eval_velocity(casadi::DMVector {q_vec,v_vec});
119  std::cout << "v(q,v)=" << res_vel << std::endl;
120 
121  BOOST_CHECK(m.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data())));
122  BOOST_CHECK(m.angular().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data()+3)));
123 
124  casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
125  casadi::Function eval_S("eval_S", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {dvel_dv});
126  std::cout << "S = " << eval_S << std::endl;
127 
128  casadi::DMVector res_S = eval_S(casadi::DMVector {q_vec,v_vec});
129  std::cout << "res_S:" << res_S << std::endl;
130  ConstraintXd::DenseBase Sref_mat = Sref.matrix();
131 
132  for(Eigen::DenseIndex i = 0; i < 6; ++i)
133  {
134  for(Eigen::DenseIndex j = 0; i < Sref.nv(); ++i)
135  BOOST_CHECK(std::fabs(Sref_mat(i,j) - (double)res_S[0](i,j)) <= Eigen::NumTraits<double>::dummy_precision());
136  }
137 }
138 
139 template<typename JointModel_> struct init;
140 
141 template<typename JointModel_>
142  struct init
143  {
144  static JointModel_ run()
145  {
146  JointModel_ jmodel;
147  jmodel.setIndexes(0,0,0);
148  return jmodel;
149  }
150 
151  static std::string name()
152  {
153  return "default " + JointModel_::classname();
154  }
155  };
156 
157  template<typename Scalar, int Options>
158  struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
159  {
161 
162  static JointModel run()
163  {
164  typedef typename JointModel::Vector3 Vector3;
165  JointModel jmodel(Vector3::Random().normalized());
166 
167  jmodel.setIndexes(0,0,0);
168  return jmodel;
169  }
170 
171  static std::string name()
172  {
173  return JointModel::classname();
174  }
175  };
176 
177  template<typename Scalar, int Options>
178  struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
179  {
181 
182  static JointModel run()
183  {
184  typedef typename JointModel::Vector3 Vector3;
185  JointModel jmodel(Vector3::Random().normalized());
186 
187  jmodel.setIndexes(0,0,0);
188  return jmodel;
189  }
190 
191  static std::string name()
192  {
193  return JointModel::classname();
194  }
195  };
196 
197  template<typename Scalar, int Options>
198  struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
199  {
201 
202  static JointModel run()
203  {
204  typedef typename JointModel::Vector3 Vector3;
205  JointModel jmodel(Vector3::Random().normalized());
206 
207  jmodel.setIndexes(0,0,0);
208  return jmodel;
209  }
210 
211  static std::string name()
212  {
213  return JointModel::classname();
214  }
215  };
216 
217  template<typename Scalar, int Options, template<typename,int> class JointCollection>
218  struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
219  {
221 
222  static JointModel run()
223  {
225  JointModel jmodel((JointModelRX()));
226 
227  jmodel.setIndexes(0,0,0);
228  return jmodel;
229  }
230 
231  static std::string name()
232  {
233  return JointModel::classname();
234  }
235  };
236 
237  template<typename Scalar, int Options, template<typename,int> class JointCollection>
238  struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
239  {
241 
242  static JointModel run()
243  {
246  JointModel jmodel((JointModelRX()));
247  jmodel.addJoint(JointModelRY());
248 
249  jmodel.setIndexes(0,0,0);
250  return jmodel;
251  }
252 
253  static std::string name()
254  {
255  return JointModel::classname();
256  }
257  };
258 
259  template<typename JointModel_>
260  struct init<pinocchio::JointModelMimic<JointModel_> >
261  {
263 
264  static JointModel run()
265  {
266  JointModel_ jmodel_ref = init<JointModel_>::run();
267 
268  JointModel jmodel(jmodel_ref,1.,0.);
269 
270  return jmodel;
271  }
272 
273  static std::string name()
274  {
275  return JointModel::classname();
276  }
277  };
278 
280 {
281  template<typename JointModel_>
283  {
284  JointModel_ jmodel = init<JointModel_>::run();
285  jmodel.setIndexes(0,0,0);
286  test(jmodel);
287  }
288 
289  // TODO: get the nq and nv quantity from LieGroups
290  template<typename JointModel_>
291  static void test(const pinocchio::JointModelMimic<JointModel_> & /*jmodel*/)
292  { /* do nothing */ }
293 
294  template<typename JointModel>
295  static void test(const pinocchio::JointModelBase<JointModel> & jmodel)
296  {
297  std::cout << "--" << std::endl;
298  std::cout << "jmodel: " << jmodel.shortname() << std::endl;
299 
300  typedef casadi::SX AD_double;
301 
302  typedef pinocchio::SE3Tpl<AD_double> SE3AD;
303  typedef pinocchio::MotionTpl<AD_double> MotionAD;
307 
308  typedef Eigen::Matrix<AD_double,Eigen::Dynamic,1> VectorXAD;
309  typedef Eigen::Matrix<AD_double,6,1> Vector6AD;
310 
311  typedef typename pinocchio::CastType<AD_double,JointModel>::type JointModelAD;
312  typedef typename JointModelAD::JointDataDerived JointDataAD;
313 
314  typedef typename JointModelAD::ConfigVector_t ConfigVectorAD;
315 
316  typedef typename JointModel::JointDataDerived JointData;
317  typedef typename JointModel::ConfigVector_t ConfigVector;
318  typedef typename JointModel::TangentVector_t TangentVector;
319 
320 
321  JointData jdata(jmodel.createData());
322  pinocchio::JointDataBase<JointData> & jdata_base = jdata;
323 
324  JointModelAD jmodel_ad = jmodel.template cast<AD_double>();
325  JointDataAD jdata_ad(jmodel_ad.createData());
326  pinocchio::JointDataBase<JointDataAD> & jdata_ad_base = jdata_ad;
327 
328  ConfigVector q(jmodel.nq());
329 
330  ConfigVector lb(ConfigVector::Constant(jmodel.nq(),-1.));
331  ConfigVector ub(ConfigVector::Constant(jmodel.nq(),1.));
332 
333  typedef pinocchio::RandomConfigurationStep<pinocchio::LieGroupMap,ConfigVector,ConfigVector,ConfigVector> RandomConfigAlgo;
334  RandomConfigAlgo::run(jmodel.derived(),typename RandomConfigAlgo::ArgsType(q,lb,ub));
335 
336  casadi::SX cs_q = casadi::SX::sym("q", jmodel.nq());
337  ConfigVectorAD q_ad(jmodel.nq());
338  for(Eigen::DenseIndex k = 0; k < jmodel.nq(); ++k)
339  {
340  q_ad[k] = cs_q(k);
341  }
342 
343  // Zero order
344  jmodel_ad.calc(jdata_ad,q_ad);
345  jmodel.calc(jdata,q);
346 
347  SE3 M1(jdata_base.M());
348  SE3AD M2(jdata_ad_base.M());
349 
350  casadi::SX cs_trans(3,1);
351  for(Eigen::DenseIndex k = 0; k < 3; ++k)
352  {
353  cs_trans(k) = M2.translation()[k];
354  }
355  casadi::SX cs_rot(3,3);
356  for(Eigen::DenseIndex i = 0; i < 3; ++i)
357  {
358  for(Eigen::DenseIndex j = 0; j < 3; ++j)
359  {
360  cs_rot(i,j) = M2.rotation()(i,j);
361  }
362  }
363 
364  casadi::Function eval_placement("eval_placement", casadi::SXVector {cs_q}, casadi::SXVector {cs_trans,cs_rot});
365  std::cout << "Joint Placement = " << eval_placement << std::endl;
366 
367  std::vector<double> q_vec((size_t)jmodel.nq());
368  Eigen::Map<ConfigVector>(q_vec.data(),jmodel.nq(),1) = q;
369  casadi::DMVector res = eval_placement(casadi::DMVector {q_vec});
370  std::cout << "M(q)=" << res << std::endl;
371 
372  BOOST_CHECK(M1.translation().isApprox(Eigen::Map<SE3::Vector3>(res[0]->data())));
373  BOOST_CHECK(M1.rotation().isApprox(Eigen::Map<SE3::Matrix3>(res[1]->data())));
374 
375  // First order
376  casadi::SX cs_v = casadi::SX::sym("v", jmodel.nv());
377  TangentVector v(TangentVector::Random(jmodel.nv()));
378  VectorXAD v_ad(jmodel_ad.nv());
379 
380  std::vector<double> v_vec((size_t)jmodel.nv());
381  Eigen::Map<TangentVector>(v_vec.data(),jmodel.nv(),1) = v;
382 
383  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
384  {
385  v_ad[k] = cs_v(k);
386  }
387 
388  jmodel.calc(jdata,q,v);
389  Motion m(jdata_base.v());
390  ConstraintXd Sref(jdata_base.S().matrix());
391 
392  jmodel_ad.calc(jdata_ad,q_ad,v_ad);
393  Vector6AD Y;
394  MotionAD m_ad(jdata_ad_base.v());
395 
396  casadi::SX cs_vel(6,1);
397  for(Eigen::DenseIndex k = 0; k < 6; ++k)
398  {
399  cs_vel(k) = m_ad.toVector()[k];
400  }
401  casadi::Function eval_velocity("eval_velocity", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {cs_vel});
402  std::cout << "Joint Velocity = " << eval_velocity << std::endl;
403 
404  casadi::DMVector res_vel = eval_velocity(casadi::DMVector {q_vec,v_vec});
405  std::cout << "v(q,v)=" << res_vel << std::endl;
406 
407  BOOST_CHECK(m.linear().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data())));
408  BOOST_CHECK(m.angular().isApprox(Eigen::Map<Motion::Vector3>(res_vel[0]->data()+3)));
409 
410  casadi::SX dvel_dv = jacobian(cs_vel, cs_v);
411  casadi::Function eval_S("eval_S", casadi::SXVector {cs_q,cs_v}, casadi::SXVector {dvel_dv});
412  std::cout << "S = " << eval_S << std::endl;
413 
414  casadi::DMVector res_S = eval_S(casadi::DMVector {q_vec,v_vec});
415  std::cout << "res_S:" << res_S << std::endl;
416  ConstraintXd::DenseBase Sref_mat = Sref.matrix();
417 
418  for(Eigen::DenseIndex i = 0; i < 6; ++i)
419  {
420  for(Eigen::DenseIndex j = 0; i < Sref.nv(); ++i)
421  BOOST_CHECK(std::fabs(Sref_mat(i,j) - (double)res_S[0](i,j)) <= Eigen::NumTraits<double>::dummy_precision());
422  }
423 
424  std::cout << "--" << std::endl << std::endl;
425  }
426 };
427 
428 BOOST_AUTO_TEST_CASE(test_all_joints)
429 {
431  boost::mpl::for_each<JointModelVariant::types>(TestADOnJoints());
432 
434 }
435 
436 BOOST_AUTO_TEST_SUITE_END()
def test(dtype)
pinocchio::JointModelMimic< JointModel_ > JointModel
q
JointDataRevoluteTpl< double, 0, 0 > JointDataRX
void setIndexes(JointIndex id, int q, int v)
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
std::string shortname() const
JointDataTpl< double > JointData
static void test(const pinocchio::JointModelMimic< JointModel_ > &)
BOOST_AUTO_TEST_CASE(test_jointRX_motion_space)
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
JointDataDerived createData() const
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
MotionTpl< double, 0 > Motion
boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelMimicRX, JointModelMimicRY, JointModelMimicRZ, JointModelFreeFlyer, JointModelPlanar, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelTranslation, JointModelRUBX, JointModelRUBY, JointModelRUBZ, JointModelRevoluteUnboundedUnaligned, boost::recursive_wrapper< JointModelComposite > > JointModelVariant
JointModelDerived & derived()
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs) const
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
float m
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
void operator()(const pinocchio::JointModelBase< JointModel_ > &) const
void setIndexes(JointIndex id, int nq, int nv)
ConstraintTpl< Eigen::Dynamic, double, 0 > ConstraintXd
Definition: constraint.hpp:18
res
JointModelTpl< double > JointModel
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
static JointModel_ run()
static std::string name()
JointCollectionDefault::JointModelVariant JointModelVariant
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
static void test(const pinocchio::JointModelBase< JointModel > &jmodel)
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: src/fwd.hpp:55
SE3Tpl< double, 0 > SE3
JointModelRevoluteTpl< double, 0, 0 > JointModelRX


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