joint-prismatic.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( JointPrismatic )
34 
35 BOOST_AUTO_TEST_CASE(spatial)
36 {
37  typedef TransformPrismaticTpl<double,0,0> TransformX;
38  typedef TransformPrismaticTpl<double,0,1> TransformY;
39  typedef TransformPrismaticTpl<double,0,2> TransformZ;
40 
41  typedef SE3::Vector3 Vector3;
42 
43  const double displacement = 0.2;
44  SE3 Mplain, Mrand(SE3::Random());
45 
46  TransformX Mx(displacement);
47  Mplain = Mx;
48  BOOST_CHECK(Mplain.translation().isApprox(Vector3(displacement,0,0)));
49  BOOST_CHECK(Mplain.rotation().isIdentity());
50  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx));
51 
52  TransformY My(displacement);
53  Mplain = My;
54  BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,displacement,0)));
55  BOOST_CHECK(Mplain.rotation().isIdentity());
56  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My));
57 
58  TransformZ Mz(displacement);
59  Mplain = Mz;
60  BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,0,displacement)));
61  BOOST_CHECK(Mplain.rotation().isIdentity());
62  BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz));
63 
64  SE3 M(SE3::Random());
65  Motion v(Motion::Random());
66 
67  MotionPrismaticTpl<double,0,0> mp_x(2.);
68  Motion mp_dense_x(mp_x);
69 
70  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
71  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
72 
73  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
74 
75  MotionPrismaticTpl<double,0,1> mp_y(2.);
76  Motion mp_dense_y(mp_y);
77 
78  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
79  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
80 
81  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
82 
83  MotionPrismaticTpl<double,0,2> mp_z(2.);
84  Motion mp_dense_z(mp_z);
85 
86  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
87  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
88 
89  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
90 }
91 
92 BOOST_AUTO_TEST_CASE( test_kinematics )
93 {
94  using namespace pinocchio;
95 
96 
97  Motion expected_v_J(Motion::Zero());
98  Motion expected_c_J(Motion::Zero());
99 
100  SE3 expected_configuration(SE3::Identity());
101 
102  JointDataPX joint_data;
103  JointModelPX joint_model;
104 
105  joint_model.setIndexes(0, 0, 0);
106 
107  Eigen::VectorXd q(Eigen::VectorXd::Zero(1));
108  Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1));
109 
110  // -------
111  q << 0. ;
112  q_dot << 0.;
113 
114  joint_model.calc(joint_data, q, q_dot);
115 
116  BOOST_CHECK(expected_configuration.rotation().isApprox(joint_data.M.rotation(), 1e-12));
117  BOOST_CHECK(expected_configuration.translation().isApprox(joint_data.M.translation(), 1e-12));
118  BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
119  BOOST_CHECK(expected_c_J.isApprox((Motion) joint_data.c, 1e-12));
120 
121  // -------
122  q << 1.;
123  q_dot << 1.;
124 
125  joint_model.calc(joint_data, q, q_dot);
126 
127  expected_configuration.translation() << 1, 0, 0;
128 
129  expected_v_J.linear() << 1., 0., 0.;
130 
131  BOOST_CHECK(expected_configuration.rotation().isApprox(joint_data.M.rotation(), 1e-12));
132  BOOST_CHECK(expected_configuration.translation().isApprox(joint_data.M.translation(), 1e-12));
133  BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
134  BOOST_CHECK(expected_c_J.isApprox((Motion) joint_data.c, 1e-12));
135 }
136 
137 BOOST_AUTO_TEST_CASE( test_rnea )
138 {
139  using namespace pinocchio;
140  typedef SE3::Vector3 Vector3;
141  typedef SE3::Matrix3 Matrix3;
142 
143  Model model;
144  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
145 
146  addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
147 
148  Data data(model);
149 
150  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
151  Eigen::VectorXd v(Eigen::VectorXd::Zero(model.nv));
152  Eigen::VectorXd a(Eigen::VectorXd::Zero(model.nv));
153 
154  rnea(model, data, q, v, a);
155 
156  Eigen::VectorXd tau_expected(Eigen::VectorXd::Zero(model.nq));
157  tau_expected << 0;
158 
159  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));
160 
161  // -----
162  q = Eigen::VectorXd::Ones(model.nq);
163  v = Eigen::VectorXd::Ones(model.nv);
164  a = Eigen::VectorXd::Ones(model.nv);
165 
166  rnea(model, data, q, v, a);
167  tau_expected << 1;
168 
169  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
170 
171  q << 3;
172  v = Eigen::VectorXd::Ones(model.nv);
173  a = Eigen::VectorXd::Ones(model.nv);
174 
175  rnea(model, data, q, v, a);
176  tau_expected << 1;
177 
178  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
179 }
180 
181 BOOST_AUTO_TEST_CASE( test_crba )
182 {
183  using namespace pinocchio;
184  using namespace std;
185  typedef SE3::Vector3 Vector3;
186  typedef SE3::Matrix3 Matrix3;
187 
188  Model model;
189  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
190 
191  addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
192 
193  Data data(model);
194 
195  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
196  Eigen::MatrixXd M_expected(model.nv,model.nv);
197 
198  crba(model, data, q);
199  M_expected << 1.0;
200 
201  BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));
202 
203  q = Eigen::VectorXd::Ones(model.nq);
204 
205  crba(model, data, q);
206 
207  BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));
208 
209  q << 3;
210 
211  crba(model, data, q);
212 
213  BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
214 }
215 
216 BOOST_AUTO_TEST_SUITE_END()
217 
218 BOOST_AUTO_TEST_SUITE(JointPrismaticUnaligned)
219 
220 BOOST_AUTO_TEST_CASE(spatial)
221 {
222  SE3 M(SE3::Random());
223  Motion v(Motion::Random());
224 
225  MotionPrismaticUnaligned mp(MotionPrismaticUnaligned::Vector3(1.,2.,3.),6.);
226  Motion mp_dense(mp);
227 
228  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
229  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
230 
231  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
232 }
233 
234 BOOST_AUTO_TEST_CASE(vsPX)
235 {
236  using namespace pinocchio;
237  typedef SE3::Vector3 Vector3;
238  typedef SE3::Matrix3 Matrix3;
239 
240  Eigen::Vector3d axis;
241  axis << 1.0, 0.0, 0.0;
242 
243  Model modelPX, modelPrismaticUnaligned;
244 
245  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
246  SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
247 
248  JointModelPrismaticUnaligned joint_model_PU(axis);
249 
250  addJointAndBody(modelPX,JointModelPX(),0,pos,"px",inertia);
251  addJointAndBody(modelPrismaticUnaligned,joint_model_PU,0,pos,"prismatic-unaligned",inertia);
252 
253  Data dataPX(modelPX);
254  Data dataPrismaticUnaligned(modelPrismaticUnaligned);
255 
256  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelPX.nq);
257  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPX.nv);
258  Eigen::VectorXd tauPX = Eigen::VectorXd::Ones(modelPX.nv);
259  Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones(modelPrismaticUnaligned.nv);
260  Eigen::VectorXd aPX = Eigen::VectorXd::Ones(modelPX.nv);
261  Eigen::VectorXd aPrismaticUnaligned(aPX);
262 
263  forwardKinematics(modelPX, dataPX, q, v);
264  forwardKinematics(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
265 
266  computeAllTerms(modelPX, dataPX, q, v);
267  computeAllTerms(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
268 
269  BOOST_CHECK(dataPrismaticUnaligned.oMi[1].isApprox(dataPX.oMi[1]));
270  BOOST_CHECK(dataPrismaticUnaligned.liMi[1].isApprox(dataPX.liMi[1]));
271  BOOST_CHECK(dataPrismaticUnaligned.Ycrb[1].matrix().isApprox(dataPX.Ycrb[1].matrix()));
272  BOOST_CHECK(dataPrismaticUnaligned.f[1].toVector().isApprox(dataPX.f[1].toVector()));
273 
274  BOOST_CHECK(dataPrismaticUnaligned.nle.isApprox(dataPX.nle));
275  BOOST_CHECK(dataPrismaticUnaligned.com[0].isApprox(dataPX.com[0]));
276 
277  // InverseDynamics == rnea
278  tauPX = rnea(modelPX, dataPX, q, v, aPX);
279  tauPrismaticUnaligned = rnea(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v, aPrismaticUnaligned);
280 
281  BOOST_CHECK(tauPX.isApprox(tauPrismaticUnaligned));
282 
283  // ForwardDynamics == aba
284  Eigen::VectorXd aAbaPX = aba(modelPX,dataPX, q, v, tauPX);
285  Eigen::VectorXd aAbaPrismaticUnaligned = aba(modelPrismaticUnaligned,dataPrismaticUnaligned, q, v, tauPrismaticUnaligned);
286 
287  BOOST_CHECK(aAbaPX.isApprox(aAbaPrismaticUnaligned));
288 
289  // crba
290  crba(modelPX, dataPX,q);
291  crba(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
292 
293  BOOST_CHECK(dataPX.M.isApprox(dataPrismaticUnaligned.M));
294 
295  // Jacobian
296  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
297  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
298  computeJointJacobians(modelPX, dataPX, q);
299  computeJointJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
300  getJointJacobian(modelPX, dataPX, 1, LOCAL, jacobianPX);
301  getJointJacobian(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, LOCAL, jacobianPrismaticUnaligned);
302 
303  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
304 }
305 
306 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::JointModelPX
JointModelPrismaticTpl< double, 0, 0 > JointModelPX
Definition: joint-prismatic.hpp:653
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...
axis
axis
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::JointModelBase::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:127
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::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:22
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:746
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(spatial)
Definition: joint-prismatic.cpp:35
pinocchio::MotionBase::cross
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
Definition: motion-base.hpp:87
pinocchio::JointModelPrismaticTpl::calc
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
Definition: joint-prismatic.hpp:602
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.
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
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::MotionTpl::Zero
static MotionTpl Zero()
Definition: motion-tpl.hpp:91
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::SE3Base::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::MotionBase::toVector
ToVectorConstReturnType toVector() const
Definition: motion-base.hpp:37
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::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
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