joint-spherical.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 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(JointSpherical)
35 
36 BOOST_AUTO_TEST_CASE(spatial)
37 {
38  SE3 M(SE3::Random());
39  Motion v(Motion::Random());
40 
41  MotionSpherical mp(MotionSpherical::Vector3(1., 2., 3.));
42  Motion mp_dense(mp);
43 
44  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
45  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
46 
47  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
48 }
49 
50 BOOST_AUTO_TEST_CASE(vsFreeFlyer)
51 {
52  using namespace pinocchio;
53  typedef SE3::Vector3 Vector3;
54  typedef Eigen::Matrix<double, 6, 1> Vector6;
55  typedef Eigen::Matrix<double, 7, 1> VectorFF;
56  typedef SE3::Matrix3 Matrix3;
57 
58  Model modelSpherical, modelFreeflyer;
59 
60  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
61  SE3 pos(1);
62  pos.translation() = SE3::LinearType(1., 0., 0.);
63 
64  addJointAndBody(modelSpherical, JointModelSpherical(), 0, pos, "spherical", inertia);
65  addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, pos, "free-flyer", inertia);
66 
67  Data dataSpherical(modelSpherical);
68  Data dataFreeFlyer(modelFreeflyer);
69 
70  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelSpherical.nq);
71  q.normalize();
72  VectorFF qff;
73  qff << 0, 0, 0, q[0], q[1], q[2], q[3];
74  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelSpherical.nv);
75  Vector6 vff;
76  vff << 0, 0, 0, 1, 1, 1;
77  Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones(modelSpherical.nv);
78  Eigen::VectorXd tauff;
79  tauff.resize(7);
80  tauff << 0, 0, 0, 1, 1, 1, 1;
81  Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones(modelSpherical.nv);
82  Eigen::VectorXd aff(vff);
83 
84  forwardKinematics(modelSpherical, dataSpherical, q, v);
85  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
86 
87  computeAllTerms(modelSpherical, dataSpherical, q, v);
88  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
89 
90  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSpherical.oMi[1]));
91  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSpherical.liMi[1]));
92  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSpherical.Ycrb[1].matrix()));
93  BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataSpherical.f[1].toVector()));
94 
95  Eigen::VectorXd nle_expected_ff(3);
96  nle_expected_ff << dataFreeFlyer.nle[3], dataFreeFlyer.nle[4], dataFreeFlyer.nle[5];
97  BOOST_CHECK(nle_expected_ff.isApprox(dataSpherical.nle));
98  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSpherical.com[0]));
99 
100  // InverseDynamics == rnea
101  tauSpherical = rnea(modelSpherical, dataSpherical, q, v, aSpherical);
102  tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
103 
104  Vector3 tau_expected;
105  tau_expected << tauff(3), tauff(4), tauff(5);
106  BOOST_CHECK(tauSpherical.isApprox(tau_expected));
107 
108  // ForwardDynamics == aba
109  Eigen::VectorXd aAbaSpherical =
110  aba(modelSpherical, dataSpherical, q, v, tauSpherical, Convention::WORLD);
111  Eigen::VectorXd aAbaFreeFlyer =
112  aba(modelFreeflyer, dataFreeFlyer, qff, vff, tauff, Convention::WORLD);
113  Vector3 a_expected;
114  a_expected << aAbaFreeFlyer[3], aAbaFreeFlyer[4], aAbaFreeFlyer[5];
115  BOOST_CHECK(aAbaSpherical.isApprox(a_expected));
116 
117  // crba
118  crba(modelSpherical, dataSpherical, q, Convention::WORLD);
119  crba(modelFreeflyer, dataFreeFlyer, qff, Convention::WORLD);
120 
121  Eigen::Matrix<double, 3, 3> M_expected(dataFreeFlyer.M.bottomRightCorner<3, 3>());
122 
123  BOOST_CHECK(dataSpherical.M.isApprox(M_expected));
124 
125  // Jacobian
126  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;
127  jacobian_planar.resize(6, 3);
128  jacobian_planar.setZero();
129  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;
130  jacobian_ff.resize(6, 6);
131  jacobian_ff.setZero();
132  computeJointJacobians(modelSpherical, dataSpherical, q);
133  computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
134  getJointJacobian(modelSpherical, dataSpherical, 1, LOCAL, jacobian_planar);
135  getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);
136 
137  Eigen::Matrix<double, 6, 3> jacobian_expected;
138  jacobian_expected << jacobian_ff.col(3), jacobian_ff.col(4), jacobian_ff.col(5);
139 
140  BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
141 }
142 BOOST_AUTO_TEST_SUITE_END()
143 
144 BOOST_AUTO_TEST_SUITE(JointSphericalZYX)
145 
146 BOOST_AUTO_TEST_CASE(spatial)
147 {
148  SE3 M(SE3::Random());
149  Motion v(Motion::Random());
150 
151  MotionSpherical mp(MotionSpherical::Vector3(1., 2., 3.));
152  Motion mp_dense(mp);
153 
154  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
155  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
156 
157  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
158 }
159 
160 BOOST_AUTO_TEST_CASE(vsFreeFlyer)
161 {
162  // WARNIG : Dynamic algorithm's results cannot be compared to FreeFlyer's ones because
163  // of the representation of the rotation and the ConstraintSubspace difference.
164  using namespace pinocchio;
165  typedef SE3::Vector3 Vector3;
166  typedef Eigen::Matrix<double, 6, 1> Vector6;
167  typedef Eigen::Matrix<double, 7, 1> VectorFF;
168  typedef SE3::Matrix3 Matrix3;
169 
170  Model modelSphericalZYX, modelFreeflyer;
171 
172  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
173  SE3 pos(1);
174  pos.translation() = SE3::LinearType(1., 0., 0.);
175 
176  addJointAndBody(modelSphericalZYX, JointModelSphericalZYX(), 0, pos, "spherical-zyx", inertia);
177  addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, pos, "free-flyer", inertia);
178 
179  Data dataSphericalZYX(modelSphericalZYX);
180  Data dataFreeFlyer(modelFreeflyer);
181 
182  Eigen::AngleAxisd rollAngle(1, Eigen::Vector3d::UnitZ());
183  Eigen::AngleAxisd yawAngle(1, Eigen::Vector3d::UnitY());
184  Eigen::AngleAxisd pitchAngle(1, Eigen::Vector3d::UnitX());
185  Eigen::Quaterniond q_sph = rollAngle * yawAngle * pitchAngle;
186 
187  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelSphericalZYX.nq);
188  VectorFF qff;
189  qff << 0, 0, 0, q_sph.x(), q_sph.y(), q_sph.z(), q_sph.w();
190  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
191  Vector6 vff;
192  vff << 0, 0, 0, 1, 1, 1;
193  Eigen::VectorXd tauSpherical = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
194  Eigen::VectorXd tauff;
195  tauff.resize(6);
196  tauff << 0, 0, 0, 1, 1, 1;
197  Eigen::VectorXd aSpherical = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
198  Eigen::VectorXd aff(vff);
199 
200  forwardKinematics(modelSphericalZYX, dataSphericalZYX, q, v);
201  forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
202 
203  computeAllTerms(modelSphericalZYX, dataSphericalZYX, q, v);
204  computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
205 
206  BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
207  BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataSphericalZYX.liMi[1]));
208  BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix()));
209 
210  BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataSphericalZYX.com[0]));
211 }
212 
213 BOOST_AUTO_TEST_CASE(test_rnea)
214 {
215  using namespace pinocchio;
216  typedef SE3::Vector3 Vector3;
217  typedef SE3::Matrix3 Matrix3;
218 
219  Model model;
220  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
221 
222  addJointAndBody(
223  model, JointModelSphericalZYX(), model.getJointId("universe"), SE3::Identity(), "root",
224  inertia);
225 
226  Data data(model);
227 
228  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
229  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
230  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
231 
232  rnea(model, data, q, v, a);
233  Vector3 tau_expected(0., -4.905, 0.);
234 
235  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));
236 
237  q = Eigen::VectorXd::Ones(model.nq);
238  v = Eigen::VectorXd::Ones(model.nv);
239  a = Eigen::VectorXd::Ones(model.nv);
240 
241  rnea(model, data, q, v, a);
242  tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;
243 
244  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
245 
246  q << 3, 2, 1;
247  v = Eigen::VectorXd::Ones(model.nv);
248  a = Eigen::VectorXd::Ones(model.nv);
249 
250  rnea(model, data, q, v, a);
251  tau_expected << 0.73934458094049, 2.7804530848031, 0.50684940972146;
252 
253  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
254 }
255 
256 BOOST_AUTO_TEST_CASE(test_crba)
257 {
258  using namespace pinocchio;
259  using namespace std;
260  typedef SE3::Vector3 Vector3;
261  typedef SE3::Matrix3 Matrix3;
262 
263  Model model;
264  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
265 
266  addJointAndBody(
267  model, JointModelSphericalZYX(), model.getJointId("universe"), SE3::Identity(), "root",
268  inertia);
269 
270  Data data(model);
271 
272  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
273  Eigen::MatrixXd M_expected(model.nv, model.nv);
274 
275  crba(model, data, q, Convention::WORLD);
276  M_expected << 1.25, 0, 0, 0, 1.25, 0, 0, 0, 1;
277 
278  BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));
279 
280  q = Eigen::VectorXd::Ones(model.nq);
281 
282  crba(model, data, q, Convention::WORLD);
283  M_expected << 1.0729816454316, -5.5511151231258e-17, -0.8414709848079, -5.5511151231258e-17, 1.25,
284  0, -0.8414709848079, 0, 1;
285 
286  BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));
287 
288  q << 3, 2, 1;
289 
290  crba(model, data, q, Convention::WORLD);
291  M_expected << 1.043294547392, 2.7755575615629e-17, -0.90929742682568, 0, 1.25, 0,
292  -0.90929742682568, 0, 1;
293 
294  BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
295 }
296 
297 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::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
pinocchio::JointModelFreeFlyer
JointModelFreeFlyerTpl< context::Scalar > JointModelFreeFlyer
Definition: multibody/joint/fwd.hpp:110
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::JointModelBase
Definition: joint-model-base.hpp:75
pinocchio::SE3Tpl< context::Scalar, context::Options >
pinocchio::Convention::WORLD
@ WORLD
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::JointModelSpherical
JointModelSphericalTpl< context::Scalar > JointModelSpherical
Definition: multibody/joint/fwd.hpp:73
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(spatial)
Definition: joint-spherical.cpp:36
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....
rnea.hpp
aba.hpp
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:56
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
pinocchio::MotionSphericalTpl
Definition: joint-spherical.hpp:20
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...
fwd.hpp
pos
pos
M
M
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
pinocchio::MotionTpl::Random
static MotionTpl Random()
Definition: motion-tpl.hpp:140
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1118
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...
std
Definition: autodiff/casadi/utils/static-if.hpp:64
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
Y
Y
joints.hpp
pinocchio::JointModelSphericalZYX
JointModelSphericalZYXTpl< context::Scalar > JointModelSphericalZYX
Definition: multibody/joint/fwd.hpp:81
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::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
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

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