GCC Code Coverage Report

Directory: ./
File: unittest/joint-helical.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 150 0.0%
Branches: 0 1280 0.0%
Line Branch Exec Source
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()
277