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