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(Model & model,
21  const JointModelBase<D> & jmodel,
22  const Model::JointIndex parent_id,
23  const SE3 & joint_placement,
24  const std::string & joint_name,
25  const Inertia & Y)
26 {
27  Model::JointIndex idx;
28 
29  idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
30  model.appendBodyToJoint(idx,Y);
31 }
32 
33 BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)
34 
35 BOOST_AUTO_TEST_CASE(vsRX)
36 {
37  using namespace pinocchio;
38  typedef SE3::Vector3 Vector3;
39  typedef SE3::Matrix3 Matrix3;
40 
41  Vector3 axis;
42  axis << 1.0, 0.0, 0.0;
43 
44  Model modelRX, modelRevoluteUnaligned;
45 
46  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
47  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
48 
49  JointModelRevoluteUnaligned joint_model_RU(axis);
50 
51  addJointAndBody(modelRX,JointModelRX(),0,pos,"rx",inertia);
52  addJointAndBody(modelRevoluteUnaligned,joint_model_RU,0,pos,"revolute-unaligned",inertia);
53 
54  Data dataRX(modelRX);
55  Data dataRevoluteUnaligned(modelRevoluteUnaligned);
56 
57  Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRX.nq);
58  Eigen::VectorXd v = Eigen::VectorXd::Ones (modelRX.nv);
59  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv);
60  Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUnaligned.nv);
61  Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv);
62  Eigen::VectorXd aRevoluteUnaligned(aRX);
63 
64  forwardKinematics(modelRX, dataRX, q, v);
65  forwardKinematics(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);
66 
67  computeAllTerms(modelRX, dataRX, q, v);
68  computeAllTerms(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);
69 
70  BOOST_CHECK(dataRevoluteUnaligned.oMi[1].isApprox(dataRX.oMi[1]));
71  BOOST_CHECK(dataRevoluteUnaligned.liMi[1].isApprox(dataRX.liMi[1]));
72  BOOST_CHECK(dataRevoluteUnaligned.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
73  BOOST_CHECK(dataRevoluteUnaligned.f[1].toVector().isApprox(dataRX.f[1].toVector()));
74 
75  BOOST_CHECK(dataRevoluteUnaligned.nle.isApprox(dataRX.nle));
76  BOOST_CHECK(dataRevoluteUnaligned.com[0].isApprox(dataRX.com[0]));
77 
78  // InverseDynamics == rnea
79  tauRX = rnea(modelRX, dataRX, q, v, aRX);
80  tauRevoluteUnaligned = rnea(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v, aRevoluteUnaligned);
81 
82  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned));
83 
84  // ForwardDynamics == aba
85  Eigen::VectorXd aAbaRX = aba(modelRX,dataRX, q, v, tauRX);
86  Eigen::VectorXd aAbaRevoluteUnaligned = aba(modelRevoluteUnaligned,dataRevoluteUnaligned, q, v, tauRevoluteUnaligned);
87 
88  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned));
89 
90  // CRBA
91  crba(modelRX, dataRX,q);
92  crba(modelRevoluteUnaligned, dataRevoluteUnaligned, q);
93 
94  BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnaligned.M));
95 
96  // Jacobian
97  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRX;jacobianRX.resize(6,1); jacobianRX.setZero();
98  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianRevoluteUnaligned;jacobianRevoluteUnaligned.resize(6,1);jacobianRevoluteUnaligned.setZero();
99  computeJointJacobians(modelRX, dataRX, q);
100  computeJointJacobians(modelRevoluteUnaligned, dataRevoluteUnaligned, q);
101  getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianRX);
102  getJointJacobian(modelRevoluteUnaligned, dataRevoluteUnaligned, 1, LOCAL, jacobianRevoluteUnaligned);
103 
104 
105  BOOST_CHECK(jacobianRX.isApprox(jacobianRevoluteUnaligned));
106 }
107 BOOST_AUTO_TEST_SUITE_END()
108 
109 BOOST_AUTO_TEST_SUITE(JointRevoluteUnboundedUnaligned)
110 
111  BOOST_AUTO_TEST_CASE(vsRUX)
112  {
113  using namespace pinocchio;
114  typedef SE3::Vector3 Vector3;
115  typedef SE3::Matrix3 Matrix3;
116 
117  Vector3 axis;
118  axis << 1.0, 0.0, 0.0;
119 
120  Model modelRUX, modelRevoluteUboundedUnaligned;
121 
122  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
123  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
124 
125  JointModelRevoluteUnboundedUnaligned joint_model_RUU(axis);
126  typedef traits< JointRevoluteUnboundedUnalignedTpl<double> >::TangentVector_t TangentVector;
127 
128  addJointAndBody(modelRUX,JointModelRUBX(),0,pos,"rux",inertia);
129  addJointAndBody(modelRevoluteUboundedUnaligned,joint_model_RUU,0,pos,"revolute-unbounded-unaligned",inertia);
130 
131  Data dataRUX(modelRUX);
132  Data dataRevoluteUnboundedUnaligned(modelRevoluteUboundedUnaligned);
133 
134  Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRUX.nq);
135  q.normalize();
136  TangentVector v = TangentVector::Ones (modelRUX.nv);
137  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRUX.nv);
138  Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUboundedUnaligned.nv);
139  Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRUX.nv);
140  Eigen::VectorXd aRevoluteUnaligned(aRX);
141 
142  forwardKinematics(modelRUX, dataRUX, q, v);
143  forwardKinematics(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v);
144 
145  computeAllTerms(modelRUX, dataRUX, q, v);
146  computeAllTerms(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v);
147 
148  BOOST_CHECK(dataRevoluteUnboundedUnaligned.oMi[1].isApprox(dataRUX.oMi[1]));
149  BOOST_CHECK(dataRevoluteUnboundedUnaligned.liMi[1].isApprox(dataRUX.liMi[1]));
150  BOOST_CHECK(dataRevoluteUnboundedUnaligned.Ycrb[1].matrix().isApprox(dataRUX.Ycrb[1].matrix()));
151  BOOST_CHECK(dataRevoluteUnboundedUnaligned.f[1].toVector().isApprox(dataRUX.f[1].toVector()));
152 
153  BOOST_CHECK(dataRevoluteUnboundedUnaligned.nle.isApprox(dataRUX.nle));
154  BOOST_CHECK(dataRevoluteUnboundedUnaligned.com[0].isApprox(dataRUX.com[0]));
155 
156  // InverseDynamics == rnea
157  tauRX = rnea(modelRUX, dataRUX, q, v, aRX);
158  tauRevoluteUnaligned = rnea(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q, v, aRevoluteUnaligned);
159 
160  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnaligned));
161 
162  // ForwardDynamics == aba
163  Eigen::VectorXd aAbaRX = aba(modelRUX, dataRUX, q, v, tauRX);
164  Eigen::VectorXd aAbaRevoluteUnaligned = aba(modelRevoluteUboundedUnaligned,dataRevoluteUnboundedUnaligned, q, v, tauRevoluteUnaligned);
165 
166  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnaligned));
167 
168  // CRBA
169  crba(modelRUX, dataRUX,q);
170  crba(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q);
171 
172  BOOST_CHECK(dataRUX.M.isApprox(dataRevoluteUnboundedUnaligned.M));
173 
174  // Jacobian
175  Data::Matrix6x jacobianRUX;jacobianRUX.resize(6,1); jacobianRUX.setZero();
176  Data::Matrix6x jacobianRevoluteUnboundedUnaligned;
177  jacobianRevoluteUnboundedUnaligned.resize(6,1); jacobianRevoluteUnboundedUnaligned.setZero();
178 
179  computeJointJacobians(modelRUX, dataRUX, q);
180  computeJointJacobians(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, q);
181  getJointJacobian(modelRUX, dataRUX, 1, LOCAL, jacobianRUX);
182  getJointJacobian(modelRevoluteUboundedUnaligned, dataRevoluteUnboundedUnaligned, 1, LOCAL, jacobianRevoluteUnboundedUnaligned);
183 
184  BOOST_CHECK(jacobianRUX.isApprox(jacobianRevoluteUnboundedUnaligned));
185  }
186 
187 BOOST_AUTO_TEST_SUITE_END()
188 
189 BOOST_AUTO_TEST_SUITE(JointRevoluteUnbounded)
190 
191  BOOST_AUTO_TEST_CASE(spatial)
192  {
193  SE3 M(SE3::Random());
194  Motion v(Motion::Random());
195 
196  MotionRevoluteTpl<double,0,0> mp_x(2.);
197  Motion mp_dense_x(mp_x);
198 
199  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
200  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
201 
202  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
203 
204  MotionRevoluteTpl<double,0,1> mp_y(2.);
205  Motion mp_dense_y(mp_y);
206 
207  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
208  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
209 
210  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
211 
212  MotionRevoluteTpl<double,0,2> mp_z(2.);
213  Motion mp_dense_z(mp_z);
214 
215  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
216  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
217 
218  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
219  }
220 
221 BOOST_AUTO_TEST_CASE(vsRX)
222 {
223  typedef SE3::Vector3 Vector3;
224  typedef SE3::Matrix3 Matrix3;
225 
226  Model modelRX, modelRevoluteUnbounded;
227 
228  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
229  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
230 
231  JointModelRUBX joint_model_RUX;
232  addJointAndBody(modelRX,JointModelRX(),0,SE3::Identity(),"rx",inertia);
233  addJointAndBody(modelRevoluteUnbounded,joint_model_RUX,0,SE3::Identity(),"revolute unbounded x",inertia);
234 
235  Data dataRX(modelRX);
236  Data dataRevoluteUnbounded(modelRevoluteUnbounded);
237 
238  Eigen::VectorXd q_rx = Eigen::VectorXd::Ones(modelRX.nq);
239  Eigen::VectorXd q_rubx = Eigen::VectorXd::Ones(modelRevoluteUnbounded.nq);
240  double ca, sa; double alpha = q_rx(0); SINCOS(alpha, &sa, &ca);
241  q_rubx(0) = ca;
242  q_rubx(1) = sa;
243  Eigen::VectorXd v_rx = Eigen::VectorXd::Ones(modelRX.nv);
244  Eigen::VectorXd v_rubx = v_rx;
245  Eigen::VectorXd tauRX = Eigen::VectorXd::Ones(modelRX.nv);
246  Eigen::VectorXd tauRevoluteUnbounded = Eigen::VectorXd::Ones(modelRevoluteUnbounded.nv);
247  Eigen::VectorXd aRX = Eigen::VectorXd::Ones(modelRX.nv);
248  Eigen::VectorXd aRevoluteUnbounded = aRX;
249 
250  forwardKinematics(modelRX, dataRX, q_rx, v_rx);
251  forwardKinematics(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);
252 
253  computeAllTerms(modelRX, dataRX, q_rx, v_rx);
254  computeAllTerms(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);
255 
256  BOOST_CHECK(dataRevoluteUnbounded.oMi[1].isApprox(dataRX.oMi[1]));
257  BOOST_CHECK(dataRevoluteUnbounded.liMi[1].isApprox(dataRX.liMi[1]));
258  BOOST_CHECK(dataRevoluteUnbounded.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
259  BOOST_CHECK(dataRevoluteUnbounded.f[1].toVector().isApprox(dataRX.f[1].toVector()));
260 
261  BOOST_CHECK(dataRevoluteUnbounded.nle.isApprox(dataRX.nle));
262  BOOST_CHECK(dataRevoluteUnbounded.com[0].isApprox(dataRX.com[0]));
263 
264  // InverseDynamics == rnea
265  tauRX = rnea(modelRX, dataRX, q_rx, v_rx, aRX);
266  tauRevoluteUnbounded = rnea(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx, aRevoluteUnbounded);
267 
268  BOOST_CHECK(tauRX.isApprox(tauRevoluteUnbounded));
269 
270  // ForwardDynamics == aba
271  Eigen::VectorXd aAbaRX= aba(modelRX,dataRX, q_rx, v_rx, tauRX);
272  Eigen::VectorXd aAbaRevoluteUnbounded = aba(modelRevoluteUnbounded,dataRevoluteUnbounded, q_rubx, v_rubx, tauRevoluteUnbounded);
273 
274  BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnbounded));
275 
276  // crba
277  crba(modelRX, dataRX,q_rx);
278  crba(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
279 
280  BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnbounded.M));
281 
282  // Jacobian
283  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
284  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
285  computeJointJacobians(modelRX, dataRX, q_rx);
286  computeJointJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
287  getJointJacobian(modelRX, dataRX, 1, LOCAL, jacobianPX);
288  getJointJacobian(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, LOCAL, jacobianPrismaticUnaligned);
289 
290  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
291 
292 }
293 BOOST_AUTO_TEST_SUITE_END()
294 
295 BOOST_AUTO_TEST_SUITE(JointRevolute)
296 
297 BOOST_AUTO_TEST_CASE(spatial)
298 {
299  typedef TransformRevoluteTpl<double,0,0> TransformX;
300  typedef TransformRevoluteTpl<double,0,1> TransformY;
301  typedef TransformRevoluteTpl<double,0,2> TransformZ;
302 
303  typedef SE3::Vector3 Vector3;
304 
305  const double alpha = 0.2;
306  double sin_alpha, cos_alpha; SINCOS(alpha,&sin_alpha,&cos_alpha);
307  SE3 Mplain, Mrand(SE3::Random());
308 
309  TransformX Mx(sin_alpha,cos_alpha);
310  Mplain = Mx;
311  BOOST_CHECK(Mplain.translation().isZero());
312  BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitX()).toRotationMatrix()));
313  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx));
314 
315  TransformY My(sin_alpha,cos_alpha);
316  Mplain = My;
317  BOOST_CHECK(Mplain.translation().isZero());
318  BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitY()).toRotationMatrix()));
319  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My));
320 
321  TransformZ Mz(sin_alpha,cos_alpha);
322  Mplain = Mz;
323  BOOST_CHECK(Mplain.translation().isZero());
324  BOOST_CHECK(Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha,Vector3::UnitZ()).toRotationMatrix()));
325  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz));
326 
327  SE3 M(SE3::Random());
328  Motion v(Motion::Random());
329 
330  MotionRevoluteTpl<double,0,0> mp_x(2.);
331  Motion mp_dense_x(mp_x);
332 
333  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
334  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
335 
336  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
337 
338  MotionRevoluteTpl<double,0,1> mp_y(2.);
339  Motion mp_dense_y(mp_y);
340 
341  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
342  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
343 
344  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
345 
346  MotionRevoluteTpl<double,0,2> mp_z(2.);
347  Motion mp_dense_z(mp_z);
348 
349  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
350  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
351 
352  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
353 }
354 
355 BOOST_AUTO_TEST_SUITE_END()
356 
357 BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)
358 
359 BOOST_AUTO_TEST_CASE(spatial)
360 {
361  SE3 M(SE3::Random());
362  Motion v(Motion::Random());
363 
364  MotionRevoluteUnaligned mp(MotionRevoluteUnaligned::Vector3(1.,2.,3.),6.);
365  Motion mp_dense(mp);
366 
367  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
368  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
369 
370  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
371 }
372 
373 BOOST_AUTO_TEST_SUITE_END()
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:746
pinocchio::aba
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::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...
axis
axis
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::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::SE3Base::actInv
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:97
pos
pos
pinocchio::rnea
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Definition: parallel/rnea.hpp:32
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or i...
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:746
pinocchio::MotionBase::cross
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
Definition: motion-base.hpp:87
pinocchio::DataTpl::nle
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis...
Definition: src/multibody/data.hpp:135
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.
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
pinocchio::ModelTpl::addJoint
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: src/multibody/data.hpp:153
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: src/multibody/model.hpp:49
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(vsRX)
Definition: joint-revolute.cpp:35
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:26
pinocchio::SE3Tpl< double, 0 >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:53
pinocchio::SE3Base::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
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::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
pinocchio::SE3Base::act
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90
pinocchio::toRotationMatrix
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition: rotation.hpp:24
M
M
pinocchio::SE3Tpl< double, 0 >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:54
pinocchio::MotionTpl::Random
static MotionTpl Random()
Definition: motion-tpl.hpp:92
pinocchio::JointModelRUBX
JointModelRevoluteUnboundedTpl< double, 0, 0 > JointModelRUBX
Definition: joint-revolute-unbounded.hpp:204
pinocchio::ModelTpl::appendBodyToJoint
void appendBodyToJoint(const JointIndex joint_index, const Inertia &Y, const SE3 &body_placement=SE3::Identity())
Append a body to a given joint of the kinematic tree.
pinocchio::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: src/multibody/data.hpp:73
pinocchio::JointModelRX
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
Definition: joint-revolute.hpp:756
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::SE3Base::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:38

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