constraint.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 //
4 
5 #include "pinocchio/spatial/se3.hpp"
6 #include "pinocchio/spatial/inertia.hpp"
7 #include "pinocchio/multibody/force-set.hpp"
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/algorithm/joint-configuration.hpp"
10 
11 #include "utils/macros.hpp"
12 
13 #include <iostream>
14 
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17 
18 using namespace pinocchio;
19 
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21 
22 BOOST_AUTO_TEST_CASE (test_ForceSet)
23 {
24  using namespace pinocchio;
25 
26  SE3 amb = SE3::Random();
27  SE3 bmc = SE3::Random();
28  SE3 amc = amb*bmc;
29 
30  ForceSet F(12);
31  ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
32  F.block(10,2) = F2;
33  BOOST_CHECK_EQUAL(F.matrix().col(10).norm() , 0.0 );
34  BOOST_CHECK(std::isnan(F.matrix()(0,9)));
35 
36  ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
37  ForceSet F4 = amb.act(F3);
38  SE3::Matrix6 aXb= amb;
39  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix(), 1e-12));
40 
41 
42  ForceSet bF = bmc.act(F3);
43  ForceSet aF = amb.act(bF);
44  ForceSet aF2 = amc.act(F3);
45  BOOST_CHECK(aF.matrix().isApprox(aF2.matrix(), 1e-12));
46 
47  ForceSet F36 = amb.act(F3.block(3,6));
48  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix(), 1e-12));
49 
50 
51  ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6));
52  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36full.matrix().block(0,3,6,6),
53  1e-12));
54 }
55 
56 BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
57 {
58  using namespace pinocchio;
59 
61  JointDataRX::Constraint_t S;
62 
63  ForceSet F1(1); F1.block(0,1) = Y*S;
64  BOOST_CHECK(F1.matrix().isApprox(Y.matrix().col(3), 1e-12));
65 
66  ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
67  Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3).matrix();
68  BOOST_CHECK(StF2.isApprox(S.matrix().transpose()*F2.matrix().block(0,5,6,3)
69  , 1e-12));
70 }
71 
72 template<typename JointModel>
74  const int & nq_ref)
75 {
76  BOOST_CHECK(jmodel.nq() == nq_ref);
77 }
78 
79 template<typename JointModel>
81  const int & nq_ref)
82 {
83  BOOST_CHECK(jmodel.jmodel().nq() == nq_ref);
84 }
85 
86 template<typename JointModel, typename ConstraintDerived>
88  const ConstraintBase<ConstraintDerived> & constraint)
89 {
90  BOOST_CHECK(constraint.nv() == jmodel.nv());
91 }
92 
93 template<typename JointModel, typename ConstraintDerived>
95  const ConstraintBase<ConstraintDerived> & constraint)
96 {
97  BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv());
98 }
99 
100 template<class JointModel>
102 {
103  static Model run(const JointModelBase<JointModel> & jmodel)
104  {
105  Model model;
106  model.addJoint(0,jmodel,SE3::Identity(),"joint");
107 
108  return model;
109  }
110 };
111 
112 template<class JointModel>
114 {
116 
117  static Model run(const JointModel_ & jmodel)
118  {
119  Model model;
120  model.addJoint(0,jmodel.jmodel(),SE3::Identity(),"joint");
121  model.addJoint(0,jmodel,SE3::Identity(),"joint_mimic");
122 
123  return model;
124  }
125 };
126 
127 template<typename JointModel>
129 {
130  typedef typename traits<JointModel>::JointDerived Joint;
131  typedef typename traits<Joint>::Constraint_t ConstraintType;
133  typedef Eigen::Matrix<typename JointModel::Scalar,6,Eigen::Dynamic> Matrix6x;
134 
135  JointData jdata = jmodel.createData();
136  typedef typename JointModel::ConfigVector_t ConfigVector_t;
137  ConfigVector_t q;
138 
139  // We need to use a model here in order to call the randomConfiguration to init q.
141 
142  test_jmodel_nq_against_nq_ref(jmodel.derived(),model.nq);
143 
144  q = randomConfiguration(model,
145  ConfigVector_t::Constant(model.nq,-1.),
146  ConfigVector_t::Constant(model.nq, 1.));
147 
148  // By calling jmodel.calc, we then have jdata.S which is initialized with non NaN quantities
149  jmodel.calc(jdata,q);
150 
151  ConstraintType constraint(jdata.S);
152 
153  test_nv_against_jmodel(jmodel.derived(),constraint);
154  BOOST_CHECK(constraint.cols() == constraint.nv());
155  BOOST_CHECK(constraint.rows() == 6);
156 
157  typedef typename JointModel::TangentVector_t TangentVector_t;
158  TangentVector_t v = TangentVector_t::Random(constraint.nv());
159 
160  typename ConstraintType::DenseBase constraint_mat = constraint.matrix();
161  Motion m = constraint * v;
162  Motion m_ref = Motion(constraint_mat * v);
163 
164  BOOST_CHECK(m.isApprox(m_ref));
165 
166  // Test SE3 action
167  {
168  SE3 M = SE3::Random();
169  typename ConstraintType::DenseBase S = M.act(constraint);
170  typename ConstraintType::DenseBase S_ref(6,constraint.nv());
171 
172  for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
173  {
174  typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
175  MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
176 
177  m_out = M.act(m_in);
178  }
179 
180  BOOST_CHECK(S.isApprox(S_ref));
181  }
182 
183  // Test SE3 action inverse
184  {
185  SE3 M = SE3::Random();
186  typename ConstraintType::DenseBase S = M.actInv(constraint);
187  typename ConstraintType::DenseBase S_ref(6,constraint.nv());
188 
189  for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
190  {
191  typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
192  MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
193 
194  m_out = M.actInv(m_in);
195  }
196 
197  BOOST_CHECK(S.isApprox(S_ref));
198  }
199 
200  // Test SE3 action and SE3 action inverse
201  {
202  const SE3 M = SE3::Random();
203  const SE3 Minv = M.inverse();
204 
205  typename ConstraintType::DenseBase S1_vice = M.actInv(constraint);
206  typename ConstraintType::DenseBase S2_vice = Minv.act(constraint);
207 
208  BOOST_CHECK(S1_vice.isApprox(S2_vice));
209 
210  typename ConstraintType::DenseBase S1_versa = M.act(constraint);
211  typename ConstraintType::DenseBase S2_versa = Minv.actInv(constraint);
212 
213  BOOST_CHECK(S1_versa.isApprox(S2_versa));
214 
215  }
216 
217  // Test Motion action
218  {
219  Motion v = Motion::Random();
220 
221  typename ConstraintType::DenseBase S = v.cross(constraint);
222  typename ConstraintType::DenseBase S_ref(6,constraint.nv());
223 
224  for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
225  {
226  typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
227  MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
228 
229  m_out = v.cross(m_in);
230  }
231  BOOST_CHECK(S.isApprox(S_ref));
232  }
233 
234  // Test transpose operations
235  {
236  const Eigen::DenseIndex dim = 20;
237  const Matrix6x Fin = Matrix6x::Random(6,dim);
238  Eigen::MatrixXd Fout = constraint.transpose() * Fin;
239  Eigen::MatrixXd Fout_ref = constraint_mat.transpose() * Fin;
240  BOOST_CHECK(Fout.isApprox(Fout_ref));
241 
242  Force force_in(Force::Random());
243  Eigen::MatrixXd Stf = (constraint.transpose() * force_in);
244  Eigen::MatrixXd Stf_ref = constraint_mat.transpose() * force_in.toVector();
245  BOOST_CHECK(Stf_ref.isApprox(Stf));
246  }
247 
248  // CRBA operations
249  {
250  const Inertia Y = Inertia::Random();
251  Eigen::MatrixXd YS = Y * constraint;
252  Eigen::MatrixXd YS_ref = Y.matrix() * constraint_mat;
253  BOOST_CHECK(YS.isApprox(YS_ref));
254  }
255 
256  // ABA operations
257  {
258  const Inertia Y = Inertia::Random();
259  const Inertia::Matrix6 Y_mat = Y.matrix();
260  Eigen::MatrixXd YS = Y_mat * constraint;
261  Eigen::MatrixXd YS_ref = Y_mat * constraint_mat;
262  BOOST_CHECK(YS.isApprox(YS_ref));
263  }
264 
265 }
266 
267 template<typename JointModel_> struct init;
268 
269 template<typename JointModel_>
270 struct init
271 {
272  static JointModel_ run()
273  {
274  JointModel_ jmodel;
275  jmodel.setIndexes(0,0,0);
276  return jmodel;
277  }
278 };
279 
280 template<typename Scalar, int Options>
281 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
282 {
284 
285  static JointModel run()
286  {
287  typedef typename JointModel::Vector3 Vector3;
288  JointModel jmodel(Vector3::Random().normalized());
289 
290  jmodel.setIndexes(0,0,0);
291  return jmodel;
292  }
293 };
294 
295 template<typename Scalar, int Options>
296 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
297 {
299 
300  static JointModel run()
301  {
302  typedef typename JointModel::Vector3 Vector3;
303  JointModel jmodel(Vector3::Random().normalized());
304 
305  jmodel.setIndexes(0,0,0);
306  return jmodel;
307  }
308 };
309 
310 template<typename Scalar, int Options>
311 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
312 {
314 
315  static JointModel run()
316  {
317  typedef typename JointModel::Vector3 Vector3;
318  JointModel jmodel(Vector3::Random().normalized());
319 
320  jmodel.setIndexes(0,0,0);
321  return jmodel;
322  }
323 };
324 
325 template<typename Scalar, int Options, template<typename,int> class JointCollection>
326 struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
327 {
329 
330  static JointModel run()
331  {
333  JointModel jmodel((JointModelRX()));
334 
335  jmodel.setIndexes(0,0,0);
336  return jmodel;
337  }
338 };
339 
340 template<typename Scalar, int Options, template<typename,int> class JointCollection>
341 struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
342 {
344 
345  static JointModel run()
346  {
350 
351  JointModel jmodel(JointModelRX(),SE3::Random());
352  jmodel.addJoint(JointModelRY(),SE3::Random());
353  jmodel.addJoint(JointModelRZ(),SE3::Random());
354 
355  jmodel.setIndexes(0,0,0);
356 
357  return jmodel;
358  }
359 };
360 
361 template<typename JointModel_>
362 struct init<pinocchio::JointModelMimic<JointModel_> >
363 {
365 
366  static JointModel run()
367  {
368  JointModel_ jmodel_ref = init<JointModel_>::run();
369 
370  JointModel jmodel(jmodel_ref,1.,0.);
371  jmodel.setIndexes(0,0,0);
372 
373  return jmodel;
374  }
375 };
376 
378 {
379 
380  template <typename JointModel>
382  {
384  jmodel.setIndexes(0,0,0);
385 
387  }
388 
389 };
390 
391 BOOST_AUTO_TEST_CASE(test_joint_constraint_operations)
392 {
394  boost::mpl::for_each<Variant::types>(TestJointConstraint());
395 }
396 
397 BOOST_AUTO_TEST_SUITE_END()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
static ForceTpl Random()
Definition: force-tpl.hpp:88
pinocchio::JointModelMimic< JointModel_ > JointModel
Definition: constraint.cpp:364
ForceSetTpl::Matrix6x matrix() const
Definition: force-set.hpp:87
void setIndexes(JointIndex id, int q, int v)
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:97
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: constraint.cpp:298
Block block(const int &idx, const int &len)
Definition: force-set.hpp:140
static InertiaTpl Random()
JointDataTpl< double > JointData
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.
int dim
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
JointDataDerived createData() const
static Model run(const JointModelBase< JointModel > &jmodel)
Definition: constraint.cpp:103
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
Definition: motion-base.hpp:87
void operator()(const JointModelBase< JointModel > &) const
Definition: constraint.cpp:381
void test_jmodel_nq_against_nq_ref(const JointModelBase< JointModel > &jmodel, const int &nq_ref)
Definition: constraint.cpp:73
Constraint_t S() const
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
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.
SE3Tpl inverse() const
aXb = bXa.inverse()
traits< SE3Tpl >::Matrix6 Matrix6
JointModelMimic< JointModel > JointModel_
Definition: constraint.cpp:115
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
MotionTpl< double, 0 > Motion
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
Definition: joint-mimic.cpp:15
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
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Matrix6x matrix() const
Definition: force-set.hpp:36
float m
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
Definition: constraint.cpp:343
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
Definition: constraint.cpp:283
void setIndexes(JointIndex id, int nq, int nv)
static Model run(const JointModel_ &jmodel)
Definition: constraint.cpp:117
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
Definition: constraint.cpp:313
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
static JointModel_ run()
Definition: constraint.cpp:272
BOOST_AUTO_TEST_CASE(test_ForceSet)
Definition: constraint.cpp:22
M
const JointModel & jmodel() const
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
Definition: constraint.cpp:328
void test_nv_against_jmodel(const JointModelBase< JointModel > &jmodel, const ConstraintBase< ConstraintDerived > &constraint)
Definition: constraint.cpp:87
static MotionTpl Random()
Definition: motion-tpl.hpp:92
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
JointCollectionTpl & model
int nq
Dimension of the configuration vector representation.
void test_constraint_operations(const JointModelBase< JointModel > &jmodel)
Definition: constraint.cpp:128


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