kinematics.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2019 CNRS INRIA
3 //
4 
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 #include "pinocchio/algorithm/kinematics.hpp"
8 #include "pinocchio/algorithm/crba.hpp"
9 #include "pinocchio/algorithm/joint-configuration.hpp"
10 #include "pinocchio/parsers/sample-models.hpp"
11 
12 #include <iostream>
13 
14 #include <boost/test/unit_test.hpp>
15 #include <boost/utility/binary.hpp>
16 
17 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
18 
19 BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
20 {
21  using namespace Eigen;
22  using namespace pinocchio;
23 
24  Model model;
25  buildModels::humanoidRandom(model);
26 
27  Data data(model);
28 
29  model.lowerPositionLimit.head<3>().fill(-1.);
30  model.upperPositionLimit.head<3>().fill(1.);
31  VectorXd q = randomConfiguration(model);
32 
33  forwardKinematics(model,data,Model::ConfigVectorType::Ones(model.nq));
34 }
35 
36 BOOST_AUTO_TEST_CASE(test_kinematics_zero)
37 {
38  using namespace Eigen;
39  using namespace pinocchio;
40 
41  Model model;
42  buildModels::humanoidRandom(model);
43 
44  Data data(model), data_ref(model);
45 
46  model.lowerPositionLimit.head<3>().fill(-1.);
47  model.upperPositionLimit.head<3>().fill(1.);
48  VectorXd q = randomConfiguration(model);
49 
50  forwardKinematics(model,data_ref,q);
51  crba(model,data,q);
52  updateGlobalPlacements(model,data);
53 
54  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
55  {
56  BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]);
57  BOOST_CHECK(data.liMi[i] == data_ref.liMi[i]);
58  }
59 }
60 
61 BOOST_AUTO_TEST_CASE(test_kinematics_first)
62 {
63  using namespace Eigen;
64  using namespace pinocchio;
65 
66  Model model;
67  buildModels::humanoidRandom(model);
68 
69  Data data(model);
70 
71  model.lowerPositionLimit.head<3>().fill(-1.);
72  model.upperPositionLimit.head<3>().fill(1.);
73  VectorXd q = randomConfiguration(model);
74  VectorXd v(VectorXd::Zero(model.nv));
75 
76  forwardKinematics(model,data,q,v);
77 
78  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
79  {
80  BOOST_CHECK(data.v[i] == Motion::Zero());
81  }
82 }
83 
84 BOOST_AUTO_TEST_CASE(test_kinematics_second)
85 {
86  using namespace Eigen;
87  using namespace pinocchio;
88 
89  Model model;
90  buildModels::humanoidRandom(model);
91 
92  Data data(model);
93 
94  model.lowerPositionLimit.head<3>().fill(-1.);
95  model.upperPositionLimit.head<3>().fill(1.);
96  VectorXd q = randomConfiguration(model);
97  VectorXd v(VectorXd::Zero(model.nv));
98  VectorXd a(VectorXd::Zero(model.nv));
99 
100  forwardKinematics(model,data,q,v,a);
101 
102  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
103  {
104  BOOST_CHECK(data.v[i] == Motion::Zero());
105  BOOST_CHECK(data.a[i] == Motion::Zero());
106  }
107 }
108 
109 BOOST_AUTO_TEST_CASE(test_get_velocity)
110 {
111  using namespace Eigen;
112  using namespace pinocchio;
113 
114  Model model;
115  buildModels::humanoidRandom(model);
116 
117  Data data(model);
118 
119  model.lowerPositionLimit.head<3>().fill(-1.);
120  model.upperPositionLimit.head<3>().fill(1.);
121  VectorXd q = randomConfiguration(model);
122  VectorXd v(VectorXd::Random(model.nv));
123 
124  forwardKinematics(model,data,q,v);
125 
126  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
127  {
128  BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i)));
129  BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i,LOCAL)));
130  BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model,data,i,WORLD)));
131  BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.v[i]).isApprox(getVelocity(model,data,i,LOCAL_WORLD_ALIGNED)));
132  }
133 }
134 
135 BOOST_AUTO_TEST_CASE(test_get_acceleration)
136 {
137  using namespace Eigen;
138  using namespace pinocchio;
139 
140  Model model;
141  buildModels::humanoidRandom(model);
142 
143  Data data(model);
144 
145  model.lowerPositionLimit.head<3>().fill(-1.);
146  model.upperPositionLimit.head<3>().fill(1.);
147  VectorXd q = randomConfiguration(model);
148  VectorXd v(VectorXd::Random(model.nv));
149  VectorXd a(VectorXd::Random(model.nv));
150 
151  forwardKinematics(model,data,q,v,a);
152 
153  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
154  {
155  BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i)));
156  BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i,LOCAL)));
157  BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model,data,i,WORLD)));
158  BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.a[i]).isApprox(getAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
159  }
160 }
161 
162 BOOST_AUTO_TEST_CASE(test_get_classical_acceleration)
163 {
164  using namespace Eigen;
165  using namespace pinocchio;
166 
167  Model model;
168  buildModels::humanoidRandom(model);
169 
170  Data data(model);
171 
172  model.lowerPositionLimit.head<3>().fill(-1.);
173  model.upperPositionLimit.head<3>().fill(1.);
174  VectorXd q = randomConfiguration(model);
175  VectorXd v(VectorXd::Random(model.nv));
176  VectorXd a(VectorXd::Random(model.nv));
177 
178  forwardKinematics(model,data,q,v,a);
179 
180  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
181  {
182  SE3 T = data.oMi[i];
183  Motion vel = data.v[i];
184  Motion acc = data.a[i];
185  Vector3d linear;
186 
187  Motion acc_classical_local = acc;
188  linear = acc.linear() + vel.angular().cross(vel.linear());
189  acc_classical_local.linear() = linear;
190 
191  BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i)));
192  BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i,LOCAL)));
193 
194  Motion vel_world = T.act(vel);
195  Motion acc_classical_world = T.act(acc);
196  linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear());
197  acc_classical_world.linear() = linear;
198 
199  BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model,data,i,WORLD)));
200 
201  Motion vel_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(vel);
202  Motion acc_classical_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(acc);
203  linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear());
204  acc_classical_aligned.linear() = linear;
205 
206  BOOST_CHECK(acc_classical_aligned.isApprox(getClassicalAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
207  }
208 }
209 
210 BOOST_AUTO_TEST_CASE(test_kinematic_getters)
211 {
212  using namespace Eigen;
213  using namespace pinocchio;
214 
215  // Build a simple 2R planar model
216  Model model;
217  JointIndex jointId = 0;
218  jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
219  jointId = model.addJoint(jointId, JointModelRZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");
220 
221  Data data(model);
222 
223  // Predetermined configuration values
224  VectorXd q(model.nq);
225  q << M_PI/2.0, 0.0;
226 
227  VectorXd v(model.nv);
228  v << 1.0, 0.0;
229 
230  VectorXd a(model.nv);
231  a << 0.0, 0.0;
232 
233  // Expected velocity
234  Motion v_local;
235  v_local.linear() = Vector3d(0.0, 1.0, 0.0);
236  v_local.angular() = Vector3d(0.0, 0.0, 1.0);
237 
238  Motion v_world;
239  v_world.linear() = Vector3d::Zero();
240  v_world.angular() = Vector3d(0.0, 0.0, 1.0);
241 
242  Motion v_align;
243  v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
244  v_align.angular() = Vector3d(0.0, 0.0, 1.0);
245 
246  // Expected classical acceleration
247  Motion ac_local;
248  ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
249  ac_local.angular() = Vector3d::Zero();
250 
251  Motion ac_world = Motion::Zero();
252 
253  Motion ac_align;
254  ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
255  ac_align.angular() = Vector3d::Zero();
256 
257  // Perform kinematics
258  forwardKinematics(model,data,q,v,a);
259 
260  // Check output velocity
261  BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId)));
262  BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId,LOCAL)));
263  BOOST_CHECK(v_world.isApprox(getVelocity(model,data,jointId,WORLD)));
264  BOOST_CHECK(v_align.isApprox(getVelocity(model,data,jointId,LOCAL_WORLD_ALIGNED)));
265 
266  // Check output acceleration (all zero)
267  BOOST_CHECK(getAcceleration(model,data,jointId).isZero());
268  BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL).isZero());
269  BOOST_CHECK(getAcceleration(model,data,jointId,WORLD).isZero());
270  BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED).isZero());
271 
272  // Check output classical
273  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId)));
274  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL)));
275  BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model,data,jointId,WORLD)));
276  BOOST_CHECK(ac_align.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED)));
277 }
278 
279 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::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...
q
q
pinocchio::isZero
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Definition: math/matrix.hpp:56
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
M_PI
#define M_PI
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
pinocchio::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:22
fill
fill
pinocchio::motionSet::act
static void act(const Eigen::MatrixBase< Mat > &iV, const ForceDense< ForceDerived > &f, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion set on a force object. The input motion set is represented by a 6xN matrix whose e...
pinocchio::MotionBase::angular
ConstAngularType angular() const
Definition: motion-base.hpp:21
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::getClassicalAcceleration
MotionTpl< Scalar, Options > getClassicalAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
Returns the "classical" acceleration of the joint expressed in the desired reference frame...
pinocchio::updateGlobalPlacements
void updateGlobalPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Update the global placement of the joints oMi according to the relative placements of the joints...
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::JointModelRZ
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
Definition: joint-revolute.hpp:764
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: src/multibody/model.hpp:49
a
Vec3f a
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::JointIndex
Index JointIndex
Definition: src/multibody/fwd.hpp:28
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::MotionDense::isApprox
bool isApprox(const MotionDense< M2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: motion-dense.hpp:171
pinocchio::SE3Base::act
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90
pinocchio::getAcceleration
MotionTpl< Scalar, Options > getAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
Returns the spatial acceleration of the joint expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
Definition: kinematics.cpp:19
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::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
pinocchio::getVelocity
MotionTpl< Scalar, Options > getVelocity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
Returns the spatial velocity of the joint expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.

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