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/multibody/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;
20  buildAllJointsModel(model);
21 
22  std::vector<Eigen::VectorXd> qs(2);
23  std::vector<Eigen::VectorXd> qdots(2);
24  std::vector<Eigen::VectorXd> results(2);
25 
26  //
27  // Test Case 0 : Integration of a config with zero velocity
28  //
29  qs[0] = Eigen::VectorXd::Ones(model.nq);
30  normalize(model, qs[0]);
31 
32  qdots[0] = Eigen::VectorXd::Zero(model.nv);
33  results[0] = integrate(model, qs[0], qdots[0]);
34 
35  BOOST_CHECK_MESSAGE(
36  results[0].isApprox(qs[0], 1e-12),
37  "integration of full body with zero velocity - wrong results");
38 }
39 
40 BOOST_AUTO_TEST_CASE(interpolate_test)
41 {
42  Model model;
43  buildAllJointsModel(model);
44 
45  Eigen::VectorXd q0(randomConfiguration(
46  model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
47  Eigen::VectorXd q1(randomConfiguration(
48  model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
49 
50  Eigen::VectorXd q01_0 = interpolate(model, q0, q1, 0.0);
51  BOOST_CHECK_MESSAGE(isSameConfiguration(model, q01_0, q0), "interpolation: q01_0 != q0");
52 
53  Eigen::VectorXd q01_1 = interpolate(model, q0, q1, 1.0);
54  BOOST_CHECK_MESSAGE(isSameConfiguration(model, q01_1, q1), "interpolation: q01_1 != q1");
55 }
56 
57 BOOST_AUTO_TEST_CASE(diff_integration_test)
58 {
59  Model model;
60  buildAllJointsModel(model);
61 
62  std::vector<Eigen::VectorXd> qs(2);
63  std::vector<Eigen::VectorXd> vs(2);
64  std::vector<Eigen::MatrixXd> results(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
65  std::vector<Eigen::MatrixXd> results_fd(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
66 
67  qs[0] = Eigen::VectorXd::Ones(model.nq);
68  normalize(model, qs[0]);
69 
70  vs[0] = Eigen::VectorXd::Zero(model.nv);
71  vs[1] = Eigen::VectorXd::Ones(model.nv);
72  dIntegrate(model, qs[0], vs[0], results[0], ARG0);
73 
74  Eigen::VectorXd q_fd(model.nq), v_fd(model.nv);
75  v_fd.setZero();
76  const double eps = 1e-8;
77  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
78  {
79  v_fd[k] = eps;
80  q_fd = integrate(model, qs[0], v_fd);
81  results_fd[0].col(k) = difference(model, qs[0], q_fd) / eps;
82  v_fd[k] = 0.;
83  }
84  BOOST_CHECK(results[0].isIdentity(sqrt(eps)));
85  BOOST_CHECK(results[0].isApprox(results_fd[0], sqrt(eps)));
86 
87  dIntegrate(model, qs[0], vs[0], results[1], ARG1);
88  BOOST_CHECK(results[1].isApprox(results[0]));
89 
90  dIntegrate(model, qs[0], vs[1], results[0], ARG0);
91  Eigen::VectorXd q_fd_intermediate(model.nq);
92  Eigen::VectorXd q0_plus_v = integrate(model, qs[0], vs[1]);
93  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
94  {
95  v_fd[k] = eps;
96  q_fd_intermediate = integrate(model, qs[0], v_fd);
97  q_fd = integrate(model, q_fd_intermediate, vs[1]);
98  results_fd[0].col(k) = difference(model, q0_plus_v, q_fd) / eps;
99  v_fd[k] = 0.;
100  }
101 
102  BOOST_CHECK(results[0].isApprox(results_fd[0], sqrt(eps)));
103 
104  dIntegrate(model, qs[0], vs[1], results[1], ARG1);
105  v_fd = vs[1];
106  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
107  {
108  v_fd[k] += eps;
109  q_fd = integrate(model, qs[0], v_fd);
110  results_fd[1].col(k) = difference(model, q0_plus_v, q_fd) / eps;
111  v_fd[k] -= eps;
112  }
113 
114  BOOST_CHECK(results[1].isApprox(results_fd[1], sqrt(eps)));
115 }
116 
117 BOOST_AUTO_TEST_CASE(diff_difference_test)
118 {
119  Model model;
120  buildAllJointsModel(model);
121 
122  std::vector<Eigen::VectorXd> qs(2);
123  std::vector<Eigen::VectorXd> vs(2);
124  std::vector<Eigen::MatrixXd> results(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
125  std::vector<Eigen::MatrixXd> results_fd(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
126 
127  qs[0] = Eigen::VectorXd::Random(model.nq);
128  normalize(model, qs[0]);
129  const Eigen::VectorXd & q0 = qs[0];
130  qs[1] = Eigen::VectorXd::Random(model.nq);
131  normalize(model, qs[1]);
132  const Eigen::VectorXd & q1 = qs[1];
133 
134  vs[0] = Eigen::VectorXd::Zero(model.nv);
135  vs[1] = Eigen::VectorXd::Ones(model.nv);
136  dDifference(model, q0, q1, results[0], ARG0);
137 
138  Eigen::VectorXd q_fd(model.nq), v_fd(model.nv);
139  v_fd.setZero();
140  const double eps = 1e-8;
141  const Eigen::VectorXd v_ref = difference(model, q0, q1);
142  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
143  {
144  v_fd[k] = eps;
145  q_fd = integrate(model, q0, v_fd);
146  results_fd[0].col(k) = (difference(model, q_fd, q1) - v_ref) / eps;
147  v_fd[k] = 0.;
148  }
149  BOOST_CHECK(results[0].isApprox(results_fd[0], sqrt(eps)));
150 
151  dDifference(model, q0, q0, results[0], ARG0);
152  BOOST_CHECK((-results[0]).isIdentity());
153 
154  dDifference(model, q0, q0, results[1], ARG1);
155  BOOST_CHECK(results[1].isIdentity());
156 
157  dDifference(model, q0, q1, results[1], ARG1);
158  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
159  {
160  v_fd[k] = eps;
161  q_fd = integrate(model, q1, v_fd);
162  results_fd[1].col(k) = (difference(model, q0, q_fd) - v_ref) / eps;
163  v_fd[k] = 0.;
164  }
165  BOOST_CHECK(results[1].isApprox(results_fd[1], sqrt(eps)));
166 }
167 
168 BOOST_AUTO_TEST_CASE(diff_difference_vs_diff_integrate)
169 {
170  Model model;
171  buildAllJointsModel(model);
172 
173  std::vector<Eigen::VectorXd> qs(2);
174  std::vector<Eigen::VectorXd> vs(2);
175  std::vector<Eigen::MatrixXd> results(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
176  std::vector<Eigen::MatrixXd> results_fd(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
177 
178  Eigen::VectorXd q0 = Eigen::VectorXd::Random(model.nq);
179  normalize(model, q0);
180 
181  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
182  Eigen::VectorXd q1 = integrate(model, q0, v);
183 
184  Eigen::VectorXd v_diff = difference(model, q0, q1);
185  BOOST_CHECK(v_diff.isApprox(v));
186 
187  Eigen::MatrixXd J_int_dq = Eigen::MatrixXd::Zero(model.nv, model.nv);
188  Eigen::MatrixXd J_int_dv = Eigen::MatrixXd::Zero(model.nv, model.nv);
189  dIntegrate(model, q0, v, J_int_dq, ARG0);
190  dIntegrate(model, q0, v, J_int_dv, ARG1);
191 
192  Eigen::MatrixXd J_diff_dq0 = Eigen::MatrixXd::Zero(model.nv, model.nv);
193  Eigen::MatrixXd J_diff_dq1 = Eigen::MatrixXd::Zero(model.nv, model.nv);
194  dDifference(model, q0, q1, J_diff_dq0, ARG0);
195  dDifference(model, q0, q1, J_diff_dq1, ARG1);
196 
197  BOOST_CHECK(J_int_dq.isApprox(Eigen::MatrixXd(-(J_int_dv * J_diff_dq0))));
198  BOOST_CHECK(Eigen::MatrixXd(J_int_dv * J_diff_dq1).isIdentity());
199 }
200 
201 BOOST_AUTO_TEST_CASE(dIntegrate_assignementop_test)
202 {
203  Model model;
204  buildAllJointsModel(model);
205 
206  std::vector<Eigen::MatrixXd> results(3, Eigen::MatrixXd::Zero(model.nv, model.nv));
207 
208  Eigen::VectorXd qs = Eigen::VectorXd::Ones(model.nq);
209  normalize(model, qs);
210 
211  Eigen::VectorXd vs = Eigen::VectorXd::Random(model.nv);
212 
213  // SETTO
214  dIntegrate(model, qs, vs, results[0], ARG0);
215  dIntegrate(model, qs, vs, results[1], ARG0, SETTO);
216  BOOST_CHECK(results[0].isApprox(results[1]));
217 
218  // ADDTO
219  results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
220  results[2] = results[1];
221  results[0].setZero();
222  dIntegrate(model, qs, vs, results[0], ARG0, SETTO);
223  dIntegrate(model, qs, vs, results[1], ARG0, ADDTO);
224  BOOST_CHECK(results[1].isApprox(results[2] + results[0]));
225 
226  // RMTO
227  results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
228  results[2] = results[1];
229  results[0].setZero();
230  dIntegrate(model, qs, vs, results[0], ARG0, SETTO);
231  dIntegrate(model, qs, vs, results[1], ARG0, RMTO);
232  BOOST_CHECK(results[1].isApprox(results[2] - results[0]));
233 
234  // SETTO
235  results[0].setZero();
236  results[1].setZero();
237  dIntegrate(model, qs, vs, results[0], ARG1);
238  dIntegrate(model, qs, vs, results[1], ARG1, SETTO);
239  BOOST_CHECK(results[0].isApprox(results[1]));
240 
241  // ADDTO
242  results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
243  results[2] = results[1];
244  results[0].setZero();
245  dIntegrate(model, qs, vs, results[0], ARG1, SETTO);
246  dIntegrate(model, qs, vs, results[1], ARG1, ADDTO);
247  BOOST_CHECK(results[1].isApprox(results[2] + results[0]));
248 
249  // RMTO
250  results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
251  results[2] = results[1];
252  results[0].setZero();
253  dIntegrate(model, qs, vs, results[0], ARG1, SETTO);
254  dIntegrate(model, qs, vs, results[1], ARG1, RMTO);
255  BOOST_CHECK(results[1].isApprox(results[2] - results[0]));
256 
257  // Transport
258  std::vector<Eigen::MatrixXd> J(2, Eigen::MatrixXd::Zero(model.nv, 2 * model.nv));
259  J[0] = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
260  results[0].setZero();
261  dIntegrate(model, qs, vs, results[0], ARG0, SETTO);
262  dIntegrateTransport(model, qs, vs, J[0], J[1], ARG0);
263  BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
264 
265  J[0] = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
266  results[0].setZero();
267  dIntegrate(model, qs, vs, results[0], ARG1, SETTO);
268  dIntegrateTransport(model, qs, vs, J[0], J[1], ARG1);
269  BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
270 
271  // TransportInPlace
272  J[1] = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
273  J[0] = J[1];
274  results[0].setZero();
275  dIntegrate(model, qs, vs, results[0], ARG0, SETTO);
276  dIntegrateTransport(model, qs, vs, J[1], ARG0);
277  BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
278 
279  J[1] = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
280  J[0] = J[1];
281  results[0].setZero();
282  dIntegrate(model, qs, vs, results[0], ARG1, SETTO);
283  dIntegrateTransport(model, qs, vs, J[1], ARG1);
284  BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
285 }
286 
287 BOOST_AUTO_TEST_CASE(integrate_difference_test)
288 {
289  Model model;
290  buildAllJointsModel(model);
291 
292  Eigen::VectorXd q0(randomConfiguration(
293  model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
294  Eigen::VectorXd q1(randomConfiguration(
295  model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
296  Eigen::VectorXd qdot(Eigen::VectorXd::Random(model.nv));
297 
298  BOOST_CHECK_MESSAGE(
299  isSameConfiguration(model, integrate(model, q0, difference(model, q0, q1)), q1),
300  "Integrate (difference) - wrong results");
301 
302  BOOST_CHECK_MESSAGE(
303  difference(model, q0, integrate(model, q0, qdot)).isApprox(qdot),
304  "difference (integrate) - wrong results");
305 }
306 
307 BOOST_AUTO_TEST_CASE(neutral_configuration_test)
308 {
309  Model model;
310  buildAllJointsModel(model);
311 
312  Eigen::VectorXd expected(model.nq);
313  expected << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
314 
315  Eigen::VectorXd neutral_config = neutral(model);
316  BOOST_CHECK_MESSAGE(
317  neutral_config.isApprox(expected, 1e-12), "neutral configuration - wrong results");
318 }
319 
320 BOOST_AUTO_TEST_CASE(distance_configuration_test)
321 {
322  Model model;
323  buildAllJointsModel(model);
324 
325  Model::ConfigVectorType q0 = neutral(model);
326  Model::ConfigVectorType q1(integrate(model, q0, Model::TangentVectorType::Ones(model.nv)));
327 
328  double dist = distance(model, q0, q1);
329 
330  BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results");
331  BOOST_CHECK_SMALL(dist - difference(model, q0, q1).norm(), 1e-12);
332 }
333 
334 BOOST_AUTO_TEST_CASE(squared_distance_test)
335 {
336  Model model;
337  buildAllJointsModel(model);
338 
339  Eigen::VectorXd q0(randomConfiguration(
340  model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
341  Eigen::VectorXd q1(randomConfiguration(
342  model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
343 
344  double dist = distance(model, q0, q1);
345  Eigen::VectorXd squaredDistance_ = squaredDistance(model, q0, q1);
346 
347  BOOST_CHECK_SMALL(dist - math::sqrt(squaredDistance_.sum()), 1e-12);
348 }
349 
350 BOOST_AUTO_TEST_CASE(uniform_sampling_test)
351 {
352  Model model;
353  buildAllJointsModel(model);
354 
355  model.lowerPositionLimit = -1 * Eigen::VectorXd::Ones(model.nq);
356  model.upperPositionLimit = Eigen::VectorXd::Ones(model.nq);
357  Eigen::VectorXd q1(randomConfiguration(model));
358 
359  for (int i = 0; i < model.nq; ++i)
360  {
361  BOOST_CHECK_MESSAGE(
362  q1[i] >= model.lowerPositionLimit[i] && q1[i] <= model.upperPositionLimit[i],
363  " UniformlySample : Generated config not in bounds");
364  }
365 }
366 
367 BOOST_AUTO_TEST_CASE(normalize_test)
368 {
369  Model model;
370  buildAllJointsModel(model);
371 
372  Eigen::VectorXd q(Eigen::VectorXd::Ones(model.nq));
373  pinocchio::normalize(model, q);
374 
375  BOOST_CHECK(q.head<3>().isApprox(Eigen::VectorXd::Ones(3)));
376  BOOST_CHECK(
377  fabs(q.segment<4>(3).norm() - 1)
378  < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
379  BOOST_CHECK(
380  fabs(q.segment<4>(7).norm() - 1)
381  < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
382  const int n = model.nq - 7 - 4 - 4; // free flyer + spherical + planar
383  BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
384 }
385 
386 BOOST_AUTO_TEST_CASE(is_normalized_test)
387 {
388  Model model;
389  buildAllJointsModel(model);
390 
391  Eigen::VectorXd q;
392  q = Eigen::VectorXd::Ones(model.nq);
393  BOOST_CHECK(!pinocchio::isNormalized(model, q));
394  BOOST_CHECK(!pinocchio::isNormalized(model, q, 1e-8));
395  BOOST_CHECK(pinocchio::isNormalized(model, q, 1e2));
396 
397  pinocchio::normalize(model, q);
398  BOOST_CHECK(pinocchio::isNormalized(model, q));
399  BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
400 
401  q = pinocchio::neutral(model);
402  BOOST_CHECK(pinocchio::isNormalized(model, q));
403  BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
404 
405  model.lowerPositionLimit = -1 * Eigen::VectorXd::Ones(model.nq);
406  model.upperPositionLimit = Eigen::VectorXd::Ones(model.nq);
407  q = pinocchio::randomConfiguration(model);
408  BOOST_CHECK(pinocchio::isNormalized(model, q));
409  BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
410 }
411 
412 BOOST_AUTO_TEST_CASE(integrateCoeffWiseJacobian_test)
413 {
414  Model model;
415  buildModels::humanoidRandom(model);
416 
417  Eigen::VectorXd q(Eigen::VectorXd::Ones(model.nq));
418  pinocchio::normalize(model, q);
419 
420  Eigen::MatrixXd jac(model.nq, model.nv);
421  jac.setZero();
422 
423  integrateCoeffWiseJacobian(model, q, jac);
424 
425  Eigen::MatrixXd jac_fd(model.nq, model.nv);
426  Eigen::VectorXd q_plus;
427  const double eps = 1e-8;
428 
429  Eigen::VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
430  for (int k = 0; k < model.nv; ++k)
431  {
432  v_eps[k] = eps;
433  q_plus = integrate(model, q, v_eps);
434  jac_fd.col(k) = (q_plus - q) / eps;
435 
436  v_eps[k] = 0.;
437  }
438  BOOST_CHECK(jac.isApprox(jac_fd, sqrt(eps)));
439 }
440 
441 BOOST_AUTO_TEST_SUITE_END()
quaternion.hpp
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:846
pinocchio::RMTO
@ RMTO
Definition: fwd.hpp:134
pinocchio::SETTO
@ SETTO
Definition: fwd.hpp:132
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(integration_test)
Definition: joint-configurations.cpp:17
codegen-rnea.jac
jac
Definition: codegen-rnea.py:41
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:487
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
inverse-kinematics-3d.J
J
Definition: inverse-kinematics-3d.py:28
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::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::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 initial tangent space of the integrate operation,...
Definition: joint-configuration.hpp:576
pinocchio::ARG0
@ ARG0
Definition: fwd.hpp:123
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
model-generator.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
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:933
joint-configuration.hpp
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::ARG1
@ ARG1
Definition: fwd.hpp:124
pinocchio::q0
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > & q0
Definition: joint-configuration.hpp:1137
dist
FCL_REAL & dist(short i)
pinocchio::ADDTO
@ ADDTO
Definition: fwd.hpp:133
pinocchio::buildAllJointsModel
void buildAllJointsModel(Model &model)
Definition: model-generator.hpp:31
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
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:249
pinocchio::q1
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Definition: joint-configuration.hpp:1138
dcrba.eps
int eps
Definition: dcrba.py:458
simulation-contact-dynamics.v_ref
v_ref
Definition: simulation-contact-dynamics.py:55
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:742
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
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: multibody/model.hpp:87
sample-models.hpp
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:1046
pinocchio::ModelTpl
Definition: context/generic.hpp:20
meshcat-viewer.qs
qs
Definition: meshcat-viewer.py:128
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:993
n
Vec3f n
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:887
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)

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