Skip to content

Commit 9f213ec

Browse files
author
ipuch
committed
tests(ongoing)
1 parent b284b0a commit 9f213ec

File tree

7 files changed

+163
-48
lines changed

7 files changed

+163
-48
lines changed

‎include/pinocchio/multibody/joint/joint-ellipsoid.hpp‎

Lines changed: 45 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -196,12 +196,12 @@ namespace pinocchio
196196

197197
data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz;
198198

199-
data.S.matrix() << c1c2, s2, Scalar(0),
200-
-c1s2, c2, Scalar(0),
201-
s1, Scalar(0), Scalar(1),
202-
c1 * (-radius_b * s0 * s1 + radius_c * c0 * c1 * s2), c1 * (radius_a - radius_c * c0 * c2), -radius_b * c1 * s0,
199+
data.S.matrix() << c1 * (-radius_b * s0 * s1 + radius_c * c0 * c1 * s2), c1 * (radius_a - radius_c * c0 * c2), -radius_b * c1 * s0,
203200
-radius_a * s1 * s1 - radius_b * c0 * c1 + radius_c * c0 * c1 * c1 * c2, radius_b * s0 * s1 + radius_c * c0 * c1 * s2, radius_a * s1,
204-
c1 * (radius_a * s1 * s2 + radius_b * c1 * c2 * s0 - radius_c * s0), radius_a * c2 * s1 + radius_b * c1 * s0 * s2 + radius_c * c0 * s1, Scalar(0);
201+
c1 * (radius_a * s1 * s2 + radius_b * c1 * c2 * s0 - radius_c * s0), radius_a * c2 * s1 + radius_b * c1 * s0 * s2 + radius_c * c0 * s1, Scalar(0),
202+
c1c2, s2, Scalar(0),
203+
-c1s2, c2, Scalar(0),
204+
s1, Scalar(0), Scalar(1);
205205
}
206206

207207
template<typename TangentVector>
@@ -240,45 +240,44 @@ namespace pinocchio
240240
Scalar Sdot_11, Sdot_21, Sdot_31, Sdot_41, Sdot_51, Sdot_61;
241241
Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62;
242242
Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63;
243-
244243

245-
// Upper part (angular)
246-
Sdot_11 = - (qdot1 * c2 * s1 + qdot2 * c1 * s2);
247-
Sdot_21 = qdot1 * s1 * s2 - qdot2 * c1 * c2;
248-
Sdot_31 = qdot1 * c1;
244+
// Lower part (angular)
245+
Sdot_41 = - (qdot1 * c2 * s1 + qdot2 * c1 * s2);
246+
Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2;
247+
Sdot_61 = qdot1 * c1;
249248

250-
Sdot_12 = qdot2 * c2;
251-
Sdot_22 = - qdot2 * s2;
252-
Sdot_32 = Scalar(0);
249+
Sdot_42 = qdot2 * c2;
250+
Sdot_52 = - qdot2 * s2;
251+
Sdot_62 = Scalar(0);
253252

254-
Sdot_13 = Scalar(0);
255-
Sdot_23 = Scalar(0);
256-
Sdot_33 = Scalar(0);
253+
Sdot_43 = Scalar(0);
254+
Sdot_53 = Scalar(0);
255+
Sdot_63 = Scalar(0);
257256

258-
// Lower part (linear)
259-
Sdot_41 = - qdot0 *c1 * (radius_b * c0 * s1 + radius_c * c0 * s0 * s2)
257+
// Upper part (linear)
258+
Sdot_11 = - qdot0 *c1 * (radius_b * c0 * s1 + radius_c * c0 * s0 * s2)
260259
+ qdot1 * (-2 * radius_b * c1 * c1 * s0 + radius_b * s0 - 2 * radius_c * c0 * c1 * s1 * s2)
261260
+ qdot2 * - radius_c * c0 * c1 * c1 * s2;
262-
Sdot_51 = qdot0 * c1 * s0 *(radius_b - radius_c * c1 * c2)
261+
Sdot_21 = qdot0 * c1 * s0 *(radius_b - radius_c * c1 * c2)
263262
- qdot1 * s1 * ( 2 * radius_a * c1 - radius_b * c0 + 2 * radius_c * c0 * c1 * c2)
264263
+ qdot2 * radius_c * c0 * c1 * c1 * s2;
265-
Sdot_61 = qdot0 * c0 * c1 *(radius_b * c1 * c2 - radius_c)
264+
Sdot_31 = qdot0 * c0 * c1 *(radius_b * c1 * c2 - radius_c)
266265
+ qdot1 * ( - 2 * radius_a *c1 * c1 * s2 + + radius_a * s2 - 2 * radius_b *c1 * c2 * s0 * s1)
267266
+ qdot2 * c1 * (radius_a * c2 * s1 + radius_b * c1 * s0 * s2);
268267

269-
Sdot_42 = qdot0 * radius_c * c1 * c2 * s0
268+
Sdot_12 = qdot0 * radius_c * c1 * c2 * s0
270269
- qdot1 * s1 * (radius_b * c1 * s0 - radius_c * c0 * s1 * s2)
271270
+ qdot2 * radius_c * c0 * c1 * c2;
272-
Sdot_52 = - qdot0 * (radius_b * c0 * c1 * s2 + radius_c * c1 *s0 * s2)
271+
Sdot_22 = - qdot0 * (radius_b * c0 * c1 * s2 + radius_c * c1 *s0 * s2)
273272
+ qdot1 *(radius_b * c1 * s0 - radius_c * c0 * s1 * s2)
274273
+ qdot2 * radius_c * c0 * c1 * c2;
275-
Sdot_62 = qdot0 * (radius_b * c0 *c1*s2 +radius_c *s0*s1)
274+
Sdot_32 = qdot0 * (radius_b * c0 *c1*s2 +radius_c *s0*s1)
276275
- qdot1 * (- radius_a * c1 * c2 + radius_b * s0 * s1 * s2 + radius_c * c0 * c1)
277276
- qdot2 * (radius_a * s1 * s2 - radius_b * c1 * c2 * s0);
278277

279-
Sdot_43 = radius_b * (-qdot0 * c0 * c1 + qdot1 * s0 * s1);
280-
Sdot_53 = -qdot1 * radius_a * c1;
281-
Sdot_63 = Scalar(0);
278+
Sdot_13 = radius_b * (-qdot0 * c0 * c1 + qdot1 * s0 * s1);
279+
Sdot_23 = -qdot1 * radius_a * c1;
280+
Sdot_33 = Scalar(0);
282281

283282
data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13,
284283
Sdot_21, Sdot_22, Sdot_23,
@@ -320,7 +319,7 @@ namespace pinocchio
320319
ny = -s0 * c1;
321320
nz = c0 * c1;
322321

323-
data.M.translation() << radius_a * nx, radius_b * ny, radius_c * nz;
322+
data.M.translation() << Scalar(0.01) * nx, radius_b * ny, radius_c * nz;
324323

325324
data.S.matrix() << c1c2, s2, Scalar(0),
326325
-c1s2, c2, Scalar(0),
@@ -343,43 +342,43 @@ namespace pinocchio
343342
Scalar Sdot_12, Sdot_22, Sdot_32, Sdot_42, Sdot_52, Sdot_62;
344343
Scalar Sdot_13, Sdot_23, Sdot_33, Sdot_43, Sdot_53, Sdot_63;
345344

346-
// Upper part (angular)
347-
Sdot_11 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2);
348-
Sdot_21 = qdot1 * s1 * s2 - qdot2 * c1 * c2;
349-
Sdot_31 = qdot1 * c1;
345+
// Lower part (angular)
346+
Sdot_41 = -(qdot1 * c2 * s1 + qdot2 * c1 * s2);
347+
Sdot_51 = qdot1 * s1 * s2 - qdot2 * c1 * c2;
348+
Sdot_61 = qdot1 * c1;
350349

351-
Sdot_12 = qdot2 * c2;
352-
Sdot_22 = -qdot2 * s2;
353-
Sdot_32 = Scalar(0);
350+
Sdot_42 = qdot2 * c2;
351+
Sdot_52 = -qdot2 * s2;
352+
Sdot_62 = Scalar(0);
354353

355-
Sdot_13 = Scalar(0);
356-
Sdot_23 = Scalar(0);
357-
Sdot_33 = Scalar(0);
354+
Sdot_43 = Scalar(0);
355+
Sdot_53 = Scalar(0);
356+
Sdot_63 = Scalar(0);
358357

359358
// Lower part (linear)
360-
Sdot_41 = -qdot0 * c1 * (radius_b * c0 * s1 + radius_c * c0 * s0 * s2)
359+
Sdot_11 = -qdot0 * c1 * (radius_b * c0 * s1 + radius_c * c0 * s0 * s2)
361360
+ qdot1 * (-2 * radius_b * c1 * c1 * s0 + radius_b * s0 - 2 * radius_c * c0 * c1 * s1 * s2)
362361
+ qdot2 * -radius_c * c0 * c1 * c1 * s2;
363-
Sdot_51 = qdot0 * c1 * s0 * (radius_b - radius_c * c1 * c2)
362+
Sdot_21 = qdot0 * c1 * s0 * (radius_b - radius_c * c1 * c2)
364363
- qdot1 * s1 * (2 * radius_a * c1 - radius_b * c0 + 2 * radius_c * c0 * c1 * c2)
365364
+ qdot2 * radius_c * c0 * c1 * c1 * s2;
366-
Sdot_61 = qdot0 * c0 * c1 * (radius_b * c1 * c2 - radius_c)
365+
Sdot_31 = qdot0 * c0 * c1 * (radius_b * c1 * c2 - radius_c)
367366
+ qdot1 * (-2 * radius_a * c1 * c1 * s2 + radius_a * s2 - 2 * radius_b * c1 * c2 * s0 * s1)
368367
+ qdot2 * c1 * (radius_a * c2 * s1 + radius_b * c1 * s0 * s2);
369368

370-
Sdot_42 = qdot0 * radius_c * c1 * c2 * s0
369+
Sdot_12 = qdot0 * radius_c * c1 * c2 * s0
371370
- qdot1 * s1 * (radius_b * c1 * s0 - radius_c * c0 * s1 * s2)
372371
+ qdot2 * radius_c * c0 * c1 * c2;
373-
Sdot_52 = -qdot0 * (radius_b * c0 * c1 * s2 + radius_c * c1 * s0 * s2)
372+
Sdot_22 = -qdot0 * (radius_b * c0 * c1 * s2 + radius_c * c1 * s0 * s2)
374373
+ qdot1 * (radius_b * c1 * s0 - radius_c * c0 * s1 * s2)
375374
+ qdot2 * radius_c * c0 * c1 * c2;
376-
Sdot_62 = qdot0 * (radius_b * c0 * c1 * s2 + radius_c * s0 * s1)
375+
Sdot_32 = qdot0 * (radius_b * c0 * c1 * s2 + radius_c * s0 * s1)
377376
- qdot1 * (-radius_a * c1 * c2 + radius_b * s0 * s1 * s2 + radius_c * c0 * c1)
378377
- qdot2 * (radius_a * s1 * s2 - radius_b * c1 * c2 * s0);
379378

380-
Sdot_43 = radius_b * (-qdot0 * c0 * c1 + qdot1 * s0 * s1);
381-
Sdot_53 = -qdot1 * radius_a * c1;
382-
Sdot_63 = Scalar(0);
379+
Sdot_13 = radius_b * (-qdot0 * c0 * c1 + qdot1 * s0 * s1);
380+
Sdot_23 = -qdot1 * radius_a * c1;
381+
Sdot_33 = Scalar(0);
383382

384383
data.Sdot.matrix() << Sdot_11, Sdot_12, Sdot_13,
385384
Sdot_21, Sdot_22, Sdot_23,

‎include/pinocchio/serialization/joints-data.hpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,16 @@ namespace boost
171171
fix::serialize(ar, static_cast<pinocchio::JointDataBase<JointType> &>(joint), version);
172172
}
173173

174+
template<class Archive, typename Scalar, int Options>
175+
void serialize(
176+
Archive & ar,
177+
pinocchio::JointDataEllipsoidTpl<Scalar, Options> & joint,
178+
const unsigned int version)
179+
{
180+
typedef pinocchio::JointDataEllipsoidTpl<Scalar, Options> JointType;
181+
fix::serialize(ar, static_cast<pinocchio::JointDataBase<JointType> &>(joint), version);
182+
}
183+
174184
template<class Archive, typename Scalar, int Options>
175185
void serialize(
176186
Archive & ar,

‎include/pinocchio/serialization/joints-model.hpp‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -205,6 +205,19 @@ namespace boost
205205
fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
206206
}
207207

208+
template<class Archive, typename Scalar, int Options>
209+
void serialize(
210+
Archive & ar,
211+
pinocchio::JointModelEllipsoidTpl<Scalar, Options> & joint,
212+
const unsigned int version)
213+
{
214+
typedef pinocchio::JointModelEllipsoidTpl<Scalar, Options> JointType;
215+
fix::serialize(ar, *static_cast<pinocchio::JointModelBase<JointType> *>(&joint), version);
216+
ar & make_nvp("radius_a", joint.radius_a);
217+
ar & make_nvp("radius_b", joint.radius_b);
218+
ar & make_nvp("radius_c", joint.radius_c);
219+
}
220+
208221
template<class Archive, typename Scalar, int Options>
209222
void serialize(
210223
Archive & ar,

‎unittest/all-joints.cpp‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -152,6 +152,24 @@ struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
152152
}
153153
};
154154

155+
template<typename Scalar, int Options>
156+
struct init<pinocchio::JointModelEllipsoidTpl<Scalar, Options>>
157+
{
158+
typedef pinocchio::JointModelEllipsoidTpl<Scalar, Options> JointModel;
159+
160+
static JointModel run()
161+
{
162+
JointModel jmodel(
163+
static_cast<Scalar>(0.01),
164+
static_cast<Scalar>(0.02),
165+
static_cast<Scalar>(0.03)
166+
);
167+
168+
jmodel.setIndexes(0, 0, 0);
169+
return jmodel;
170+
}
171+
};
172+
155173
template<typename Scalar, int Options>
156174
struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
157175
{

‎unittest/finite-differences.cpp‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -163,6 +163,23 @@ struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
163163
}
164164
};
165165

166+
template<typename Scalar, int Options>
167+
struct init<pinocchio::JointModelEllipsoidTpl<Scalar, Options>>
168+
{
169+
typedef pinocchio::JointModelEllipsoidTpl<Scalar, Options> JointModel;
170+
171+
static JointModel run()
172+
{
173+
JointModel jmodel(Scalar(0.01),
174+
Scalar(0),
175+
Scalar(0)
176+
);
177+
178+
jmodel.setIndexes(0, 0, 0);
179+
return jmodel;
180+
}
181+
};
182+
166183
template<typename Scalar, int Options, template<typename, int> class JointCollection>
167184
struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
168185
{
@@ -236,22 +253,30 @@ struct FiniteDiffJoint
236253
typedef JointDataBase<typename JointModel::JointDataDerived> DataBaseType;
237254
DataBaseType & jdata = static_cast<DataBaseType &>(jdata_);
238255

256+
std::cout << std::setprecision(15);
257+
239258
CV q = LieGroupType().random();
259+
std::cout << "q: " << q << std::endl;
240260
jmodel.calc(jdata.derived(), q);
241261
SE3 M_ref(jdata.M());
242262

243263
CV q_int(q);
264+
std::cout << "q_int: " << q_int << std::endl;
244265
const Eigen::DenseIndex nv = jdata.S().nv();
245266
TV v(nv);
246267
v.setZero();
247268
double eps = 1e-8;
248269

249270
Eigen::Matrix<double, 6, JointModel::NV> S(6, nv), S_ref(jdata.S().matrix());
250271

272+
std::cout << "M_ref: " << M_ref << std::endl;
273+
251274
for (int k = 0; k < nv; ++k)
252275
{
253276
v[k] = eps;
254277
q_int = LieGroupType().integrate(q, v);
278+
std::cout << "k: " << k << std::endl;
279+
std::cout << "q_int: " << q_int << std::endl;
255280
jmodel.calc(jdata.derived(), q_int);
256281
SE3 M_int = jdata.M();
257282

‎unittest/joint-ellipsoid.cpp‎

Lines changed: 35 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,8 @@ BOOST_AUTO_TEST_CASE(vsFreeFlyer)
6161
Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv);
6262

6363
// ForwardDynamics == aba
64-
// Eigen::VectorXd aAbaEllipsoid =
65-
// aba(modelEllipsoid, dataEllipsoid, q, v, tauEllipsoid, Convention::WORLD);
64+
Eigen::VectorXd aAbaEllipsoid =
65+
aba(modelEllipsoid, dataEllipsoid, q, v, tauEllipsoid, Convention::WORLD);
6666

6767
// Calculer jdata.S().transpose() * data.f[i]
6868
}
@@ -96,7 +96,30 @@ BOOST_AUTO_TEST_CASE(vsRandomForce)
9696
// Verify both methods give the same result
9797
BOOST_CHECK(tau1.isApprox(tau2));
9898

99+
Model modelComposite;
100+
101+
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
102+
SE3 pos(1);
103+
pos.translation() = SE3::LinearType(1., 0., 0.);
104+
105+
JointModelComposite jmodel_composite((JointModelRZ()));
106+
jmodel_composite.addJoint(JointModelRY());
107+
jmodel_composite.addJoint(JointModelRX());
108+
99109

110+
addJointAndBody(modelComposite, jmodel_composite, 0, pos, "composite", inertia);
111+
Data dataComposite(modelComposite);
112+
113+
114+
Eigen::VectorXd q = Eigen::VectorXd::Ones(modelComposite.nq);
115+
Eigen::VectorXd v = Eigen::VectorXd::Ones(modelComposite.nv);
116+
117+
forwardKinematics(modelComposite, dataComposite, q, v);
118+
119+
Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelComposite.nv);
120+
121+
// ABA
122+
aba(modelComposite,dataComposite,q,v,tau);
100123

101124
// for later
102125
// addJointAndBody(modelEllipsoid, JointModelEllipsoid(1, 2,3), 0, pos, "ellipsoid", inertia);
@@ -117,3 +140,13 @@ BOOST_AUTO_TEST_CASE(vsRandomForce)
117140
}
118141

119142
BOOST_AUTO_TEST_SUITE_END()
143+
144+
// BEAU TEST
145+
// Comparer spherical zyx avec ellipsoid
146+
// base sur helical et universal
147+
// vsPXRX ça teste les rotations
148+
// {
149+
// TODO
150+
// }
151+
152+
// Test translations

‎unittest/serialization.cpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -349,6 +349,23 @@ struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
349349
}
350350
};
351351

352+
template<typename Scalar, int Options>
353+
struct init<pinocchio::JointModelEllipsoidTpl<Scalar, Options>>
354+
{
355+
typedef pinocchio::JointModelEllipsoidTpl<Scalar, Options> JointModel;
356+
357+
static JointModel run()
358+
{
359+
JointModel jmodel(
360+
static_cast<Scalar>(0.01),
361+
static_cast<Scalar>(0.02),
362+
static_cast<Scalar>(0.03));
363+
364+
jmodel.setIndexes(0, 0, 0);
365+
return jmodel;
366+
}
367+
};
368+
352369
template<typename Scalar, int Options, template<typename, int> class JointCollection>
353370
struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
354371
{

0 commit comments

Comments
 (0)