regressor.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2020 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/parsers/sample-models.hpp"
13 
14 #include <iostream>
15 
16 #include <boost/test/unit_test.hpp>
17 #include <boost/utility/binary.hpp>
18 
19 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
20 
21 BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint)
22 {
23  using namespace Eigen;
24  using namespace pinocchio;
25 
26  pinocchio::Model model; buildModels::humanoidRandom(model);
27 
28  pinocchio::Data data(model);
29  pinocchio::Data data_ref(model);
30 
31  model.lowerPositionLimit.head<7>().fill(-1.);
32  model.upperPositionLimit.head<7>().fill(1.);
33 
34 // const std::string joint_name = "larm5_joint";
35 // const JointIndex joint_id = model.getJointId(joint_name);
36 
37  const VectorXd q = randomConfiguration(model);
38 
39  forwardKinematics(model,data,q);
40 
41  const double eps = 1e-8;
42  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
43  {
44  Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
45  Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
46  Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
47 
48  Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
49  Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
50  Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
51 
52  computeJointKinematicRegressor(model, data, joint_id, LOCAL, kinematic_regressor_L);
53  computeJointKinematicRegressor(model, data, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA);
54  computeJointKinematicRegressor(model, data, joint_id, WORLD, kinematic_regressor_W);
55 
56  Model model_plus = model; Data data_plus(model_plus);
57  const SE3 & oMi = data.oMi[joint_id];
58  const SE3 Mi_LWA = SE3(oMi.rotation(),SE3::Vector3::Zero());
59  const SE3 & oMi_plus = data_plus.oMi[joint_id];
60  for(int i = 1; i < model.njoints; ++i)
61  {
62  Motion::Vector6 v = Motion::Vector6::Zero();
63  const SE3 & M_placement = model.jointPlacements[(JointIndex)i];
64  SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i];
65  for(Eigen::DenseIndex k = 0; k < 6; ++k)
66  {
67  v[k] = eps;
68  M_placement_plus = M_placement * exp6(Motion(v));
69 
70  forwardKinematics(model_plus,data_plus,q);
71 
72  const Motion diff_L = log6(oMi.actInv(oMi_plus));
73  kinematic_regressor_L_fd.middleCols<6>(6*(i-1)).col(k) = diff_L.toVector()/eps;
74  const Motion diff_LWA = Mi_LWA.act(diff_L);
75  kinematic_regressor_LWA_fd.middleCols<6>(6*(i-1)).col(k) = diff_LWA.toVector()/eps;
76  const Motion diff_W = oMi.act(diff_L);
77  kinematic_regressor_W_fd.middleCols<6>(6*(i-1)).col(k) = diff_W.toVector()/eps;
78  v[k] = 0.;
79  }
80 
81  M_placement_plus = M_placement;
82  }
83 
84  BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd,sqrt(eps)));
85  BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd,sqrt(eps)));
86  BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd,sqrt(eps)));
87  }
88 }
89 
90 BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint_placement)
91 {
92  using namespace Eigen;
93  using namespace pinocchio;
94 
95  pinocchio::Model model; buildModels::humanoidRandom(model);
96 
97  pinocchio::Data data(model);
98  pinocchio::Data data_ref(model);
99 
100  model.lowerPositionLimit.head<7>().fill(-1.);
101  model.upperPositionLimit.head<7>().fill(1.);
102 
103  const VectorXd q = randomConfiguration(model);
104 
105  forwardKinematics(model,data,q);
106  forwardKinematics(model,data_ref,q);
107 
108  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
109  {
110  Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
111  Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
112  Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
113 
114  computeJointKinematicRegressor(model, data, joint_id, LOCAL, SE3::Identity(), kinematic_regressor_L);
115  computeJointKinematicRegressor(model, data, joint_id, LOCAL_WORLD_ALIGNED, SE3::Identity(), kinematic_regressor_LWA);
116  computeJointKinematicRegressor(model, data, joint_id, WORLD, SE3::Identity(), kinematic_regressor_W);
117 
118  Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
119  Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
120  Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
121 
122  computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL, kinematic_regressor_L_ref);
123  computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA_ref);
124  computeJointKinematicRegressor(model, data_ref, joint_id, WORLD, kinematic_regressor_W_ref);
125 
126  BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref));
127  BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref));
128  BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref));
129  }
130 }
131 
132 BOOST_AUTO_TEST_CASE(test_kinematic_regressor_frame)
133 {
134  using namespace Eigen;
135  using namespace pinocchio;
136 
137  pinocchio::Model model; buildModels::humanoidRandom(model);
138 
139  model.lowerPositionLimit.head<7>().fill(-1.);
140  model.upperPositionLimit.head<7>().fill(1.);
141 
142  const std::string joint_name = "larm5_joint";
143  const JointIndex joint_id = model.getJointId(joint_name);
144  model.addBodyFrame("test_body", joint_id, SE3::Random(), -1);
145 
146  pinocchio::Data data(model);
147  pinocchio::Data data_ref(model);
148 
149  const VectorXd q = randomConfiguration(model);
150 
151  forwardKinematics(model,data,q);
152  updateFramePlacements(model,data);
153  forwardKinematics(model,data_ref,q);
154 
155  const double eps = 1e-8;
156  for(FrameIndex frame_id = 1; frame_id < (FrameIndex)model.nframes; ++frame_id)
157  {
158  const Frame & frame = model.frames[frame_id];
159 
160  Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
161  Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
162  Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
163 
164  computeFrameKinematicRegressor(model, data, frame_id, LOCAL, kinematic_regressor_L);
165  computeFrameKinematicRegressor(model, data, frame_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA);
166  computeFrameKinematicRegressor(model, data, frame_id, WORLD, kinematic_regressor_W);
167 
168  Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
169  Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
170  Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
171 
172  computeJointKinematicRegressor(model, data_ref, frame.parent, LOCAL, frame.placement, kinematic_regressor_L_ref);
173  computeJointKinematicRegressor(model, data_ref, frame.parent, LOCAL_WORLD_ALIGNED, frame.placement, kinematic_regressor_LWA_ref);
174  computeJointKinematicRegressor(model, data_ref, frame.parent, WORLD, frame.placement, kinematic_regressor_W_ref);
175 
176  BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref));
177  BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref));
178  BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref));
179 
180  Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
181  Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
182  Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1)));
183 
184  Model model_plus = model; Data data_plus(model_plus);
185  const SE3 & oMf = data.oMf[frame_id];
186  const SE3 Mf_LWA = SE3(oMf.rotation(),SE3::Vector3::Zero());
187  const SE3 & oMf_plus = data_plus.oMf[frame_id];
188  for(int i = 1; i < model.njoints; ++i)
189  {
190  Motion::Vector6 v = Motion::Vector6::Zero();
191  const SE3 & M_placement = model.jointPlacements[(JointIndex)i];
192  SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i];
193  for(Eigen::DenseIndex k = 0; k < 6; ++k)
194  {
195  v[k] = eps;
196  M_placement_plus = M_placement * exp6(Motion(v));
197 
198  forwardKinematics(model_plus,data_plus,q);
199  updateFramePlacements(model_plus,data_plus);
200 
201  const Motion diff_L = log6(oMf.actInv(oMf_plus));
202  kinematic_regressor_L_fd.middleCols<6>(6*(i-1)).col(k) = diff_L.toVector()/eps;
203  const Motion diff_LWA = Mf_LWA.act(diff_L);
204  kinematic_regressor_LWA_fd.middleCols<6>(6*(i-1)).col(k) = diff_LWA.toVector()/eps;
205  const Motion diff_W = oMf.act(diff_L);
206  kinematic_regressor_W_fd.middleCols<6>(6*(i-1)).col(k) = diff_W.toVector()/eps;
207  v[k] = 0.;
208  }
209 
210  M_placement_plus = M_placement;
211  }
212 
213  BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd,sqrt(eps)));
214  BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd,sqrt(eps)));
215  BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd,sqrt(eps)));
216  }
217 }
218 
219 BOOST_AUTO_TEST_CASE(test_static_regressor)
220 {
221  using namespace Eigen;
222  using namespace pinocchio;
223 
224  pinocchio::Model model; buildModels::humanoidRandom(model);
225 
226  pinocchio::Data data(model);
227  pinocchio::Data data_ref(model);
228 
229  model.lowerPositionLimit.head<7>().fill(-1.);
230  model.upperPositionLimit.head<7>().fill(1.);
231 
232  VectorXd q = randomConfiguration(model);
233  computeStaticRegressor(model,data,q);
234 
235  VectorXd phi(4*(model.njoints-1));
236  for(int k = 1; k < model.njoints; ++k)
237  {
238  const Inertia & Y = model.inertias[(size_t)k];
239  phi.segment<4>(4*(k-1)) << Y.mass(), Y.mass() * Y.lever();
240  }
241 
242  Vector3d com = centerOfMass(model,data_ref,q);
243  Vector3d static_com_ref;
244  static_com_ref << com;
245 
246  Vector3d static_com = data.staticRegressor * phi;
247 
248  BOOST_CHECK(static_com.isApprox(static_com_ref));
249 }
250 
251 BOOST_AUTO_TEST_CASE(test_body_regressor)
252 {
253  using namespace Eigen;
254  using namespace pinocchio;
255 
256  Inertia I(Inertia::Random());
257  Motion v(Motion::Random());
258  Motion a(Motion::Random());
259 
260  Force f = I*a + I.vxiv(v);
261 
262  Inertia::Vector6 f_regressor = bodyRegressor(v,a) * I.toDynamicParameters();
263 
264  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
265 }
266 
267 BOOST_AUTO_TEST_CASE(test_joint_body_regressor)
268 {
269  using namespace Eigen;
270  using namespace pinocchio;
271 
272  pinocchio::Model model;
273  buildModels::manipulator(model);
274  pinocchio::Data data(model);
275 
276  JointIndex JOINT_ID = JointIndex(model.njoints) - 1;
277 
278  VectorXd q = randomConfiguration(model);
279  VectorXd v = Eigen::VectorXd::Random(model.nv);
280  VectorXd a = Eigen::VectorXd::Random(model.nv);
281 
282  rnea(model,data,q,v,a);
283 
284  Force f = data.f[JOINT_ID];
285 
286  Inertia::Vector6 f_regressor = jointBodyRegressor(model,data,JOINT_ID) * model.inertias[JOINT_ID].toDynamicParameters();
287 
288  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
289 }
290 
291 BOOST_AUTO_TEST_CASE(test_frame_body_regressor)
292 {
293  using namespace Eigen;
294  using namespace pinocchio;
295 
296  pinocchio::Model model;
297  buildModels::manipulator(model);
298 
299  JointIndex JOINT_ID = JointIndex(model.njoints) - 1;
300 
301  const SE3 & framePlacement = SE3::Random();
302  FrameIndex FRAME_ID = model.addBodyFrame ("test_body", JOINT_ID, framePlacement, -1);
303 
304  pinocchio::Data data(model);
305 
306  VectorXd q = randomConfiguration(model);
307  VectorXd v = Eigen::VectorXd::Random(model.nv);
308  VectorXd a = Eigen::VectorXd::Random(model.nv);
309 
310  rnea(model,data,q,v,a);
311 
312  Force f = framePlacement.actInv(data.f[JOINT_ID]);
313  Inertia I = framePlacement.actInv(model.inertias[JOINT_ID]);
314 
315  Inertia::Vector6 f_regressor = frameBodyRegressor(model,data,FRAME_ID) * I.toDynamicParameters();
316 
317  BOOST_CHECK(f_regressor.isApprox(f.toVector()));
318 }
319 
320 BOOST_AUTO_TEST_CASE(test_joint_torque_regressor)
321 {
322  using namespace Eigen;
323  using namespace pinocchio;
324 
325  pinocchio::Model model;
326  buildModels::humanoidRandom(model);
327 
328  model.lowerPositionLimit.head<7>().fill(-1.);
329  model.upperPositionLimit.head<7>().fill(1.);
330 
331  pinocchio::Data data(model);
332  pinocchio::Data data_ref(model);
333 
334  VectorXd q = randomConfiguration(model);
335  VectorXd v = Eigen::VectorXd::Random(model.nv);
336  VectorXd a = Eigen::VectorXd::Random(model.nv);
337 
338  rnea(model,data_ref,q,v,a);
339 
340  Eigen::VectorXd params(10*(model.njoints-1));
341  for(JointIndex i=1; i<(Model::JointIndex)model.njoints; ++i)
342  params.segment<10>((int)((i-1)*10)) = model.inertias[i].toDynamicParameters();
343 
344  computeJointTorqueRegressor(model,data,q,v,a);
345 
346  Eigen::VectorXd tau_regressor = data.jointTorqueRegressor * params;
347 
348  BOOST_CHECK(tau_regressor.isApprox(data_ref.tau));
349 }
350 
351 BOOST_AUTO_TEST_SUITE_END()
pinocchio::WORLD
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
Definition: src/multibody/fwd.hpp:46
pinocchio::InertiaTpl::vxiv
Force vxiv(const Motion &v) const
Definition: src/spatial/inertia.hpp:570
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: src/multibody/data.hpp:128
pinocchio::FrameIndex
Index FrameIndex
Definition: src/multibody/fwd.hpp:30
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint)
Definition: regressor.cpp:21
q
q
pinocchio::DataTpl::jointTorqueRegressor
MatrixXs jointTorqueRegressor
Matrix related to joint torque regressor.
Definition: src/multibody/data.hpp:359
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::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: src/multibody/frame.hpp:32
dcrba.eps
int eps
Definition: dcrba.py:384
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: src/multibody/model.hpp:82
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: src/multibody/model.hpp:139
pinocchio::ForceBase::toVector
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:81
pinocchio::jointBodyRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType & jointBodyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, JointIndex jointId)
Computes the regressor for the dynamic parameters of a rigid body attached to a given joint...
pinocchio::FrameTpl::parent
JointIndex parent
Index of the parent joint.
Definition: src/multibody/frame.hpp:123
pinocchio::InertiaTpl::toDynamicParameters
Vector10 toDynamicParameters() const
Definition: src/spatial/inertia.hpp:335
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition: src/multibody/model.hpp:145
v
v
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: src/spatial/explog.hpp:373
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:224
data
data
pinocchio::frameBodyRegressor
DataTpl< Scalar, Options, JointCollectionTpl >::BodyRegressorType & frameBodyRegressor(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, FrameIndex frameId)
Computes the regressor for the dynamic parameters of a rigid body attached to a given frame...
pinocchio::ModelTpl::nframes
int nframes
Number of operational frames.
Definition: src/multibody/model.hpp:88
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::InertiaTpl::lever
const Vector3 & lever() const
Definition: src/spatial/inertia.hpp:544
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::SE3Base::actInv
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:97
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
pinocchio::Motion
MotionTpl< double, 0 > Motion
Definition: src/spatial/fwd.hpp:57
pinocchio::SE3Base::act
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90
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...
pinocchio::FrameTpl::placement
SE3 placement
Placement of the frame wrt the parent joint.
Definition: src/multibody/frame.hpp:129
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:30
pinocchio::JointIndex
Index JointIndex
Definition: src/multibody/fwd.hpp:28
pinocchio::MotionBase::toVector
ToVectorConstReturnType toVector() const
Definition: motion-base.hpp:37
pinocchio::LOCAL_WORLD_ALIGNED
The LOCAL_WORLD_ALIGNED frame convention corresponds to the frame centered on the moving part (Joint...
Definition: src/multibody/fwd.hpp:48
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
pinocchio::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
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.
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 ...
Definition: regressor.hpp:176
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)
Computes the kinematic regressor that links the joint placements variations of the whole kinematic tr...
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...
a
list a
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
pinocchio::DataTpl::staticRegressor
Matrix3x staticRegressor
Matrix related to static regressor.
Definition: src/multibody/data.hpp:353
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)
Computes the kinematic regressor that links the joint placement variations of the whole kinematic tre...
pinocchio::exp6
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
Definition: src/spatial/explog.hpp:296
pinocchio::SE3
SE3Tpl< double, 0 > SE3
Definition: src/spatial/fwd.hpp:53
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: src/multibody/data.hpp:71
pinocchio::SE3Base::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
pinocchio::buildModels::manipulator
void manipulator(ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Create a 6-DOF kinematic chain shoulder-elbow-wrist.
pinocchio::ModelTpl::addBodyFrame
FrameIndex addBodyFrame(const std::string &body_name, const JointIndex &parentJoint, const SE3 &body_placement=SE3::Identity(), int previousFrame=-1)
Add a body to the frame tree.
fill
void fill(Eigen::Ref< MatType > mat, const typename MatType::Scalar &value)

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:04