casadi/algorithms.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2021 INRIA
3 //
4 
5 #include "pinocchio/autodiff/casadi.hpp"
6 
7 #include "pinocchio/multibody/model.hpp"
8 #include "pinocchio/multibody/data.hpp"
9 
10 #include "pinocchio/algorithm/kinematics.hpp"
11 #include "pinocchio/algorithm/frames.hpp"
12 #include "pinocchio/algorithm/jacobian.hpp"
13 #include "pinocchio/algorithm/crba.hpp"
14 #include "pinocchio/algorithm/rnea.hpp"
15 #include "pinocchio/algorithm/aba.hpp"
16 #include "pinocchio/algorithm/joint-configuration.hpp"
17 #include "pinocchio/algorithm/energy.hpp"
18 
19 #include "pinocchio/multibody/sample-models.hpp"
20 
21 #include <boost/test/unit_test.hpp>
22 #include <boost/utility/binary.hpp>
23 
24 #include "casadi-utils.hpp"
25 
26 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
27 
28 BOOST_AUTO_TEST_CASE(test_jacobian)
29 {
30  typedef double Scalar;
31  typedef casadi::SX ADScalar;
32 
33  typedef pinocchio::ModelTpl<Scalar> Model;
34  typedef Model::Data Data;
35 
36  typedef pinocchio::ModelTpl<ADScalar> ADModel;
37  typedef ADModel::Data ADData;
38 
39  Model model;
40  pinocchio::buildModels::humanoidRandom(model);
41  model.lowerPositionLimit.head<3>().fill(-1.);
42  model.upperPositionLimit.head<3>().fill(1.);
43  Data data(model);
44 
45  typedef Model::ConfigVectorType ConfigVector;
46  typedef Model::TangentVectorType TangentVector;
47  ConfigVector q(model.nq);
48  q = pinocchio::randomConfiguration(model);
49  TangentVector v(TangentVector::Random(model.nv));
50 
51  typedef ADModel::ConfigVectorType ConfigVectorAD;
52  typedef ADModel::TangentVectorType TangentVectorAD;
53  ADModel ad_model = model.cast<ADScalar>();
54  ADData ad_data(ad_model);
55 
56  Model::Index joint_id =
57  model.existJointName("rarm2") ? model.getJointId("rarm2") : (Model::Index)(model.njoints - 1);
58  Data::Matrix6x jacobian_local(6, model.nv), jacobian_world(6, model.nv);
59  jacobian_local.setZero();
60  jacobian_world.setZero();
61 
62  BOOST_CHECK(jacobian_local.isZero() && jacobian_world.isZero());
63 
64  pinocchio::computeJointJacobians(model, data, q);
65  pinocchio::getJointJacobian(model, data, joint_id, pinocchio::WORLD, jacobian_world);
66  pinocchio::getJointJacobian(model, data, joint_id, pinocchio::LOCAL, jacobian_local);
67 
68  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
69  ConfigVectorAD q_ad(model.nq);
70  for (Eigen::DenseIndex k = 0; k < model.nq; ++k)
71  {
72  q_ad[k] = cs_q(k);
73  }
74  std::cout << "q =\n " << q_ad << std::endl;
75 
76  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
77  TangentVectorAD v_ad(model.nv);
78  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
79  {
80  v_ad[k] = cs_v(k);
81  }
82  std::cout << "v =\n " << v_ad << std::endl;
83 
84  pinocchio::forwardKinematics(ad_model, ad_data, q_ad, v_ad);
85  typedef pinocchio::MotionTpl<ADScalar> MotionAD;
86  MotionAD & v_local = ad_data.v[(size_t)joint_id];
87  MotionAD v_world = ad_data.oMi[(size_t)joint_id].act(v_local);
88 
89  casadi::SX cs_v_local(6, 1), cs_v_world(6, 1);
90  for (Eigen::DenseIndex k = 0; k < 6; ++k)
91  {
92  cs_v_local(k) = v_local.toVector()[k];
93  cs_v_world(k) = v_world.toVector()[k];
94  }
95  std::cout << "v_local = " << cs_v_local << std::endl;
96  std::cout << "v_world = " << cs_v_world << std::endl;
97 
98  casadi::Function eval_velocity_local(
99  "eval_velocity_local", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{cs_v_local});
100  std::cout << "eval_velocity_local = " << eval_velocity_local << std::endl;
101 
102  casadi::Function eval_velocity_world(
103  "eval_velocity_world", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{cs_v_world});
104  std::cout << "eval_velocity_world = " << eval_velocity_world << std::endl;
105 
106  casadi::SX dv_dv_local = jacobian(cs_v_local, cs_v);
107  casadi::Function eval_jacobian_local(
108  "eval_jacobian_local", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{dv_dv_local});
109  std::cout << "eval_jacobian_local = " << eval_jacobian_local << std::endl;
110 
111  casadi::SX dv_dv_world = jacobian(cs_v_world, cs_v);
112  casadi::Function eval_jacobian_world(
113  "eval_jacobian_world", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{dv_dv_world});
114 
115  std::vector<double> q_vec((size_t)model.nq);
116  Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
117 
118  std::vector<double> v_vec((size_t)model.nv);
119  Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
120 
121  casadi::DMVector v_local_res = eval_velocity_local(casadi::DMVector{q_vec, v_vec});
122  casadi::DMVector J_local_res = eval_jacobian_local(casadi::DMVector{q_vec, v_vec});
123  std::cout << "J_local_res:" << J_local_res << std::endl;
124 
125  std::vector<double> v_local_vec(static_cast<std::vector<double>>(v_local_res[0]));
126  BOOST_CHECK(
127  (jacobian_local * v).isApprox(Eigen::Map<pinocchio::Motion::Vector6>(v_local_vec.data())));
128 
129  casadi::DMVector v_world_res = eval_velocity_world(casadi::DMVector{q_vec, v_vec});
130  casadi::DMVector J_world_res = eval_jacobian_world(casadi::DMVector{q_vec, v_vec});
131 
132  std::vector<double> v_world_vec(static_cast<std::vector<double>>(v_world_res[0]));
133  BOOST_CHECK(
134  (jacobian_world * v).isApprox(Eigen::Map<pinocchio::Motion::Vector6>(v_world_vec.data())));
135 
136  Data::Matrix6x J_local_mat(6, model.nv), J_world_mat(6, model.nv);
137 
138  std::vector<double> J_local_vec(static_cast<std::vector<double>>(J_local_res[0]));
139  J_local_mat = Eigen::Map<Data::Matrix6x>(J_local_vec.data(), 6, model.nv);
140  BOOST_CHECK(jacobian_local.isApprox(J_local_mat));
141 
142  std::vector<double> J_world_vec(static_cast<std::vector<double>>(J_world_res[0]));
143  J_world_mat = Eigen::Map<Data::Matrix6x>(J_world_vec.data(), 6, model.nv);
144  BOOST_CHECK(jacobian_world.isApprox(J_world_mat));
145 }
146 
147 BOOST_AUTO_TEST_CASE(test_fk)
148 {
149  typedef double Scalar;
150  typedef casadi::SX ADScalar;
151 
152  typedef pinocchio::ModelTpl<Scalar> Model;
153  typedef Model::Data Data;
154 
155  typedef pinocchio::ModelTpl<ADScalar> ADModel;
156  typedef ADModel::Data ADData;
157 
158  Model model;
159  pinocchio::buildModels::humanoidRandom(model);
160  model.lowerPositionLimit.head<3>().fill(-1.);
161  model.upperPositionLimit.head<3>().fill(1.);
162  Data data(model);
163 
164  typedef Model::ConfigVectorType ConfigVector;
165  typedef Model::TangentVectorType TangentVector;
166  ConfigVector q(model.nq);
167  q = pinocchio::randomConfiguration(model);
168  TangentVector v(TangentVector::Random(model.nv));
169  TangentVector a(TangentVector::Random(model.nv));
170 
171  pinocchio::forwardKinematics(model, data, q);
172 
173  typedef ADModel::ConfigVectorType ConfigVectorAD;
174  typedef ADModel::TangentVectorType TangentVectorAD;
175  ADModel ad_model = model.cast<ADScalar>();
176  ADData ad_data(ad_model);
177 
178  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
179  ConfigVectorAD q_ad(model.nq);
180  pinocchio::casadi::copy(cs_q, q_ad);
181 
182  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
183  TangentVectorAD v_ad(model.nv);
184  pinocchio::casadi::copy(cs_v, v_ad);
185 
186  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
187  TangentVectorAD a_ad(model.nv);
188  pinocchio::casadi::copy(cs_a, a_ad);
189 
190  pinocchio::forwardKinematics(ad_model, ad_data, q_ad, v_ad, a_ad);
191  pinocchio::updateGlobalPlacements(ad_model, ad_data);
192  pinocchio::updateFramePlacements(ad_model, ad_data);
193  // typedef pinocchio::MotionTpl<ADScalar> MotionAD;
194 }
195 
196 BOOST_AUTO_TEST_CASE(test_rnea)
197 {
198  typedef double Scalar;
199  typedef casadi::SX ADScalar;
200 
201  typedef pinocchio::ModelTpl<Scalar> Model;
202  typedef Model::Data Data;
203 
204  typedef pinocchio::ModelTpl<ADScalar> ADModel;
205  typedef ADModel::Data ADData;
206 
207  Model model;
208  pinocchio::buildModels::humanoidRandom(model);
209  model.lowerPositionLimit.head<3>().fill(-1.);
210  model.upperPositionLimit.head<3>().fill(1.);
211  Data data(model);
212 
213  typedef Model::ConfigVectorType ConfigVector;
214  typedef Model::TangentVectorType TangentVector;
215  ConfigVector q(model.nq);
216  q = pinocchio::randomConfiguration(model);
217  TangentVector v(TangentVector::Random(model.nv));
218  TangentVector a(TangentVector::Random(model.nv));
219 
220  typedef ADModel::ConfigVectorType ConfigVectorAD;
221  typedef ADModel::TangentVectorType TangentVectorAD;
222  ADModel ad_model = model.cast<ADScalar>();
223  ADData ad_data(ad_model);
224 
225  pinocchio::rnea(model, data, q, v, a);
226 
227  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
228  ConfigVectorAD q_ad(model.nq);
229  q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
230 
231  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
232  TangentVectorAD v_ad(model.nv);
233  v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
234 
235  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
236  TangentVectorAD a_ad(model.nv);
237  a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
238 
239  rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
240  casadi::SX tau_ad(model.nv, 1);
241  // Eigen::Map<TangentVectorAD>(tau_ad->data(),model.nv,1)
242  // = ad_data.tau;
243  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
244  tau_ad(k) = ad_data.tau[k];
245  casadi::Function eval_rnea(
246  "eval_rnea", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{tau_ad});
247 
248  std::vector<double> q_vec((size_t)model.nq);
249  Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
250 
251  std::vector<double> v_vec((size_t)model.nv);
252  Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
253 
254  std::vector<double> a_vec((size_t)model.nv);
255  Eigen::Map<TangentVector>(a_vec.data(), model.nv, 1) = a;
256  casadi::DM tau_res = eval_rnea(casadi::DMVector{q_vec, v_vec, a_vec})[0];
257  std::cout << "tau_res = " << tau_res << std::endl;
258  Data::TangentVectorType tau_vec = Eigen::Map<Data::TangentVectorType>(
259  static_cast<std::vector<double>>(tau_res).data(), model.nv, 1);
260 
261  BOOST_CHECK(data.tau.isApprox(tau_vec));
262 }
263 
264 BOOST_AUTO_TEST_CASE(test_crba)
265 {
266  typedef double Scalar;
267  typedef casadi::SX ADScalar;
268 
269  typedef pinocchio::ModelTpl<Scalar> Model;
270  typedef Model::Data Data;
271 
272  typedef pinocchio::ModelTpl<ADScalar> ADModel;
273  typedef ADModel::Data ADData;
274 
275  Model model;
276  pinocchio::buildModels::humanoidRandom(model);
277  model.lowerPositionLimit.head<3>().fill(-1.);
278  model.upperPositionLimit.head<3>().fill(1.);
279  Data data(model);
280 
281  typedef Model::ConfigVectorType ConfigVector;
282  typedef Model::TangentVectorType TangentVector;
283  ConfigVector q(model.nq);
284  q = pinocchio::randomConfiguration(model);
285  TangentVector v(TangentVector::Random(model.nv));
286  TangentVector a(TangentVector::Random(model.nv));
287 
288  typedef ADModel::ConfigVectorType ConfigVectorAD;
289  typedef ADModel::TangentVectorType TangentVectorAD;
290  ADModel ad_model = model.cast<ADScalar>();
291  ADData ad_data(ad_model);
292 
293  pinocchio::crba(model, data, q, pinocchio::Convention::WORLD);
294  data.M.triangularView<Eigen::StrictlyLower>() =
295  data.M.transpose().triangularView<Eigen::StrictlyLower>();
296  pinocchio::rnea(model, data, q, v, a);
297 
298  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
299  ConfigVectorAD q_ad(model.nq);
300  q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
301 
302  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
303  TangentVectorAD v_ad(model.nv);
304  v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
305 
306  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
307  TangentVectorAD a_ad(model.nv);
308  a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
309 
310  // RNEA
311  rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
312  casadi::SX cs_tau(model.nv, 1);
313  // Eigen::Map<TangentVectorAD>(tau_ad->data(),model.nv,1)
314  // = ad_data.tau;
315  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
316  cs_tau(k) = ad_data.tau[k];
317  casadi::Function eval_rnea(
318  "eval_rnea", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{cs_tau});
319  // CRBA
320  crba(ad_model, ad_data, q_ad, pinocchio::Convention::WORLD);
321  ad_data.M.triangularView<Eigen::StrictlyLower>() =
322  ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
323  casadi::SX M_ad(model.nv, model.nv);
324  for (Eigen::DenseIndex j = 0; j < model.nv; ++j)
325  {
326  for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
327  {
328  M_ad(i, j) = ad_data.M(i, j);
329  }
330  }
331 
332  std::vector<double> q_vec((size_t)model.nq);
333  Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
334 
335  std::vector<double> v_vec((size_t)model.nv);
336  Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
337 
338  std::vector<double> a_vec((size_t)model.nv);
339  Eigen::Map<TangentVector>(a_vec.data(), model.nv, 1) = a;
340 
341  casadi::Function eval_crba("eval_crba", casadi::SXVector{cs_q}, casadi::SXVector{M_ad});
342  casadi::DM M_res = eval_crba(casadi::DMVector{q_vec})[0];
343  Data::MatrixXs M_mat =
344  Eigen::Map<Data::MatrixXs>(static_cast<std::vector<double>>(M_res).data(), model.nv, model.nv);
345 
346  BOOST_CHECK(data.M.isApprox(M_mat));
347 
348  casadi::SX dtau_da = jacobian(cs_tau, cs_a);
349  casadi::Function eval_dtau_da(
350  "eval_dtau_da", casadi::SXVector{cs_q, cs_v, cs_a}, casadi::SXVector{dtau_da});
351  casadi::DM dtau_da_res = eval_dtau_da(casadi::DMVector{q_vec, v_vec, a_vec})[0];
352  Data::MatrixXs dtau_da_mat = Eigen::Map<Data::MatrixXs>(
353  static_cast<std::vector<double>>(dtau_da_res).data(), model.nv, model.nv);
354  BOOST_CHECK(data.M.isApprox(dtau_da_mat));
355 }
356 
357 BOOST_AUTO_TEST_CASE(test_aba)
358 {
359  typedef double Scalar;
360  typedef casadi::SX ADScalar;
361 
362  typedef pinocchio::ModelTpl<Scalar> Model;
363  typedef Model::Data Data;
364 
365  typedef pinocchio::ModelTpl<ADScalar> ADModel;
366  typedef ADModel::Data ADData;
367 
368  Model model;
369  pinocchio::buildModels::humanoidRandom(model);
370  model.lowerPositionLimit.head<3>().fill(-1.);
371  model.upperPositionLimit.head<3>().fill(1.);
372  Data data(model);
373 
374  typedef Model::ConfigVectorType ConfigVector;
375  typedef Model::TangentVectorType TangentVector;
376  ConfigVector q(model.nq);
377  q = pinocchio::randomConfiguration(model);
378  TangentVector v(TangentVector::Random(model.nv));
379  TangentVector tau(TangentVector::Random(model.nv));
380 
381  typedef ADModel::ConfigVectorType ConfigVectorAD;
382  typedef ADModel::TangentVectorType TangentVectorAD;
383  ADModel ad_model = model.cast<ADScalar>();
384  ADData ad_data(ad_model);
385 
386  pinocchio::aba(model, data, q, v, tau, pinocchio::Convention::WORLD);
387 
388  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
389  ConfigVectorAD q_ad(model.nq);
390  q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
391 
392  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
393  TangentVectorAD v_ad(model.nv);
394  v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
395 
396  casadi::SX cs_tau = casadi::SX::sym("tau", model.nv);
397  TangentVectorAD tau_ad(model.nv);
398  tau_ad =
399  Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
400 
401  // ABA
402  pinocchio::aba(ad_model, ad_data, q_ad, v_ad, tau_ad, pinocchio::Convention::WORLD);
403  casadi::SX cs_ddq(model.nv, 1);
404  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
405  cs_ddq(k) = ad_data.ddq[k];
406  casadi::Function eval_aba(
407  "eval_aba", casadi::SXVector{cs_q, cs_v, cs_tau}, casadi::SXVector{cs_ddq});
408 
409  std::vector<double> q_vec((size_t)model.nq);
410  Eigen::Map<ConfigVector>(q_vec.data(), model.nq, 1) = q;
411 
412  std::vector<double> v_vec((size_t)model.nv);
413  Eigen::Map<TangentVector>(v_vec.data(), model.nv, 1) = v;
414 
415  std::vector<double> tau_vec((size_t)model.nv);
416  Eigen::Map<TangentVector>(tau_vec.data(), model.nv, 1) = tau;
417 
418  casadi::DM ddq_res = eval_aba(casadi::DMVector{q_vec, v_vec, tau_vec})[0];
419  Data::TangentVectorType ddq_mat = Eigen::Map<Data::TangentVectorType>(
420  static_cast<std::vector<double>>(ddq_res).data(), model.nv, 1);
421 
422  BOOST_CHECK(ddq_mat.isApprox(data.ddq));
423 }
424 
425 void test_interp_for_model(const pinocchio::ModelTpl<double> & model)
426 {
427  using casadi::DMVector;
428  using casadi::SX;
429  using casadi::SXVector;
430  typedef SX ADScalar;
431  typedef pinocchio::ModelTpl<double> Model;
432  typedef pinocchio::ModelTpl<ADScalar> ADModel;
433  std::cout << model;
434 
435  ADModel ad_model = model.cast<ADScalar>();
436  int nq = model.nq;
437  int nv = model.nv;
438 
439  SX cs_q0 = SX::sym("q0", nq);
440  SX cs_q1 = SX::sym("q1", nq);
441  SX cs_dq0 = SX::sym("dq0", nv);
442  SX cs_dq1 = SX::sym("dq0", nv);
443  SX zero_vec(SX::zeros(2 * nv));
444 
445  typedef ADModel::ConfigVectorType cConfig_t;
446  typedef ADModel::TangentVectorType cTangent_t;
447 
448  cConfig_t cq0(nq), cq1(nq);
449  cTangent_t cdq0(nv), cdq1(nv);
450  pinocchio::casadi::copy(cs_q0, cq0);
451  pinocchio::casadi::copy(cs_q1, cq1);
452  pinocchio::casadi::copy(cs_dq0, cdq0);
453  pinocchio::casadi::copy(cs_dq1, cdq1);
454  ADScalar cs_dqall = vertcat(cs_dq0, cs_dq1);
455 
456  auto cq0_i = pinocchio::integrate(ad_model, cq0, cdq0);
457  auto cq1_i = pinocchio::integrate(ad_model, cq1, cdq1);
458 
459  ADScalar alpha(0.5);
460 
461  cConfig_t qinterp = pinocchio::interpolate(ad_model, cq0_i, cq1_i, alpha);
462  cConfig_t qneutral = pinocchio::neutral(ad_model);
463  cTangent_t log_interp = pinocchio::difference(ad_model, qinterp, qneutral);
464 
465  auto norm_interp = log_interp.dot(log_interp);
466  auto Jnorm_interp = jacobian(norm_interp, cs_dqall);
467 
468  casadi::Function Jnorm_eval(
469  "Jnorm", SXVector{cs_q0, cs_q1}, SXVector{substitute(Jnorm_interp, cs_dqall, zero_vec)});
470  std::cout << Jnorm_eval << '\n';
471 
472  const auto q0 = pinocchio::neutral(model);
473  const auto q1 = pinocchio::randomConfiguration(model);
474  const auto q2 = pinocchio::randomConfiguration(model);
475 
476  typedef Eigen::Map<Model::ConfigVectorType> ConfigMap_t;
477  std::vector<double> q0_vec((size_t)nq);
478  std::vector<double> q1_vec((size_t)nq);
479  std::vector<double> q2_vec((size_t)nq);
480  ConfigMap_t(q0_vec.data(), nq, 1) = q0;
481  ConfigMap_t(q1_vec.data(), nq, 1) = q1;
482  ConfigMap_t(q2_vec.data(), nq, 1) = q2;
483 
484  std::cout << Jnorm_eval(DMVector{q0_vec, q0_vec})[0] << '\n';
485  std::cout << Jnorm_eval(DMVector{q0_vec, q1_vec})[0] << '\n';
486  std::cout << Jnorm_eval(DMVector{q1_vec, q1_vec})[0] << '\n';
487  std::cout << Jnorm_eval(DMVector{q2_vec, q2_vec})[0] << '\n';
488 }
489 
490 BOOST_AUTO_TEST_CASE(test_interp)
491 {
492  typedef pinocchio::ModelTpl<double> Model;
493  Model model;
494  pinocchio::buildModels::humanoidRandom(model, true);
495  model.lowerPositionLimit.head<3>().fill(-1.);
496  model.upperPositionLimit.head<3>().fill(1.);
497 
498  test_interp_for_model(model);
499 
500  Model model2;
501  typedef pinocchio::SE3Tpl<double> SE3;
502  size_t baseId = model2.addJoint(0, pinocchio::JointModelSpherical(), SE3::Identity(), "base");
503  model2.addJoint(baseId, pinocchio::JointModelRX(), SE3::Random(), "pole");
504  model2.lowerPositionLimit.tail<1>().fill(-4.);
505  model2.upperPositionLimit.tail<1>().fill(4.);
506 
507  test_interp_for_model(model2);
508 }
509 
510 BOOST_AUTO_TEST_CASE(test_kinetic_energy)
511 {
512  using casadi::DMVector;
513  using casadi::SX;
514  using casadi::SXVector;
515  typedef SX ADScalar;
516  typedef pinocchio::ModelTpl<double> Model;
517  typedef pinocchio::ModelTpl<ADScalar> ADModel;
518  typedef ADModel::Data ADData;
519 
520  Model model;
521  // pinocchio::buildModels::humanoidRandom(model, true);
522  model.addJoint(0, pinocchio::JointModelSpherical(), pinocchio::SE3::Identity(), "base");
523  model.appendBodyToJoint(1, pinocchio::Inertia::Identity());
524  std::cout << model;
525  model.lowerPositionLimit.head<3>().fill(-1.);
526  model.upperPositionLimit.head<3>().fill(1.);
527 
528  ADModel ad_model = model.cast<ADScalar>();
529  ADData ad_data(ad_model);
530  int nq = model.nq;
531  int nv = model.nv;
532 
533  SX cs_q0 = SX::sym("q0", nq);
534  SX cs_q1 = SX::sym("q1", nq);
535  SX cs_dq0 = SX::sym("dq0", nv);
536  SX cs_dq1 = SX::sym("dq0", nv);
537  SX zero_vec(SX::zeros(2 * nv));
538 
539  typedef ADModel::ConfigVectorType cConfig_t;
540  typedef ADModel::TangentVectorType cTangent_t;
541 
542  cConfig_t cq0(nq), cq1(nq);
543  cTangent_t cdq0(nv), cdq1(nv);
544  pinocchio::casadi::copy(cs_q0, cq0);
545  pinocchio::casadi::copy(cs_q1, cq1);
546  pinocchio::casadi::copy(cs_dq0, cdq0);
547  pinocchio::casadi::copy(cs_dq1, cdq1);
548  ADScalar cs_dqall = vertcat(cs_dq0, cs_dq1);
549 
550  cConfig_t cq0_i = pinocchio::integrate(ad_model, cq0, cdq0);
551  cConfig_t cq1_i = pinocchio::integrate(ad_model, cq1, cdq1);
552 
553  const double dt(0.05);
554 
555  cTangent_t cv = pinocchio::difference(ad_model, cq0_i, cq1_i) / dt;
556  pinocchio::computeKineticEnergy(ad_model, ad_data, cq0_i, cv);
557  const auto KE_expr = ad_data.kinetic_energy;
558  const auto gKE_expr = jacobian(KE_expr, cs_dqall);
559  const auto HKE_expr = jacobian(gKE_expr, cs_dqall);
560 
561  casadi::Function KE_eval(
562  "KE", SXVector{cs_q0, cs_q1}, SXVector{substitute(KE_expr, cs_dqall, zero_vec)});
563  casadi::Function gKE_eval(
564  "gKE", SXVector{cs_q0, cs_q1}, SXVector{substitute(gKE_expr, cs_dqall, zero_vec)});
565  casadi::Function HKE_eval(
566  "HKE", SXVector{cs_q0, cs_q1}, SXVector{substitute(HKE_expr, cs_dqall, zero_vec)});
567  std::cout << HKE_eval << std::endl;
568 
569  auto q0 = pinocchio::neutral(model);
570  auto q1 = pinocchio::randomConfiguration(model);
571  auto q2 = pinocchio::randomConfiguration(model);
572  casadi::DM q0d(nq);
573  pinocchio::casadi::copy(q0, q0d);
574  casadi::DM q1d(nq);
575  pinocchio::casadi::copy(q1, q1d);
576  casadi::DM q2d(nq);
577  pinocchio::casadi::copy(q2, q2d);
578 
579  const static double eps = 1e-8;
580 
581  auto fd_grad_lambda =
582  [&model, &KE_eval](const Model::ConfigVectorType q0_, const Model::ConfigVectorType & q1_) {
583  auto nv = model.nv;
584  // finite differencing
585  Model::TangentVectorType dq0(nv), dq1(nv);
586  dq0.setZero();
587  dq1.setZero();
588  Eigen::VectorXd jac_fd(2 * nv);
589 
590  const casadi::DM dm = KE_eval(DMVector{eigenToDM(q0_), eigenToDM(q1_)})[0];
591  for (int i = 0; i < nv; i++)
592  {
593  dq0[i] = eps;
594  dq1[i] = eps;
595 
596  Model::ConfigVectorType q0_i = pinocchio::integrate(model, q0_, dq0);
597  // std::cout << "\nq0_i: " << q0_i.transpose() << std::endl;
598  casadi::DM dp1 = KE_eval(DMVector{eigenToDM(q0_i), eigenToDM(q1_)})[0];
599  // std::cout << "dp1: " << dp1 << std::endl;
600  casadi::DM diff1 = (dp1 - dm) / eps;
601 
602  Model::ConfigVectorType q1_i = pinocchio::integrate(model, q1_, dq1);
603  casadi::DM dp2 = KE_eval(DMVector{eigenToDM(q0_), eigenToDM(q1_i)})[0];
604  // std::cout << "dp2: " << dp2 << std::endl;
605  casadi::DM diff2 = (dp2 - dm) / eps;
606 
607  jac_fd[i] = static_cast<double>(diff1);
608  jac_fd[i + nv] = static_cast<double>(diff2);
609 
610  dq0[i] = 0.;
611  dq1[i] = 0.;
612  }
613  return jac_fd;
614  };
615 
616  std::cout << "eval: {q0d,q0d}: " << KE_eval(DMVector{q0d, q0d}) << std::endl;
617  std::cout << "grad: {q0d,q0d}: " << gKE_eval(DMVector{q0d, q0d}) << std::endl;
618  std::cout << "FD grad: {q0d,q0d}:" << fd_grad_lambda(q0, q0).transpose() << std::endl;
619  std::cout << "---" << std::endl;
620 
621  std::cout << "eval: {q1d,q1d}: " << KE_eval(DMVector{q1d, q1d}) << std::endl;
622  std::cout << "grad: {q1d,q1d}: " << gKE_eval(DMVector{q1d, q1d}) << std::endl;
623  std::cout << "FD grad: {q1d,q1d}:" << fd_grad_lambda(q1, q1).transpose() << std::endl;
624  std::cout << "---" << std::endl;
625 
626  std::cout << "eval: {q1d,q2d}: " << KE_eval(DMVector{q1d, q2d}) << std::endl;
627  std::cout << "grad: {q1d,q2d}: " << gKE_eval(DMVector{q1d, q2d}) << std::endl;
628  std::cout << "FD grad: {q1d,q2d}:" << fd_grad_lambda(q1, q2).transpose() << std::endl;
629  std::cout << "---" << std::endl;
630 
631  std::cout << "eval: {q2d,q2d}: " << KE_eval(DMVector{q2d, q2d}) << std::endl;
632  std::cout << "grad: {q2d,q2d}: " << gKE_eval(DMVector{q2d, q2d}) << std::endl;
633  std::cout << "FD grad: {q2d,q2d}:" << fd_grad_lambda(q2, q2).transpose() << std::endl;
634  std::cout << "---" << std::endl;
635  std::cout << '\n';
636 
637  auto fd_hess_ambda =
638  [&model, &gKE_eval](const Model::ConfigVectorType & q0_, const Model::ConfigVectorType & q1_) {
639  auto nv = model.nv;
640  // finite differencing
641  Model::TangentVectorType dq0(nv), dq1(nv);
642  dq0.setZero();
643  dq1.setZero();
644  Eigen::MatrixXd jac_fd(2 * nv, 2 * nv);
645  for (int i = 0; i < nv; i++)
646  {
647  dq0(i, 0) = eps;
648  dq1(i, 0) = eps;
649 
650  Model::ConfigVectorType q0_i = pinocchio::integrate(model, q0_, dq0);
651  auto dp = gKE_eval(DMVector{eigenToDM(q0_i), eigenToDM(q1_)});
652  auto dm = gKE_eval(DMVector{eigenToDM(q0_), eigenToDM(q1_)});
653  auto diff1 = (dp[0] - dm[0]) / eps;
654 
655  Model::ConfigVectorType q1_i = pinocchio::integrate(model, q1_, dq1);
656  dp = gKE_eval(DMVector{eigenToDM(q0_), eigenToDM(q1_i)});
657  auto diff2 = (dp[0] - dm[0]) / eps;
658 
659  for (int j = 0; j < jac_fd.rows(); j++)
660  {
661  jac_fd(j, i) = static_cast<double>(diff1(j));
662  jac_fd(j, i + nv) = static_cast<double>(diff2(j));
663  }
664 
665  dq0(i, 0) = 0.;
666  dq1(i, 0) = 0.;
667  }
668  return jac_fd;
669  };
670 
671  std::cout << HKE_eval(DMVector{q0d, q0d})[0] << '\n';
672  auto jac_fd = fd_hess_ambda(q0, q0);
673  std::cout << jac_fd << '\n';
674 
675  std::cout << HKE_eval(DMVector{q1d, q2d})[0] << '\n';
676  jac_fd = fd_hess_ambda(q1, q2);
677  std::cout << jac_fd << '\n';
678 
679  std::cout << HKE_eval(DMVector{q0d, q2d})[0] << '\n';
680  jac_fd = fd_hess_ambda(q0, q2);
681  std::cout << jac_fd << '\n';
682 
683  std::cout << HKE_eval(DMVector{q2d, q2d})[0] << '\n';
684  jac_fd = fd_hess_ambda(q2, q2);
685  std::cout << jac_fd << '\n';
686 }
687 
688 BOOST_AUTO_TEST_SUITE_END()
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
frames.hpp
kinematics-derivatives.dv_dv_local
dv_dv_local
Definition: kinematics-derivatives.py:34
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::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.
append-urdf-model-with-another-model.model2
model2
Definition: append-urdf-model-with-another-model.py:24
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.
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_jacobian)
Definition: casadi/algorithms.cpp:28
pinocchio.casadi::sym
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
Definition: autodiff/casadi.hpp:230
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
pinocchio::Convention::WORLD
@ WORLD
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
kinematics.hpp
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
q2
q2
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
test_interp_for_model
void test_interp_for_model(const pinocchio::ModelTpl< double > &model)
Definition: casadi/algorithms.cpp:425
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::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
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::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::difference
void difference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &dvout)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.
Definition: joint-configuration.hpp:193
pinocchio::python::context::Model
ModelTpl< Scalar, Options > Model
Definition: bindings/python/context/generic.hpp:63
meshcat-viewer.q1
q1
Definition: meshcat-viewer.py:84
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
rnea.hpp
pinocchio::interpolate
void interpolate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Scalar &u, const Eigen::MatrixBase< ReturnType > &qout)
Interpolate two configurations for a given model.
Definition: joint-configuration.hpp:127
aba.hpp
pinocchio::Index
PINOCCHIO_COMPILER_DIAGNOSTIC_POP typedef std::size_t Index
Definition: multibody/fwd.hpp:22
anymal-simulation.model
model
Definition: anymal-simulation.py:8
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
codegen-rnea.nq
nq
Definition: codegen-rnea.py:18
joint-configuration.hpp
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
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::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:70
pinocchio::python::context::Data
DataTpl< Scalar, Options > Data
Definition: bindings/python/context/generic.hpp:64
pinocchio::context::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: context/generic.hpp:49
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...
cartpole.v
v
Definition: cartpole.py:153
data.hpp
pinocchio::JointModelSphericalTpl
Definition: multibody/joint/fwd.hpp:73
q
q
pinocchio::jacobian
void jacobian(const ConstraintModelTpl< Scalar, Options, ConstraintCollectionTpl > &cmodel, ConstraintDataTpl< Scalar, Options, ConstraintCollectionTpl > &cdata, const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< JacobianMatrix > &jacobian_matrix)
Definition: constraint-model-visitor.hpp:199
pinocchio::JointModelRevoluteTpl
Definition: multibody/joint/fwd.hpp:33
autodiff-rnea.dtau_da
dtau_da
Definition: autodiff-rnea.py:29
a
Vec3f a
pinocchio::computeKineticEnergy
Scalar computeKineticEnergy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the kinetic energy of the system. The result is accessible through data.kinetic_energy.
Matrix6x
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
Definition: joint-mimic.cpp:15
fill
fill
pinocchio::rnea
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(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 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
pinocchio::InertiaTpl< context::Scalar, context::Options >::Identity
static InertiaTpl Identity()
Definition: spatial/inertia.hpp:349
pinocchio.casadi::copy
void copy(::casadi::Matrix< Scalar > const &src, Eigen::MatrixBase< MT > &dst)
Definition: autodiff/casadi.hpp:154
pinocchio::SE3Tpl::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
dcrba.eps
int eps
Definition: dcrba.py:458
codegen-rnea.nv
nv
Definition: codegen-rnea.py:19
casadi.hpp
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:363
energy.hpp
pinocchio::python::context::SE3
SE3Tpl< Scalar, Options > SE3
Definition: bindings/python/context/generic.hpp:53
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
ur5x4.q0
q0
Definition: ur5x4.py:44
crba.hpp
cartpole.dt
float dt
Definition: cartpole.py:145
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)
eigenToDM
casadi::DM eigenToDM(const Eigen::MatrixBase< Derived > &x)
Without copy.
Definition: casadi-utils.hpp:11
casadi-utils.hpp
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:25