joint-revolute.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #include "pinocchio/math/fwd.hpp"
7 #include "pinocchio/multibody/joint/joints.hpp"
8 #include "pinocchio/algorithm/rnea.hpp"
9 #include "pinocchio/algorithm/aba.hpp"
10 #include "pinocchio/algorithm/crba.hpp"
11 #include "pinocchio/algorithm/jacobian.hpp"
12 #include "pinocchio/algorithm/compute-all-terms.hpp"
13 
14 #include <boost/test/unit_test.hpp>
15 #include <iostream>
16 
17 using namespace pinocchio;
18 
19 template<typename D>
20 void addJointAndBody(
21  Model & model,
22  const JointModelBase<D> & jmodel,
23  const Model::JointIndex parent_id,
24  const SE3 & joint_placement,
25  const std::string & joint_name,
26  const Inertia & Y)
27 {
28  Model::JointIndex idx;
29 
30  idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
31  model.appendBodyToJoint(idx, Y);
32 }
33 
34 BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)
35 
36 BOOST_AUTO_TEST_CASE(vsRX)
37 {
38  using namespace pinocchio;
39  typedef SE3::Vector3 Vector3;
40  typedef SE3::Matrix3 Matrix3;
41 
42  Vector3 axis;
43  axis << 1.0, 0.0, 0.0;
44 
45  Model modelRX, modelRevoluteUnaligned;
46 
47  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
48  SE3 pos(1);
49  pos.translation() = SE3::LinearType(1., 0., 0.);
50 
51  JointModelRevoluteUnaligned joint_model_RU(axis);
52 
53  addJointAndBody(modelRX, JointModelRX(), 0, pos, "rx", inertia);
54  addJointAndBody(modelRevoluteUnaligned, joint_model_RU, 0, pos, "revolute-unaligned", inertia);
55 
56  Data dataRX(modelRX);
57  Data dataRevoluteUnaligned(modelRevoluteUnaligned);
58 
59  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelRX.nq);
60  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelRX.nv);
61  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones(modelRX.nv);
62  Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones(modelRevoluteUnaligned.nv);
63  Eigen::VectorXd aRX = Eigen::VectorXd::Ones(modelRX.nv);
64  Eigen::VectorXd aRevoluteUnaligned(aRX);
65 
66  forwardKinematics(modelRX, dataRX, q, v);
67  forwardKinematics(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);
68 
69  computeAllTerms(modelRX, dataRX, q, v);
70  computeAllTerms(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);
71 
72  BOOST_CHECK(dataRevoluteUnaligned.oMi[1].isApprox(dataRX.oMi[1]));
73  BOOST_CHECK(dataRevoluteUnaligned.liMi[1].isApprox(dataRX.liMi[1]));
74  BOOST_CHECK(dataRevoluteUnaligned.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
75  BOOST_CHECK(dataRevoluteUnaligned.f[1].toVector().isApprox(dataRX.f[1].toVector()));
76 
77  BOOST_CHECK(dataRevoluteUnaligned.nle.isApprox(dataRX.nle));
78  BOOST_CHECK(dataRevoluteUnaligned.com[0].isApprox(dataRX.com[0]));
79 
80  // InverseDynamics == rnea
81  tauRX = rnea(modelRX, dataRX, q, v, aRX);
82  tauRevoluteUnaligned =
83  rnea(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v, aRevoluteUnaligned);
84 
85  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned));
86 
87  // ForwardDynamics == aba
88  Eigen::VectorXd aAbaRX = aba(modelRX, dataRX, q, v, tauRX, Convention::WORLD);
89  Eigen::VectorXd aAbaRevoluteUnaligned = aba(
90  modelRevoluteUnaligned, dataRevoluteUnaligned, q, v, tauRevoluteUnaligned, Convention::WORLD);
91 
92  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned));
93 
94  // CRBA
95  crba(modelRX, dataRX, q, Convention::WORLD);
96  crba(modelRevoluteUnaligned, dataRevoluteUnaligned, q, Convention::WORLD);
97 
98  BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnaligned.M));
99 
100  // Jacobian
101  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRX;
102  jacobianRX.resize(6, 1);
103  jacobianRX.setZero();
104  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRevoluteUnaligned;
105  jacobianRevoluteUnaligned.resize(6, 1);
106  jacobianRevoluteUnaligned.setZero();
107  computeJointJacobians(modelRX, dataRX, q);
108  computeJointJacobians(modelRevoluteUnaligned, dataRevoluteUnaligned, q);
109  getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianRX);
110  getJointJacobian(
111  modelRevoluteUnaligned, dataRevoluteUnaligned, 1, LOCAL, jacobianRevoluteUnaligned);
112 
113  BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned));
114 }
115 BOOST_AUTO_TEST_SUITE_END()
116 
117 BOOST_AUTO_TEST_SUITE(JointRevoluteUnboundedUnaligned)
118 
119 BOOST_AUTO_TEST_CASE(vsRUX)
120 {
121  using namespace pinocchio;
122  typedef SE3::Vector3 Vector3;
123  typedef SE3::Matrix3 Matrix3;
124 
125  Vector3 axis;
126  axis << 1.0, 0.0, 0.0;
127 
128  Model modelRUX, modelRevoluteUboundedUnaligned;
129 
130  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
131  SE3 pos(1);
132  pos.translation() = SE3::LinearType(1., 0., 0.);
133 
134  JointModelRevoluteUnboundedUnaligned joint_model_RUU(axis);
135  typedef traits<JointRevoluteUnboundedUnalignedTpl<double>>::TangentVector_t TangentVector;
136 
137  addJointAndBody(modelRUX, JointModelRUBX(), 0, pos, "rux", inertia);
138  addJointAndBody(
139  modelRevoluteUboundedUnaligned, joint_model_RUU, 0, pos, "revolute-unbounded-unaligned",
140  inertia);
141 
142  Data dataRUX(modelRUX);
143  Data dataRevoluteUnboundedUnaligned(modelRevoluteUboundedUnaligned);
144 
145  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelRUX.nq);
146  q.normalize();
147  TangentVector v = TangentVector::Ones(modelRUX.nv);
148  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones(modelRUX.nv);
149  Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones(modelRevoluteUboundedUnaligned.nv);
150  Eigen::VectorXd aRX = Eigen::VectorXd::Ones(modelRUX.nv);
151  Eigen::VectorXd aRevoluteUnaligned(aRX);
152 
153  forwardKinematics(modelRUX, dataRUX, q, v);
154  forwardKinematics(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v);
155 
156  computeAllTerms(modelRUX, dataRUX, q, v);
157  computeAllTerms(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v);
158 
159  BOOST_CHECK(dataRevoluteUnboundedUnaligned.oMi[1].isApprox(dataRUX.oMi[1]));
160  BOOST_CHECK(dataRevoluteUnboundedUnaligned.liMi[1].isApprox(dataRUX.liMi[1]));
161  BOOST_CHECK(dataRevoluteUnboundedUnaligned.Ycrb[1].matrix().isApprox(dataRUX.Ycrb[1].matrix()));
162  BOOST_CHECK(dataRevoluteUnboundedUnaligned.f[1].toVector().isApprox(dataRUX.f[1].toVector()));
163 
164  BOOST_CHECK(dataRevoluteUnboundedUnaligned.nle.isApprox(dataRUX.nle));
165  BOOST_CHECK(dataRevoluteUnboundedUnaligned.com[0].isApprox(dataRUX.com[0]));
166 
167  // InverseDynamics == rnea
168  tauRX = rnea(modelRUX, dataRUX, q, v, aRX);
169  tauRevoluteUnaligned =
170  rnea(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v, aRevoluteUnaligned);
171 
172  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned));
173 
174  // ForwardDynamics == aba
175  Eigen::VectorXd aAbaRX = aba(modelRUX, dataRUX, q, v, tauRX, Convention::WORLD);
176  Eigen::VectorXd aAbaRevoluteUnaligned = aba(
177  modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v, tauRevoluteUnaligned,
178  Convention::WORLD);
179 
180  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned));
181 
182  // CRBA
183  crba(modelRUX, dataRUX, q, Convention::WORLD);
184  crba(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, Convention::WORLD);
185 
186  BOOST_CHECK(dataRUX.M.isApprox(dataRevoluteUnboundedUnaligned.M));
187 
188  // Jacobian
189  Data::Matrix6x jacobianRUX;
190  jacobianRUX.resize(6, 1);
191  jacobianRUX.setZero();
192  Data::Matrix6x jacobianRevoluteUnboundedUnaligned;
193  jacobianRevoluteUnboundedUnaligned.resize(6, 1);
194  jacobianRevoluteUnboundedUnaligned.setZero();
195 
196  computeJointJacobians(modelRUX, dataRUX, q);
197  computeJointJacobians(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q);
198  getJointJacobian(modelRUX, dataRUX, 1, LOCAL, jacobianRUX);
199  getJointJacobian(
200  modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, 1, LOCAL,
201  jacobianRevoluteUnboundedUnaligned);
202 
203  BOOST_CHECK(jacobianRUX.isApprox(jacobianRevoluteUnboundedUnaligned));
204 }
205 
206 BOOST_AUTO_TEST_SUITE_END()
207 
208 BOOST_AUTO_TEST_SUITE(JointRevoluteUnbounded)
209 
210 BOOST_AUTO_TEST_CASE(spatial)
211 {
212  SE3 M(SE3::Random());
213  Motion v(Motion::Random());
214 
215  MotionRevoluteTpl<double, 0, 0> mp_x(2.);
216  Motion mp_dense_x(mp_x);
217 
218  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
219  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
220 
221  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
222 
223  MotionRevoluteTpl<double, 0, 1> mp_y(2.);
224  Motion mp_dense_y(mp_y);
225 
226  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
227  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
228 
229  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
230 
231  MotionRevoluteTpl<double, 0, 2> mp_z(2.);
232  Motion mp_dense_z(mp_z);
233 
234  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
235  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
236 
237  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
238 }
239 
240 BOOST_AUTO_TEST_CASE(vsRX)
241 {
242  typedef SE3::Vector3 Vector3;
243  typedef SE3::Matrix3 Matrix3;
244 
245  Model modelRX, modelRevoluteUnbounded;
246 
247  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
248  SE3 pos(1);
249  pos.translation() = SE3::LinearType(1., 0., 0.);
250 
251  JointModelRUBX joint_model_RUX;
252  addJointAndBody(modelRX, JointModelRX(), 0, SE3::Identity(), "rx", inertia);
253  addJointAndBody(
254  modelRevoluteUnbounded, joint_model_RUX, 0, SE3::Identity(), "revolute unbounded x", inertia);
255 
256  Data dataRX(modelRX);
257  Data dataRevoluteUnbounded(modelRevoluteUnbounded);
258 
259  Eigen::VectorXd q_rx = Eigen::VectorXd::Ones(modelRX.nq);
260  Eigen::VectorXd q_rubx = Eigen::VectorXd::Ones(modelRevoluteUnbounded.nq);
261  double ca, sa;
262  double alpha = q_rx(0);
263  SINCOS(alpha, &sa, &ca);
264  q_rubx(0) = ca;
265  q_rubx(1) = sa;
266  Eigen::VectorXd v_rx = Eigen::VectorXd::Ones(modelRX.nv);
267  Eigen::VectorXd v_rubx = v_rx;
268  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones(modelRX.nv);
269  Eigen::VectorXd tauRevoluteUnbounded = Eigen::VectorXd::Ones(modelRevoluteUnbounded.nv);
270  Eigen::VectorXd aRX = Eigen::VectorXd::Ones(modelRX.nv);
271  Eigen::VectorXd aRevoluteUnbounded = aRX;
272 
273  forwardKinematics(modelRX, dataRX, q_rx, v_rx);
274  forwardKinematics(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);
275 
276  computeAllTerms(modelRX, dataRX, q_rx, v_rx);
277  computeAllTerms(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);
278 
279  BOOST_CHECK(dataRevoluteUnbounded.oMi[1].isApprox(dataRX.oMi[1]));
280  BOOST_CHECK(dataRevoluteUnbounded.liMi[1].isApprox(dataRX.liMi[1]));
281  BOOST_CHECK(dataRevoluteUnbounded.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
282  BOOST_CHECK(dataRevoluteUnbounded.f[1].toVector().isApprox(dataRX.f[1].toVector()));
283 
284  BOOST_CHECK(dataRevoluteUnbounded.nle.isApprox(dataRX.nle));
285  BOOST_CHECK(dataRevoluteUnbounded.com[0].isApprox(dataRX.com[0]));
286 
287  // InverseDynamics == rnea
288  tauRX = rnea(modelRX, dataRX, q_rx, v_rx, aRX);
289  tauRevoluteUnbounded =
290  rnea(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx, aRevoluteUnbounded);
291 
292  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnbounded));
293 
294  // ForwardDynamics == aba
295  Eigen::VectorXd aAbaRX = aba(modelRX, dataRX, q_rx, v_rx, tauRX, Convention::WORLD);
296  Eigen::VectorXd aAbaRevoluteUnbounded = aba(
297  modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx, tauRevoluteUnbounded,
298  Convention::WORLD);
299 
300  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnbounded));
301 
302  // crba
303  crba(modelRX, dataRX, q_rx, Convention::WORLD);
304  crba(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, Convention::WORLD);
305 
306  BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnbounded.M));
307 
308  // Jacobian
309  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;
310  jacobianPX.resize(6, 1);
311  jacobianPX.setZero();
312  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;
313  jacobianPrismaticUnaligned.resize(6, 1);
314  jacobianPrismaticUnaligned.setZero();
315  computeJointJacobians(modelRX, dataRX, q_rx);
316  computeJointJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
317  getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianPX);
318  getJointJacobian(
319  modelRevoluteUnbounded, dataRevoluteUnbounded, 1, LOCAL, jacobianPrismaticUnaligned);
320 
321  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
322 }
323 BOOST_AUTO_TEST_SUITE_END()
324 
325 BOOST_AUTO_TEST_SUITE(JointRevolute)
326 
327 BOOST_AUTO_TEST_CASE(spatial)
328 {
329  typedef TransformRevoluteTpl<double, 0, 0> TransformX;
330  typedef TransformRevoluteTpl<double, 0, 1> TransformY;
331  typedef TransformRevoluteTpl<double, 0, 2> TransformZ;
332 
333  typedef SE3::Vector3 Vector3;
334 
335  const double alpha = 0.2;
336  double sin_alpha, cos_alpha;
337  SINCOS(alpha, &sin_alpha, &cos_alpha);
338  SE3 Mplain, Mrand(SE3::Random());
339 
340  TransformX Mx(sin_alpha, cos_alpha);
341  Mplain = Mx;
342  BOOST_CHECK(Mplain.translation().isZero());
343  BOOST_CHECK(
344  Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitX()).toRotationMatrix()));
345  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mx));
346 
347  TransformY My(sin_alpha, cos_alpha);
348  Mplain = My;
349  BOOST_CHECK(Mplain.translation().isZero());
350  BOOST_CHECK(
351  Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitY()).toRotationMatrix()));
352  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * My));
353 
354  TransformZ Mz(sin_alpha, cos_alpha);
355  Mplain = Mz;
356  BOOST_CHECK(Mplain.translation().isZero());
357  BOOST_CHECK(
358  Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitZ()).toRotationMatrix()));
359  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mz));
360 
361  SE3 M(SE3::Random());
362  Motion v(Motion::Random());
363 
364  MotionRevoluteTpl<double, 0, 0> mp_x(2.);
365  Motion mp_dense_x(mp_x);
366 
367  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
368  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
369 
370  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
371 
372  MotionRevoluteTpl<double, 0, 1> mp_y(2.);
373  Motion mp_dense_y(mp_y);
374 
375  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
376  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
377 
378  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
379 
380  MotionRevoluteTpl<double, 0, 2> mp_z(2.);
381  Motion mp_dense_z(mp_z);
382 
383  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
384  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
385 
386  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
387 }
388 
389 BOOST_AUTO_TEST_SUITE_END()
390 
391 BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)
392 
393 BOOST_AUTO_TEST_CASE(spatial)
394 {
395  SE3 M(SE3::Random());
396  Motion v(Motion::Random());
397 
398  MotionRevoluteUnaligned mp(MotionRevoluteUnaligned::Vector3(1., 2., 3.), 6.);
399  Motion mp_dense(mp);
400 
401  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
402  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
403 
404  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
405 }
406 
407 BOOST_AUTO_TEST_SUITE_END()
pinocchio::InertiaTpl
Definition: spatial/fwd.hpp:58
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:199
pinocchio::addJointAndBody
void addJointAndBody(Model &model, const JointModelBase< D > &jmodel, const Model::JointIndex parent_id, const SE3 &joint_placement, const std::string &name, const Inertia &Y)
Definition: model-generator.hpp:11
pinocchio::JointModelRevoluteUnboundedUnalignedTpl
Definition: multibody/joint/fwd.hpp:46
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
pinocchio::DataTpl
Definition: context/generic.hpp:25
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::MotionRevoluteTpl
Definition: joint-revolute.hpp:20
compute-all-terms.hpp
pinocchio::JointModelBase
Definition: joint-model-base.hpp:75
pinocchio::SE3Tpl< context::Scalar, context::Options >
pinocchio::Convention::WORLD
@ WORLD
pinocchio::JointModelRX
JointModelRevoluteTpl< context::Scalar, context::Options, 0 > JointModelRX
Definition: joint-revolute.hpp:855
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::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
rnea.hpp
aba.hpp
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:56
pinocchio::SE3Tpl::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:52
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:68
append-urdf-model-with-another-model.parent_id
int parent_id
Definition: append-urdf-model-with-another-model.py:28
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:159
pinocchio::MotionRevoluteUnalignedTpl
Definition: joint-revolute-unaligned.hpp:21
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
fwd.hpp
pinocchio::TransformRevoluteTpl
Definition: joint-revolute.hpp:64
pos
pos
M
M
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
axis
axis
pinocchio::MotionTpl::Random
static MotionTpl Random()
Definition: motion-tpl.hpp:140
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
pinocchio::JointModelRUBX
JointModelRevoluteUnboundedTpl< context::Scalar, context::Options, 0 > JointModelRUBX
Definition: joint-revolute-unbounded.hpp:246
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1118
pinocchio::JointModelRevoluteUnalignedTpl
Definition: multibody/joint/fwd.hpp:38
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(vsRX)
Definition: joint-revolute.cpp:36
append-urdf-model-with-another-model.joint_name
string joint_name
Definition: append-urdf-model-with-another-model.py:33
pinocchio::SE3Tpl< context::Scalar, context::Options >::Random
static SE3Tpl Random()
Definition: spatial/se3-tpl.hpp:154
append-urdf-model-with-another-model.joint_placement
joint_placement
Definition: append-urdf-model-with-another-model.py:29
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::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: multibody/model.hpp:102
pinocchio::SE3Tpl< context::Scalar, context::Options >::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
Y
Y
joints.hpp
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
pinocchio::MotionTpl
Definition: context/casadi.hpp:27
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
crba.hpp
pinocchio::computeAllTerms
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...
pinocchio::JointModelRevoluteUnboundedTpl
Definition: multibody/joint/fwd.hpp:55
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: multibody/model.hpp:99
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::DataTpl::nle
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis,...
Definition: multibody/data.hpp:179
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:48