joint-helical.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2022 CNRS INRIA
3 //
4 
5 #include "pinocchio/math/fwd.hpp"
6 #include "pinocchio/multibody/joint/joints.hpp"
7 #include "pinocchio/algorithm/rnea.hpp"
8 #include "pinocchio/algorithm/aba.hpp"
9 #include "pinocchio/algorithm/crba.hpp"
10 #include "pinocchio/algorithm/jacobian.hpp"
11 #include "pinocchio/algorithm/compute-all-terms.hpp"
12 
13 #include <boost/test/unit_test.hpp>
14 
15 using namespace pinocchio;
16 
17 template<typename D>
18 void addJointAndBody(
19  Model & model,
20  const JointModelBase<D> & jmodel,
21  const Model::JointIndex parent_id,
22  const SE3 & joint_placement,
23  const std::string & joint_name,
24  const Inertia & Y)
25 {
26  Model::JointIndex idx;
27 
28  idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
29  model.appendBodyToJoint(idx, Y);
30 }
31 
32 BOOST_AUTO_TEST_SUITE(JointHelical)
33 
34 BOOST_AUTO_TEST_CASE(vsPXRX)
35 {
36  typedef SE3::Vector3 Vector3;
37  typedef SE3::Matrix3 Matrix3;
38 
39  Model modelHX, modelPXRX;
40 
41  Inertia inertia(1., Vector3(0., 0., 0.0), Matrix3::Identity());
42  // Important to have the same mass for both systems, otherwise COM position not the same
43  Inertia inertia_zero_mass(0., Vector3(0.0, 0.0, 0.0), Matrix3::Identity());
44  const double h = 0.4;
45 
46  JointModelHX joint_model_HX(h);
47  addJointAndBody(modelHX, joint_model_HX, 0, SE3::Identity(), "helical x", inertia);
48 
49  JointModelPX joint_model_PX;
50  JointModelRX joint_model_RX;
51  addJointAndBody(modelPXRX, joint_model_PX, 0, SE3::Identity(), "prismatic x", inertia);
52  addJointAndBody(modelPXRX, joint_model_RX, 1, SE3::Identity(), "revolute x", inertia_zero_mass);
53 
54  Data dataHX(modelHX);
55  Data dataPXRX(modelPXRX);
56 
57  // Set the prismatic joint to corresponding displacement, velocit and acceleration
58  Eigen::VectorXd q_hx = Eigen::VectorXd::Ones(modelHX.nq); // dim 1
59  Eigen::VectorXd q_PXRX = Eigen::VectorXd::Ones(modelPXRX.nq); // dim 2
60  q_PXRX(0) = q_hx(0) * h;
61 
62  Eigen::VectorXd v_hx = Eigen::VectorXd::Ones(modelHX.nv);
63  Eigen::VectorXd v_PXRX = Eigen::VectorXd::Ones(modelPXRX.nv);
64  v_PXRX(0) = v_hx(0) * h;
65 
66  Eigen::VectorXd tauHX = Eigen::VectorXd::Ones(modelHX.nv);
67  Eigen::VectorXd tauPXRX = Eigen::VectorXd::Ones(modelPXRX.nv);
68  Eigen::VectorXd aHX = Eigen::VectorXd::Ones(modelHX.nv);
69  Eigen::VectorXd aPXRX = Eigen::VectorXd::Ones(modelPXRX.nv);
70  aPXRX(0) = aHX(0) * h * h;
71 
72  forwardKinematics(modelHX, dataHX, q_hx, v_hx);
73  forwardKinematics(modelPXRX, dataPXRX, q_PXRX, v_PXRX);
74 
75  computeAllTerms(modelHX, dataHX, q_hx, v_hx);
76  computeAllTerms(modelPXRX, dataPXRX, q_PXRX, v_PXRX);
77 
78  BOOST_CHECK(dataPXRX.oMi[2].isApprox(dataHX.oMi[1]));
79  BOOST_CHECK((dataPXRX.liMi[2] * dataPXRX.liMi[1]).isApprox(dataHX.liMi[1]));
80  BOOST_CHECK(dataPXRX.Ycrb[2].matrix().isApprox(dataHX.Ycrb[1].matrix()));
81  BOOST_CHECK((dataPXRX.liMi[2].actInv(dataPXRX.f[1])).toVector().isApprox(dataHX.f[1].toVector()));
82  BOOST_CHECK(
83  (Eigen::Matrix<double, 1, 1>(dataPXRX.nle.dot(Eigen::VectorXd::Ones(2)))).isApprox(dataHX.nle));
84  BOOST_CHECK(dataPXRX.com[0].isApprox(dataHX.com[0]));
85 
86  // InverseDynamics == rnea
87  tauHX = rnea(modelHX, dataHX, q_hx, v_hx, aHX);
88  tauPXRX = rnea(modelPXRX, dataPXRX, q_PXRX, v_PXRX, aPXRX);
89  BOOST_CHECK(tauHX.isApprox(Eigen::Matrix<double, 1, 1>(tauPXRX.dot(Eigen::VectorXd::Ones(2)))));
90 
91  // ForwardDynamics == aba
92  Eigen::VectorXd aAbaHX = aba(modelHX, dataHX, q_hx, v_hx, tauHX, Convention::WORLD);
93  Eigen::VectorXd aAbaPXRX = aba(modelPXRX, dataPXRX, q_PXRX, v_PXRX, tauPXRX, Convention::WORLD);
94 
95  BOOST_CHECK(aAbaHX.isApprox(aHX));
96  BOOST_CHECK(aAbaPXRX.isApprox(aPXRX));
97  BOOST_CHECK(aAbaPXRX.isApprox(Eigen::Matrix<double, 2, 1>(aHX(0) * h * h, aHX(0))));
98 
99  aAbaHX = aba(modelHX, dataHX, q_hx, v_hx, tauHX, Convention::LOCAL);
100  aAbaPXRX = aba(modelPXRX, dataPXRX, q_PXRX, v_PXRX, tauPXRX, Convention::LOCAL);
101 
102  BOOST_CHECK(aAbaHX.isApprox(aHX));
103  BOOST_CHECK(aAbaPXRX.isApprox(aPXRX));
104  BOOST_CHECK(aAbaPXRX.isApprox(Eigen::Matrix<double, 2, 1>(aHX(0) * h * h, aHX(0))));
105 
106  // crba
107  crba(modelHX, dataHX, q_hx, Convention::WORLD);
108  crba(modelPXRX, dataPXRX, q_PXRX, Convention::WORLD);
109 
110  tauHX = dataHX.M * aHX;
111  tauPXRX = dataPXRX.M * aPXRX;
112 
113  BOOST_CHECK(tauHX.isApprox(Eigen::Matrix<double, 1, 1>(tauPXRX.dot(Eigen::VectorXd::Ones(2)))));
114 
115  crba(modelHX, dataHX, q_hx, Convention::LOCAL);
116  crba(modelPXRX, dataPXRX, q_PXRX, Convention::LOCAL);
117 
118  tauHX = dataHX.M * aHX;
119  tauPXRX = dataPXRX.M * aPXRX;
120 
121  BOOST_CHECK(tauHX.isApprox(Eigen::Matrix<double, 1, 1>(tauPXRX.dot(Eigen::VectorXd::Ones(2)))));
122 
123  // Jacobian
124  computeJointJacobians(modelHX, dataHX, q_hx);
125  computeJointJacobians(modelPXRX, dataPXRX, q_PXRX);
126  Eigen::VectorXd v_body_hx = dataHX.J * v_hx;
127  Eigen::VectorXd v_body_PXRX = dataPXRX.J * v_PXRX;
128  BOOST_CHECK(v_body_hx.isApprox(v_body_PXRX));
129 }
130 
131 BOOST_AUTO_TEST_CASE(spatial)
132 {
133  typedef TransformHelicalTpl<double, 0, 0> TransformX;
134  typedef TransformHelicalTpl<double, 0, 1> TransformY;
135  typedef TransformHelicalTpl<double, 0, 2> TransformZ;
136 
137  typedef SE3::Vector3 Vector3;
138 
139  const double alpha = 0.2, h = 0.1;
140  double sin_alpha, cos_alpha;
141  SINCOS(alpha, &sin_alpha, &cos_alpha);
142  SE3 Mplain, Mrand(SE3::Random());
143 
144  TransformX Mx(sin_alpha, cos_alpha, alpha * h);
145  Mplain = Mx;
146  BOOST_CHECK(Mplain.translation().isApprox(Vector3::UnitX() * alpha * h));
147  BOOST_CHECK(
148  Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitX()).toRotationMatrix()));
149  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mx));
150 
151  TransformY My(sin_alpha, cos_alpha, alpha * h);
152  Mplain = My;
153  BOOST_CHECK(Mplain.translation().isApprox(Vector3::UnitY() * alpha * h));
154  BOOST_CHECK(
155  Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitY()).toRotationMatrix()));
156  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * My));
157 
158  TransformZ Mz(sin_alpha, cos_alpha, alpha * h);
159  Mplain = Mz;
160  BOOST_CHECK(Mplain.translation().isApprox(Vector3::UnitZ() * alpha * h));
161  BOOST_CHECK(
162  Mplain.rotation().isApprox(Eigen::AngleAxisd(alpha, Vector3::UnitZ()).toRotationMatrix()));
163  BOOST_CHECK((Mrand * Mplain).isApprox(Mrand * Mz));
164 
165  SE3 M(SE3::Random());
166  Motion v(Motion::Random());
167 
168  MotionHelicalTpl<double, 0, 0> mh_x(2., h);
169  Motion mh_dense_x(mh_x);
170 
171  BOOST_CHECK(M.act(mh_x).isApprox(M.act(mh_dense_x)));
172  BOOST_CHECK(M.actInv(mh_x).isApprox(M.actInv(mh_dense_x)));
173 
174  BOOST_CHECK(v.cross(mh_x).isApprox(v.cross(mh_dense_x)));
175 
176  MotionHelicalTpl<double, 0, 1> mh_y(2., h);
177  Motion mh_dense_y(mh_y);
178 
179  BOOST_CHECK(M.act(mh_y).isApprox(M.act(mh_dense_y)));
180  BOOST_CHECK(M.actInv(mh_y).isApprox(M.actInv(mh_dense_y)));
181 
182  BOOST_CHECK(v.cross(mh_y).isApprox(v.cross(mh_dense_y)));
183 
184  MotionHelicalTpl<double, 0, 2> mh_z(2., h);
185  Motion mh_dense_z(mh_z);
186 
187  BOOST_CHECK(M.act(mh_z).isApprox(M.act(mh_dense_z)));
188  BOOST_CHECK(M.actInv(mh_z).isApprox(M.actInv(mh_dense_z)));
189 
190  BOOST_CHECK(v.cross(mh_z).isApprox(v.cross(mh_dense_z)));
191 }
192 BOOST_AUTO_TEST_SUITE_END()
193 
194 BOOST_AUTO_TEST_SUITE(JointHelicalUnaligned)
195 
196 BOOST_AUTO_TEST_CASE(vsHX)
197 {
198  using namespace pinocchio;
199  typedef SE3::Vector3 Vector3;
200  typedef SE3::Matrix3 Matrix3;
201 
202  Vector3 axis;
203  axis << 1.0, 0.0, 0.0;
204  const double h = 0.2;
205 
206  Model modelHX, modelHelicalUnaligned;
207 
208  Inertia inertia(1., Vector3(0.0, 0., 0.0), Matrix3::Identity());
209  SE3 pos(1);
210  pos.translation() = SE3::LinearType(1., 0., 0.);
211 
212  JointModelHelicalUnaligned joint_model_HU(axis, h);
213 
214  addJointAndBody(modelHX, JointModelHX(h), 0, pos, "HX", inertia);
215  addJointAndBody(modelHelicalUnaligned, joint_model_HU, 0, pos, "Helical-unaligned", inertia);
216 
217  Data dataHX(modelHX);
218  Data dataHelicalUnaligned(modelHelicalUnaligned);
219 
220  Eigen::VectorXd q = 3 * Eigen::VectorXd::Ones(modelHX.nq);
221  Eigen::VectorXd v = 30 * Eigen::VectorXd::Ones(modelHX.nv);
222  Eigen::VectorXd tauHX = Eigen::VectorXd::Ones(modelHX.nv);
223  Eigen::VectorXd tauHelicalUnaligned = Eigen::VectorXd::Ones(modelHelicalUnaligned.nv);
224  Eigen::VectorXd aHX = 5 * Eigen::VectorXd::Ones(modelHX.nv);
225  Eigen::VectorXd aHelicalUnaligned(aHX);
226 
227  forwardKinematics(modelHX, dataHX, q, v);
228  forwardKinematics(modelHelicalUnaligned, dataHelicalUnaligned, q, v);
229 
230  computeAllTerms(modelHX, dataHX, q, v);
231  computeAllTerms(modelHelicalUnaligned, dataHelicalUnaligned, q, v);
232 
233  BOOST_CHECK(dataHelicalUnaligned.oMi[1].isApprox(dataHX.oMi[1]));
234  BOOST_CHECK(dataHelicalUnaligned.liMi[1].isApprox(dataHX.liMi[1]));
235  BOOST_CHECK(dataHelicalUnaligned.Ycrb[1].matrix().isApprox(dataHX.Ycrb[1].matrix()));
236  BOOST_CHECK(dataHelicalUnaligned.f[1].toVector().isApprox(dataHX.f[1].toVector()));
237 
238  BOOST_CHECK(dataHelicalUnaligned.nle.isApprox(dataHX.nle));
239  BOOST_CHECK(dataHelicalUnaligned.com[0].isApprox(dataHX.com[0]));
240 
241  // InverseDynamics == rnea
242  tauHX = rnea(modelHX, dataHX, q, v, aHX);
243  tauHelicalUnaligned = rnea(modelHelicalUnaligned, dataHelicalUnaligned, q, v, aHelicalUnaligned);
244 
245  BOOST_CHECK(tauHX.isApprox(tauHelicalUnaligned));
246 
247  // ForwardDynamics == aba
248  Eigen::VectorXd aAbaHX = aba(modelHX, dataHX, q, v, tauHX, Convention::WORLD);
249  Eigen::VectorXd aAbaHelicalUnaligned =
250  aba(modelHelicalUnaligned, dataHelicalUnaligned, q, v, tauHelicalUnaligned, Convention::WORLD);
251 
252  BOOST_CHECK(aAbaHX.isApprox(aAbaHelicalUnaligned));
253 
254  // crba
255  crba(modelHX, dataHX, q, Convention::WORLD);
256  crba(modelHelicalUnaligned, dataHelicalUnaligned, q, Convention::WORLD);
257 
258  BOOST_CHECK(dataHX.M.isApprox(dataHelicalUnaligned.M));
259 
260  // Jacobian
261  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;
262  jacobianPX.resize(6, 1);
263  jacobianPX.setZero();
264  Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;
265  jacobianPrismaticUnaligned.resize(6, 1);
266  jacobianPrismaticUnaligned.setZero();
267  computeJointJacobians(modelHX, dataHX, q);
268  computeJointJacobians(modelHelicalUnaligned, dataHelicalUnaligned, q);
269  getJointJacobian(modelHX, dataHX, 1, LOCAL, jacobianPX);
270  getJointJacobian(
271  modelHelicalUnaligned, dataHelicalUnaligned, 1, LOCAL, jacobianPrismaticUnaligned);
272 
273  BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
274 }
275 
276 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::JointModelHelicalTpl
Definition: multibody/joint/fwd.hpp:60
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::JointModelHX
JointModelHelicalTpl< context::Scalar, context::Options, 0 > JointModelHX
Definition: joint-helical.hpp:960
pinocchio::JointModelBase
Definition: joint-model-base.hpp:75
pinocchio::SE3Tpl< context::Scalar, context::Options >
pinocchio::Convention::WORLD
@ WORLD
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....
pinocchio::SINCOS
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
rnea.hpp
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:28
pinocchio::JointModelPrismaticTpl
Definition: multibody/joint/fwd.hpp:89
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
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::TransformHelicalTpl
Definition: joint-helical.hpp:63
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
pinocchio::JointModelHelicalUnalignedTpl
Definition: multibody/joint/fwd.hpp:65
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::JointModelRevoluteTpl
Definition: multibody/joint/fwd.hpp:33
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(vsPXRX)
Definition: joint-helical.cpp:34
ur5x4.h
h
Definition: ur5x4.py:50
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:360
append-urdf-model-with-another-model.joint_name
string joint_name
Definition: append-urdf-model-with-another-model.py:33
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: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...
pinocchio::Convention::LOCAL
@ LOCAL
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::MotionHelicalTpl
Definition: joint-helical.hpp:19
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 Sun Dec 22 2024 03:41:10