casadi-algo.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019 INRIA
3 //
4 
5 #include "pinocchio/autodiff/casadi.hpp"
6 
7 #include "pinocchio/algorithm/kinematics.hpp"
8 #include "pinocchio/algorithm/frames.hpp"
9 #include "pinocchio/algorithm/jacobian.hpp"
10 #include "pinocchio/algorithm/crba.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include "pinocchio/algorithm/aba.hpp"
13 #include "pinocchio/algorithm/joint-configuration.hpp"
14 
15 #include "pinocchio/parsers/sample-models.hpp"
16 
17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
19 
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21 
22 BOOST_AUTO_TEST_CASE(test_jacobian)
23 {
24  typedef double Scalar;
25  typedef casadi::SX ADScalar;
26 
27  typedef pinocchio::ModelTpl<Scalar> Model;
28  typedef Model::Data Data;
29 
30  typedef pinocchio::ModelTpl<ADScalar> ADModel;
31  typedef ADModel::Data ADData;
32 
33  Model model;
34  pinocchio::buildModels::humanoidRandom(model);
35  model.lowerPositionLimit.head<3>().fill(-1.);
36  model.upperPositionLimit.head<3>().fill(1.);
37  Data data(model);
38 
39  typedef Model::ConfigVectorType ConfigVector;
40  typedef Model::TangentVectorType TangentVector;
41  ConfigVector q(model.nq);
42  q = pinocchio::randomConfiguration(model);
43  TangentVector v(TangentVector::Random(model.nv));
44 
45  typedef ADModel::ConfigVectorType ConfigVectorAD;
46  typedef ADModel::TangentVectorType TangentVectorAD;
47  ADModel ad_model = model.cast<ADScalar>();
48  ADData ad_data(ad_model);
49 
50  Model::Index joint_id = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
51  Data::Matrix6x jacobian_local(6,model.nv), jacobian_world(6,model.nv);
52  jacobian_local.setZero(); jacobian_world.setZero();
53 
54  BOOST_CHECK(jacobian_local.isZero() && jacobian_world.isZero());
55 
56  pinocchio::computeJointJacobians(model,data,q);
57  pinocchio::getJointJacobian(model,data,joint_id,pinocchio::WORLD,jacobian_world);
58  pinocchio::getJointJacobian(model,data,joint_id,pinocchio::LOCAL,jacobian_local);
59 
60  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
61  ConfigVectorAD q_ad(model.nq);
62  for(Eigen::DenseIndex k = 0; k < model.nq; ++k)
63  {
64  q_ad[k] = cs_q(k);
65  }
66  std::cout << "q =\n " << q_ad << std::endl;
67 
68  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
69  TangentVectorAD v_ad(model.nv);
70  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
71  {
72  v_ad[k] = cs_v(k);
73  }
74  std::cout << "v =\n " << v_ad << std::endl;
75 
76  pinocchio::forwardKinematics(ad_model, ad_data, q_ad, v_ad);
77  typedef pinocchio::MotionTpl<ADScalar> MotionAD;
78  MotionAD & v_local = ad_data.v[(size_t)joint_id];
79  MotionAD v_world = ad_data.oMi[(size_t)joint_id].act(v_local);
80 
81  casadi::SX cs_v_local(6,1), cs_v_world(6,1);
82  for(Eigen::DenseIndex k = 0; k < 6; ++k)
83  {
84  cs_v_local(k) = v_local.toVector()[k];
85  cs_v_world(k) = v_world.toVector()[k];
86  }
87  std::cout << "v_local = " << cs_v_local << std::endl;
88  std::cout << "v_world = " << cs_v_world << std::endl;
89 
90  casadi::Function eval_velocity_local("eval_velocity_local",
91  casadi::SXVector {cs_q, cs_v},
92  casadi::SXVector {cs_v_local});
93  std::cout << "eval_velocity_local = " << eval_velocity_local << std::endl;
94 
95  casadi::Function eval_velocity_world("eval_velocity_world",
96  casadi::SXVector {cs_q, cs_v},
97  casadi::SXVector {cs_v_world});
98  std::cout << "eval_velocity_world = " << eval_velocity_world << std::endl;
99 
100  casadi::SX dv_dv_local = jacobian(cs_v_local, cs_v);
101  casadi::Function eval_jacobian_local("eval_jacobian_local",
102  casadi::SXVector {cs_q,cs_v},
103  casadi::SXVector {dv_dv_local});
104  std::cout << "eval_jacobian_local = " << eval_jacobian_local << std::endl;
105 
106  casadi::SX dv_dv_world = jacobian(cs_v_world, cs_v);
107  casadi::Function eval_jacobian_world("eval_jacobian_world",
108  casadi::SXVector {cs_q,cs_v},
109  casadi::SXVector {dv_dv_world});
110 
111  std::vector<double> q_vec((size_t)model.nq);
112  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;
113 
114  std::vector<double> v_vec((size_t)model.nv);
115  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;
116 
117  casadi::DMVector v_local_res = eval_velocity_local(casadi::DMVector {q_vec,v_vec});
118  casadi::DMVector J_local_res = eval_jacobian_local(casadi::DMVector {q_vec,v_vec});
119  std::cout << "J_local_res:" << J_local_res << std::endl;
120 
121  std::vector<double> v_local_vec(static_cast< std::vector<double> >(v_local_res[0]));
122  BOOST_CHECK((jacobian_local*v).isApprox(Eigen::Map<pinocchio::Motion::Vector6>(v_local_vec.data())));
123 
124  casadi::DMVector v_world_res = eval_velocity_world(casadi::DMVector {q_vec,v_vec});
125  casadi::DMVector J_world_res = eval_jacobian_world(casadi::DMVector {q_vec,v_vec});
126 
127  std::vector<double> v_world_vec(static_cast< std::vector<double> >(v_world_res[0]));
128  BOOST_CHECK((jacobian_world*v).isApprox(Eigen::Map<pinocchio::Motion::Vector6>(v_world_vec.data())));
129 
130  Data::Matrix6x J_local_mat(6,model.nv), J_world_mat(6,model.nv);
131 
132  std::vector<double> J_local_vec(static_cast< std::vector<double> >(J_local_res[0]));
133  J_local_mat = Eigen::Map<Data::Matrix6x>(J_local_vec.data(),6,model.nv);
134  BOOST_CHECK(jacobian_local.isApprox(J_local_mat));
135 
136  std::vector<double> J_world_vec(static_cast< std::vector<double> >(J_world_res[0]));
137  J_world_mat = Eigen::Map<Data::Matrix6x>(J_world_vec.data(),6,model.nv);
138  BOOST_CHECK(jacobian_world.isApprox(J_world_mat));
139 }
140 
141  BOOST_AUTO_TEST_CASE(test_fk)
142  {
143  typedef double Scalar;
144  typedef casadi::SX ADScalar;
145 
146  typedef pinocchio::ModelTpl<Scalar> Model;
147  typedef Model::Data Data;
148 
149  typedef pinocchio::ModelTpl<ADScalar> ADModel;
150  typedef ADModel::Data ADData;
151 
152  Model model;
153  pinocchio::buildModels::humanoidRandom(model);
154  model.lowerPositionLimit.head<3>().fill(-1.);
155  model.upperPositionLimit.head<3>().fill(1.);
156  Data data(model);
157 
158  typedef Model::ConfigVectorType ConfigVector;
159  typedef Model::TangentVectorType TangentVector;
160  ConfigVector q(model.nq);
161  q = pinocchio::randomConfiguration(model);
162  TangentVector v(TangentVector::Random(model.nv));
163  TangentVector a(TangentVector::Random(model.nv));
164 
165  pinocchio::forwardKinematics(model,data,q);
166 
167  typedef ADModel::ConfigVectorType ConfigVectorAD;
168  typedef ADModel::TangentVectorType TangentVectorAD;
169  ADModel ad_model = model.cast<ADScalar>();
170  ADData ad_data(ad_model);
171 
172  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
173  ConfigVectorAD q_ad(model.nq);
174  pinocchio::casadi::copy(cs_q,q_ad);
175 
176  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
177  TangentVectorAD v_ad(model.nv);
178  pinocchio::casadi::copy(cs_v,v_ad);
179 
180  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
181  TangentVectorAD a_ad(model.nv);
182  pinocchio::casadi::copy(cs_a,a_ad);
183 
184  pinocchio::forwardKinematics(ad_model, ad_data, q_ad, v_ad, a_ad);
185  pinocchio::updateGlobalPlacements(ad_model, ad_data);
186  pinocchio::updateFramePlacements(ad_model, ad_data);
187 // typedef pinocchio::MotionTpl<ADScalar> MotionAD;
188  }
189 
190 BOOST_AUTO_TEST_CASE(test_rnea)
191 {
192  typedef double Scalar;
193  typedef casadi::SX ADScalar;
194 
195  typedef pinocchio::ModelTpl<Scalar> Model;
196  typedef Model::Data Data;
197 
198  typedef pinocchio::ModelTpl<ADScalar> ADModel;
199  typedef ADModel::Data ADData;
200 
201  Model model;
202  pinocchio::buildModels::humanoidRandom(model);
203  model.lowerPositionLimit.head<3>().fill(-1.);
204  model.upperPositionLimit.head<3>().fill(1.);
205  Data data(model);
206 
207  typedef Model::ConfigVectorType ConfigVector;
208  typedef Model::TangentVectorType TangentVector;
209  ConfigVector q(model.nq);
210  q = pinocchio::randomConfiguration(model);
211  TangentVector v(TangentVector::Random(model.nv));
212  TangentVector a(TangentVector::Random(model.nv));
213 
214  typedef ADModel::ConfigVectorType ConfigVectorAD;
215  typedef ADModel::TangentVectorType TangentVectorAD;
216  ADModel ad_model = model.cast<ADScalar>();
217  ADData ad_data(ad_model);
218 
219  pinocchio::rnea(model,data,q,v,a);
220 
221  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
222  ConfigVectorAD q_ad(model.nq);
223  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
224 
225  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
226  TangentVectorAD v_ad(model.nv);
227  v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);
228 
229  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
230  TangentVectorAD a_ad(model.nv);
231  a_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_a).data(),model.nv,1);
232 
233  rnea(ad_model,ad_data,q_ad,v_ad,a_ad);
234  casadi::SX tau_ad(model.nv,1);
235  // Eigen::Map<TangentVectorAD>(tau_ad->data(),model.nv,1)
236  // = ad_data.tau;
237  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
238  tau_ad(k) = ad_data.tau[k];
239  casadi::Function eval_rnea("eval_rnea",
240  casadi::SXVector {cs_q, cs_v, cs_a},
241  casadi::SXVector {tau_ad});
242 
243  std::vector<double> q_vec((size_t)model.nq);
244  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;
245 
246  std::vector<double> v_vec((size_t)model.nv);
247  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;
248 
249  std::vector<double> a_vec((size_t)model.nv);
250  Eigen::Map<TangentVector>(a_vec.data(),model.nv,1) = a;
251  casadi::DM tau_res = eval_rnea(casadi::DMVector {q_vec,v_vec,a_vec})[0];
252  std::cout << "tau_res = " << tau_res << std::endl;
253  Data::TangentVectorType tau_vec = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(tau_res).data(),model.nv,1);
254 
255  BOOST_CHECK(data.tau.isApprox(tau_vec));
256 }
257 
258 BOOST_AUTO_TEST_CASE(test_crba)
259 {
260  typedef double Scalar;
261  typedef casadi::SX ADScalar;
262 
263  typedef pinocchio::ModelTpl<Scalar> Model;
264  typedef Model::Data Data;
265 
266  typedef pinocchio::ModelTpl<ADScalar> ADModel;
267  typedef ADModel::Data ADData;
268 
269  Model model;
270  pinocchio::buildModels::humanoidRandom(model);
271  model.lowerPositionLimit.head<3>().fill(-1.);
272  model.upperPositionLimit.head<3>().fill(1.);
273  Data data(model);
274 
275  typedef Model::ConfigVectorType ConfigVector;
276  typedef Model::TangentVectorType TangentVector;
277  ConfigVector q(model.nq);
278  q = pinocchio::randomConfiguration(model);
279  TangentVector v(TangentVector::Random(model.nv));
280  TangentVector a(TangentVector::Random(model.nv));
281 
282  typedef ADModel::ConfigVectorType ConfigVectorAD;
283  typedef ADModel::TangentVectorType TangentVectorAD;
284  ADModel ad_model = model.cast<ADScalar>();
285  ADData ad_data(ad_model);
286 
287  pinocchio::crba(model,data,q);
288  data.M.triangularView<Eigen::StrictlyLower>()
289  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
290  pinocchio::rnea(model,data,q,v,a);
291 
292  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
293  ConfigVectorAD q_ad(model.nq);
294  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
295 
296  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
297  TangentVectorAD v_ad(model.nv);
298  v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);
299 
300  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
301  TangentVectorAD a_ad(model.nv);
302  a_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_a).data(),model.nv,1);
303 
304  // RNEA
305  rnea(ad_model,ad_data,q_ad,v_ad,a_ad);
306  casadi::SX cs_tau(model.nv,1);
307  // Eigen::Map<TangentVectorAD>(tau_ad->data(),model.nv,1)
308  // = ad_data.tau;
309  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
310  cs_tau(k) = ad_data.tau[k];
311  casadi::Function eval_rnea("eval_rnea",
312  casadi::SXVector {cs_q, cs_v, cs_a},
313  casadi::SXVector {cs_tau});
314  // CRBA
315  crba(ad_model,ad_data,q_ad);
316  ad_data.M.triangularView<Eigen::StrictlyLower>()
317  = ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
318  casadi::SX M_ad(model.nv,model.nv);
319  for(Eigen::DenseIndex j = 0; j < model.nv; ++j)
320  {
321  for(Eigen::DenseIndex i = 0; i < model.nv; ++i)
322  {
323  M_ad(i,j) = ad_data.M(i,j);
324  }
325  }
326 
327  std::vector<double> q_vec((size_t)model.nq);
328  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;
329 
330  std::vector<double> v_vec((size_t)model.nv);
331  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;
332 
333  std::vector<double> a_vec((size_t)model.nv);
334  Eigen::Map<TangentVector>(a_vec.data(),model.nv,1) = a;
335 
336  casadi::Function eval_crba("eval_crba",
337  casadi::SXVector {cs_q},
338  casadi::SXVector {M_ad});
339  casadi::DM M_res = eval_crba(casadi::DMVector {q_vec})[0];
340  Data::MatrixXs M_mat = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(M_res).data(),
341  model.nv,model.nv);
342 
343  BOOST_CHECK(data.M.isApprox(M_mat));
344 
345  casadi::SX dtau_da = jacobian(cs_tau,cs_a);
346  casadi::Function eval_dtau_da("eval_dtau_da",
347  casadi::SXVector {cs_q,cs_v,cs_a},
348  casadi::SXVector {dtau_da});
349  casadi::DM dtau_da_res = eval_dtau_da(casadi::DMVector {q_vec, v_vec, a_vec})[0];
350  Data::MatrixXs dtau_da_mat = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(dtau_da_res).data(),
351  model.nv,model.nv);
352  BOOST_CHECK(data.M.isApprox(dtau_da_mat));
353 }
354 
355  BOOST_AUTO_TEST_CASE(test_aba)
356  {
357  typedef double Scalar;
358  typedef casadi::SX ADScalar;
359 
360  typedef pinocchio::ModelTpl<Scalar> Model;
361  typedef Model::Data Data;
362 
363  typedef pinocchio::ModelTpl<ADScalar> ADModel;
364  typedef ADModel::Data ADData;
365 
366  Model model;
367  pinocchio::buildModels::humanoidRandom(model);
368  model.lowerPositionLimit.head<3>().fill(-1.);
369  model.upperPositionLimit.head<3>().fill(1.);
370  Data data(model);
371 
372  typedef Model::ConfigVectorType ConfigVector;
373  typedef Model::TangentVectorType TangentVector;
374  ConfigVector q(model.nq);
375  q = pinocchio::randomConfiguration(model);
376  TangentVector v(TangentVector::Random(model.nv));
377  TangentVector tau(TangentVector::Random(model.nv));
378 
379  typedef ADModel::ConfigVectorType ConfigVectorAD;
380  typedef ADModel::TangentVectorType TangentVectorAD;
381  ADModel ad_model = model.cast<ADScalar>();
382  ADData ad_data(ad_model);
383 
384  pinocchio::aba(model,data,q,v,tau);
385 
386  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
387  ConfigVectorAD q_ad(model.nq);
388  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
389 
390  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
391  TangentVectorAD v_ad(model.nv);
392  v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);
393 
394  casadi::SX cs_tau = casadi::SX::sym("tau", model.nv);
395  TangentVectorAD tau_ad(model.nv);
396  tau_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_tau).data(),model.nv,1);
397 
398  // ABA
399  aba(ad_model,ad_data,q_ad,v_ad,tau_ad);
400  casadi::SX cs_ddq(model.nv,1);
401  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
402  cs_ddq(k) = ad_data.ddq[k];
403  casadi::Function eval_aba("eval_aba",
404  casadi::SXVector {cs_q, cs_v, cs_tau},
405  casadi::SXVector {cs_ddq});
406 
407  std::vector<double> q_vec((size_t)model.nq);
408  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;
409 
410  std::vector<double> v_vec((size_t)model.nv);
411  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;
412 
413  std::vector<double> tau_vec((size_t)model.nv);
414  Eigen::Map<TangentVector>(tau_vec.data(),model.nv,1) = tau;
415 
416  casadi::DM ddq_res = eval_aba(casadi::DMVector {q_vec, v_vec, tau_vec})[0];
417  Data::TangentVectorType ddq_mat = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(ddq_res).data(),
418  model.nv,1);
419 
420  BOOST_CHECK(ddq_mat.isApprox(data.ddq));
421  }
422 
423 BOOST_AUTO_TEST_SUITE_END()
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)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::WORLD
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
Definition: src/multibody/fwd.hpp:46
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_jacobian)
Definition: casadi-algo.cpp:22
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::Model
ModelTpl< double > Model
Definition: src/multibody/fwd.hpp:35
q
q
pinocchio::rnea
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Definition: parallel/rnea.hpp:32
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:224
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or i...
fill
fill
pinocchio::motionSet::act
static void act(const Eigen::MatrixBase< Mat > &iV, const ForceDense< ForceDerived > &f, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion set on a force object. The input motion set is represented by a 6xN matrix whose e...
pinocchio::casadi::sym
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
Definition: autodiff/casadi.hpp:170
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
pinocchio::updateGlobalPlacements
void updateGlobalPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Update the global placement of the joints oMi according to the relative placements of the joints...
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
Matrix6x
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
Definition: joint-mimic.cpp:15
a
Vec3f a
pinocchio::Data
DataTpl< double > Data
Definition: src/multibody/fwd.hpp:36
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame...
pinocchio::updateFramePlacements
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
pinocchio::Index
std::size_t Index
Definition: src/multibody/fwd.hpp:25
pinocchio::casadi::copy
void copy(::casadi::Matrix< Scalar > const &src, Eigen::MatrixBase< MT > &dst)
Definition: autodiff/casadi.hpp:139
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
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29