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 |
|
|
|