pv-solver.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2023-2024 INRIA
3 // Copyright (c) 2023 KU Leuven
4 //
5 
6 #include "pinocchio/algorithm/aba.hpp"
7 #include "pinocchio/algorithm/jacobian.hpp"
8 #include "pinocchio/algorithm/contact-info.hpp"
9 #include "pinocchio/algorithm/compute-all-terms.hpp"
10 #include "pinocchio/algorithm/constrained-dynamics.hpp"
11 #include "pinocchio/algorithm/joint-configuration.hpp"
12 #include "pinocchio/multibody/sample-models.hpp"
13 #include "pinocchio/spatial/classic-acceleration.hpp"
14 #include "pinocchio/algorithm/pv.hpp"
15 
16 #include <boost/test/tools/old/interface.hpp>
17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
19 
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21 
22 // TODO: add tests on J_ref*ddq - rhs and on the OSIM matrix for PV. Add tests for proxLTLs
23 
24 pinocchio::Motion computeAcceleration(
26  const pinocchio::Model & model,
27  pinocchio::Data & data,
28  const pinocchio::JointIndex & joint_id,
29  pinocchio::ReferenceFrame reference_frame,
30  const pinocchio::ContactType type,
31  const pinocchio::SE3 & placement = pinocchio::SE3::Identity())
32 {
33  PINOCCHIO_UNUSED_VARIABLE(model);
34  using namespace pinocchio;
35  Motion res(Motion::Zero());
36 
37  const Data::SE3 & oMi = data.oMi[joint_id];
38  const Data::SE3 & iMc = placement;
39  const Data::SE3 oMc = oMi * iMc;
40 
41  const Motion ov = oMi.act(data.v[joint_id]);
42  const Motion oa = oMi.act(data.a[joint_id]);
43 
44  switch (reference_frame)
45  {
46  case WORLD:
47  if (type == CONTACT_3D)
48  classicAcceleration(ov, oa, res.linear());
49  else
50  res.linear() = oa.linear();
51  res.angular() = oa.angular();
52  break;
53  case LOCAL_WORLD_ALIGNED:
54  if (type == CONTACT_3D)
55  res.linear() = oMc.rotation() * classicAcceleration(data.v[joint_id], data.a[joint_id], iMc);
56  else
57  res.linear() = oMc.rotation() * (iMc.actInv(data.a[joint_id])).linear();
58  res.angular() = oMi.rotation() * data.a[joint_id].angular();
59  break;
60  case LOCAL:
61  if (type == CONTACT_3D)
62  classicAcceleration(data.v[joint_id], data.a[joint_id], iMc, res.linear());
63  else
64  res.linear() = (iMc.actInv(data.a[joint_id])).linear();
65  res.angular() = iMc.rotation().transpose() * data.a[joint_id].angular();
66  break;
67  default:
68  break;
69  }
70 
71  return res;
72 }
73 
74 BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_empty)
75 {
76  using namespace Eigen;
77  using namespace pinocchio;
78 
79  pinocchio::Model model;
80  pinocchio::buildModels::humanoidRandom(model, true);
81  pinocchio::Data data(model), data_ref(model);
82 
83  model.lowerPositionLimit.head<3>().fill(-1.);
84  model.upperPositionLimit.head<3>().fill(1.);
85  VectorXd q = randomConfiguration(model);
86 
87  VectorXd v = VectorXd::Random(model.nv);
88  VectorXd tau = VectorXd::Random(model.nv);
89 
90  // Contact models and data
91  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models;
92  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) empty_contact_datas;
93 
94  const double mu0 = 0.;
95  ProximalSettings prox_settings(1e-12, mu0, 1);
96 
97  initPvSolver(model, data, empty_contact_models);
98  pv(model, data, q, v, tau, empty_contact_models, empty_contact_datas, prox_settings);
99 
100  // Check solutions
101  aba(model, data_ref, q, v, tau, Convention::WORLD);
102  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
103 
104  // Checking if solving again the same problem gives the same solution
105  pv(model, data, q, v, tau, empty_contact_models, empty_contact_datas, prox_settings);
106  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
107 
108  prox_settings.mu = 1e-5;
109  constrainedABA(model, data, q, v, tau, empty_contact_models, empty_contact_datas, prox_settings);
110  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
111 }
112 
113 BOOST_AUTO_TEST_CASE(test_forward_dynamics_in_contact_6D_LOCAL_humanoid)
114 {
115  using namespace Eigen;
116  using namespace pinocchio;
117 
118  pinocchio::Model model;
119  pinocchio::buildModels::humanoidRandom(model, true);
120  pinocchio::Data data(model), data_ref(model);
121 
122  model.lowerPositionLimit.head<3>().fill(-1.);
123  model.upperPositionLimit.head<3>().fill(1.);
124  VectorXd q = randomConfiguration(model);
125 
126  VectorXd v = VectorXd::Random(model.nv);
127  VectorXd tau = VectorXd::Random(model.nv);
128 
129  const std::string RF = "rleg6_joint";
130  const Model::JointIndex RF_id = model.getJointId(RF);
131  const std::string LF = "lleg6_joint";
132  const Model::JointIndex LF_id = model.getJointId(LF);
133 
134  // Contact models and data
135  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
136  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
137  RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL);
138  ci_RF.joint1_placement.setRandom();
139  contact_models.push_back(ci_RF);
140  contact_datas.push_back(RigidConstraintData(ci_RF));
141  RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL);
142  ci_LF.joint1_placement.setRandom();
143  contact_models.push_back(ci_LF);
144  contact_datas.push_back(RigidConstraintData(ci_LF));
145 
146  const double mu0 = 0.0;
147 
148  ProximalSettings prox_settings(1e-12, mu0, 1);
149  initConstraintDynamics(model, data_ref, contact_models);
150  constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas, prox_settings);
151 
152  initPvSolver(model, data, contact_models);
153  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
154  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
155 
156  prox_settings.mu = 0.0;
157 
158  // Check the solver works the second time for new random inputs
159  q = randomConfiguration(model);
160  v = VectorXd::Random(model.nv);
161  tau = VectorXd::Random(model.nv);
162 
163  constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas, prox_settings);
164  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
165 
166  // Warning: the test below is not guaranteed to work for different constraints since the order of
167  // constraints in PV and ProxLTL can vary.
168  data_ref.osim = data_ref.contact_chol.getInverseOperationalSpaceInertiaMatrix();
169  data.LA[0].template triangularView<Eigen::StrictlyUpper>() =
170  data.LA[0].template triangularView<Eigen::StrictlyLower>().transpose();
171  BOOST_CHECK(data_ref.osim.isApprox(data.LA[0]));
172 
173  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
174 
175  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
176  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
177 
178  prox_settings.mu = 1e-4;
179  prox_settings.max_iter = 6;
180 
181  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
182  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
183 
184  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
185  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
186 
187  // Send non-zero mu and max 1 iteration and the solution should not match
188  prox_settings.mu = 1e-3;
189  prox_settings.max_iter = 1;
190  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
191  BOOST_CHECK(!data.ddq.isApprox(data_ref.ddq));
192 
193  // Change max iter to 10 and now should work
194  prox_settings.max_iter = 10;
195  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
196  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
197 }
198 
199 BOOST_AUTO_TEST_CASE(test_forward_dynamics_3D_humanoid)
200 {
201  using namespace Eigen;
202  using namespace pinocchio;
203 
204  pinocchio::Model model;
205  pinocchio::buildModels::humanoidRandom(model, true);
206  pinocchio::Data data(model), data_ref(model);
207 
208  model.lowerPositionLimit.head<3>().fill(-1.);
209  model.upperPositionLimit.head<3>().fill(1.);
210  VectorXd q = randomConfiguration(model);
211 
212  VectorXd v = VectorXd::Random(model.nv);
213  VectorXd tau = VectorXd::Random(model.nv);
214 
215  const std::string RF = "rleg6_joint";
216  const Model::JointIndex RF_id = model.getJointId(RF);
217 
218  // Contact models and data
219  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
220  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
221  RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL);
222  ci_RF.joint1_placement.setRandom();
223  contact_models.push_back(ci_RF);
224  contact_datas.push_back(RigidConstraintData(ci_RF));
225 
226  const double mu0 = 0.0;
227 
228  ProximalSettings prox_settings(1e-12, mu0, 1);
229  initConstraintDynamics(model, data_ref, contact_models);
230  constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas, prox_settings);
231 
232  initPvSolver(model, data, contact_models);
233  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
234  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
235 
236  // Check the solver works the second time for new random inputs
237  q = randomConfiguration(model);
238  v = VectorXd::Random(model.nv);
239  tau = VectorXd::Random(model.nv);
240 
241  constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas, prox_settings);
242  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
243  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
244 
245  data_ref.osim = data_ref.contact_chol.getInverseOperationalSpaceInertiaMatrix();
246  data.LA[0].template triangularView<Eigen::StrictlyUpper>() =
247  data.LA[0].template triangularView<Eigen::StrictlyLower>().transpose();
248  BOOST_CHECK(data_ref.osim.isApprox(data.LA[0]));
249 
250  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
251  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
252 
253  initPvSolver(model, data, contact_models);
254  prox_settings.mu = 1e-4;
255  prox_settings.max_iter = 6;
256  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
257  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
258 
259  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
260  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
261 }
262 
263 BOOST_AUTO_TEST_CASE(test_forward_dynamics_repeating_3D_humanoid)
264 {
265  using namespace Eigen;
266  using namespace pinocchio;
267 
268  pinocchio::Model model;
269  pinocchio::buildModels::humanoidRandom(model, true);
270  pinocchio::Data data(model), data_ref(model);
271 
272  model.lowerPositionLimit.head<3>().fill(-1.);
273  model.upperPositionLimit.head<3>().fill(1.);
274  VectorXd q = randomConfiguration(model);
275 
276  VectorXd v = VectorXd::Random(model.nv);
277  VectorXd tau = VectorXd::Random(model.nv);
278 
279  const std::string RF = "rleg6_joint";
280  const Model::JointIndex RF_id = model.getJointId(RF);
281 
282  // Contact models and data
283  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
284  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
285  RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL);
286  ci_RF.joint1_placement.setRandom();
287  ci_RF.corrector.Kd.setZero();
288  ci_RF.corrector.Kp.setZero();
289  contact_models.push_back(ci_RF);
290  contact_datas.push_back(RigidConstraintData(ci_RF));
291  RigidConstraintModel ci_RF2(CONTACT_6D, model, model.getJointId("rleg5_joint"), LOCAL);
292  ci_RF2.joint1_placement.setRandom();
293  ci_RF2.corrector.Kd.setZero();
294  ci_RF2.corrector.Kp.setZero();
295  contact_models.push_back(ci_RF2);
296  contact_datas.push_back(RigidConstraintData(ci_RF2));
297 
298  const double mu0 = 1e-3;
299 
300  ProximalSettings prox_settings(1e-14, mu0, 10);
301  initConstraintDynamics(model, data_ref, contact_models);
302  constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas, prox_settings);
303 
304  computeAllTerms(model, data_ref, q, v);
305 
306  Eigen::DenseIndex constraint_dim = 0;
307  for (size_t k = 0; k < contact_models.size(); ++k)
308  constraint_dim += contact_models[k].size();
309  Eigen::MatrixXd J_ref(constraint_dim, model.nv);
310  J_ref.setZero();
311  Data::Matrix6x Jtmp = Data::Matrix6x::Zero(6, model.nv);
312 
313  getJointJacobian(model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, Jtmp);
314  J_ref.middleRows<6>(0) = ci_RF.joint1_placement.inverse().toActionMatrix() * Jtmp;
315 
316  Jtmp.setZero();
317  getJointJacobian(model, data_ref, ci_RF2.joint1_id, ci_RF2.reference_frame, Jtmp);
318  J_ref.middleRows<6>(6) = ci_RF2.joint1_placement.inverse().toActionMatrix() * Jtmp;
319 
320  Eigen::VectorXd rhs_ref(constraint_dim);
321  rhs_ref.segment<6>(0) =
322  computeAcceleration(
323  model, data_ref, ci_RF.joint1_id, ci_RF.reference_frame, ci_RF.type, ci_RF.joint1_placement)
324  .toVector();
325  rhs_ref.segment<6>(6) = computeAcceleration(
326  model, data_ref, ci_RF2.joint1_id, ci_RF2.reference_frame, ci_RF2.type,
327  ci_RF2.joint1_placement)
328  .toVector();
329 
330  BOOST_CHECK((J_ref.transpose() * (J_ref * data_ref.ddq + rhs_ref)).isZero(1e-11));
331 
332  initPvSolver(model, data, contact_models);
333  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
334 
335  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
336  BOOST_CHECK((J_ref.transpose() * (J_ref * data.ddq + rhs_ref)).isZero(1e-11));
337 
338  initPvSolver(model, data, contact_models);
339  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
340  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
341  BOOST_CHECK((J_ref.transpose() * (J_ref * data.ddq + rhs_ref)).isZero(1e-11));
342 
343  // Check the solver works the second time for new random inputs
344  q = randomConfiguration(model);
345  v = VectorXd::Random(model.nv);
346  tau = VectorXd::Random(model.nv);
347 
348  constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas, prox_settings);
349  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
350  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
351 
352  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
353  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
354 
355  initPvSolver(model, data, contact_models);
356  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
357  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
358 
359  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
360  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
361 }
362 
363 BOOST_AUTO_TEST_CASE(test_FD_humanoid_redundant_baumgarte)
364 {
365  using namespace Eigen;
366  using namespace pinocchio;
367 
368  pinocchio::Model model;
369  pinocchio::buildModels::humanoidRandom(model, true);
370  pinocchio::Data data(model), data_ref(model);
371 
372  model.lowerPositionLimit.head<3>().fill(-1.);
373  model.upperPositionLimit.head<3>().fill(1.);
374  VectorXd q = randomConfiguration(model);
375 
376  VectorXd v = VectorXd::Random(model.nv);
377  VectorXd tau = VectorXd::Random(model.nv);
378 
379  const std::string RF = "rleg6_joint";
380  const Model::JointIndex RF_id = model.getJointId(RF);
381 
382  // Contact models and data
383  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
384  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
385  RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL);
386  ci_RF.joint1_placement.setRandom();
387  ci_RF.corrector.Kd.setIdentity();
388  ci_RF.corrector.Kp.setIdentity();
389  contact_models.push_back(ci_RF);
390  contact_datas.push_back(RigidConstraintData(ci_RF));
391  RigidConstraintModel ci_RF2(CONTACT_6D, model, model.getJointId("rleg5_joint"), LOCAL);
392  ci_RF2.joint1_placement.setRandom();
393  ci_RF2.corrector.Kd.setIdentity();
394  ci_RF2.corrector.Kp.setZero();
395  contact_models.push_back(ci_RF2);
396  contact_datas.push_back(RigidConstraintData(ci_RF2));
397 
398  const double mu0 = 1e-4;
399 
400  ProximalSettings prox_settings(1e-14, mu0, 10);
401  initConstraintDynamics(model, data_ref, contact_models);
402  constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas, prox_settings);
403 
404  initPvSolver(model, data, contact_models);
405  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
406  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
407 
408  // Check the solver works the second time for new random inputs
409  q = randomConfiguration(model);
410  v = VectorXd::Random(model.nv);
411  tau = VectorXd::Random(model.nv);
412 
413  constraintDynamics(model, data_ref, q, v, tau, contact_models, contact_datas, prox_settings);
414  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
415  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
416 
417  pv(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
418  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
419 
420  initPvSolver(model, data, contact_models);
421  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
422  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq, 1e-11));
423 
424  constrainedABA(model, data, q, v, tau, contact_models, contact_datas, prox_settings);
425  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq, 1e-11));
426 }
427 
428 BOOST_AUTO_TEST_SUITE_END()
pinocchio::RigidConstraintData
RigidConstraintDataTpl< context::Scalar, context::Options > RigidConstraintData
Definition: algorithm/fwd.hpp:27
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
display-shapes-meshcat.placement
placement
Definition: display-shapes-meshcat.py:24
pinocchio::DataTpl::ddq
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: multibody/data.hpp:256
common_symbols.type
type
Definition: common_symbols.py:35
Eigen
pinocchio::constrainedABA
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & constrainedABA(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ContactModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ContactDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
The constrained Articulated Body Algorithm (constrainedABA). It computes constrained forward dynamics...
pinocchio::DataTpl
Definition: context/generic.hpp:25
compute-all-terms.hpp
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: multibody/fwd.hpp:46
pinocchio::pv
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & pv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ContactModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ContactDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
The Popov-Vereshchagin algorithm. It computes constrained forward dynamics, aka the joint acceleratio...
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::classicAcceleration
void classicAcceleration(const MotionDense< Motion1 > &spatial_velocity, const MotionDense< Motion2 > &spatial_acceleration, const Eigen::MatrixBase< Vector3Like > &res)
Computes the classic acceleration from a given spatial velocity and spatial acceleration.
Definition: spatial/classic-acceleration.hpp:29
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::aba
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::initPvSolver
void initPvSolver(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the data according to the contact information contained in contact_models.
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::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition: algorithm/contact-info.hpp:22
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
simulation-contact-dynamics.contact_datas
list contact_datas
Definition: simulation-contact-dynamics.py:60
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
joint-configuration.hpp
pinocchio::ProximalSettingsTpl
Structure containing all the settings parameters for the proximal algorithms.
Definition: algorithm/fwd.hpp:13
pinocchio::placement
const MotionDense< Motion2 > const SE3Tpl< SE3Scalar, SE3Options > & placement
Definition: spatial/classic-acceleration.hpp:122
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
pinocchio::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
cartpole.v
v
Definition: cartpole.py:153
pinocchio::initConstraintDynamics
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
size
FCL_REAL size() const
q
q
constraintDynamics
VectorXd constraintDynamics(const Model &model, Data &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Model::JointIndex id)
Definition: unittest/constrained-dynamics-derivatives.cpp:185
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
fill
fill
pinocchio::MotionTpl::Zero
static MotionTpl Zero()
Definition: motion-tpl.hpp:136
pinocchio::ContactType
ContactType
&#160;
Definition: algorithm/contact-info.hpp:19
pinocchio::SE3Tpl::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:22
contact-info.hpp
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_sparse_forward_dynamics_empty)
Definition: pv-solver.cpp:74
computeAcceleration
pinocchio::Motion computeAcceleration(const pinocchio::Model &model, pinocchio::Data &data, const pinocchio::JointIndex &joint_id, pinocchio::ReferenceFrame reference_frame, const pinocchio::ContactType type, const pinocchio::SE3 &placement=pinocchio::SE3::Identity())
Computes motions in the world frame.
Definition: pv-solver.cpp:25
anymal-simulation.constraint_dim
constraint_dim
Definition: anymal-simulation.py:52
sample-models.hpp
pinocchio::MotionTpl
Definition: context/casadi.hpp:27
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
constrained-dynamics.hpp
simulation-closed-kinematic-chains.prox_settings
prox_settings
Definition: simulation-closed-kinematic-chains.py:163
classic-acceleration.hpp
pinocchio::CONTACT_3D
@ CONTACT_3D
Definition: algorithm/contact-info.hpp:21
pinocchio::computeAllTerms
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...
pinocchio::DataTpl::osim
MatrixXs osim
Operational space inertia matrix;.
Definition: multibody/data.hpp:412
pinocchio::DataTpl::contact_chol
ContactCholeskyDecomposition contact_chol
Cholesky decomposition of the KKT contact matrix.
Definition: multibody/data.hpp:532
pv.hpp
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
PINOCCHIO_UNUSED_VARIABLE
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
Definition: include/pinocchio/macros.hpp:72
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

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