Skip to content

Commit d9b49c9

Browse files
author
ipuch
committed
tests(joint-ellipsoid): add composite test
delete extra commented test
1 parent bc79b2d commit d9b49c9

File tree

1 file changed

+102
-190
lines changed

1 file changed

+102
-190
lines changed

‎unittest/joint-ellipsoid.cpp‎

Lines changed: 102 additions & 190 deletions
Original file line numberDiff line numberDiff line change
@@ -38,185 +38,6 @@ void addJointAndBody(
3838

3939
BOOST_AUTO_TEST_SUITE(JointEllipsoid)
4040

41-
BOOST_AUTO_TEST_CASE(vsFreeFlyer)
42-
{
43-
using namespace pinocchio;
44-
typedef SE3::Vector3 Vector3;
45-
typedef Eigen::Matrix<double, 6, 1> Vector6;
46-
typedef Eigen::Matrix<double, 7, 1> VectorFF;
47-
typedef SE3::Matrix3 Matrix3;
48-
49-
Model modelEllipsoid, modelFreeflyer;
50-
51-
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
52-
SE3 pos(1);
53-
pos.translation() = SE3::LinearType(1., 0., 0.);
54-
55-
addJointAndBody(modelEllipsoid, JointModelEllipsoid(1, 2, 3), 0, pos, "ellipsoid", inertia);
56-
addJointAndBody(modelFreeflyer, JointModelFreeFlyer(), 0, pos, "free-flyer", inertia);
57-
58-
Data dataEllipsoid(modelEllipsoid);
59-
60-
Eigen::VectorXd q = Eigen::VectorXd::Ones(modelEllipsoid.nq);
61-
Eigen::VectorXd v = Eigen::VectorXd::Ones(modelEllipsoid.nv);
62-
63-
forwardKinematics(modelEllipsoid, dataEllipsoid, q, v);
64-
65-
Eigen::VectorXd tauEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv);
66-
67-
Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv);
68-
69-
// ForwardDynamics == aba
70-
Eigen::VectorXd aAbaEllipsoid =
71-
aba(modelEllipsoid, dataEllipsoid, q, v, tauEllipsoid, Convention::WORLD);
72-
73-
// Calculer jdata.S().transpose() * data.f[i]
74-
}
75-
76-
// BOOST_AUTO_TEST_CASE(vsSphericalZYX)
77-
// {
78-
// using namespace pinocchio;
79-
// typedef SE3::Vector3 Vector3;
80-
// typedef Eigen::Matrix<double, 6, 1> Vector6;
81-
// typedef SE3::Matrix3 Matrix3;
82-
83-
// // Build models using ModelGraph to enable configuration conversion
84-
// graph::ModelGraph g;
85-
// Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
86-
// SE3 pos(1);
87-
// pos.translation() = SE3::LinearType(1., 0., 0.);
88-
89-
// g.addBody("root_body", inertia);
90-
// g.addBody("end_body", inertia);
91-
92-
// // Create SphericalZYX joint (uses ZYX Euler angles)
93-
// g.addJoint(
94-
// "spherical_joint",
95-
// graph::JointSphericalZYX(),
96-
// "root_body",
97-
// SE3::Identity(),
98-
// "end_body",
99-
// pos);
100-
101-
// // Build SphericalZYX model
102-
// const auto forward_build = graph::buildModelWithBuildInfo(g, "root_body", SE3::Identity());
103-
// const Model & modelSphericalZYX = forward_build.model;
104-
// Data dataSphericalZYX(modelSphericalZYX);
105-
106-
// Eigen::VectorXd q_zyx(3);
107-
// q_zyx << 0.5, 1.2, -0.8;
108-
// Eigen::VectorXd v_zyx(3);
109-
// v_zyx << 0.1, -0.3, 0.7;
110-
// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx);
111-
// updateFramePlacements(modelSphericalZYX, dataSphericalZYX);
112-
113-
// auto end_index = modelSphericalZYX.getFrameId("end_body", BODY);
114-
// auto X_end = dataSphericalZYX.oMf[end_index];
115-
// const auto backward_build =
116-
// graph::buildModelWithBuildInfo(g, "end_body", X_end);
117-
118-
// const Model & XYZlacirehpSledom = backward_build.model;
119-
120-
// // Create converter from SphericalZYX to Ellipsoid
121-
// auto converter = graph::createConverter(
122-
// modelSphericalZYX, XYZlacirehpSledom, forward_build.build_info, backward_build.build_info);
123-
124-
// // Convert to Ellipsoid configuration
125-
// Eigen::VectorXd q_xyz = Eigen::VectorXd::Zero(XYZlacirehpSledom.nq);
126-
// Eigen::VectorXd v_xyz = Eigen::VectorXd::Zero(XYZlacirehpSledom.nv);
127-
// converter.convertConfigurationVector(q_zyx, q_xyz);
128-
// converter.convertTangentVector(q_zyx, v_zyx, v_xyz);
129-
130-
// std::cout << "\n=== Configuration Conversion ===" << std::endl;
131-
// std::cout << "q_zyx (SphericalZYX): " << q_zyx.transpose() << std::endl;
132-
// std::cout << "q_xyz (converted): " << q_xyz.transpose() << std::endl;
133-
// std::cout << "v_zyx (SphericalZYX): " << v_zyx.transpose() << std::endl;
134-
// std::cout << "v_xyz (converted): " << v_xyz.transpose() << std::endl;
135-
136-
// Model modelEllipsoid;
137-
// addJointAndBody(modelEllipsoid, JointModelEllipsoid(0, 0, 0), 0, pos, "ellipsoid", inertia);
138-
// Data dataEllipsoid(modelEllipsoid);
139-
140-
// // Test forward kinematics with converted configurations
141-
// forwardKinematics(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz);
142-
// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx);
143-
144-
// // Check that the transformations are identical
145-
// std::cout << "\n=== Forward Kinematics Comparison ===" << std::endl;
146-
// std::cout << "oMi (Ellipsoid):\n" << dataEllipsoid.oMi[1] << std::endl;
147-
// std::cout << "oMi (SphericalZYX):\n" << dataSphericalZYX.oMi[1] << std::endl;
148-
// BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
149-
150-
// BOOST_CHECK(dataEllipsoid.liMi[1].isApprox(dataSphericalZYX.liMi[1]));
151-
152-
// // Check that velocities match
153-
// BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector()));
154-
155-
// // Test computeAllTerms
156-
// computeAllTerms(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz);
157-
// computeAllTerms(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx);
158-
159-
// BOOST_CHECK(dataEllipsoid.Ycrb[1].matrix().isApprox(dataSphericalZYX.Ycrb[1].matrix()));
160-
161-
// BOOST_CHECK(dataEllipsoid.f[1].toVector().isApprox(dataSphericalZYX.f[1].toVector()));
162-
163-
// BOOST_CHECK(dataEllipsoid.nle.isApprox(dataSphericalZYX.nle));
164-
165-
// BOOST_CHECK(dataEllipsoid.com[0].isApprox(dataSphericalZYX.com[0]));
166-
167-
// // Test inverse dynamics (RNEA)
168-
// Eigen::VectorXd aEllipsoid = Eigen::VectorXd::Ones(modelEllipsoid.nv);
169-
// Eigen::VectorXd aSphericalZYX = Eigen::VectorXd::Ones(modelSphericalZYX.nv);
170-
171-
// Eigen::VectorXd tauEllipsoid = rnea(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, aEllipsoid);
172-
// Eigen::VectorXd tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx,
173-
// aSphericalZYX);
174-
175-
// BOOST_CHECK(tauEllipsoid.isApprox(tauSphericalZYX));
176-
177-
// // Test forward dynamics (ABA)
178-
// Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv);
179-
180-
// Eigen::VectorXd aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau,
181-
// Convention::WORLD); Eigen::VectorXd aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX,
182-
// q_zyx, v_zyx, tau, Convention::WORLD);
183-
184-
// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX));
185-
186-
// // Test with LOCAL convention
187-
// aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::LOCAL);
188-
// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau,
189-
// Convention::LOCAL);
190-
191-
// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX));
192-
193-
// // Test with different configurations
194-
// q_zyx << 0.2, -0.5, 1.1;
195-
// v_zyx << 0.3, 0.1, -0.2;
196-
197-
// converter.convertConfigurationVector(q_zyx, q_xyz);
198-
// converter.convertTangentVector(q_zyx, v_zyx, v_xyz);
199-
200-
// forwardKinematics(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz);
201-
// forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx);
202-
203-
// BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
204-
205-
// BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector()));
206-
207-
// tauEllipsoid = rnea(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, aEllipsoid);
208-
// tauSphericalZYX = rnea(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, aSphericalZYX);
209-
210-
// BOOST_CHECK(tauEllipsoid.isApprox(tauSphericalZYX));
211-
212-
// aAbaEllipsoid = aba(modelEllipsoid, dataEllipsoid, q_xyz, v_xyz, tau, Convention::WORLD);
213-
// aAbaSphericalZYX = aba(modelSphericalZYX, dataSphericalZYX, q_zyx, v_zyx, tau,
214-
// Convention::WORLD);
215-
216-
// BOOST_CHECK(aAbaEllipsoid.isApprox(aAbaSphericalZYX));
217-
218-
// }
219-
22041
BOOST_AUTO_TEST_CASE(vsSphericalZYX)
22142
{
22243
using namespace pinocchio;
@@ -319,7 +140,6 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX)
319140

320141
BOOST_CHECK(jDataEllipsoidFK.S.matrix().isApprox(jDataEllipsoidFK2.S.matrix()));
321142
BOOST_CHECK(jDataEllipsoidFK.S.matrix().isApprox(jDataEllipsoidFK3.S.matrix()));
322-
std::cout << "✓ Motion subspace matrices match for the three calc(...) methods" << std::endl;
323143

324144
JointDataSphericalZYX jDataSphereFK = jmodel_s.createData();
325145
jmodel_s.calc(jDataSphereFK, q_s, qd_s);
@@ -330,10 +150,7 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX)
330150
Eigen::Matrix<double, 6, 1> joint_vel_s = jDataSphereFK.S.matrix() * qd_s;
331151

332152
BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataSphericalZYX.v[1].toVector()));
333-
std::cout << "✓ Spatial velocities match" << std::endl;
334-
335153
BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataSphericalZYX.oMi[1]));
336-
std::cout << "✓ Full oMi[1] matches" << std::endl;
337154

338155
Matrix3 Sdot_e = jDataEllipsoidFK.Sdot.matrix().bottomRows<3>(); // Angular part
339156

@@ -348,31 +165,126 @@ BOOST_AUTO_TEST_CASE(vsSphericalZYX)
348165
Eigen::Vector3d wdot_s = jDataSphereFK.c.angular() + S_s * qdotdot_s;
349166
Eigen::Vector3d wdot_e = Sdot_e * qd_e + S_e * qdotdot_e;
350167
BOOST_CHECK(wdot_s.isApprox(wdot_e));
351-
std::cout << "✓ Angular accelerations from joint accelerations match" << std::endl;
352168

353169
forwardKinematics(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e);
354170
forwardKinematics(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s);
355171

356172
BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataSphericalZYX.a[1].toVector()));
357173

358-
// Test RNEA (Recursive Newton-Euler Algorithm)
359-
std::cout << "\n=== RNEA Test ===" << std::endl;
360-
174+
// Test RNEA (Recursive Newton-Euler Algorithm) - spatial forces should match
361175
rnea(modelEllipsoid, dataEllipsoid, q_e, qd_e, qdotdot_e);
362176
rnea(modelSphericalZYX, dataSphericalZYX, q_s, qd_s, qdotdot_s);
363177

364178
BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataSphericalZYX.f[1]));
365-
std::cout << "✓ RNEA f match" << std::endl;
366179

367180
// Test ABA (Articulated-Body Algorithm)
368-
std::cout << "\n=== ABA Test ===" << std::endl;
369181
Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv);
370-
371182
Eigen::VectorXd aAbaEllipsoid =
372183
aba(modelEllipsoid, dataEllipsoid, q_e, qd_e, dataEllipsoid.tau, Convention::WORLD);
373184

374185
BOOST_CHECK(dataEllipsoid.ddq.isApprox(qdotdot_e));
375186
std::cout << "✓ ABA computed accelerations match input accelerations" << std::endl;
376187
}
377188

189+
BOOST_AUTO_TEST_CASE(vsCompositeTxTyTzRxRyRz)
190+
{
191+
using namespace pinocchio;
192+
typedef SE3::Vector3 Vector3;
193+
typedef SE3::Matrix3 Matrix3;
194+
195+
// Ellipsoid parameters
196+
double radius_a = 2.0;
197+
double radius_b = 1.5;
198+
double radius_c = 1.0;
199+
200+
Inertia inertia = Inertia::Identity();
201+
SE3 pos = SE3::Identity();
202+
203+
// Create Ellipsoid model
204+
Model modelEllipsoid;
205+
JointModelEllipsoid jointModelEllipsoid(radius_a, radius_b, radius_c);
206+
addJointAndBody(
207+
modelEllipsoid, jointModelEllipsoid, 0, pos, "ellipsoid",
208+
inertia);
209+
210+
// Create Composite model (Tx, Ty, Tz, Rx, Ry, Rz)
211+
Model modelComposite;
212+
JointModelComposite jComposite;
213+
jComposite.addJoint(JointModelPX());
214+
jComposite.addJoint(JointModelPY());
215+
jComposite.addJoint(JointModelPZ());
216+
jComposite.addJoint(JointModelRX());
217+
jComposite.addJoint(JointModelRY());
218+
jComposite.addJoint(JointModelRZ());
219+
addJointAndBody(modelComposite, jComposite, 0, pos, "composite", inertia);
220+
221+
Data dataEllipsoid(modelEllipsoid);
222+
Data dataComposite(modelComposite);
223+
224+
// Test positions of ellispoid vs composite
225+
Eigen::VectorXd q_ellipsoid(3);
226+
q_ellipsoid << 1.0, 2.0, 3.0; // rx, ry, rz
227+
Eigen::Vector3d t = jointModelEllipsoid.computeTranslations(q_ellipsoid);
228+
Eigen::VectorXd q_composite(6);
229+
q_composite << t, q_ellipsoid;
230+
231+
forwardKinematics(modelEllipsoid, dataEllipsoid, q_ellipsoid);
232+
forwardKinematics(modelComposite, dataComposite, q_composite);
233+
234+
BOOST_CHECK(dataEllipsoid.oMi[1].isApprox(dataComposite.oMi[1]));
235+
236+
// Velocity test
237+
Eigen::VectorXd qdot_ellipsoid(3);
238+
qdot_ellipsoid << 0.1, 0.2, 0.3;
239+
240+
Eigen::Vector3d v_linear = jointModelEllipsoid.computeTranslationVelocities(q_ellipsoid, qdot_ellipsoid);
241+
Eigen::VectorXd qdot_composite(6);
242+
qdot_composite << v_linear, qdot_ellipsoid;
243+
244+
forwardKinematics(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid);
245+
forwardKinematics(modelComposite, dataComposite, q_composite, qdot_composite);
246+
247+
BOOST_CHECK(dataEllipsoid.v[1].toVector().isApprox(dataComposite.v[1].toVector()));
248+
249+
// Acceleration test
250+
Eigen::VectorXd qddot_ellipsoid(3);
251+
qddot_ellipsoid << 0.01, 0.02, 0.03;
252+
Eigen::Vector3d a_linear = jointModelEllipsoid.computeTranslationAccelerations(
253+
q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid);
254+
Eigen::VectorXd qddot_composite(6);
255+
qddot_composite << a_linear, qddot_ellipsoid;
256+
257+
forwardKinematics(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid);
258+
forwardKinematics(modelComposite, dataComposite, q_composite, qdot_composite, qddot_composite);
259+
260+
BOOST_CHECK(dataEllipsoid.a[1].toVector().isApprox(dataComposite.a[1].toVector()));
261+
262+
// Test RNEA - spatial forces and torques should match
263+
rnea(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, qddot_ellipsoid);
264+
rnea(modelComposite, dataComposite, q_composite, qdot_composite, qddot_composite);
265+
BOOST_CHECK(dataEllipsoid.f[1].isApprox(dataComposite.f[1]));
266+
267+
// Torque doesn't match yet.
268+
std::cout << "Ellipsoid torques: " << dataEllipsoid.tau.transpose() << std::endl;
269+
std::cout << "Composite torques: " << dataComposite.tau.transpose() << std::endl;
270+
BOOST_CHECK(dataEllipsoid.tau.isApprox(dataComposite.tau));
271+
std::cout << "✓ RNEA torques match" << std::endl;
272+
273+
274+
// Test ABA (Articulated-Body Algorithm)
275+
Eigen::VectorXd tau = Eigen::VectorXd::Ones(modelEllipsoid.nv);
276+
Eigen::VectorXd aAbaEllipsoid =
277+
aba(modelEllipsoid, dataEllipsoid, q_ellipsoid, qdot_ellipsoid, tau, Convention::WORLD);
278+
Eigen::VectorXd tau_composite = Eigen::VectorXd::Ones(modelComposite.nv);
279+
Eigen::VectorXd aAbaComposite =
280+
aba(modelComposite, dataComposite, q_composite, qdot_composite, tau_composite, Convention::WORLD);
281+
282+
std::cout << "Ellipsoid ABA ddq: " << dataEllipsoid.ddq.transpose() << std::endl;
283+
std::cout << "Composite ABA ddq: " << dataComposite.ddq.transpose() << std::endl;
284+
std::cout << "Composite ABA linear ddq: " << aAbaComposite.head<3>().transpose() << std::endl;
285+
std::cout << "Composite ABA angular ddq: " << aAbaComposite.tail<3>().transpose() << std::endl;
286+
287+
BOOST_CHECK(dataEllipsoid.ddq.isApprox(aAbaComposite.tail<3>()));
288+
}
289+
378290
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)