Skip to content

Commit f464bc1

Browse files
MegMllMegMll
committed
Spline - Add TU with finite difference
1 parent 1559120 commit f464bc1

File tree

1 file changed

+67
-74
lines changed

1 file changed

+67
-74
lines changed

‎unittest/joint-spline.cpp‎

Lines changed: 67 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,6 @@ BOOST_AUTO_TEST_CASE(vsPrismatic)
4646
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
4747
ctrlFrames.push_back(SE3::Identity());
4848
ctrlFrames.push_back(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.)));
49-
std::cout << ctrlFrames.size() << std::endl;
5049

5150
JointModelSpline jmodel(ctrlFrames, 1);
5251
JointDataSpline jdata = jmodel.createData();
@@ -63,9 +62,8 @@ BOOST_AUTO_TEST_CASE(vsPrismatic)
6362
BOOST_CHECK(expected_configuration.rotation().isApprox(jdata.M.rotation(), 1e-12));
6463
BOOST_CHECK(expected_configuration.translation().isApprox(jdata.M.translation(), 1e-12));
6564

66-
Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1));
67-
6865
// -------
66+
Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1));
6967
q << 0.3;
7068
q_dot << 0.4;
7169

@@ -80,76 +78,71 @@ BOOST_AUTO_TEST_CASE(vsPrismatic)
8078
BOOST_CHECK(expected_c_J.isApprox((Motion)jdata.c, 1e-12));
8179
}
8280

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-
// PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
93-
// ctrlFrames.push_back(SE3::Identity());
94-
95-
// Eigen::Matrix3d R1;
96-
// R1 << 0.866025, 0, 0.5,
97-
// 0, 1, 0 ,
98-
// -0.5, 0, 0.866025;
99-
// // R1 << 0.770151, -0.219024, 0.599079,
100-
// // 0.420735, 0.880347, -0.219024,
101-
// // -0.479426, 0.420735, 0.770151;
102-
// ctrlFrames.push_back(SE3(R1, Eigen::Vector3d::Zero()));
103-
104-
// // 0.5
105-
// Eigen::Matrix3d R2;
106-
107-
// R2 << 0.707107, 0, 0.707107,
108-
// 0, 1, 0 ,
109-
// -0.707107, 0, 0.707107;
110-
// // R1 << 0.770151, -0.219024, 0.599079,
111-
// // 0.420735, 0.880347, -0.219024,
112-
// // -0.479426, 0.420735, 0.770151;
113-
// ctrlFrames.push_back(SE3(R2, Eigen::Vector3d::Zero()));
114-
115-
// Eigen::Matrix3d R3;
116-
117-
// R3 << 0.5, 0, 0.866025,
118-
// 0, 1, 0 ,
119-
// -0.866025, 0, 0.5;
120-
// // R1 << 0.770151, -0.219024, 0.599079,
121-
// // 0.420735, 0.880347, -0.219024,
122-
// // -0.479426, 0.420735, 0.770151;
123-
// ctrlFrames.push_back(SE3(R3, Eigen::Vector3d::Zero()));
124-
125-
// // 1
126-
// Eigen::Matrix3d R4;
127-
// R4 << 0, 0, 1, 0, 1, 0, -1, 0, 0;
128-
// // R2 << 0.291927, -0.072075, 0.953721,
129-
// // 0.454649, 0.88775, -0.072075,
130-
// // -0.841471, 0.454649, 0.291927;
131-
// ctrlFrames.push_back(SE3(R4, Eigen::Vector3d::Zero()));
132-
133-
// Model modelSpline;
134-
// addJointAndBody(modelSpline, JointModelSpline(ctrlFrames), 0, SE3::Identity(), "spline_joint",
135-
// inertia); Data dataSpline(modelSpline);
136-
137-
// Model modelSph;
138-
// addJointAndBody(modelSph, JointModelSphericalZYX(), 0, SE3::Identity(), "sph_joint", inertia);
139-
// Data dataSph(modelSph);
140-
141-
// Eigen::VectorXd qSpline(Eigen::VectorXd::Zero(1));
142-
// qSpline << 0.5;
143-
144-
// Eigen::Vector3d qSph;
145-
// qSph << 0, M_PI / 3, 0;
146-
147-
// forwardKinematics(modelSpline, dataSpline, qSpline);
148-
// forwardKinematics(modelSph, dataSph, qSph);
149-
150-
// BOOST_CHECK(dataSpline.oMi[1].isApprox(dataSph.oMi[1]));
151-
// std::cout << dataSpline.oMi[1] << std::endl;
152-
// std::cout << dataSph.oMi[1] << std::endl;
153-
// }
81+
BOOST_AUTO_TEST_CASE(vsFiniteDiff)
82+
{
83+
using namespace pinocchio;
84+
typedef SE3::Vector3 Vector3;
85+
typedef SE3::Matrix3 Matrix3;
86+
87+
typedef typename JointModelSpline::ConfigVector_t CV;
88+
typedef typename JointModelSpline::TangentVector_t TV;
89+
typedef typename LieGroup<JointModelSpline>::type LieGroupType;
90+
91+
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
92+
ctrlFrames.push_back(SE3::Identity());
93+
ctrlFrames.push_back(SE3::Random());
94+
ctrlFrames.push_back(SE3::Random());
95+
ctrlFrames.push_back(SE3::Random());
96+
97+
JointModelSpline jmodel(ctrlFrames);
98+
JointDataSpline jdata = jmodel.createData();
99+
100+
jmodel.setIndexes(0, 0, 0);
101+
102+
double eps = 1e-8;
103+
CV q_ref(1);
104+
q_ref[0] = 0.6;
105+
CV q(q_ref);
106+
107+
const Eigen::DenseIndex nv = jdata.S.nv();
108+
TV q_dot(nv);
109+
TV q_dot_ref(nv);
110+
q_dot.setZero();
111+
112+
q_dot[0] = eps;
113+
q_dot_ref[0] = 0.3;
114+
115+
q = LieGroupType().integrate(q_ref, q_dot);
116+
117+
{
118+
// Check S
119+
jmodel.calc(jdata, q_ref);
120+
SE3 M_ref(jdata.M);
121+
Eigen::Matrix<double, 6, JointModelSpline::NV> S(6, JointModelSpline::NV),
122+
S_ref(jdata.S.matrix());
123+
124+
jmodel.calc(jdata, q);
125+
SE3 M_ = jdata.M;
126+
127+
S.col(0) = log6(M_ref.inverse() * M_).toVector();
128+
S.col(0) /= eps;
129+
130+
BOOST_CHECK(S.isApprox(S_ref, eps * 1e1));
131+
}
132+
// Check bias
133+
{
134+
jmodel.calc(jdata, q_ref, q_dot_ref);
135+
const Motion & c_ref = jdata.c;
136+
Eigen::Matrix<double, 6, JointModelSpline::NV> S_ref(jdata.S.matrix());
137+
138+
jmodel.calc(jdata, q);
139+
Eigen::Matrix<double, 6, JointModelSpline::NV> S_(jdata.S.matrix());
140+
141+
Motion dSdq_fd((S_ - S_ref) / eps);
142+
Motion c_fd = dSdq_fd * q_dot_ref[0];
143+
144+
BOOST_CHECK(c_ref.isApprox(c_fd, eps * 1e1));
145+
}
146+
}
154147

155148
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)