joint-configurations.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #include "utils/model-generator.hpp"
6 #include "pinocchio/parsers/sample-models.hpp"
7 #include "pinocchio/algorithm/joint-configuration.hpp"
8 #include "pinocchio/math/quaternion.hpp"
9 
10 #include <boost/test/unit_test.hpp>
11 #include <boost/utility/binary.hpp>
12 
13 using namespace pinocchio;
14 
15 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
16 
17 BOOST_AUTO_TEST_CASE ( integration_test )
18 {
19  Model model; buildAllJointsModel(model);
20 
21  std::vector<Eigen::VectorXd> qs(2);
22  std::vector<Eigen::VectorXd> qdots(2);
23  std::vector<Eigen::VectorXd> results(2);
24 
25  //
26  // Test Case 0 : Integration of a config with zero velocity
27  //
28  qs[0] = Eigen::VectorXd::Ones(model.nq);
29  normalize(model,qs[0]);
30 
31  qdots[0] = Eigen::VectorXd::Zero(model.nv);
32  results[0] = integrate(model,qs[0],qdots[0]);
33 
34  BOOST_CHECK_MESSAGE(results[0].isApprox(qs[0], 1e-12), "integration of full body with zero velocity - wrong results");
35 }
36 
37 BOOST_AUTO_TEST_CASE ( interpolate_test )
38 {
39  Model model; buildAllJointsModel(model);
40 
41  Eigen::VectorXd q0(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
42  Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
43 
44  Eigen::VectorXd q01_0 = interpolate(model,q0,q1,0.0);
45  BOOST_CHECK_MESSAGE(isSameConfiguration(model,q01_0,q0), "interpolation: q01_0 != q0");
46 
47  Eigen::VectorXd q01_1 = interpolate(model,q0,q1,1.0);
48  BOOST_CHECK_MESSAGE(isSameConfiguration(model,q01_1,q1), "interpolation: q01_1 != q1");
49 }
50 
51 BOOST_AUTO_TEST_CASE ( diff_integration_test )
52 {
53  Model model; buildAllJointsModel(model);
54 
55  std::vector<Eigen::VectorXd> qs(2);
56  std::vector<Eigen::VectorXd> vs(2);
57  std::vector<Eigen::MatrixXd> results(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
58  std::vector<Eigen::MatrixXd> results_fd(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
59 
60  qs[0] = Eigen::VectorXd::Ones(model.nq);
61  normalize(model,qs[0]);
62 
63  vs[0] = Eigen::VectorXd::Zero(model.nv);
64  vs[1] = Eigen::VectorXd::Ones(model.nv);
65  dIntegrate(model,qs[0],vs[0],results[0],ARG0);
66 
67  Eigen::VectorXd q_fd(model.nq), v_fd(model.nv); v_fd.setZero();
68  const double eps = 1e-8;
69  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
70  {
71  v_fd[k] = eps;
72  q_fd = integrate(model,qs[0],v_fd);
73  results_fd[0].col(k) = difference(model,qs[0],q_fd)/eps;
74  v_fd[k] = 0.;
75  }
76  BOOST_CHECK(results[0].isIdentity(sqrt(eps)));
77  BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));
78 
79  dIntegrate(model,qs[0],vs[0],results[1],ARG1);
80  BOOST_CHECK(results[1].isApprox(results[0]));
81 
82  dIntegrate(model,qs[0],vs[1],results[0],ARG0);
83  Eigen::VectorXd q_fd_intermediate(model.nq);
84  Eigen::VectorXd q0_plus_v = integrate(model,qs[0],vs[1]);
85  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
86  {
87  v_fd[k] = eps;
88  q_fd_intermediate = integrate(model,qs[0],v_fd);
89  q_fd = integrate(model,q_fd_intermediate,vs[1]);
90  results_fd[0].col(k) = difference(model,q0_plus_v,q_fd)/eps;
91  v_fd[k] = 0.;
92  }
93 
94  BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));
95 
96  dIntegrate(model,qs[0],vs[1],results[1],ARG1);
97  v_fd = vs[1];
98  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
99  {
100  v_fd[k] += eps;
101  q_fd = integrate(model,qs[0],v_fd);
102  results_fd[1].col(k) = difference(model,q0_plus_v,q_fd)/eps;
103  v_fd[k] -= eps;
104  }
105 
106  BOOST_CHECK(results[1].isApprox(results_fd[1],sqrt(eps)));
107 }
108 
109 BOOST_AUTO_TEST_CASE ( diff_difference_test )
110 {
111  Model model; buildAllJointsModel(model);
112 
113  std::vector<Eigen::VectorXd> qs(2);
114  std::vector<Eigen::VectorXd> vs(2);
115  std::vector<Eigen::MatrixXd> results(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
116  std::vector<Eigen::MatrixXd> results_fd(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
117 
118  qs[0] = Eigen::VectorXd::Random(model.nq);
119  normalize(model,qs[0]);
120  const Eigen::VectorXd & q0 = qs[0];
121  qs[1] = Eigen::VectorXd::Random(model.nq);
122  normalize(model,qs[1]);
123  const Eigen::VectorXd & q1 = qs[1];
124 
125  vs[0] = Eigen::VectorXd::Zero(model.nv);
126  vs[1] = Eigen::VectorXd::Ones(model.nv);
127  dDifference(model,q0,q1,results[0],ARG0);
128 
129  Eigen::VectorXd q_fd(model.nq), v_fd(model.nv); v_fd.setZero();
130  const double eps = 1e-8;
131  const Eigen::VectorXd v_ref = difference(model,q0,q1);
132  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
133  {
134  v_fd[k] = eps;
135  q_fd = integrate(model,q0,v_fd);
136  results_fd[0].col(k) = (difference(model,q_fd,q1) - v_ref)/eps;
137  v_fd[k] = 0.;
138  }
139  BOOST_CHECK(results[0].isApprox(results_fd[0],sqrt(eps)));
140 
141  dDifference(model,q0,q0,results[0],ARG0);
142  BOOST_CHECK((-results[0]).isIdentity());
143 
144  dDifference(model,q0,q0,results[1],ARG1);
145  BOOST_CHECK(results[1].isIdentity());
146 
147  dDifference(model,q0,q1,results[1],ARG1);
148  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
149  {
150  v_fd[k] = eps;
151  q_fd = integrate(model,q1,v_fd);
152  results_fd[1].col(k) = (difference(model,q0,q_fd) - v_ref)/eps;
153  v_fd[k] = 0.;
154  }
155  BOOST_CHECK(results[1].isApprox(results_fd[1],sqrt(eps)));
156 }
157 
158 BOOST_AUTO_TEST_CASE ( diff_difference_vs_diff_integrate )
159 {
160  Model model; buildAllJointsModel(model);
161 
162  std::vector<Eigen::VectorXd> qs(2);
163  std::vector<Eigen::VectorXd> vs(2);
164  std::vector<Eigen::MatrixXd> results(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
165  std::vector<Eigen::MatrixXd> results_fd(2,Eigen::MatrixXd::Zero(model.nv,model.nv));
166 
167  Eigen::VectorXd q0 = Eigen::VectorXd::Random(model.nq);
168  normalize(model,q0);
169 
170  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
171  Eigen::VectorXd q1 = integrate(model,q0,v);
172 
173  Eigen::VectorXd v_diff = difference(model,q0,q1);
174  BOOST_CHECK(v_diff.isApprox(v));
175 
176  Eigen::MatrixXd J_int_dq = Eigen::MatrixXd::Zero(model.nv,model.nv);
177  Eigen::MatrixXd J_int_dv = Eigen::MatrixXd::Zero(model.nv,model.nv);
178  dIntegrate(model,q0,v,J_int_dq,ARG0);
179  dIntegrate(model,q0,v,J_int_dv,ARG1);
180 
181  Eigen::MatrixXd J_diff_dq0 = Eigen::MatrixXd::Zero(model.nv,model.nv);
182  Eigen::MatrixXd J_diff_dq1 = Eigen::MatrixXd::Zero(model.nv,model.nv);
183  dDifference(model,q0,q1,J_diff_dq0,ARG0);
184  dDifference(model,q0,q1,J_diff_dq1,ARG1);
185 
186  BOOST_CHECK(J_int_dq.isApprox(Eigen::MatrixXd(-(J_int_dv * J_diff_dq0))));
187  BOOST_CHECK(Eigen::MatrixXd(J_int_dv * J_diff_dq1).isIdentity());
188 }
189 
190 
191 BOOST_AUTO_TEST_CASE ( dIntegrate_assignementop_test )
192 {
193  Model model; buildAllJointsModel(model);
194 
195  std::vector<Eigen::MatrixXd> results(3,Eigen::MatrixXd::Zero(model.nv,model.nv));
196 
197  Eigen::VectorXd qs = Eigen::VectorXd::Ones(model.nq);
198  normalize(model,qs);
199 
200  Eigen::VectorXd vs = Eigen::VectorXd::Random(model.nv);
201 
202  //SETTO
203  dIntegrate(model,qs,vs,results[0],ARG0);
204  dIntegrate(model,qs,vs,results[1],ARG0,SETTO);
205  BOOST_CHECK(results[0].isApprox(results[1]));
206 
207  //ADDTO
208  results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
209  results[2] = results[1];
210  results[0].setZero();
211  dIntegrate(model,qs,vs,results[0],ARG0,SETTO);
212  dIntegrate(model,qs,vs,results[1],ARG0,ADDTO);
213  BOOST_CHECK(results[1].isApprox(results[2] + results[0]));
214 
215  //RMTO
216  results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
217  results[2] = results[1];
218  results[0].setZero();
219  dIntegrate(model,qs,vs,results[0],ARG0,SETTO);
220  dIntegrate(model,qs,vs,results[1],ARG0,RMTO);
221  BOOST_CHECK(results[1].isApprox(results[2] - results[0]));
222 
223  //SETTO
224  results[0].setZero();
225  results[1].setZero();
226  dIntegrate(model,qs,vs,results[0],ARG1);
227  dIntegrate(model,qs,vs,results[1],ARG1,SETTO);
228  BOOST_CHECK(results[0].isApprox(results[1]));
229 
230  //ADDTO
231  results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
232  results[2] = results[1];
233  results[0].setZero();
234  dIntegrate(model,qs,vs,results[0],ARG1,SETTO);
235  dIntegrate(model,qs,vs,results[1],ARG1,ADDTO);
236  BOOST_CHECK(results[1].isApprox(results[2] + results[0]));
237 
238  //RMTO
239  results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
240  results[2] = results[1];
241  results[0].setZero();
242  dIntegrate(model,qs,vs,results[0],ARG1,SETTO);
243  dIntegrate(model,qs,vs,results[1],ARG1,RMTO);
244  BOOST_CHECK(results[1].isApprox(results[2] - results[0]));
245 
246  //Transport
247  std::vector<Eigen::MatrixXd> J(2,Eigen::MatrixXd::Zero(model.nv,2*model.nv));
248  J[0] = Eigen::MatrixXd::Random(model.nv, 2*model.nv);
249  results[0].setZero();
250  dIntegrate(model,qs,vs,results[0],ARG0,SETTO);
251  dIntegrateTransport(model,qs,vs,J[0],J[1],ARG0);
252  BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
253 
254  J[0] = Eigen::MatrixXd::Random(model.nv, 2*model.nv);
255  results[0].setZero();
256  dIntegrate(model,qs,vs,results[0],ARG1,SETTO);
257  dIntegrateTransport(model,qs,vs,J[0],J[1],ARG1);
258  BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
259 
260  //TransportInPlace
261  J[1] = Eigen::MatrixXd::Random(model.nv, 2*model.nv);
262  J[0] = J[1];
263  results[0].setZero();
264  dIntegrate(model,qs,vs,results[0],ARG0,SETTO);
265  dIntegrateTransport(model,qs,vs,J[1],ARG0);
266  BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
267 
268  J[1] = Eigen::MatrixXd::Random(model.nv, 2*model.nv);
269  J[0] = J[1];
270  results[0].setZero();
271  dIntegrate(model,qs,vs,results[0],ARG1,SETTO);
272  dIntegrateTransport(model,qs,vs,J[1],ARG1);
273  BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
274 
275 }
276 
277 BOOST_AUTO_TEST_CASE ( integrate_difference_test )
278 {
279  Model model; buildAllJointsModel(model);
280 
281  Eigen::VectorXd q0(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
282  Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
283  Eigen::VectorXd qdot(Eigen::VectorXd::Random(model.nv));
284 
285  BOOST_CHECK_MESSAGE(isSameConfiguration(model, integrate(model, q0, difference(model, q0,q1)), q1), "Integrate (difference) - wrong results");
286 
287  BOOST_CHECK_MESSAGE(difference(model, q0, integrate(model,q0, qdot)).isApprox(qdot),"difference (integrate) - wrong results");
288 }
289 
290 
291 BOOST_AUTO_TEST_CASE ( neutral_configuration_test )
292 {
293  Model model; buildAllJointsModel(model);
294 
295  Eigen::VectorXd expected(model.nq);
296  expected << 0,0,0,0,0,0,1,
297  0,0,0,1,
298  0,0,1,0,
299  0,
300  0,
301  0,
302  0,
303  0,0,0,
304  0,0,0;
305 
306 
307  Eigen::VectorXd neutral_config = neutral(model);
308  BOOST_CHECK_MESSAGE(neutral_config.isApprox(expected, 1e-12), "neutral configuration - wrong results");
309 }
310 
311 BOOST_AUTO_TEST_CASE ( distance_configuration_test )
312 {
313  Model model; buildAllJointsModel(model);
314 
315  Model::ConfigVectorType q0 = neutral(model);
316  Model::ConfigVectorType q1(integrate (model, q0, Model::TangentVectorType::Ones(model.nv)));
317 
318  double dist = distance(model,q0,q1);
319 
320  BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results");
321  BOOST_CHECK_SMALL(dist-difference(model,q0,q1).norm(), 1e-12);
322 }
323 
324 BOOST_AUTO_TEST_CASE ( squared_distance_test )
325 {
326  Model model; buildAllJointsModel(model);
327 
328  Eigen::VectorXd q0(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
329  Eigen::VectorXd q1(randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) ));
330 
331  double dist = distance(model,q0,q1);
332  Eigen::VectorXd squaredDistance_ = squaredDistance(model,q0,q1);
333 
334  BOOST_CHECK_SMALL(dist-math::sqrt(squaredDistance_.sum()), 1e-12);
335 }
336 
337 BOOST_AUTO_TEST_CASE ( uniform_sampling_test )
338 {
339  Model model; buildAllJointsModel(model);
340 
341  model.lowerPositionLimit = -1 * Eigen::VectorXd::Ones(model.nq);
342  model.upperPositionLimit = Eigen::VectorXd::Ones(model.nq);
343  Eigen::VectorXd q1(randomConfiguration(model));
344 
345  for (int i = 0; i < model.nq; ++i)
346  {
347  BOOST_CHECK_MESSAGE(q1[i] >= model.lowerPositionLimit[i] && q1[i] <= model.upperPositionLimit[i], " UniformlySample : Generated config not in bounds");
348  }
349 }
350 
351 BOOST_AUTO_TEST_CASE ( normalize_test )
352 {
353  Model model; buildAllJointsModel(model);
354 
355  Eigen::VectorXd q (Eigen::VectorXd::Ones(model.nq));
356  pinocchio::normalize(model, q);
357 
358  BOOST_CHECK(q.head<3>().isApprox(Eigen::VectorXd::Ones(3)));
359  BOOST_CHECK(fabs(q.segment<4>(3).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
360  BOOST_CHECK(fabs(q.segment<4>(7).norm() - 1) < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
361  const int n = model.nq - 7 - 4 - 4; // free flyer + spherical + planar
362  BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
363 }
364 
365 BOOST_AUTO_TEST_CASE ( is_normalized_test )
366 {
367  Model model; buildAllJointsModel(model);
368 
369  Eigen::VectorXd q;
370  q = Eigen::VectorXd::Ones(model.nq);
371  BOOST_CHECK(!pinocchio::isNormalized(model, q));
372  BOOST_CHECK(!pinocchio::isNormalized(model, q, 1e-8));
373  BOOST_CHECK(pinocchio::isNormalized(model, q, 1e2));
374 
375  pinocchio::normalize(model, q);
376  BOOST_CHECK(pinocchio::isNormalized(model, q));
377  BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
378 
379  q = pinocchio::neutral(model);
380  BOOST_CHECK(pinocchio::isNormalized(model, q));
381  BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
382 
383  model.lowerPositionLimit = -1 * Eigen::VectorXd::Ones(model.nq);
384  model.upperPositionLimit = Eigen::VectorXd::Ones(model.nq);
385  q = pinocchio::randomConfiguration(model);
386  BOOST_CHECK(pinocchio::isNormalized(model, q));
387  BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
388 }
389 
390 BOOST_AUTO_TEST_CASE ( integrateCoeffWiseJacobian_test )
391 {
392  Model model; buildModels::humanoidRandom(model);
393 
394  Eigen::VectorXd q(Eigen::VectorXd::Ones(model.nq));
395  pinocchio::normalize(model, q);
396 
397  Eigen::MatrixXd jac(model.nq,model.nv); jac.setZero();
398 
399  integrateCoeffWiseJacobian(model,q,jac);
400 
401 
402  Eigen::MatrixXd jac_fd(model.nq,model.nv);
403  Eigen::VectorXd q_plus;
404  const double eps = 1e-8;
405 
406  Eigen::VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
407  for(int k = 0; k < model.nv; ++k)
408  {
409  v_eps[k] = eps;
410  q_plus = integrate(model,q,v_eps);
411  jac_fd.col(k) = (q_plus - q)/eps;
412 
413  v_eps[k] = 0.;
414  }
415  BOOST_CHECK(jac.isApprox(jac_fd,sqrt(eps)));
416 }
417 
418 BOOST_AUTO_TEST_SUITE_END ()
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:746
pinocchio::distance
Scalar distance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1)
Distance between two configuration vectors, namely .
Definition: joint-configuration.hpp:581
n
Vec3f n
pinocchio::integrateCoeffWiseJacobian
void integrateCoeffWiseJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector > &q, const Eigen::MatrixBase< JacobianMatrix > &jacobian)
Return the Jacobian of the integrate function for the components of the config vector.
Definition: joint-configuration.hpp:721
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:54
pinocchio::dDifference
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...
Definition: joint-configuration.hpp:507
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:142
dcrba.eps
int eps
Definition: dcrba.py:384
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: src/multibody/model.hpp:139
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(integration_test)
Definition: joint-configurations.cpp:17
pinocchio::isNormalized
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec...
Definition: joint-configuration.hpp:641
pinocchio::isSameConfiguration
bool isSameConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q1, const Eigen::MatrixBase< ConfigVectorIn2 > &q2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Return true if the given configurations are equivalents, within the given precision.
Definition: joint-configuration.hpp:683
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::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:746
pinocchio::dIntegrateTransport
void dIntegrateTransport(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType1 > &Jin, const Eigen::MatrixBase< JacobianMatrixType2 > &Jout, const ArgumentPosition arg)
Transport a matrix from the terminal to the originate tangent space of the integrate operation...
Definition: joint-configuration.hpp:398
pinocchio::dIntegrate
void dIntegrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType > &J, const ArgumentPosition arg, const AssignmentOperatorType op=SETTO)
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the...
Definition: joint-configuration.hpp:337
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
pinocchio::buildAllJointsModel
void buildAllJointsModel(Model &model)
Definition: model-generator.hpp:33
pinocchio::q1
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Definition: joint-configuration.hpp:784
dist
FCL_REAL dist(short i) const
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::q0
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Definition: joint-configuration.hpp:784
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:257
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
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:96
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: src/multibody/model.hpp:66
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:609
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
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::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:746
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
pinocchio::squaredDistance
void squaredDistance(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &out)
Squared distance between two configuration vectors.
Definition: joint-configuration.hpp:179

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