joint-generic.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-2019 CNRS INRIA
3 //
4 
5 #include "pinocchio/multibody/joint/joint-generic.hpp"
6 
7 #include "pinocchio/multibody/liegroup/liegroup.hpp"
8 
9 #include "pinocchio/multibody/model.hpp"
10 
11 #include "pinocchio/algorithm/joint-configuration.hpp"
12 
13 #include <boost/test/unit_test.hpp>
14 #include <boost/utility/binary.hpp>
15 
16 using namespace pinocchio;
17 
18 template <typename JointModel>
21 {
22  typedef typename LieGroup<JointModel>::type LieGroupType;
23 
24  std::cout << "Testing Joint over " << jmodel.shortname() << std::endl;
25 
27  q1 = LieGroupType().random();
28  q2 = LieGroupType().random();
29 
31  v1(Eigen::VectorXd::Random(jdata.S().nv())),
32  v2(Eigen::VectorXd::Random(jdata.S().nv()));
33 
34  Inertia::Matrix6
35  Ia(pinocchio::Inertia::Random().matrix()),
36  Ia2(pinocchio::Inertia::Random().matrix());
37  bool update_I = false;
38 
39 
40  jmodel.calc(jdata.derived(), q1, v1);
41  jmodel.calc_aba(jdata.derived(), Ia, update_I);
42 
43  pinocchio::JointModel jma(jmodel);
44  BOOST_CHECK(jmodel == jma);
45  BOOST_CHECK(jma == jmodel);
46  BOOST_CHECK(jma.hasSameIndexes(jmodel));
47 
48  pinocchio::JointData jda(jdata);
49  BOOST_CHECK(jda == jdata);
50  BOOST_CHECK(jdata == jda);
51 
52  jma.calc(jda, q1, v1);
53  jma.calc_aba(jda, Ia, update_I);
54 
55  pinocchio::JointData jda_other(jdata);
56 
57  jma.calc(jda_other, q2, v2);
58  jma.calc_aba(jda_other, Ia2, update_I);
59 
60  BOOST_CHECK(jda_other != jda);
61  BOOST_CHECK(jda != jda_other);
62  BOOST_CHECK(jda_other != jdata);
63  BOOST_CHECK(jdata != jda_other);
64 
65  const std::string error_prefix("JointModel on " + jma.shortname());
66  BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq "));
67  BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv "));
68 
69  BOOST_CHECK_MESSAGE(jmodel.idx_q() == jma.idx_q() ,std::string(error_prefix + " - Idx_q "));
70  BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v() ,std::string(error_prefix + " - Idx_v "));
71  BOOST_CHECK_MESSAGE(jmodel.id() == jma.id() ,std::string(error_prefix + " - JointId "));
72 
73  BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S().matrix()),std::string(error_prefix + " - ConstraintXd "));
74  BOOST_CHECK_MESSAGE( (jda.M()).isApprox((jdata.M())),std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
75  BOOST_CHECK_MESSAGE( (jda.v()).isApprox( (pinocchio::Motion(jdata.v()))),std::string(error_prefix + " - Joint motions "));
76  BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c()),std::string(error_prefix + " - Joint bias "));
77 
78  BOOST_CHECK_MESSAGE((jda.U()).isApprox(jdata.U()),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
79  BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox(jdata.Dinv()),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
80  BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox(jdata.UDinv()),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
81 
82  // Test vxS
83  typedef typename JointModel::Constraint_t Constraint_t;
84  typedef typename Constraint_t::DenseBase ConstraintDense;
85 
87  ConstraintDense vxS(v.cross(jdata.S()));
88  ConstraintDense vxS_ref = v.toActionMatrix() * jdata.S().matrix();
89 
90  BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref),std::string(error_prefix + "- Joint vxS operation "));
91 
92  // Test Y*S
93  const Inertia Isparse(Inertia::Random());
94  const Inertia::Matrix6 Idense(Isparse.matrix());
95 
96  const ConstraintDense IsparseS = Isparse * jdata.S();
97  const ConstraintDense IdenseS = Idense * jdata.S();
98 
99  BOOST_CHECK_MESSAGE(IdenseS.isApprox(IsparseS),std::string(error_prefix + "- Joint YS operation "));
100 
101 }
102 
103 template<typename JointModel_> struct init;
104 
105 template<typename JointModel_>
106 struct init
107 {
108  static JointModel_ run()
109  {
110  JointModel_ jmodel;
111  jmodel.setIndexes(0,0,0);
112  return jmodel;
113  }
114 };
115 
116 template<typename Scalar, int Options>
117 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
118 {
120 
121  static JointModel run()
122  {
123  typedef typename JointModel::Vector3 Vector3;
124  JointModel jmodel(Vector3::Random().normalized());
125 
126  jmodel.setIndexes(0,0,0);
127  return jmodel;
128  }
129 };
130 
131 template<typename Scalar, int Options>
132 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
133 {
135 
136  static JointModel run()
137  {
138  typedef typename JointModel::Vector3 Vector3;
139  JointModel jmodel(Vector3::Random().normalized());
140 
141  jmodel.setIndexes(0,0,0);
142  return jmodel;
143  }
144 };
145 
146 template<typename Scalar, int Options>
147 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
148 {
150 
151  static JointModel run()
152  {
153  typedef typename JointModel::Vector3 Vector3;
154  JointModel jmodel(Vector3::Random().normalized());
155 
156  jmodel.setIndexes(0,0,0);
157  return jmodel;
158  }
159 };
160 
161 template<typename Scalar, int Options, template<typename,int> class JointCollection>
162 struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
163 {
165 
166  static JointModel run()
167  {
169  JointModel jmodel((JointModelRX()));
170 
171  jmodel.setIndexes(0,0,0);
172  return jmodel;
173  }
174 };
175 
176 template<typename Scalar, int Options, template<typename,int> class JointCollection>
177 struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
178 {
180 
181  static JointModel run()
182  {
185  JointModel jmodel((JointModelRX()));
186  jmodel.addJoint(JointModelRY());
187 
188  jmodel.setIndexes(0,0,0);
189  return jmodel;
190  }
191 };
192 
193 template<typename JointModel_>
194 struct init<pinocchio::JointModelMimic<JointModel_> >
195 {
197 
198  static JointModel run()
199  {
200  JointModel_ jmodel_ref = init<JointModel_>::run();
201 
202  JointModel jmodel(jmodel_ref,1.,0.);
203  jmodel.setIndexes(0,0,0);
204 
205  return jmodel;
206  }
207 };
208 
209 struct TestJoint{
210 
211  template <typename JointModel>
213  {
215  jmodel.setIndexes(0,0,0);
216  typename JointModel::JointDataDerived jdata = jmodel.createData();
217 
218  test_joint_methods(jmodel, jdata);
219  }
220 
222  {
223 
224  }
225 
226 };
227 
228 namespace pinocchio
229 {
230 
231  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointTest;
232  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointModelTest;
233  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointDataTest;
234 
235  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
236  struct traits< JointDataTest<Scalar,Options,JointCollectionTpl> >
238 
239  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
240  struct traits< JointModelTest<Scalar,Options,JointCollectionTpl> >
242 
243  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
244  struct traits< JointTest<_Scalar,_Options,JointCollectionTpl> >
245  {
246  enum {
247  Options = _Options,
248  NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
249  NV = Eigen::Dynamic
250  };
251  typedef _Scalar Scalar;
252 
259 
260  // [ABA]
261  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
262  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
263  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
264 
265  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
266  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
267  };
268 
269  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
270  struct JointModelTest
271  : JointCollectionTpl<_Scalar,_Options>::JointModelVariant
272  , JointModelBase< JointModelTest<_Scalar,_Options,JointCollectionTpl> >
273  {
275  typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
278 
279  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
280 
281  JointModelTest(const JointModelVariant & jmodel)
282  : VariantBase(jmodel)
283  {}
284 
285  };
286 
287 }
288 
289 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
290 
291 BOOST_AUTO_TEST_CASE(test_joint_from_joint_composite)
292 {
293  typedef JointCollectionDefault JointCollection;
295 
296  JointModelRX jmodel_revolute_x;
297  JointModel jmodel_generic(jmodel_revolute_x);
298  JointModelVariant jmodel_variant(jmodel_revolute_x);
299 
300  JointModelTest<double,0,JointCollectionDefaultTpl> jmodel_test(jmodel_revolute_x);
301  std::vector< JointModelTest<double,0,JointCollectionDefaultTpl> > jmodel_test_vector;
302  jmodel_test_vector.push_back(JointModelTest<double,0,JointCollectionDefaultTpl>(jmodel_revolute_x));
303 
304  std::vector<JointModelVariant> jmodel_variant_vector;
305  jmodel_variant_vector.push_back(jmodel_revolute_x);
306 
307  std::vector<JointModel> jmodel_generic_vector;
308  jmodel_generic_vector.push_back((JointModel)jmodel_revolute_x);
309  JointModelComposite jmodel_composite(jmodel_revolute_x);
310 }
311 
312 BOOST_AUTO_TEST_CASE ( test_all_joints )
313 {
314  boost::mpl::for_each<JointModelVariant::types>(TestJoint());
315 }
316 
317 BOOST_AUTO_TEST_CASE(test_empty_model)
318 {
319  JointModel jmodel;
320  std::cout << "nq " << jmodel.nq() << std::endl;
321  std::cout << "nv " << jmodel.nv() << std::endl;
322  std::cout << "idx_q " << jmodel.idx_q() << std::endl;
323  std::cout << "idx_v " << jmodel.idx_v() << std::endl;
324  std::cout << "id " << jmodel.id() << std::endl;
325  std::cout << "name " << jmodel.shortname() << std::endl;
326 
327  BOOST_CHECK(jmodel.idx_q() == -1);
328  BOOST_CHECK(jmodel.idx_v() == -1);
329 }
330 
332 {
333  JointModelRX joint_revolutex;
334  JointModelRY joint_revolutey;
335 
336  BOOST_CHECK(joint_revolutex != joint_revolutey);
337 
338  JointModel jmodelx(joint_revolutex);
339  jmodelx.setIndexes(0,0,0);
340 
341  JointModel jmodelx_copy = jmodelx;
342  BOOST_CHECK(jmodelx_copy == jmodelx);
343  BOOST_CHECK(jmodelx_copy == jmodelx.derived());
344 
345  JointModel jmodely(joint_revolutey);
346  // TODO: the comparison of two variants is not supported by some old version of BOOST
347 // BOOST_CHECK(jmodely.toVariant() != jmodelx.toVariant());
348  BOOST_CHECK(jmodely != jmodelx);
349 }
350 
352 {
353  JointModelRX joint_revolutex;
354 
355  JointModel jmodelx(joint_revolutex);
356  jmodelx.setIndexes(0,0,0);
357 
358  BOOST_CHECK(jmodelx.cast<double>() == jmodelx);
359  BOOST_CHECK(jmodelx.cast<long double>().cast<double>() == jmodelx);
360 }
361 
363 {
364 
365  template <typename JointModel>
367  {
368  JointModel jmodel_init = init<JointModel>::run();
369  typedef typename JointModel::JointDataDerived JointData;
370 
371  Model model;
372  model.addJoint(0,jmodel_init,SE3::Random(),"toto");
373  model.lowerPositionLimit.fill(-1.);
374  model.upperPositionLimit.fill( 1.);
375 
376  const JointModel & jmodel = boost::get<JointModel>(model.joints[1]);
377 
379  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
380 
381  JointData jdata = jmodel.createData();
382 
383  jmodel.calc(jdata,q,v);
384  Inertia::Matrix6 I = Inertia::Matrix6::Identity();
385  jmodel.calc_aba(jdata,I,false);
386  test(jdata);
387  }
388 
389  template <typename JointModel>
391  {
392 
393  }
394 
395  template<typename JointData>
396  static void test(const JointData & jdata)
397  {
398  pinocchio::JointData jdata_generic1(jdata);
399 
400  std::cout << "name: " << jdata_generic1.shortname() << std::endl;
401  BOOST_CHECK(jdata_generic1 == jdata_generic1);
402  }
403 
404 };
405 
406 BOOST_AUTO_TEST_CASE(test_operator_equal)
407 {
408  boost::mpl::for_each<JointModelVariant::types>(TestJointOperatorEqual());
409 }
410 
411 BOOST_AUTO_TEST_SUITE_END ()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
void test_joint_methods(JointModelBase< JointModel > &jmodel, JointDataBase< typename JointModel::JointDataDerived > &jdata)
def test(dtype)
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > UD_t
int NQ
Definition: dpendulum.py:8
pinocchio::JointModelMimic< JointModel_ > JointModel
void operator()(const pinocchio::JointModelMimic< JointModel > &) const
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
void setIndexes(JointIndex id, int q, int v)
void operator()(const JointModelBase< JointModel > &) const
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
UDTypeConstRef UDinv() const
BiasTypeConstRef c() const
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > U_t
static InertiaTpl Random()
DTypeConstRef Dinv() const
std::string shortname() const
static void test(const JointData &jdata)
NewScalar cast(const Scalar &value)
Definition: cast.hpp:13
JointDataTpl< double > JointData
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
JointDataTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
TansformTypeConstRef M() const
MotionTypeConstRef v() const
void operator()(const pinocchio::JointModelComposite &) const
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
bool isEqual(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
BOOST_AUTO_TEST_CASE(test_joint_from_joint_composite)
JointModelTest(const JointModelVariant &jmodel)
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_t
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
Definition: motion-base.hpp:87
SE3::Scalar Scalar
Definition: conversions.cpp:13
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
std::string shortname() const
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
JointCollection::JointModelVariant VariantBase
bool hasSameIndexes(const JointModelBase< JointModelDerived > &other) const
JointTpl< Scalar, Options, JointCollectionTpl > JointDerived
void operator()(const JointModelBase< JointModel > &) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
JointCollection::JointModelVariant JointModelVariant
JointModelDerived & derived()
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs) const
#define BOOST_TEST_MODULE
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I=false) const
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
std::string shortname() const
void setIndexes(JointIndex id, int nq, int nv)
Main pinocchio namespace.
Definition: timings.cpp:28
q2
int nv
Dimension of the velocity vector space.
JointCollectionTpl< _Scalar, _Options > JointCollection
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
static JointModel_ run()
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
UTypeConstRef U() const
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
ConstraintTypeConstRef S() const
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
JointCollectionDefault::JointModelVariant JointModelVariant
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
NV
Definition: dcrba.py:444
JointDataDerived createData() const
static MotionTpl Random()
Definition: motion-tpl.hpp:92
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q) const
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
ActionMatrixType toActionMatrix() const
Definition: motion-base.hpp:41
JointCollectionTpl & model
JointTest< _Scalar, _Options, JointCollectionTpl > JointDerived
ConstraintTpl< Eigen::Dynamic, Scalar, Options > Constraint_t
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


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