Skip to content

Commit e48451c

Browse files
MegMllMegMll
committed
Unittest - Add TU for spline joint
1 parent 639fe6b commit e48451c

File tree

2 files changed

+160
-0
lines changed

2 files changed

+160
-0
lines changed

‎unittest/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -284,6 +284,7 @@ add_pinocchio_unit_test(joint-composite)
284284
add_pinocchio_unit_test(joint-mimic)
285285
add_pinocchio_unit_test(joint-helical)
286286
add_pinocchio_unit_test(joint-universal)
287+
add_pinocchio_unit_test(joint-spline)
287288

288289
# Main corpus
289290
add_pinocchio_unit_test(model COLLISION_OPTIONAL)

‎unittest/joint-spline.cpp‎

Lines changed: 159 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
//
2+
// Copyright (c) 2025 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+
#include <iostream>
15+
16+
using namespace pinocchio;
17+
18+
template<typename D>
19+
void addJointAndBody(
20+
Model & model,
21+
const JointModelBase<D> & jmodel,
22+
const Model::JointIndex parent_id,
23+
const SE3 & joint_placement,
24+
const std::string & joint_name,
25+
const Inertia & Y)
26+
{
27+
Model::JointIndex idx;
28+
29+
idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name);
30+
model.appendBodyToJoint(idx, Y);
31+
}
32+
33+
BOOST_AUTO_TEST_SUITE(JointSpline)
34+
35+
BOOST_AUTO_TEST_CASE(vsPrismatic)
36+
{
37+
using namespace pinocchio;
38+
typedef SE3::Vector3 Vector3;
39+
typedef SE3::Matrix3 Matrix3;
40+
41+
Motion expected_v_J(Motion::Zero());
42+
Motion expected_c_J(Motion::Zero());
43+
44+
SE3 expected_configuration(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 0.2)));
45+
46+
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
47+
ctrlFrames.push_back(SE3::Identity());
48+
ctrlFrames.push_back(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
49+
std::cout << ctrlFrames.size() << std::endl;
50+
51+
JointModelSpline jmodel(ctrlFrames, 1);
52+
JointDataSpline jdata = jmodel.createData();
53+
54+
jmodel.setIndexes(0, 0, 0);
55+
56+
Eigen::VectorXd q(Eigen::VectorXd::Zero(1));
57+
58+
// -------
59+
q << 0.2;
60+
61+
jmodel.calc(jdata, q);
62+
63+
BOOST_CHECK(expected_configuration.rotation().isApprox(jdata.M.rotation(), 1e-12));
64+
BOOST_CHECK(expected_configuration.translation().isApprox(jdata.M.translation(), 1e-12));
65+
66+
Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1));
67+
68+
// -------
69+
q << 0.3;
70+
q_dot << 0.4;
71+
72+
jmodel.calc(jdata, q, q_dot);
73+
74+
expected_configuration.translation() << 0, 0, 0.3;
75+
expected_v_J.linear() << 0., 0., 0.4;
76+
77+
BOOST_CHECK(expected_configuration.rotation().isApprox(jdata.M.rotation(), 1e-12));
78+
BOOST_CHECK(expected_configuration.translation().isApprox(jdata.M.translation(), 1e-12));
79+
BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion)jdata.v).toVector(), 1e-12));
80+
BOOST_CHECK(expected_c_J.isApprox((Motion)jdata.c, 1e-12));
81+
}
82+
83+
// BOOST_AUTO_TEST_CASE(vsSphericalXYZ)
84+
// {
85+
86+
// using namespace pinocchio;
87+
// typedef SE3::Vector3 Vector3;
88+
// typedef SE3::Matrix3 Matrix3;
89+
90+
// Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
91+
92+
93+
// PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
94+
// ctrlFrames.push_back(SE3::Identity());
95+
96+
// Eigen::Matrix3d R1;
97+
// R1 << 0.866025, 0, 0.5,
98+
// 0, 1, 0 ,
99+
// -0.5, 0, 0.866025;
100+
// // R1 << 0.770151, -0.219024, 0.599079,
101+
// // 0.420735, 0.880347, -0.219024,
102+
// // -0.479426, 0.420735, 0.770151;
103+
// ctrlFrames.push_back(SE3(R1, Eigen::Vector3d::Zero()));
104+
105+
// // 0.5
106+
// Eigen::Matrix3d R2;
107+
108+
// R2 << 0.707107, 0, 0.707107,
109+
// 0, 1, 0 ,
110+
// -0.707107, 0, 0.707107;
111+
// // R1 << 0.770151, -0.219024, 0.599079,
112+
// // 0.420735, 0.880347, -0.219024,
113+
// // -0.479426, 0.420735, 0.770151;
114+
// ctrlFrames.push_back(SE3(R2, Eigen::Vector3d::Zero()));
115+
116+
// Eigen::Matrix3d R3;
117+
118+
// R3 << 0.5, 0, 0.866025,
119+
// 0, 1, 0 ,
120+
// -0.866025, 0, 0.5;
121+
// // R1 << 0.770151, -0.219024, 0.599079,
122+
// // 0.420735, 0.880347, -0.219024,
123+
// // -0.479426, 0.420735, 0.770151;
124+
// ctrlFrames.push_back(SE3(R3, Eigen::Vector3d::Zero()));
125+
126+
// // 1
127+
// Eigen::Matrix3d R4;
128+
// R4 << 0, 0, 1, 0, 1, 0, -1, 0, 0;
129+
// // R2 << 0.291927, -0.072075, 0.953721,
130+
// // 0.454649, 0.88775, -0.072075,
131+
// // -0.841471, 0.454649, 0.291927;
132+
// ctrlFrames.push_back(SE3(R4, Eigen::Vector3d::Zero()));
133+
134+
// Model modelSpline;
135+
// addJointAndBody(modelSpline, JointModelSpline(ctrlFrames), 0, SE3::Identity(), "spline_joint", inertia);
136+
// Data dataSpline(modelSpline);
137+
138+
// Model modelSph;
139+
// addJointAndBody(modelSph, JointModelSphericalZYX(), 0, SE3::Identity(), "sph_joint", inertia);
140+
// Data dataSph(modelSph);
141+
142+
// Eigen::VectorXd qSpline(Eigen::VectorXd::Zero(1));
143+
// qSpline << 0.5;
144+
145+
// Eigen::Vector3d qSph;
146+
// qSph << 0, M_PI / 3, 0;
147+
148+
// forwardKinematics(modelSpline, dataSpline, qSpline);
149+
// forwardKinematics(modelSph, dataSph, qSph);
150+
151+
// BOOST_CHECK(dataSpline.oMi[1].isApprox(dataSph.oMi[1]));
152+
// std::cout << dataSpline.oMi[1] << std::endl;
153+
// std::cout << dataSph.oMi[1] << std::endl;
154+
// }
155+
156+
157+
158+
159+
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)