joint-motion-subspace.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 //
4 
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  ForceSet bF = bmc.act(F3);
42  ForceSet aF = amb.act(bF);
43  ForceSet aF2 = amc.act(F3);
44  BOOST_CHECK(aF.matrix().isApprox(aF2.matrix(), 1e-12));
45 
46  ForceSet F36 = amb.act(F3.block(3, 6));
47  BOOST_CHECK(
48  (aXb.transpose().inverse() * F3.matrix().block(0, 3, 6, 6)).isApprox(F36.matrix(), 1e-12));
49 
50  ForceSet F36full(12);
51  F36full.block(3, 6) = amb.act(F3.block(3, 6));
52  BOOST_CHECK((aXb.transpose().inverse() * F3.matrix().block(0, 3, 6, 6))
53  .isApprox(F36full.matrix().block(0, 3, 6, 6), 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);
64  F1.block(0, 1) = Y * S;
65  BOOST_CHECK(F1.matrix().isApprox(Y.matrix().col(3), 1e-12));
66 
67  ForceSet F2(Eigen::Matrix<double, 3, 9>::Random(), Eigen::Matrix<double, 3, 9>::Random());
68  Eigen::MatrixXd StF2 = S.transpose() * F2.block(5, 3).matrix();
69  BOOST_CHECK(StF2.isApprox(S.matrix().transpose() * F2.matrix().block(0, 5, 6, 3), 1e-12));
70 }
71 
72 template<typename JointModel>
73 void test_jmodel_nq_against_nq_ref(const JointModelBase<JointModel> & jmodel, const int & nq_ref)
74 {
75  BOOST_CHECK(jmodel.nq() == nq_ref);
76 }
77 
78 template<typename JointModel>
79 void test_jmodel_nq_against_nq_ref(const JointModelMimic<JointModel> & jmodel, const int & nq_ref)
80 {
81  BOOST_CHECK(jmodel.jmodel().nq() == nq_ref);
82 }
83 
84 template<typename JointModel, typename ConstraintDerived>
86  const JointModelBase<JointModel> & jmodel,
88 {
89  BOOST_CHECK(constraint.nv() == jmodel.nv());
90 }
91 
92 template<typename JointModel, typename ConstraintDerived>
94  const JointModelMimic<JointModel> & jmodel,
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 
143 
145  model, ConfigVector_t::Constant(model.nq, -1.), ConfigVector_t::Constant(model.nq, 1.));
146 
147  // By calling jmodel.calc, we then have jdata.S which is initialized with non NaN quantities
148  jmodel.calc(jdata, q);
149 
150  ConstraintType constraint(jdata.S);
151 
152  test_nv_against_jmodel(jmodel.derived(), constraint);
153  BOOST_CHECK(constraint.cols() == constraint.nv());
154  BOOST_CHECK(constraint.rows() == 6);
155 
156  typedef typename JointModel::TangentVector_t TangentVector_t;
157  TangentVector_t v = TangentVector_t::Random(constraint.nv());
158 
159  typename ConstraintType::DenseBase constraint_mat = constraint.matrix();
160  Motion m = constraint * v;
161  Motion m_ref = Motion(constraint_mat * v);
162 
163  BOOST_CHECK(m.isApprox(m_ref));
164 
165  // Test SE3 action
166  {
167  SE3 M = SE3::Random();
168  typename ConstraintType::DenseBase S = M.act(constraint);
169  typename ConstraintType::DenseBase S_ref(6, constraint.nv());
170 
171  for (Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
172  {
173  typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
174  MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
175 
176  m_out = M.act(m_in);
177  }
178 
179  BOOST_CHECK(S.isApprox(S_ref));
180  }
181 
182  // Test SE3 action inverse
183  {
184  SE3 M = SE3::Random();
185  typename ConstraintType::DenseBase S = M.actInv(constraint);
186  typename ConstraintType::DenseBase S_ref(6, constraint.nv());
187 
188  for (Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
189  {
190  typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
191  MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
192 
193  m_out = M.actInv(m_in);
194  }
195 
196  BOOST_CHECK(S.isApprox(S_ref));
197  }
198 
199  // Test SE3 action and SE3 action inverse
200  {
201  const SE3 M = SE3::Random();
202  const SE3 Minv = M.inverse();
203 
204  typename ConstraintType::DenseBase S1_vice = M.actInv(constraint);
205  typename ConstraintType::DenseBase S2_vice = Minv.act(constraint);
206 
207  BOOST_CHECK(S1_vice.isApprox(S2_vice));
208 
209  typename ConstraintType::DenseBase S1_versa = M.act(constraint);
210  typename ConstraintType::DenseBase S2_versa = Minv.actInv(constraint);
211 
212  BOOST_CHECK(S1_versa.isApprox(S2_versa));
213  }
214 
215  // Test Motion action
216  {
217  Motion v = Motion::Random();
218 
219  typename ConstraintType::DenseBase S = v.cross(constraint);
220  typename ConstraintType::DenseBase S_ref(6, constraint.nv());
221 
222  for (Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
223  {
224  typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
225  MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
226 
227  m_out = v.cross(m_in);
228  }
229  BOOST_CHECK(S.isApprox(S_ref));
230  }
231 
232  // Test transpose operations
233  {
234  const Eigen::DenseIndex dim = 20;
235  const Matrix6x Fin = Matrix6x::Random(6, dim);
236  Eigen::MatrixXd Fout = constraint.transpose() * Fin;
237  Eigen::MatrixXd Fout_ref = constraint_mat.transpose() * Fin;
238  BOOST_CHECK(Fout.isApprox(Fout_ref));
239 
240  Force force_in(Force::Random());
241  Eigen::MatrixXd Stf = (constraint.transpose() * force_in);
242  Eigen::MatrixXd Stf_ref = constraint_mat.transpose() * force_in.toVector();
243  BOOST_CHECK(Stf_ref.isApprox(Stf));
244  }
245 
246  // CRBA operations
247  {
248  const Inertia Y = Inertia::Random();
249  Eigen::MatrixXd YS = Y * constraint;
250  Eigen::MatrixXd YS_ref = Y.matrix() * constraint_mat;
251  BOOST_CHECK(YS.isApprox(YS_ref));
252  }
253 
254  // ABA operations
255  {
256  const Inertia Y = Inertia::Random();
257  const Inertia::Matrix6 Y_mat = Y.matrix();
258  Eigen::MatrixXd YS = Y_mat * constraint;
259  Eigen::MatrixXd YS_ref = Y_mat * constraint_mat;
260  BOOST_CHECK(YS.isApprox(YS_ref));
261  }
262 
263  // Test constrainst operations
264  {
265  Eigen::MatrixXd StS = constraint.transpose() * constraint;
266  Eigen::MatrixXd StS_ref = constraint_mat.transpose() * constraint_mat;
267  BOOST_CHECK(StS.isApprox(StS_ref));
268  }
269 }
270 
271 template<typename JointModel_>
272 struct init;
273 
274 template<typename JointModel_>
275 struct init
276 {
277  static JointModel_ run()
278  {
279  JointModel_ jmodel;
280  jmodel.setIndexes(0, 0, 0);
281  return jmodel;
282  }
283 };
284 
285 template<typename Scalar, int Options>
286 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options>>
287 {
289 
290  static JointModel run()
291  {
292  typedef typename JointModel::Vector3 Vector3;
293  JointModel jmodel(Vector3::Random().normalized());
294 
295  jmodel.setIndexes(0, 0, 0);
296  return jmodel;
297  }
298 };
299 
300 template<typename Scalar, int Options>
302 {
304 
305  static JointModel run()
306  {
307  typedef typename JointModel::Vector3 Vector3;
308  JointModel jmodel(Vector3::Random().normalized());
309 
310  jmodel.setIndexes(0, 0, 0);
311  return jmodel;
312  }
313 };
314 
315 template<typename Scalar, int Options>
316 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options>>
317 {
319 
320  static JointModel run()
321  {
322  typedef typename JointModel::Vector3 Vector3;
323  JointModel jmodel(Vector3::Random().normalized());
324 
325  jmodel.setIndexes(0, 0, 0);
326  return jmodel;
327  }
328 };
329 
330 template<typename Scalar, int Options, template<typename, int> class JointCollection>
331 struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
332 {
334 
335  static JointModel run()
336  {
338  JointModel jmodel((JointModelRX()));
339 
340  jmodel.setIndexes(0, 0, 0);
341  return jmodel;
342  }
343 };
344 
345 template<typename Scalar, int Options>
346 struct init<pinocchio::JointModelUniversalTpl<Scalar, Options>>
347 {
349 
350  static JointModel run()
351  {
353 
354  jmodel.setIndexes(0, 0, 0);
355  return jmodel;
356  }
357 };
358 
359 template<typename Scalar, int Options, template<typename, int> class JointCollection>
360 struct init<pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection>>
361 {
363 
364  static JointModel run()
365  {
369 
370  JointModel jmodel(JointModelRX(), SE3::Random());
371  jmodel.addJoint(JointModelRY(), SE3::Random());
372  jmodel.addJoint(JointModelRZ(), SE3::Random());
373 
374  jmodel.setIndexes(0, 0, 0);
375 
376  return jmodel;
377  }
378 };
379 
380 template<typename JointModel_>
381 struct init<pinocchio::JointModelMimic<JointModel_>>
382 {
384 
385  static JointModel run()
386  {
387  JointModel_ jmodel_ref = init<JointModel_>::run();
388 
389  JointModel jmodel(jmodel_ref, 1., 0.);
390  jmodel.setIndexes(0, 0, 0);
391 
392  return jmodel;
393  }
394 };
395 
396 template<typename Scalar, int Options, int axis>
397 struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
398 {
400 
401  static JointModel run()
402  {
403  JointModel jmodel(static_cast<Scalar>(0.5));
404 
405  jmodel.setIndexes(0, 0, 0);
406  return jmodel;
407  }
408 };
409 
410 template<typename Scalar, int Options>
411 struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
412 {
414 
415  static JointModel run()
416  {
417  typedef typename JointModel::Vector3 Vector3;
418  JointModel jmodel(Vector3::Random().normalized());
419 
420  jmodel.setIndexes(0, 0, 0);
421  return jmodel;
422  }
423 };
424 
425 struct TestJointConstraint
426 {
427 
428  template<typename JointModel>
430  {
432  jmodel.setIndexes(0, 0, 0);
433 
435  }
436 };
437 
438 BOOST_AUTO_TEST_CASE(test_joint_constraint_operations)
439 {
441  boost::mpl::for_each<Variant::types>(TestJointConstraint());
442 }
443 
444 BOOST_AUTO_TEST_SUITE_END()
pinocchio::InertiaTpl< context::Scalar, context::Options >
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: joint-motion-subspace.cpp:318
pinocchio::JointMotionSubspaceBase::nv
int nv() const
Definition: joint-motion-subspace-base.hpp:91
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_ForceSet)
Definition: joint-motion-subspace.cpp:22
pinocchio::JointModelRevoluteUnboundedUnalignedTpl
Definition: multibody/joint/fwd.hpp:46
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:25
pinocchio::ForceSetTpl::matrix
Matrix6x matrix() const
Definition: force-set.hpp:49
pinocchio::JointModelHelicalTpl
Definition: multibody/joint/fwd.hpp:60
init< pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > >::JointModel
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
Definition: joint-motion-subspace.cpp:362
pinocchio::JointModelMimic
Definition: joint-mimic.hpp:307
pinocchio::JointModelRevoluteUnboundedUnalignedTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
init< pinocchio::JointModelMimic< JointModel_ > >::JointModel
pinocchio::JointModelMimic< JointModel_ > JointModel
Definition: joint-motion-subspace.cpp:383
pinocchio::JointModelBase
Definition: joint-model-base.hpp:75
pinocchio::JointMotionSubspaceBase
Definition: joint-motion-subspace-base.hpp:59
pinocchio::SE3Tpl< context::Scalar, context::Options >
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix6
traits< SE3Tpl >::Matrix6 Matrix6
Definition: spatial/se3-tpl.hpp:59
model.hpp
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: joint-motion-subspace.cpp:335
inertia.hpp
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::JointDataTpl< context::Scalar >
pinocchio::JointModelUniversalTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
pinocchio::Joint
JointTpl< context::Scalar > Joint
Definition: joint-generic.hpp:22
pinocchio::randomConfiguration
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.
Definition: joint-configuration.hpp:315
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
pinocchio::CartesianAxis::vector
static Eigen::Matrix< Scalar, 3, 1 > vector()
Definition: cartesian-axis.hpp:79
init< pinocchio::JointModelMimic< JointModel_ > >::run
static JointModel run()
Definition: joint-motion-subspace.cpp:385
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: joint-motion-subspace.cpp:305
joint-configuration.hpp
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: joint-motion-subspace.cpp:413
simulation-contact-dynamics.S
S
Definition: simulation-contact-dynamics.py:80
pinocchio::JointData
JointDataTpl< context::Scalar > JointData
Definition: multibody/joint/fwd.hpp:162
init< pinocchio::JointModelHelicalUnalignedTpl< Scalar, Options > >::run
static JointModel run()
Definition: joint-motion-subspace.cpp:415
pinocchio::JointModelHelicalUnalignedTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Definition: joint-helical-unaligned.hpp:635
TestJointConstraint
Definition: joint-mimic.cpp:91
se3.hpp
pinocchio::JointModelRevoluteUnboundedUnalignedTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: joint-revolute-unbounded-unaligned.hpp:138
pinocchio::JointModelTpl< context::Scalar >
pinocchio::JointModelHelicalUnalignedTpl
Definition: multibody/joint/fwd.hpp:65
buildModel::run
static Model run(const JointModelBase< JointModel > &jmodel)
Definition: joint-motion-subspace.cpp:103
pinocchio::ForceTpl
Definition: context/casadi.hpp:25
pinocchio::JointModelRZ
JointModelRevoluteTpl< context::Scalar, context::Options, 2 > JointModelRZ
Definition: joint-revolute.hpp:881
init< pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > >::JointModel
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
Definition: joint-motion-subspace.cpp:303
init< pinocchio::JointModelHelicalTpl< Scalar, Options, axis > >::JointModel
pinocchio::JointModelHelicalTpl< Scalar, Options, axis > JointModel
Definition: joint-motion-subspace.cpp:399
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: joint-motion-subspace.cpp:333
pinocchio::InertiaTpl< context::Scalar, context::Options >::Random
static InertiaTpl Random()
Definition: spatial/inertia.hpp:361
M
M
init< pinocchio::JointModelUniversalTpl< Scalar, Options > >::JointModel
pinocchio::JointModelUniversalTpl< Scalar, Options > JointModel
Definition: joint-motion-subspace.cpp:348
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: joint-motion-subspace.cpp:320
buildModel< JointModelMimic< JointModel > >::JointModel_
JointModelMimic< JointModel > JointModel_
Definition: joint-motion-subspace.cpp:115
test_jmodel_nq_against_nq_ref
void test_jmodel_nq_against_nq_ref(const JointModelBase< JointModel > &jmodel, const int &nq_ref)
Definition: joint-motion-subspace.cpp:73
pinocchio::JointModelBase::nq
int nq() const
Definition: joint-model-base.hpp:145
pinocchio::JointDataTpl::S
Constraint_t S() const
Definition: joint-generic.hpp:123
pinocchio::MotionTpl::Random
static MotionTpl Random()
Definition: motion-tpl.hpp:140
pinocchio::JointModelRevoluteTpl
Definition: multibody/joint/fwd.hpp:33
pinocchio::Motion
MotionTpl<::CppAD::AD< double >, 0 > Motion
Definition: context/cppad.hpp:37
pinocchio::ForceTpl::Random
static ForceTpl Random()
Definition: force-tpl.hpp:114
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
init< pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > >::JointModel
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
Definition: joint-motion-subspace.cpp:288
pinocchio::JointModelHelicalUnalignedTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
Matrix6x
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
Definition: joint-mimic.cpp:15
pinocchio::JointModelMimic::jmodel
const JointModel & jmodel() const
Definition: joint-mimic.hpp:719
pinocchio::ForceSetTpl
Definition: force-set.hpp:14
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
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::SE3Tpl::Random
static SE3Tpl Random()
Definition: spatial/se3-tpl.hpp:154
init::run
static JointModel_ run()
Definition: joint-motion-subspace.cpp:277
pinocchio::ForceSetTpl::Block::matrix
ForceSetTpl::Matrix6x matrix() const
Definition: force-set.hpp:123
TestJointConstraint::operator()
void operator()(const JointModelBase< JointModel > &) const
Definition: joint-motion-subspace.cpp:429
pinocchio::SE3Tpl::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
pinocchio::JointModelRevoluteUnalignedTpl::Vector3
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Definition: joint-revolute-unaligned.hpp:614
pinocchio::JointModelBase::calc
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVectorType > &qs) const
Definition: joint-model-base.hpp:107
test_nv_against_jmodel
void test_nv_against_jmodel(const JointModelBase< JointModel > &jmodel, const JointMotionSubspaceBase< ConstraintDerived > &constraint)
Definition: joint-motion-subspace.cpp:85
test_constraint_operations
void test_constraint_operations(const JointModelBase< JointModel > &jmodel)
Definition: joint-motion-subspace.cpp:128
pinocchio::JointModelPrismaticUnalignedTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
Y
Y
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
buildModel
Definition: joint-motion-subspace.cpp:101
buildModel< JointModelMimic< JointModel > >::run
static Model run(const JointModel_ &jmodel)
Definition: joint-motion-subspace.cpp:117
pinocchio::JointModelBase::nv
int nv() const
Definition: joint-model-base.hpp:141
dim
int dim
force-set.hpp
pinocchio::MotionTpl
Definition: context/casadi.hpp:27
pinocchio::ModelTpl
Definition: context/generic.hpp:20
init
Definition: all-joints.cpp:20
pinocchio::MotionRef
Definition: context/casadi.hpp:38
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: joint-motion-subspace.cpp:401
pinocchio::ForceSetTpl::block
Block block(const int &idx, const int &len)
Definition: force-set.hpp:179
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: joint-motion-subspace.cpp:290
pinocchio::JointModelBase::derived
JointModelDerived & derived()
Definition: joint-model-base.hpp:82
init< pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > >::run
static JointModel run()
Definition: joint-motion-subspace.cpp:364
init< pinocchio::JointModelUniversalTpl< Scalar, Options > >::run
static JointModel run()
Definition: joint-motion-subspace.cpp:350
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


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