all-joints.cpp
Go to the documentation of this file.
1 //
2 // Copyright(c) 2015-2021 CNRS INRIA
3 // Copyright(c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #include "pinocchio/math/fwd.hpp"
13 
14 #include <boost/test/unit_test.hpp>
15 #include <iostream>
16 
17 using namespace pinocchio;
18 
19 template<typename JointModel_>
20 struct init;
21 
22 template<typename JointModel_>
23 struct init
24 {
25  static JointModel_ run()
26  {
27  JointModel_ jmodel;
28  jmodel.setIndexes(0, 0, 0);
29  return jmodel;
30  }
31 };
32 
33 template<typename Scalar, int Options>
35 {
37 
38  static JointModel run()
39  {
40  typedef typename JointModel::Vector3 Vector3;
41  JointModel jmodel(Vector3::Random().normalized());
42 
43  jmodel.setIndexes(0, 0, 0);
44  return jmodel;
45  }
46 };
47 
48 template<typename Scalar, int Options>
50 {
52 
53  static JointModel run()
54  {
55  typedef typename JointModel::Vector3 Vector3;
56  JointModel jmodel(Vector3::Random().normalized());
57 
58  jmodel.setIndexes(0, 0, 0);
59  return jmodel;
60  }
61 };
62 
63 template<typename Scalar, int Options>
65 {
67 
68  static JointModel run()
69  {
70  typedef typename JointModel::Vector3 Vector3;
71  JointModel jmodel(Vector3::Random().normalized());
72 
73  jmodel.setIndexes(0, 0, 0);
74  return jmodel;
75  }
76 };
77 
78 template<typename Scalar, int Options, template<typename, int> class JointCollection>
79 struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
80 {
82 
83  static JointModel run()
84  {
86  JointModel jmodel((JointModelRX()));
87 
88  jmodel.setIndexes(0, 0, 0);
89  return jmodel;
90  }
91 };
92 
93 template<typename Scalar, int Options, template<typename, int> class JointCollection>
94 struct init<pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection>>
95 {
97 
98  static JointModel run()
99  {
102  JointModel jmodel((JointModelRX()));
103  jmodel.addJoint(JointModelRY());
104 
105  jmodel.setIndexes(0, 0, 0);
106  return jmodel;
107  }
108 };
109 
110 template<typename JointModel_>
111 struct init<pinocchio::JointModelMimic<JointModel_>>
112 {
114 
115  static JointModel run()
116  {
117  JointModel_ jmodel_ref = init<JointModel_>::run();
118 
119  JointModel jmodel(jmodel_ref, 1., 0.);
120  jmodel.setIndexes(0, 0, 0);
121 
122  return jmodel;
123  }
124 };
125 
126 template<typename Scalar, int Options>
127 struct init<pinocchio::JointModelUniversalTpl<Scalar, Options>>
128 {
130 
131  static JointModel run()
132  {
134 
135  jmodel.setIndexes(0, 0, 0);
136  return jmodel;
137  }
138 };
139 
140 template<typename Scalar, int Options, int axis>
141 struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
142 {
144 
145  static JointModel run()
146  {
147  JointModel jmodel(static_cast<Scalar>(0.5));
148 
149  jmodel.setIndexes(0, 0, 0);
150  return jmodel;
151  }
152 };
153 
154 template<typename Scalar, int Options>
156 {
158 
159  static JointModel run()
160  {
161  typedef typename JointModel::Vector3 Vector3;
162  JointModel jmodel(Vector3::Random().normalized());
163 
164  jmodel.setIndexes(0, 0, 0);
165  return jmodel;
166  }
167 };
168 
169 BOOST_AUTO_TEST_SUITE(joint_model_base_test)
170 
171 template<typename TestDerived>
173 {
174  template<typename JointModel>
176  {
178  return TestDerived::test(jmodel);
179  }
180 };
181 
182 struct TestJointModelIsEqual : TestJointModel<TestJointModelIsEqual>
183 {
184  template<typename JointModel>
185  static void test(const JointModelBase<JointModel> & jmodel)
186  {
187  JointModel jmodel_copy = jmodel.derived();
188  BOOST_CHECK(jmodel_copy == jmodel.derived());
189 
190  JointModel jmodel_any;
191  BOOST_CHECK(jmodel_any != jmodel.derived());
192  BOOST_CHECK(!jmodel_any.isEqual(jmodel.derived()));
193  }
194 };
195 
196 struct TestJointModelTransform : TestJointModel<TestJointModelTransform>
197 {
198  template<typename JointModel>
199  static void test(const JointModelBase<JointModel> & jmodel)
200  {
201  typedef typename JointModel::JointDataDerived JointData;
202  JointData jdata = jmodel.createData();
203  Eigen::Matrix<typename JointModel::Scalar, 3, 1> v = jdata.M_accessor().translation();
204  Eigen::Matrix<typename JointModel::Scalar, 3, 3> R = jdata.M_accessor().rotation();
205  }
206 };
207 
209 {
211  boost::mpl::for_each<JointModelVariant::types>(TestJointModelIsEqual());
212 
213  JointModelRX joint_revolutex;
214  JointModelRY joint_revolutey;
215 
216  BOOST_CHECK(joint_revolutex != joint_revolutey);
217 
218  JointModel jmodelx(joint_revolutex);
219  jmodelx.setIndexes(0, 0, 0);
221 
222  JointModel jmodel_any;
223  BOOST_CHECK(jmodel_any != jmodelx);
224 }
225 
227 {
229  boost::mpl::for_each<JointModelVariant::types>(TestJointModelTransform());
230 
232 }
233 
234 struct TestJointModelCast : TestJointModel<TestJointModelCast>
235 {
236  template<typename JointModel>
237  static void test(const JointModelBase<JointModel> & jmodel)
238  {
239  typedef typename JointModel::Scalar Scalar;
240  BOOST_CHECK(jmodel == jmodel);
241  BOOST_CHECK(jmodel.template cast<Scalar>().isEqual(jmodel));
242  BOOST_CHECK(jmodel.template cast<Scalar>() == jmodel);
243  BOOST_CHECK_MESSAGE(
244  jmodel.template cast<long double>().template cast<double>() == jmodel,
245  std::string("Error when casting " + jmodel.shortname() + " from long double to double."));
246  }
247 };
248 
250 {
252  boost::mpl::for_each<JointModelVariant::types>(TestJointModelCast());
253 
255 }
256 
257 struct TestJointModelDisp : TestJointModel<TestJointModelDisp>
258 {
259  template<typename JointModel>
260  static void test(const JointModelBase<JointModel> & jmodel)
261  {
262  typedef typename JointModel::JointDataDerived JointData;
263 
264  std::cout << "shortname: " << jmodel.shortname() << std::endl;
265  std::cout << "classname: " << jmodel.classname() << std::endl;
266  std::cout << "disp:\n" << jmodel << std::endl;
267 
268  JointData jdata = jmodel.createData();
269 
270  std::cout << "shortname: " << jdata.shortname() << std::endl;
271  std::cout << "classname: " << jdata.classname() << std::endl;
272  std::cout << "disp:\n" << jdata << std::endl;
273  }
274 };
275 
277 {
279  boost::mpl::for_each<JointModelVariant::types>(TestJointModelDisp());
280 
282 }
283 
284 BOOST_AUTO_TEST_SUITE_END()
pinocchio::JointModelUniversalTpl
Definition: multibody/joint/fwd.hpp:102
pinocchio::JointModelRevoluteUnalignedTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
pinocchio::JointModelBase::createData
JointDataDerived createData() const
Definition: joint-model-base.hpp:91
init< pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > >::JointModel
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Definition: all-joints.cpp:66
TestJointModelTransform
Definition: all-joints.cpp:196
pinocchio::JointModelRevoluteUnboundedUnalignedTpl
Definition: multibody/joint/fwd.hpp:46
pinocchio::JointModelHelicalTpl
Definition: multibody/joint/fwd.hpp:60
TestJointModelIsEqual
Definition: all-joints.cpp:182
init< pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > >::JointModel
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
Definition: all-joints.cpp:96
pinocchio::JointModelMimic
Definition: joint-mimic.hpp:307
pinocchio::JointModelRevoluteUnboundedUnalignedTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
compute-all-terms.hpp
init< pinocchio::JointModelMimic< JointModel_ > >::JointModel
pinocchio::JointModelMimic< JointModel_ > JointModel
Definition: all-joints.cpp:113
pinocchio::JointModelBase
Definition: joint-model-base.hpp:75
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(isEqual)
Definition: all-joints.cpp:208
pinocchio::JointModelRX
JointModelRevoluteTpl< context::Scalar, context::Options, 0 > JointModelRX
Definition: joint-revolute.hpp:873
init< pinocchio::JointModelTpl< Scalar, Options, JointCollection > >::run
static JointModel run()
Definition: all-joints.cpp:83
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
TestJointModelCast
Definition: all-joints.cpp:234
pinocchio::JointDataTpl< context::Scalar >
pinocchio::JointModelUniversalTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
pinocchio::JointModelTpl::isEqual
bool isEqual(const JointModelBase< JointModelDerived > &other) const
Definition: joint-generic.hpp:323
R
R
pinocchio::cast
NewScalar cast(const Scalar &value)
Definition: utils/cast.hpp:13
pinocchio::JointModelCompositeTpl::addJoint
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
Definition: joint-composite.hpp:278
rnea.hpp
TestJointModelDisp
Definition: all-joints.cpp:257
aba.hpp
pinocchio::CartesianAxis::vector
static Eigen::Matrix< Scalar, 3, 1 > vector()
Definition: cartesian-axis.hpp:79
init< pinocchio::JointModelMimic< JointModel_ > >::run
static JointModel run()
Definition: all-joints.cpp:115
pinocchio::JointModelRY
JointModelRevoluteTpl< context::Scalar, context::Options, 1 > JointModelRY
Definition: joint-revolute.hpp:877
pinocchio::JointModelTpl::setIndexes
void setIndexes(JointIndex id, int nq, int nv)
Definition: joint-generic.hpp:414
init< pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > >::run
static JointModel run()
Definition: all-joints.cpp:53
pinocchio::JointModelBase::classname
static std::string classname()
Definition: joint-model-base.hpp:218
pinocchio::JointModelHelicalTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
init< pinocchio::JointModelHelicalUnalignedTpl< Scalar, Options > >::JointModel
pinocchio::JointModelHelicalUnalignedTpl< Scalar, Options > JointModel
Definition: all-joints.cpp:157
TestJointModel::operator()
void operator()(const pinocchio::JointModelBase< JointModel > &) const
Definition: all-joints.cpp:175
pinocchio::JointData
JointDataTpl< context::Scalar > JointData
Definition: multibody/joint/fwd.hpp:162
init< pinocchio::JointModelHelicalUnalignedTpl< Scalar, Options > >::run
static JointModel run()
Definition: all-joints.cpp:159
TestJointModelCast::test
static void test(const JointModelBase< JointModel > &jmodel)
Definition: all-joints.cpp:237
pinocchio::JointModelHelicalUnalignedTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Definition: joint-helical-unaligned.hpp:635
pinocchio::JointDataTpl::classname
static std::string classname()
Definition: joint-generic.hpp:217
pinocchio::JointModelVariant
JointCollectionDefault::JointModelVariant JointModelVariant
Definition: joint-collection.hpp:205
pinocchio::JointModelRevoluteUnboundedUnalignedTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: joint-revolute-unbounded-unaligned.hpp:138
pinocchio::JointModelTpl
Definition: multibody/joint/fwd.hpp:155
pinocchio::JointModelBase::shortname
std::string shortname() const
Definition: joint-model-base.hpp:214
pinocchio::JointModelHelicalUnalignedTpl
Definition: multibody/joint/fwd.hpp:65
fwd.hpp
init< pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > >::JointModel
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
Definition: all-joints.cpp:51
init< pinocchio::JointModelHelicalTpl< Scalar, Options, axis > >::JointModel
pinocchio::JointModelHelicalTpl< Scalar, Options, axis > JointModel
Definition: all-joints.cpp:143
pinocchio::JointModelPrismaticUnalignedTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Definition: joint-prismatic-unaligned.hpp:596
init< pinocchio::JointModelTpl< Scalar, Options, JointCollection > >::JointModel
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
Definition: all-joints.cpp:81
init< pinocchio::JointModelUniversalTpl< Scalar, Options > >::JointModel
pinocchio::JointModelUniversalTpl< Scalar, Options > JointModel
Definition: all-joints.cpp:129
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
init< pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > >::run
static JointModel run()
Definition: all-joints.cpp:68
pinocchio::JointModel
JointModelTpl< context::Scalar > JointModel
Definition: multibody/joint/fwd.hpp:155
TestJointModel
Definition: all-joints.cpp:172
pinocchio::JointModelRevoluteTpl
Definition: multibody/joint/fwd.hpp:33
init< pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > >::JointModel
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
Definition: all-joints.cpp:36
pinocchio::JointModelHelicalUnalignedTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
TestJointModelIsEqual::test
static void test(const JointModelBase< JointModel > &jmodel)
Definition: all-joints.cpp:185
pinocchio::JointModelRevoluteUnalignedTpl
Definition: multibody/joint/fwd.hpp:38
pinocchio::JointModelPrismaticUnalignedTpl
Definition: multibody/joint/fwd.hpp:94
pinocchio::JointModelCompositeTpl
Definition: multibody/joint/fwd.hpp:141
pinocchio::isEqual
bool isEqual(const ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > &cdata_generic, const ConstraintDataBase< ConstraintDataDerived > &cdata)
Definition: constraint-model-visitor.hpp:273
TestJointModelDisp::test
static void test(const JointModelBase< JointModel > &jmodel)
Definition: all-joints.cpp:260
init::run
static JointModel_ run()
Definition: all-joints.cpp:25
pinocchio::JointModelRevoluteUnalignedTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Definition: joint-revolute-unaligned.hpp:614
pinocchio::JointModelPrismaticUnalignedTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
joints.hpp
TestJointModelTransform::test
static void test(const JointModelBase< JointModel > &jmodel)
Definition: all-joints.cpp:199
pinocchio::JointDataTpl::shortname
std::string shortname() const
Definition: joint-generic.hpp:221
jacobian.hpp
init
Definition: all-joints.cpp:20
crba.hpp
pinocchio::JointModelMimic::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
init< pinocchio::JointModelHelicalTpl< Scalar, Options, axis > >::run
static JointModel run()
Definition: all-joints.cpp:145
pinocchio::JointCollectionDefaultTpl::JointModelVariant
boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelMimicRX, JointModelMimicRY, JointModelMimicRZ, JointModelFreeFlyer, JointModelPlanar, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelTranslation, JointModelRUBX, JointModelRUBY, JointModelRUBZ, JointModelRevoluteUnboundedUnaligned, JointModelHx, JointModelHy, JointModelHz, JointModelHelicalUnaligned, JointModelUniversal, boost::recursive_wrapper< JointModelComposite > > JointModelVariant
Definition: joint-collection.hpp:113
init< pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > >::run
static JointModel run()
Definition: all-joints.cpp:38
pinocchio::JointModelBase::isEqual
bool isEqual(const JointModelBase< OtherDerived > &) const
Definition: joint-model-base.hpp:242
pinocchio::JointModelBase::derived
JointModelDerived & derived()
Definition: joint-model-base.hpp:82
init< pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > >::run
static JointModel run()
Definition: all-joints.cpp:98
init< pinocchio::JointModelUniversalTpl< Scalar, Options > >::run
static JointModel run()
Definition: all-joints.cpp:131
pinocchio::JointDataTpl::M_accessor
Transformation_t M_accessor() const
Definition: joint-generic.hpp:188
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


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