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 <boost/test/unit_test.hpp>
7 #include <iostream>
8 
9 #include "pinocchio/math/fwd.hpp"
10 #include "pinocchio/multibody/joint/joints.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include "pinocchio/algorithm/aba.hpp"
13 #include "pinocchio/algorithm/crba.hpp"
14 #include "pinocchio/algorithm/jacobian.hpp"
15 #include "pinocchio/algorithm/compute-all-terms.hpp"
16 
17 using namespace pinocchio;
18 
19 template<typename JointModel_> struct init;
20 
21 template<typename JointModel_>
22 struct init
23 {
24  static JointModel_ run()
25  {
26  JointModel_ jmodel;
27  jmodel.setIndexes(0,0,0);
28  return jmodel;
29  }
30 };
31 
32 template<typename Scalar, int Options>
34 {
36 
37  static JointModel run()
38  {
39  typedef typename JointModel::Vector3 Vector3;
40  JointModel jmodel(Vector3::Random().normalized());
41 
42  jmodel.setIndexes(0,0,0);
43  return jmodel;
44  }
45 };
46 
47 template<typename Scalar, int Options>
49 {
51 
52  static JointModel run()
53  {
54  typedef typename JointModel::Vector3 Vector3;
55  JointModel jmodel(Vector3::Random().normalized());
56 
57  jmodel.setIndexes(0,0,0);
58  return jmodel;
59  }
60 };
61 
62 template<typename Scalar, int Options>
64 {
66 
67  static JointModel run()
68  {
69  typedef typename JointModel::Vector3 Vector3;
70  JointModel jmodel(Vector3::Random().normalized());
71 
72  jmodel.setIndexes(0,0,0);
73  return jmodel;
74  }
75 };
76 
77 template<typename Scalar, int Options, template<typename,int> class JointCollection>
78 struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
79 {
81 
82  static JointModel run()
83  {
85  JointModel jmodel((JointModelRX()));
86 
87  jmodel.setIndexes(0,0,0);
88  return jmodel;
89  }
90 };
91 
92 template<typename Scalar, int Options, template<typename,int> class JointCollection>
93 struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
94 {
96 
97  static JointModel run()
98  {
101  JointModel jmodel((JointModelRX()));
102  jmodel.addJoint(JointModelRY());
103 
104  jmodel.setIndexes(0,0,0);
105  return jmodel;
106  }
107 };
108 
109 template<typename JointModel_>
110 struct init<pinocchio::JointModelMimic<JointModel_> >
111 {
113 
114  static JointModel run()
115  {
116  JointModel_ jmodel_ref = init<JointModel_>::run();
117 
118  JointModel jmodel(jmodel_ref,1.,0.);
119  jmodel.setIndexes(0,0,0);
120 
121  return jmodel;
122  }
123 };
124 
125 BOOST_AUTO_TEST_SUITE(joint_model_base_test)
126 
127 template<typename TestDerived>
129 {
130  template<typename JointModel>
132  {
134  return TestDerived::test(jmodel);
135  }
136 };
137 
138 struct TestJointModelIsEqual : TestJointModel<TestJointModelIsEqual>
139 {
140  template<typename JointModel>
141  static void test(const JointModelBase<JointModel> & jmodel)
142  {
143  JointModel jmodel_copy = jmodel.derived();
144  BOOST_CHECK(jmodel_copy == jmodel);
145 
146  JointModel jmodel_any;
147  BOOST_CHECK(jmodel_any != jmodel);
148  BOOST_CHECK(!jmodel_any.isEqual(jmodel));
149  }
150 };
151 
153 {
155  boost::mpl::for_each<JointModelVariant::types>(TestJointModelIsEqual());
156 
157  JointModelRX joint_revolutex;
158  JointModelRY joint_revolutey;
159 
160  BOOST_CHECK(joint_revolutex != joint_revolutey);
161 
162  JointModel jmodelx(joint_revolutex);
163  jmodelx.setIndexes(0,0,0);
165 
166  JointModel jmodel_any;
167  BOOST_CHECK(jmodel_any != jmodelx);
168 }
169 
170 struct TestJointModelCast : TestJointModel<TestJointModelCast>
171 {
172  template<typename JointModel>
173  static void test(const JointModelBase<JointModel> & jmodel)
174  {
175  typedef typename JointModel::Scalar Scalar;
176  BOOST_CHECK(jmodel == jmodel);
177  BOOST_CHECK(jmodel.template cast<Scalar>().isEqual(jmodel));
178  BOOST_CHECK(jmodel.template cast<Scalar>() == jmodel);
179  BOOST_CHECK(jmodel.template cast<long double>().template cast<double>() == jmodel);
180  }
181 };
182 
184 {
186  boost::mpl::for_each<JointModelVariant::types>(TestJointModelCast());
187 
189 }
190 
191 struct TestJointModelDisp : TestJointModel<TestJointModelDisp>
192 {
193  template<typename JointModel>
194  static void test(const JointModelBase<JointModel> & jmodel)
195  {
196  typedef typename JointModel::JointDataDerived JointData;
197 
198  std::cout << "shortname: " << jmodel.shortname() << std::endl;
199  std::cout << "classname: " << jmodel.classname() << std::endl;
200  std::cout << "disp:\n" << jmodel << std::endl;
201 
202  JointData jdata = jmodel.createData();
203 
204  std::cout << "shortname: " << jdata.shortname() << std::endl;
205  std::cout << "classname: " << jdata.classname() << std::endl;
206  std::cout << "disp:\n" << jdata << std::endl;
207  }
208 };
209 
211 {
213  boost::mpl::for_each<JointModelVariant::types>(TestJointModelDisp());
214 
216 }
217 
218 BOOST_AUTO_TEST_SUITE_END()
static std::string classname()
pinocchio::JointModelMimic< JointModel_ > JointModel
Definition: all-joints.cpp:112
void operator()(const pinocchio::JointModelBase< JointModel > &) const
Definition: all-joints.cpp:131
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
Definition: all-joints.cpp:50
std::string shortname() const
NewScalar cast(const Scalar &value)
Definition: cast.hpp:13
JointDataTpl< double > JointData
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
bool isEqual(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived.
JointDataDerived createData() const
SE3::Scalar Scalar
Definition: conversions.cpp:13
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
std::string shortname() const
bool isEqual(const JointModelBase< OtherDerived > &) const
BOOST_AUTO_TEST_CASE(isEqual)
Definition: all-joints.cpp:152
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()
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
Definition: all-joints.cpp:95
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
Definition: all-joints.cpp:35
void setIndexes(JointIndex id, int nq, int nv)
static std::string classname()
Main pinocchio namespace.
Definition: timings.cpp:28
static void test(const JointModelBase< JointModel > &jmodel)
Definition: all-joints.cpp:141
JointModelTpl< double > JointModel
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Definition: all-joints.cpp:65
static JointModel_ run()
Definition: all-joints.cpp:24
bool isEqual(const JointModelBase< JointModelDerived > &other) const
JointCollectionDefault::JointModelVariant JointModelVariant
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
Definition: all-joints.cpp:80
static void test(const JointModelBase< JointModel > &jmodel)
Definition: all-joints.cpp:194
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
static void test(const JointModelBase< JointModel > &jmodel)
Definition: all-joints.cpp:173


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