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(
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(JointPrismatic)
35 
36 BOOST_AUTO_TEST_CASE(spatial)
37 {
38  typedef TransformPrismaticTpl<double, 0, 0> TransformX;
39  typedef TransformPrismaticTpl<double, 0, 1> TransformY;
40  typedef TransformPrismaticTpl<double, 0, 2> TransformZ;
41 
42  typedef SE3::Vector3 Vector3;
43 
44  const double displacement = 0.2;
45  SE3 Mplain, Mrand(SE3::Random());
46 
47  TransformX Mx(displacement);
48  Mplain = Mx;
49  BOOST_CHECK(Mplain.translation().isApprox(Vector3(displacement, 0, 0)));
50  BOOST_CHECK(Mplain.rotation().isIdentity());
51  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mx));
52 
53  TransformY My(displacement);
54  Mplain = My;
55  BOOST_CHECK(Mplain.translation().isApprox(Vector3(0, displacement, 0)));
56  BOOST_CHECK(Mplain.rotation().isIdentity());
57  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * My));
58 
59  TransformZ Mz(displacement);
60  Mplain = Mz;
61  BOOST_CHECK(Mplain.translation().isApprox(Vector3(0, 0, displacement)));
62  BOOST_CHECK(Mplain.rotation().isIdentity());
63  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mz));
64 
65  SE3 M(SE3::Random());
66  Motion v(Motion::Random());
67 
68  MotionPrismaticTpl<double, 0, 0> mp_x(2.);
69  Motion mp_dense_x(mp_x);
70 
71  BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
72  BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
73 
74  BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
75 
76  MotionPrismaticTpl<double, 0, 1> mp_y(2.);
77  Motion mp_dense_y(mp_y);
78 
79  BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
80  BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
81 
82  BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
83 
84  MotionPrismaticTpl<double, 0, 2> mp_z(2.);
85  Motion mp_dense_z(mp_z);
86 
87  BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
88  BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
89 
90  BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
91 }
92 
93 BOOST_AUTO_TEST_CASE(test_kinematics)
94 {
95  using namespace pinocchio;
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(
147  model, JointModelPX(), model.getJointId("universe"), SE3::Identity(), "root", inertia);
148 
149  Data data(model);
150 
151  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
152  Eigen::VectorXd v(Eigen::VectorXd::Zero(model.nv));
153  Eigen::VectorXd a(Eigen::VectorXd::Zero(model.nv));
154 
155  rnea(model, data, q, v, a);
156 
157  Eigen::VectorXd tau_expected(Eigen::VectorXd::Zero(model.nq));
158  tau_expected << 0;
159 
160  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));
161 
162  // -----
163  q = Eigen::VectorXd::Ones(model.nq);
164  v = Eigen::VectorXd::Ones(model.nv);
165  a = Eigen::VectorXd::Ones(model.nv);
166 
167  rnea(model, data, q, v, a);
168  tau_expected << 1;
169 
170  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
171 
172  q << 3;
173  v = Eigen::VectorXd::Ones(model.nv);
174  a = Eigen::VectorXd::Ones(model.nv);
175 
176  rnea(model, data, q, v, a);
177  tau_expected << 1;
178 
179  BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
180 }
181 
182 BOOST_AUTO_TEST_CASE(test_crba)
183 {
184  using namespace pinocchio;
185  using namespace std;
186  typedef SE3::Vector3 Vector3;
187  typedef SE3::Matrix3 Matrix3;
188 
189  Model model;
190  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
191 
192  addJointAndBody(
193  model, JointModelPX(), model.getJointId("universe"), SE3::Identity(), "root", inertia);
194 
195  Data data(model);
196 
197  Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
198  Eigen::MatrixXd M_expected(model.nv, model.nv);
199 
200  crba(model, data, q, Convention::WORLD);
201  M_expected << 1.0;
202 
203  BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));
204 
205  q = Eigen::VectorXd::Ones(model.nq);
206 
207  crba(model, data, q, Convention::WORLD);
208 
209  BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));
210 
211  q << 3;
212 
213  crba(model, data, q, Convention::WORLD);
214 
215  BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
216 }
217 
218 BOOST_AUTO_TEST_SUITE_END()
219 
220 BOOST_AUTO_TEST_SUITE(JointPrismaticUnaligned)
221 
222 BOOST_AUTO_TEST_CASE(spatial)
223 {
224  SE3 M(SE3::Random());
225  Motion v(Motion::Random());
226 
227  MotionPrismaticUnaligned mp(MotionPrismaticUnaligned::Vector3(1., 2., 3.), 6.);
228  Motion mp_dense(mp);
229 
230  BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
231  BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
232 
233  BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
234 }
235 
236 BOOST_AUTO_TEST_CASE(vsPX)
237 {
238  using namespace pinocchio;
239  typedef SE3::Vector3 Vector3;
240  typedef SE3::Matrix3 Matrix3;
241 
242  Eigen::Vector3d axis;
243  axis << 1.0, 0.0, 0.0;
244 
245  Model modelPX, modelPrismaticUnaligned;
246 
247  Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
248  SE3 pos(1);
249  pos.translation() = SE3::LinearType(1., 0., 0.);
250 
251  JointModelPrismaticUnaligned joint_model_PU(axis);
252 
253  addJointAndBody(modelPX, JointModelPX(), 0, pos, "px", inertia);
254  addJointAndBody(modelPrismaticUnaligned, joint_model_PU, 0, pos, "prismatic-unaligned", inertia);
255 
256  Data dataPX(modelPX);
257  Data dataPrismaticUnaligned(modelPrismaticUnaligned);
258 
259  Eigen::VectorXd q = Eigen::VectorXd::Ones(modelPX.nq);
260  Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPX.nv);
261  Eigen::VectorXd tauPX = Eigen::VectorXd::Ones(modelPX.nv);
262  Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones(modelPrismaticUnaligned.nv);
263  Eigen::VectorXd aPX = Eigen::VectorXd::Ones(modelPX.nv);
264  Eigen::VectorXd aPrismaticUnaligned(aPX);
265 
266  forwardKinematics(modelPX, dataPX, q, v);
267  forwardKinematics(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
268 
269  computeAllTerms(modelPX, dataPX, q, v);
270  computeAllTerms(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
271 
272  BOOST_CHECK(dataPrismaticUnaligned.oMi[1].isApprox(dataPX.oMi[1]));
273  BOOST_CHECK(dataPrismaticUnaligned.liMi[1].isApprox(dataPX.liMi[1]));
274  BOOST_CHECK(dataPrismaticUnaligned.Ycrb[1].matrix().isApprox(dataPX.Ycrb[1].matrix()));
275  BOOST_CHECK(dataPrismaticUnaligned.f[1].toVector().isApprox(dataPX.f[1].toVector()));
276 
277  BOOST_CHECK(dataPrismaticUnaligned.nle.isApprox(dataPX.nle));
278  BOOST_CHECK(dataPrismaticUnaligned.com[0].isApprox(dataPX.com[0]));
279 
280  // InverseDynamics == rnea
281  tauPX = rnea(modelPX, dataPX, q, v, aPX);
282  tauPrismaticUnaligned =
283  rnea(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v, aPrismaticUnaligned);
284 
285  BOOST_CHECK(tauPX.isApprox(tauPrismaticUnaligned));
286 
287  // ForwardDynamics == aba
288  Eigen::VectorXd aAbaPX = aba(modelPX, dataPX, q, v, tauPX, Convention::WORLD);
289  Eigen::VectorXd aAbaPrismaticUnaligned = aba(
290  modelPrismaticUnaligned, dataPrismaticUnaligned, q, v, tauPrismaticUnaligned,
291  Convention::WORLD);
292 
293  BOOST_CHECK(aAbaPX.isApprox(aAbaPrismaticUnaligned));
294 
295  // crba
296  crba(modelPX, dataPX, q, Convention::WORLD);
297  crba(modelPrismaticUnaligned, dataPrismaticUnaligned, q, Convention::WORLD);
298 
299  BOOST_CHECK(dataPX.M.isApprox(dataPrismaticUnaligned.M));
300 
301  // Jacobian
302  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;
303  jacobianPX.resize(6, 1);
304  jacobianPX.setZero();
305  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;
306  jacobianPrismaticUnaligned.resize(6, 1);
307  jacobianPrismaticUnaligned.setZero();
308  computeJointJacobians(modelPX, dataPX, q);
309  computeJointJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
310  getJointJacobian(modelPX, dataPX, 1, LOCAL, jacobianPX);
311  getJointJacobian(
312  modelPrismaticUnaligned, dataPrismaticUnaligned, 1, LOCAL, jacobianPrismaticUnaligned);
313 
314  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
315 }
316 
317 BOOST_AUTO_TEST_SUITE_END()
pinocchio::InertiaTpl
Definition: spatial/fwd.hpp:58
pinocchio::MotionPrismaticUnalignedTpl
Definition: joint-prismatic-unaligned.hpp:20
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:199
pinocchio::TransformPrismaticTpl
Definition: joint-prismatic.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::MotionPrismaticTpl
Definition: joint-prismatic.hpp:20
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::JointDataPrismaticTpl::v
Motion_t v
Definition: joint-prismatic.hpp:626
pinocchio::JointDataPrismaticTpl
Definition: multibody/joint/fwd.hpp:91
pinocchio::SE3Tpl< context::Scalar, context::Options >
pinocchio::JointDataPrismaticTpl::M
Transformation_t M
Definition: joint-prismatic.hpp:625
pinocchio::Convention::WORLD
@ WORLD
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::JointModelPrismaticTpl::calc
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
Definition: joint-prismatic.hpp:696
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
pinocchio::JointDataPrismaticTpl::c
Bias_t c
Definition: joint-prismatic.hpp:627
aba.hpp
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix3
traits< SE3Tpl >::Matrix3 Matrix3
Definition: spatial/se3-tpl.hpp:56
pinocchio::SE3Tpl::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:52
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
append-urdf-model-with-another-model.parent_id
int parent_id
Definition: append-urdf-model-with-another-model.py:27
pinocchio::JointModelPrismaticTpl
Definition: multibody/joint/fwd.hpp:89
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...
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
fwd.hpp
pos
pos
M
M
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
axis
axis
pinocchio::MotionTpl::Random
static MotionTpl Random()
Definition: motion-tpl.hpp:140
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(spatial)
Definition: joint-prismatic.cpp:36
pinocchio::JointModelPrismaticUnalignedTpl
Definition: multibody/joint/fwd.hpp:94
append-urdf-model-with-another-model.joint_name
string joint_name
Definition: append-urdf-model-with-another-model.py:32
pinocchio::SE3Tpl::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:28
pinocchio::JointModelPrismaticTpl::setIndexes
void setIndexes(JointIndex id, int q, int v)
Definition: joint-model-base.hpp:186
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::MotionTpl::Zero
static MotionTpl Zero()
Definition: motion-tpl.hpp:136
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: multibody/model.hpp:101
pinocchio::SE3Tpl::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
Y
Y
joints.hpp
pinocchio::JointModelPX
JointModelPrismaticTpl< context::Scalar, context::Options, 0 > JointModelPX
Definition: joint-prismatic.hpp:777
q_dot
#define q_dot
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:98
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
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
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Fri Nov 1 2024 02:41:46