unittest/regressor.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2024 CNRS INRIA
3 //
4 
5 #include "pinocchio/spatial/fwd.hpp"
6 #include "pinocchio/spatial/explog.hpp"
7 #include "pinocchio/algorithm/regressor.hpp"
8 #include "pinocchio/algorithm/rnea.hpp"
9 #include "pinocchio/algorithm/frames.hpp"
10 #include "pinocchio/algorithm/joint-configuration.hpp"
11 #include "pinocchio/algorithm/center-of-mass.hpp"
12 #include "pinocchio/multibody/sample-models.hpp"
13 #include "pinocchio/algorithm/compute-all-terms.hpp"
14 
15 #include <iostream>
16 
17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
19 
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21 
22 BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint)
23 {
24  using namespace Eigen;
25  using namespace pinocchio;
26 
27  pinocchio::Model model;
28  buildModels::humanoidRandom(model);
29 
30  pinocchio::Data data(model);
31  pinocchio::Data data_ref(model);
32 
33  model.lowerPositionLimit.head<7>().fill(-1.);
34  model.upperPositionLimit.head<7>().fill(1.);
35 
36  // const std::string joint_name = "larm5_joint";
37  // const JointIndex joint_id = model.getJointId(joint_name);
38 
39  const VectorXd q = randomConfiguration(model);
40 
41  forwardKinematics(model, data, q);
42 
43  const double eps = 1e-8;
44  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
45  {
46  Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
47  Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
48  Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
49 
50  Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
51  Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
52  Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
53 
54  computeJointKinematicRegressor(model, data, joint_id, LOCAL, kinematic_regressor_L);
55  computeJointKinematicRegressor(
56  model, data, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA);
57  computeJointKinematicRegressor(model, data, joint_id, WORLD, kinematic_regressor_W);
58 
59  Model model_plus = model;
60  Data data_plus(model_plus);
61  const SE3 & oMi = data.oMi[joint_id];
62  const SE3 Mi_LWA = SE3(oMi.rotation(), SE3::Vector3::Zero());
63  const SE3 & oMi_plus = data_plus.oMi[joint_id];
64  for (int i = 1; i < model.njoints; ++i)
65  {
66  Motion::Vector6 v = Motion::Vector6::Zero();
67  const SE3 & M_placement = model.jointPlacements[(JointIndex)i];
68  SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i];
69  for (Eigen::DenseIndex k = 0; k < 6; ++k)
70  {
71  v[k] = eps;
72  M_placement_plus = M_placement * exp6(Motion(v));
73 
74  forwardKinematics(model_plus, data_plus, q);
75 
76  const Motion diff_L = log6(oMi.actInv(oMi_plus));
77  kinematic_regressor_L_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_L.toVector() / eps;
78  const Motion diff_LWA = Mi_LWA.act(diff_L);
79  kinematic_regressor_LWA_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_LWA.toVector() / eps;
80  const Motion diff_W = oMi.act(diff_L);
81  kinematic_regressor_W_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_W.toVector() / eps;
82  v[k] = 0.;
83  }
84 
85  M_placement_plus = M_placement;
86  }
87 
88  BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd, sqrt(eps)));
89  BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd, sqrt(eps)));
90  BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd, sqrt(eps)));
91  }
92 }
93 
94 BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint_placement)
95 {
96  using namespace Eigen;
97  using namespace pinocchio;
98 
99  pinocchio::Model model;
100  buildModels::humanoidRandom(model);
101 
102  pinocchio::Data data(model);
103  pinocchio::Data data_ref(model);
104 
105  model.lowerPositionLimit.head<7>().fill(-1.);
106  model.upperPositionLimit.head<7>().fill(1.);
107 
108  const VectorXd q = randomConfiguration(model);
109 
110  forwardKinematics(model, data, q);
111  forwardKinematics(model, data_ref, q);
112 
113  for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
114  {
115  Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
116  Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
117  Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
118 
119  computeJointKinematicRegressor(
120  model, data, joint_id, LOCAL, SE3::Identity(), kinematic_regressor_L);
121  computeJointKinematicRegressor(
122  model, data, joint_id, LOCAL_WORLD_ALIGNED, SE3::Identity(), kinematic_regressor_LWA);
123  computeJointKinematicRegressor(
124  model, data, joint_id, WORLD, SE3::Identity(), kinematic_regressor_W);
125 
126  Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
127  Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
128  Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
129 
130  computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL, kinematic_regressor_L_ref);
131  computeJointKinematicRegressor(
132  model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA_ref);
133  computeJointKinematicRegressor(model, data_ref, joint_id, WORLD, kinematic_regressor_W_ref);
134 
135  BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref));
136  BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref));
137  BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref));
138  }
139 }
140 
141 BOOST_AUTO_TEST_CASE(test_kinematic_regressor_frame)
142 {
143  using namespace Eigen;
144  using namespace pinocchio;
145 
146  pinocchio::Model model;
147  buildModels::humanoidRandom(model);
148 
149  model.lowerPositionLimit.head<7>().fill(-1.);
150  model.upperPositionLimit.head<7>().fill(1.);
151 
152  const std::string joint_name = "larm5_joint";
153  const JointIndex joint_id = model.getJointId(joint_name);
154  model.addBodyFrame("test_body", joint_id, SE3::Random(), -1);
155 
156  pinocchio::Data data(model);
157  pinocchio::Data data_ref(model);
158 
159  const VectorXd q = randomConfiguration(model);
160 
161  forwardKinematics(model, data, q);
162  updateFramePlacements(model, data);
163  forwardKinematics(model, data_ref, q);
164 
165  const double eps = 1e-8;
166  for (FrameIndex frame_id = 1; frame_id < (FrameIndex)model.nframes; ++frame_id)
167  {
168  const Frame & frame = model.frames[frame_id];
169 
170  Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
171  Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
172  Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
173 
174  computeFrameKinematicRegressor(model, data, frame_id, LOCAL, kinematic_regressor_L);
175  computeFrameKinematicRegressor(
176  model, data, frame_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA);
177  computeFrameKinematicRegressor(model, data, frame_id, WORLD, kinematic_regressor_W);
178 
179  Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
180  Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
181  Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
182 
183  computeJointKinematicRegressor(
184  model, data_ref, frame.parentJoint, LOCAL, frame.placement, kinematic_regressor_L_ref);
185  computeJointKinematicRegressor(
186  model, data_ref, frame.parentJoint, LOCAL_WORLD_ALIGNED, frame.placement,
187  kinematic_regressor_LWA_ref);
188  computeJointKinematicRegressor(
189  model, data_ref, frame.parentJoint, WORLD, frame.placement, kinematic_regressor_W_ref);
190 
191  BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref));
192  BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref));
193  BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref));
194 
195  Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
196  Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
197  Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1)));
198 
199  Model model_plus = model;
200  Data data_plus(model_plus);
201  const SE3 & oMf = data.oMf[frame_id];
202  const SE3 Mf_LWA = SE3(oMf.rotation(), SE3::Vector3::Zero());
203  const SE3 & oMf_plus = data_plus.oMf[frame_id];
204  for (int i = 1; i < model.njoints; ++i)
205  {
206  Motion::Vector6 v = Motion::Vector6::Zero();
207  const SE3 & M_placement = model.jointPlacements[(JointIndex)i];
208  SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i];
209  for (Eigen::DenseIndex k = 0; k < 6; ++k)
210  {
211  v[k] = eps;
212  M_placement_plus = M_placement * exp6(Motion(v));
213 
214  forwardKinematics(model_plus, data_plus, q);
215  updateFramePlacements(model_plus, data_plus);
216 
217  const Motion diff_L = log6(oMf.actInv(oMf_plus));
218  kinematic_regressor_L_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_L.toVector() / eps;
219  const Motion diff_LWA = Mf_LWA.act(diff_L);
220  kinematic_regressor_LWA_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_LWA.toVector() / eps;
221  const Motion diff_W = oMf.act(diff_L);
222  kinematic_regressor_W_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_W.toVector() / eps;
223  v[k] = 0.;
224  }
225 
226  M_placement_plus = M_placement;
227  }
228 
229  BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd, sqrt(eps)));
230  BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd, sqrt(eps)));
231  BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd, sqrt(eps)));
232  }
233 }
234 
235 BOOST_AUTO_TEST_CASE(test_static_regressor)
236 {
237  using namespace Eigen;
238  using namespace pinocchio;
239 
240  pinocchio::Model model;
241  buildModels::humanoidRandom(model);
242 
243  pinocchio::Data data(model);
244  pinocchio::Data data_ref(model);
245 
246  model.lowerPositionLimit.head<7>().fill(-1.);
247  model.upperPositionLimit.head<7>().fill(1.);
248 
249  VectorXd q = randomConfiguration(model);
250  computeStaticRegressor(model, data, q);
251 
252  VectorXd phi(4 * (model.njoints - 1));
253  for (int k = 1; k < model.njoints; ++k)
254  {
255  const Inertia & Y = model.inertias[(size_t)k];
256  phi.segment<4>(4 * (k - 1)) << Y.mass(), Y.mass() * Y.lever();
257  }
258 
259  Vector3d com = centerOfMass(model, data_ref, q);
260  Vector3d static_com_ref;
261  static_com_ref << com;
262 
263  Vector3d static_com = data.staticRegressor * phi;
264 
265  BOOST_CHECK(static_com.isApprox(static_com_ref));
266 }
267 
268 BOOST_AUTO_TEST_CASE(test_body_regressor)
269 {
270  using namespace Eigen;
271  using namespace pinocchio;
272 
273  Inertia I(Inertia::Random());
274  Motion v(Motion::Random());
275  Motion a(Motion::Random());
276 
277  Force f = I * a + I.vxiv(v);
278 
279  Inertia::Vector6 f_regressor = bodyRegressor(v, a) * I.toDynamicParameters();
280 
281  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
282 }
283 
284 BOOST_AUTO_TEST_CASE(test_joint_body_regressor)
285 {
286  using namespace Eigen;
287  using namespace pinocchio;
288 
289  pinocchio::Model model;
290  buildModels::manipulator(model);
291  pinocchio::Data data(model);
292 
293  JointIndex JOINT_ID = JointIndex(model.njoints) - 1;
294 
295  VectorXd q = randomConfiguration(model);
296  VectorXd v = Eigen::VectorXd::Random(model.nv);
297  VectorXd a = Eigen::VectorXd::Random(model.nv);
298 
299  rnea(model, data, q, v, a);
300 
301  Force f = data.f[JOINT_ID];
302 
303  Inertia::Vector6 f_regressor =
304  jointBodyRegressor(model, data, JOINT_ID) * model.inertias[JOINT_ID].toDynamicParameters();
305 
306  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
307 }
308 
309 BOOST_AUTO_TEST_CASE(test_frame_body_regressor)
310 {
311  using namespace Eigen;
312  using namespace pinocchio;
313 
314  pinocchio::Model model;
315  buildModels::manipulator(model);
316 
317  JointIndex JOINT_ID = JointIndex(model.njoints) - 1;
318 
319  const SE3 & framePlacement = SE3::Random();
320  FrameIndex FRAME_ID = model.addBodyFrame("test_body", JOINT_ID, framePlacement, -1);
321 
322  pinocchio::Data data(model);
323 
324  VectorXd q = randomConfiguration(model);
325  VectorXd v = Eigen::VectorXd::Random(model.nv);
326  VectorXd a = Eigen::VectorXd::Random(model.nv);
327 
328  rnea(model, data, q, v, a);
329 
330  Force f = framePlacement.actInv(data.f[JOINT_ID]);
331  Inertia I = framePlacement.actInv(model.inertias[JOINT_ID]);
332 
333  Inertia::Vector6 f_regressor =
334  frameBodyRegressor(model, data, FRAME_ID) * I.toDynamicParameters();
335 
336  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
337 }
338 
339 BOOST_AUTO_TEST_CASE(test_joint_torque_regressor)
340 {
341  using namespace Eigen;
342  using namespace pinocchio;
343 
344  pinocchio::Model model;
345  buildModels::humanoidRandom(model);
346 
347  model.lowerPositionLimit.head<7>().fill(-1.);
348  model.upperPositionLimit.head<7>().fill(1.);
349 
350  pinocchio::Data data(model);
351  pinocchio::Data data_ref(model);
352 
353  VectorXd q = randomConfiguration(model);
354  VectorXd v = Eigen::VectorXd::Random(model.nv);
355  VectorXd a = Eigen::VectorXd::Random(model.nv);
356 
357  rnea(model, data_ref, q, v, a);
358 
359  Eigen::VectorXd params(10 * (model.njoints - 1));
360  for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
361  params.segment<10>((int)((i - 1) * 10)) = model.inertias[i].toDynamicParameters();
362 
363  computeJointTorqueRegressor(model, data, q, v, a);
364 
365  Eigen::VectorXd tau_regressor = data.jointTorqueRegressor * params;
366 
367  BOOST_CHECK(tau_regressor.isApprox(data_ref.tau));
368 }
369 
370 BOOST_AUTO_TEST_CASE(test_kinetic_energy_regressor)
371 {
372  using namespace Eigen;
373  using namespace pinocchio;
374 
375  pinocchio::Model model;
376  buildModels::humanoidRandom(model);
377 
378  model.lowerPositionLimit.head<7>().fill(-1.);
379  model.upperPositionLimit.head<7>().fill(1.);
380 
381  pinocchio::Data data(model);
382  pinocchio::Data data_ref(model);
383 
384  const VectorXd q = randomConfiguration(model);
385  const VectorXd v = Eigen::VectorXd::Random(model.nv);
386 
387  computeAllTerms(model, data_ref, q, v);
388  auto target_energy = computeKineticEnergy(model, data_ref);
389 
390  const auto regressor = computeKineticEnergyRegressor(model, data, q, v);
391 
392  Eigen::VectorXd params(10 * (model.njoints - 1));
393  for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
394  params.segment<10>(Eigen::DenseIndex((i - 1) * 10)) = model.inertias[i].toDynamicParameters();
395 
396  const double kinetic_energy_regressor = data.kineticEnergyRegressor * params;
397 
398  BOOST_CHECK_CLOSE(kinetic_energy_regressor, target_energy, 1e-12);
399 }
400 
401 BOOST_AUTO_TEST_CASE(test_potential_energy_regressor)
402 {
403  using namespace Eigen;
404  using namespace pinocchio;
405 
406  pinocchio::Model model;
407  buildModels::humanoidRandom(model);
408 
409  model.lowerPositionLimit.head<7>().fill(-1.);
410  model.upperPositionLimit.head<7>().fill(1.);
411 
412  pinocchio::Data data(model);
413  pinocchio::Data data_ref(model);
414 
415  const VectorXd q = randomConfiguration(model);
416  const VectorXd v = Eigen::VectorXd::Random(model.nv);
417 
418  computeAllTerms(model, data_ref, q, v);
419  const double target_energy = computePotentialEnergy(model, data_ref);
420 
421  Eigen::VectorXd params(10 * (model.njoints - 1));
422  for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
423  params.segment<10>(Eigen::DenseIndex((i - 1) * 10)) = model.inertias[i].toDynamicParameters();
424 
425  computePotentialEnergyRegressor(model, data, q);
426  const double potential_energy_regressor = data.potentialEnergyRegressor * params;
427 
428  BOOST_CHECK_CLOSE(potential_energy_regressor, target_energy, 1e-12);
429 }
430 
431 BOOST_AUTO_TEST_SUITE_END()
pinocchio::InertiaTpl< context::Scalar, context::Options >
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
pinocchio::FrameIndex
Index FrameIndex
Definition: multibody/fwd.hpp:28
frames.hpp
fwd.hpp
pinocchio::computePotentialEnergy
Scalar computePotentialEnergy(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the potential energy of the system, i.e. the potential energy linked to the gravity field....
Eigen
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.
compute-all-terms.hpp
pinocchio::ModelTpl::jointPlacements
SE3Vector jointPlacements
Vector of joint placements: placement of a joint i wrt its parent joint frame.
Definition: multibody/model.hpp:116
pinocchio::SE3Tpl< context::Scalar, context::Options >
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
pinocchio::computeJointTorqueRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & computeJointTorqueRegressor(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)
Computes the joint torque regressor that links the joint torque to the dynamic parameters of each lin...
setup.data
data
Definition: cmake/cython/setup.in.py:48
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.
explog.hpp
pinocchio::InertiaTpl::vxiv
Force vxiv(const MotionDense< MotionDerived > &v) const
Definition: spatial/inertia.hpp:891
pinocchio::python::context::Motion
MotionTpl< Scalar, Options > Motion
Definition: bindings/python/context/generic.hpp:54
pinocchio::frameBodyRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType & frameBodyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, FrameIndex frame_id)
Computes the regressor for the dynamic parameters of a rigid body attached to a given frame,...
pinocchio::buildModels::manipulator
void manipulator(ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Create a 6-DOF kinematic chain shoulder-elbow-wrist.
autodiff-rnea.f
f
Definition: autodiff-rnea.py:24
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::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: multibody/frame.hpp:55
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint)
Definition: unittest/regressor.cpp:22
center-of-mass.hpp
joint-configuration.hpp
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.
regressor.hpp
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: multibody/data.hpp:173
pinocchio::computePotentialEnergyRegressor
const DataTpl< Scalar, Options, JointCollectionTpl >::RowVectorXs & computePotentialEnergyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
cartpole.v
v
Definition: cartpole.py:153
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
pinocchio::ForceTpl
Definition: context/casadi.hpp:25
q
q
contact-cholesky.frame
frame
Definition: contact-cholesky.py:24
pinocchio::jointBodyRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType & jointBodyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, JointIndex joint_id)
Computes the regressor for the dynamic parameters of a rigid body attached to a given joint,...
pinocchio::InertiaTpl::toDynamicParameters
Vector10 toDynamicParameters() const
Definition: spatial/inertia.hpp:541
pinocchio::computeJointKinematicRegressor
void computeJointKinematicRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame rf, const SE3Tpl< Scalar, Options > &placement, const Eigen::MatrixBase< Matrix6xReturnType > &kinematic_regressor)
a
Vec3f a
contact-cholesky.frame_id
frame_id
Definition: contact-cholesky.py:19
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.
pinocchio::computeFrameKinematicRegressor
void computeFrameKinematicRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xReturnType > &kinematic_regressor)
append-urdf-model-with-another-model.joint_name
string joint_name
Definition: append-urdf-model-with-another-model.py:33
fill
fill
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: spatial/explog.hpp:435
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::bodyRegressor
void bodyRegressor(const MotionDense< MotionVelocity > &v, const MotionDense< MotionAcceleration > &a, const Eigen::MatrixBase< OutputType > &regressor)
Computes the regressor for the dynamic parameters of a single rigid body.
pinocchio::quaternion::exp6
void exp6(const MotionDense< MotionDerived > &motion, Eigen::MatrixBase< Config_t > &qout)
The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
Definition: explog-quaternion.hpp:92
pinocchio::computeStaticRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & computeStaticRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the static regressor that links the center of mass positions of all the links to the center ...
dcrba.eps
int eps
Definition: dcrba.py:458
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
Y
Y
inverse-kinematics-3d.JOINT_ID
int JOINT_ID
Definition: inverse-kinematics-3d.py:8
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
pinocchio::ModelTpl
Definition: context/generic.hpp:20
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...
robot-wrapper-viewer.com
com
Definition: robot-wrapper-viewer.py:45
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::centerOfMass
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & centerOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
Computes the center of mass position of a given model according to a particular joint configuration....
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

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