joint-mimic.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019 INRIA
3 //
4 
5 #include "pinocchio/multibody/joint/joint-generic.hpp"
6 #include "pinocchio/multibody/liegroup/liegroup.hpp"
7 
8 #include <boost/test/unit_test.hpp>
9 #include <boost/utility/binary.hpp>
10 
11 using namespace pinocchio;
12 
13 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
14 
15 typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
16 
17 template<typename JointModel>
19 {
20  typedef typename traits<JointModel>::JointDerived Joint;
21  typedef typename traits<Joint>::Constraint_t ConstraintType;
23  typedef ScaledConstraint<ConstraintType> ScaledConstraintType;
24 
25  JointData jdata = jmodel.createData();
26 
27  const double scaling_factor = 2.;
28  ConstraintType constraint_ref(jdata.S), constraint_ref_shared(jdata.S);
29  ScaledConstraintType scaled_constraint(constraint_ref_shared,scaling_factor);
30 
31  BOOST_CHECK(constraint_ref.nv() == scaled_constraint.nv());
32 
33  typedef typename JointModel::TangentVector_t TangentVector_t;
34  TangentVector_t v = TangentVector_t::Random();
35 
36  Motion m = scaled_constraint * v;
37  Motion m_ref = scaling_factor * (Motion)(constraint_ref * v);
38 
39  BOOST_CHECK(m.isApprox(m_ref));
40 
41  {
42  SE3 M = SE3::Random();
43  typename ScaledConstraintType::DenseBase S = M.act(scaled_constraint);
44  typename ScaledConstraintType::DenseBase S_ref = scaling_factor * M.act(constraint_ref);
45 
46  BOOST_CHECK(S.isApprox(S_ref));
47  }
48 
49  {
50  typename ScaledConstraintType::DenseBase S = scaled_constraint.matrix();
51  typename ScaledConstraintType::DenseBase S_ref = scaling_factor * constraint_ref.matrix();
52 
53  BOOST_CHECK(S.isApprox(S_ref));
54  }
55 
56  {
57  Motion v = Motion::Random();
58  typename ScaledConstraintType::DenseBase S = v.cross(scaled_constraint);
59  typename ScaledConstraintType::DenseBase S_ref = scaling_factor * v.cross(constraint_ref);
60 
61  BOOST_CHECK(S.isApprox(S_ref));
62  }
63 
64  // Test transpose operations
65  {
66  const Eigen::DenseIndex dim = 20;
67  const Matrix6x Fin = Matrix6x::Random(6,dim);
68  Eigen::MatrixXd Fout = scaled_constraint.transpose() * Fin;
69  Eigen::MatrixXd Fout_ref = scaling_factor * (constraint_ref.transpose() * Fin);
70  BOOST_CHECK(Fout.isApprox(Fout_ref));
71 
72  Force force_in(Force::Random());
73  Eigen::MatrixXd Stf = (scaled_constraint.transpose() * force_in);
74  Eigen::MatrixXd Stf_ref = scaling_factor * (constraint_ref.transpose() * force_in);
75  BOOST_CHECK(Stf_ref.isApprox(Stf));
76  }
77 
78  // CRBA Y*S
79  {
81  Eigen::MatrixXd YS = Y * scaled_constraint;
82  Eigen::MatrixXd YS_ref = scaling_factor * (Y * constraint_ref);
83 
84  BOOST_CHECK(YS.isApprox(YS_ref));
85  }
86 
87 }
88 
90 {
91 
92  template <typename JointModel>
94  {
95  JointModel jmodel;
96  jmodel.setIndexes(0,0,0);
97 
98  test_constraint_mimic(jmodel);
99  }
100 
102  {
103  JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
104  jmodel.setIndexes(0,0,0);
105 
106  test_constraint_mimic(jmodel);
107  }
108 
110  {
111  JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
112  jmodel.setIndexes(0,0,0);
113 
114  test_constraint_mimic(jmodel);
115  }
116 
117 };
118 
119 BOOST_AUTO_TEST_CASE(test_constraint)
120 {
121  using namespace pinocchio;
122  typedef boost::variant<
128  > Variant;
129 
130  boost::mpl::for_each<Variant::types>(TestJointConstraint());
131 }
132 
133 template<typename JointModel>
135 {
136  typedef typename traits<JointModel>::JointDerived Joint;
138 
139  JointData jdata = jmodel.createData();
140 
141  const double scaling_factor = 1.;
142  const double offset = 0.;
143 
144  typedef JointMimic<Joint> JointMimicType;
145  typedef typename traits<JointMimicType>::JointModelDerived JointModelMimicType;
146  typedef typename traits<JointMimicType>::JointDataDerived JointDataMimicType;
147 
148  // test constructor
149  JointModelMimicType jmodel_mimic(jmodel.derived(),scaling_factor,offset);
150  JointDataMimicType jdata_mimic = jmodel_mimic.createData();
151 
152  BOOST_CHECK(jmodel_mimic.nq() == 0);
153  BOOST_CHECK(jmodel_mimic.nv() == 0);
154 
155  BOOST_CHECK(jmodel_mimic.idx_q() == jmodel.idx_q());
156  BOOST_CHECK(jmodel_mimic.idx_v() == jmodel.idx_v());
157 
158  BOOST_CHECK(jmodel_mimic.idx_q() == 0);
159  BOOST_CHECK(jmodel_mimic.idx_v() == 0);
160 
161  typedef typename JointModelMimicType::ConfigVector_t ConfigVectorType;
162  typedef typename LieGroup<JointModel>::type LieGroupType;
163  ConfigVectorType q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones());
164 
165  jmodel.calc(jdata,q0);
166  jmodel_mimic.calc(jdata_mimic,q0);
167 
168  BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M()));
169  BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix()));
170 
171  typedef typename JointModelMimicType::TangentVector_t TangentVectorType;
172 
173  q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones());
174  TangentVectorType v0 = TangentVectorType::Random();
175  jmodel.calc(jdata,q0,v0);
176  jmodel_mimic.calc(jdata_mimic,q0,v0);
177 
178  BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M()));
179  BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix()));
180  BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic.v()));
181 }
182 
184 {
185 
186  template <typename JointModel>
188  {
189  JointModel jmodel;
190  jmodel.setIndexes(0,0,0);
191 
192  test_joint_mimic(jmodel);
193  }
194 
196  {
197  JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
198  jmodel.setIndexes(0,0,0);
199 
200  test_joint_mimic(jmodel);
201  }
202 
204  {
205  JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
206  jmodel.setIndexes(0,0,0);
207 
208  test_joint_mimic(jmodel);
209  }
210 
211 };
212 
214 {
215  using namespace pinocchio;
216  typedef boost::variant<
222  > Variant;
223 
224  boost::mpl::for_each<Variant::types>(TestJointMimic());
225 }
226 
227 BOOST_AUTO_TEST_CASE(test_transform_linear_affine)
228 {
229  typedef JointModelRX::ConfigVector_t ConfigVectorType;
230  double scaling = 1., offset = 0.;
231 
232  ConfigVectorType q0 = ConfigVectorType::Random();
233  ConfigVectorType q1;
234  LinearAffineTransform::run(q0,scaling,offset,q1);
235  BOOST_CHECK(q0 == q1);
236 
237  offset = 2.;
238  LinearAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1);
239  BOOST_CHECK(q1 == ConfigVectorType::Constant(offset));
240 }
241 
242 BOOST_AUTO_TEST_CASE(test_transform_linear_revolute)
243 {
244  typedef JointModelRUBX::ConfigVector_t ConfigVectorType;
245  double scaling = 1., offset = 0.;
246 
247  ConfigVectorType q0 = ConfigVectorType::Random().normalized();
248  ConfigVectorType q1;
249  UnboundedRevoluteAffineTransform::run(q0,scaling,offset,q1);
250  BOOST_CHECK(q0.isApprox(q1));
251 
252  offset = 2.;
253  UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1);
254  BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset),math::sin(offset)));
255 }
256 
257 BOOST_AUTO_TEST_CASE(test_joint_generic_cast)
258 {
259  JointModelRX jmodel_ref;
260  jmodel_ref.setIndexes(1,2,3);
261 
262  JointModelMimic<JointModelRX> jmodel(jmodel_ref,2.,1.);
263  jmodel.setIndexes(1,-1,-1);
264 
265  BOOST_CHECK(jmodel.id() == jmodel_ref.id());
266  BOOST_CHECK(jmodel.idx_q() == jmodel_ref.idx_q());
267  BOOST_CHECK(jmodel.idx_v() == jmodel_ref.idx_v());
268 
269  JointModel jmodel_generic(jmodel);
270  jmodel_generic.setIndexes(1,-2,-2);
271 
272  BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id());
273 }
274 BOOST_AUTO_TEST_SUITE_END()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
JointModelPrismaticTpl< double, 0, 0 > JointModelPX
void test_constraint_mimic(const JointModelBase< JointModel > &jmodel)
Definition: joint-mimic.cpp:18
static ForceTpl Random()
Definition: force-tpl.hpp:88
void test_joint_mimic(const JointModelBase< JointModel > &jmodel)
void operator()(const JointModelBase< JointModelRevoluteUnaligned > &) const
void setIndexes(JointIndex id, int q, int v)
static InertiaTpl Random()
JointModelRevoluteUnboundedTpl< double, 0, 1 > JointModelRUBY
JointModelRevoluteUnalignedTpl< double > JointModelRevoluteUnaligned
JointDataTpl< double > JointData
JointModelPrismaticTpl< double, 0, 1 > JointModelPY
int dim
Transformation_t M() const
void operator()(const JointModelBase< JointModel > &) const
JointDataDerived createData() const
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
Definition: motion-base.hpp:87
void operator()(const JointModelBase< JointModel > &) const
Definition: joint-mimic.cpp:93
Constraint_t S() const
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
BOOST_AUTO_TEST_CASE(test_constraint)
void operator()(const JointModelBase< JointModelPrismaticUnaligned > &) const
void operator()(const JointModelBase< JointModelPrismaticUnaligned > &) const
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
MotionTpl< double, 0 > Motion
static void run(const Eigen::MatrixBase< ConfigVectorIn > &q, const Scalar &scaling, const Scalar &offset, const Eigen::MatrixBase< ConfigVectorOut > &dest)
void operator()(const JointModelBase< JointModelRevoluteUnaligned > &) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
Definition: joint-mimic.cpp:15
static void run(const Eigen::MatrixBase< ConfigVectorIn > &q, const Scalar &scaling, const Scalar &offset, const Eigen::MatrixBase< ConfigVectorOut > &dest)
JointModelDerived & derived()
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs) const
float m
void setIndexes(JointIndex id, int nq, int nv)
Main pinocchio namespace.
Definition: timings.cpp:28
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
JointModelPrismaticTpl< double, 0, 2 > JointModelPZ
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
JointTpl< double > Joint
bool isApprox(const MotionDense< M2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90
M
static MotionTpl Random()
Definition: motion-tpl.hpp:92
JointModelRevoluteUnboundedTpl< double, 0, 0 > JointModelRUBX
JointModelPrismaticUnalignedTpl< double > JointModelPrismaticUnaligned
JointModelRevoluteTpl< double, 0, 0 > JointModelRX


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