cppad-joints.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/multibody/joint/joint-generic.hpp"
8 #include "pinocchio/multibody/liegroup/liegroup.hpp"
9 #include "pinocchio/multibody/liegroup/liegroup-algo.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 BOOST_AUTO_TEST_CASE(test_jointRX_motion_space)
19 {
20  using CppAD::AD;
21  using CppAD::NearEqual;
22 
23  typedef AD<double> AD_double;
24  typedef pinocchio::JointCollectionDefaultTpl<AD_double> JointCollectionAD;
25  typedef pinocchio::JointCollectionDefaultTpl<double> JointCollection;
26 
27  typedef pinocchio::SE3Tpl<AD_double> SE3AD;
28  typedef pinocchio::MotionTpl<AD_double> MotionAD;
32 
33  typedef Eigen::Matrix<AD_double,Eigen::Dynamic,1> VectorXAD;
34  typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> MatrixX;
35 
36  typedef JointCollectionAD::JointModelRX JointModelRXAD;
37  typedef JointModelRXAD::ConfigVector_t ConfigVectorAD;
38 // typedef JointModelRXAD::TangentVector_t TangentVectorAD;
39  typedef JointCollectionAD::JointDataRX JointDataRXAD;
40 
42  typedef JointModelRX::ConfigVector_t ConfigVector;
43  typedef JointModelRX::TangentVector_t TangentVector;
45 
46  JointModelRX jmodel; jmodel.setIndexes(0,0,0);
47  JointDataRX jdata(jmodel.createData());
48 
49  JointModelRXAD jmodel_ad = jmodel.cast<AD_double>();
50  JointDataRXAD jdata_ad(jmodel_ad.createData());
51 
52  typedef pinocchio::LieGroup<JointModelRX>::type JointOperation;
53  ConfigVector q(jmodel.nq()); JointOperation().random(q);
54  ConfigVectorAD q_ad(q.cast<AD_double>());
55 
56  // Zero order
57  jmodel_ad.calc(jdata_ad,q_ad);
58  jmodel.calc(jdata,q);
59 
60  SE3 M1(jdata.M);
61  SE3AD M2(jdata_ad.M);
62  BOOST_CHECK(M1.isApprox(M2.cast<double>()));
63 
64  // First order
65  TangentVector v(TangentVector::Random(jmodel.nv()));
66  VectorXAD X(jmodel_ad.nv());
67 
68  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
69  {
70  X[k] = v[k];
71  }
72  CppAD::Independent(X);
73  jmodel_ad.calc(jdata_ad,q_ad,X);
74  jmodel.calc(jdata,q,v);
75  VectorXAD Y(6);
76  MotionAD m_ad(jdata_ad.v);
77  Motion m(jdata.v);
78  ConstraintXd Sref(jdata.S.matrix());
79 
80  for(Eigen::DenseIndex k = 0; k < 3; ++k)
81  {
82  Y[k+Motion::LINEAR] = m_ad.linear()[k];
83  Y[k+Motion::ANGULAR] = m_ad.angular()[k];
84  }
85 
86  CppAD::ADFun<double> vjoint(X,Y);
87 
88  CPPAD_TESTVECTOR(double) x((size_t)jmodel_ad.nv());
89  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
90  {
91  x[(size_t)k] = v[k];
92  }
93 
94  CPPAD_TESTVECTOR(double) jac = vjoint.Jacobian(x);
95  MatrixX S(6,jac.size()/6);
96  S = Eigen::Map<MatrixX>(jac.data(),S.rows(),S.cols());
97 
98  BOOST_CHECK(m.isApprox(m_ad.cast<double>()));
99 
100  BOOST_CHECK(Sref.matrix().isApprox(S));
101 }
102 
103 struct TestADOnJoints
104 {
105  template<typename JointModel>
107  {
108  JointModel jmodel;
109  jmodel.setIndexes(0,0,0);
110 
111  test(jmodel);
112  }
113 
114  template<typename Scalar, int Options>
116  {
118  typedef typename JointModel::Vector3 Vector3;
119  JointModel jmodel(Vector3::Random().normalized());
120  jmodel.setIndexes(0,0,0);
121 
122  test(jmodel);
123  }
124 
125  template<typename Scalar, int Options>
127  {
129  typedef typename JointModel::Vector3 Vector3;
130  JointModel jmodel(Vector3::Random().normalized());
131  jmodel.setIndexes(0,0,0);
132 
133  test(jmodel);
134  }
135 
136  template<typename Scalar, int Options>
138  {
140  typedef typename JointModel::Vector3 Vector3;
141  JointModel jmodel(Vector3::Random().normalized());
142  jmodel.setIndexes(0,0,0);
143 
144  test(jmodel);
145  }
146 
147  template<typename Scalar, int Options, template<typename,int> class JointCollection>
149  {
152  JointModel jmodel((JointModelRX()));
153  jmodel.setIndexes(0,0,0);
154 
155  test(jmodel);
156  }
157 
158  // TODO: implement it
159  template<typename JointModel_>
161  {
162  /* do nothing */
163  }
164 
165  template<typename Scalar, int Options, template<typename,int> class JointCollection>
167  {
171  JointModel jmodel((JointModelRX()));
172  jmodel.addJoint(JointModelRY());
173  jmodel.setIndexes(0,0,0);
174 
175  test(jmodel);
176  }
177 
178  template<typename JointModel>
179  static void test(const pinocchio::JointModelBase<JointModel> & jmodel)
180  {
181  using CppAD::AD;
182  using CppAD::NearEqual;
183 
184  typedef typename JointModel::Scalar Scalar;
185  typedef typename JointModel::JointDataDerived JointData;
186 
187  typedef AD<Scalar> AD_scalar;
188 
189  typedef pinocchio::SE3Tpl<AD_scalar> SE3AD;
190  typedef pinocchio::MotionTpl<AD_scalar> MotionAD;
194 
195  typedef Eigen::Matrix<AD_scalar,Eigen::Dynamic,1> VectorXAD;
196  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic> MatrixX;
197 
198  typedef typename pinocchio::CastType<AD_scalar,JointModel>::type JointModelAD;
199  typedef typename JointModelAD::JointDataDerived JointDataAD;
200 
201  typedef typename JointModelAD::ConfigVector_t ConfigVectorAD;
202 
203  typedef typename JointModel::ConfigVector_t ConfigVector;
204  typedef typename JointModel::TangentVector_t TangentVector;
205 
206  JointData jdata(jmodel.createData());
207  pinocchio::JointDataBase<JointData> & jdata_base = jdata;
208 
209  JointModelAD jmodel_ad = jmodel.template cast<AD_scalar>();
210  JointDataAD jdata_ad(jmodel_ad.createData());
211  pinocchio::JointDataBase<JointDataAD> & jdata_ad_base = jdata_ad;
212 
213  ConfigVector q(jmodel.nq());
214  ConfigVector lb(ConfigVector::Constant(jmodel.nq(),-1.));
215  ConfigVector ub(ConfigVector::Constant(jmodel.nq(),1.));
216 
217  typedef pinocchio::RandomConfigurationStep<pinocchio::LieGroupMap,ConfigVector,ConfigVector,ConfigVector> RandomConfigAlgo;
218  RandomConfigAlgo::run(jmodel.derived(),typename RandomConfigAlgo::ArgsType(q,lb,ub));
219 
220  ConfigVectorAD q_ad(q.template cast<AD_scalar>());
221 
222  // Zero order
223  jmodel_ad.calc(jdata_ad,q_ad);
224  jmodel.calc(jdata,q);
225 
226  SE3 M1(jdata_base.M());
227  SE3AD M2(jdata_ad_base.M());
228  BOOST_CHECK(M1.isApprox(M2.template cast<Scalar>()));
229 
230  // First order
231  TangentVector v(TangentVector::Random(jmodel.nv()));
232  VectorXAD X(jmodel_ad.nv());
233 
234  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
235  {
236  X[k] = v[k];
237  }
238  CppAD::Independent(X);
239  jmodel_ad.calc(jdata_ad,q_ad,X);
240  jmodel.calc(jdata,q,v);
241  VectorXAD Y(6);
242  MotionAD m_ad(jdata_ad_base.v());
243  Motion m(jdata_base.v());
244  ConstraintXd Sref(jdata_base.S().matrix());
245 
246  for(Eigen::DenseIndex k = 0; k < 3; ++k)
247  {
248  Y[k+Motion::LINEAR] = m_ad.linear()[k];
249  Y[k+Motion::ANGULAR] = m_ad.angular()[k];
250  }
251 
252  CppAD::ADFun<Scalar> vjoint(X,Y);
253 
254  CPPAD_TESTVECTOR(Scalar) x((size_t)jmodel_ad.nv());
255  for(Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k)
256  {
257  x[(size_t)k] = v[k];
258  }
259 
260  CPPAD_TESTVECTOR(Scalar) jac = vjoint.Jacobian(x);
261  MatrixX S(6,jac.size()/6);
262  S = Eigen::Map<typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixX)>(jac.data(),S.rows(),S.cols());
263 
264  BOOST_CHECK(m.isApprox(m_ad.template cast<Scalar>()));
265 
266  BOOST_CHECK(Sref.matrix().isApprox(S));
267  }
268 };
269 
270 BOOST_AUTO_TEST_CASE(test_all_joints)
271 {
273  boost::mpl::for_each<JointModelVariant::types>(TestADOnJoints());
274 
276 }
277 
278 BOOST_AUTO_TEST_SUITE_END()
q
JointDataRevoluteTpl< double, 0, 0 > JointDataRX
JointDataTpl< double > JointData
static void test(const pinocchio::JointModelMimic< JointModel_ > &)
void operator()(const pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > &) const
JointDataDerived createData() const
SE3::Scalar Scalar
Definition: conversions.cpp:13
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
void operator()(const pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > &) const
x
void operator()(const pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > &) const
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
float m
void setIndexes(JointIndex id, int nq, int nv)
ConstraintTpl< Eigen::Dynamic, double, 0 > ConstraintXd
Definition: constraint.hpp:18
BOOST_AUTO_TEST_CASE(test_jointRX_motion_space)
JointModelTpl< double > JointModel
void operator()(const pinocchio::JointModelMimic< JointModel_ > &) const
JointCollectionDefault::JointModelVariant JointModelVariant
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
void operator()(const pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > &) const
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
void operator()(const pinocchio::JointModelTpl< Scalar, Options, JointCollection > &) const
void operator()(const pinocchio::JointModelBase< JointModel > &) const


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