SpatialAlgebraTests.cc
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 
3 #include "UnitTestUtils.hpp"
4 #include "rdl_dynamics/Body.h"
5 
6 using namespace std;
7 using namespace RobotDynamics;
8 using namespace RobotDynamics::Math;
9 
10 struct SpatialAlgebraTests : public testing::Test
11 {
12 
13 };
14 
15 const double TEST_PREC = 1.0e-14;
16 
17 SpatialMatrix spatial_adjoint(const SpatialMatrix &m)
18 {
19  SpatialMatrix res(m);
20  res.block<3, 3>(3, 0) = m.block<3, 3>(0, 3);
21  res.block<3, 3>(0, 3) = m.block<3, 3>(3, 0);
22  return res;
23 }
24 
25 SpatialMatrix spatial_inverse(const SpatialMatrix &m)
26 {
27  SpatialMatrix res(m);
28  res.block<3, 3>(0, 0) = m.block<3, 3>(0, 0).transpose();
29  res.block<3, 3>(3, 0) = m.block<3, 3>(3, 0).transpose();
30  res.block<3, 3>(0, 3) = m.block<3, 3>(0, 3).transpose();
31  res.block<3, 3>(3, 3) = m.block<3, 3>(3, 3).transpose();
32  return res;
33 }
34 
35 Matrix3d get_rotation(const SpatialMatrix &m)
36 {
37  return m.block<3, 3>(0, 0);
38 }
39 
40 Vector3d get_translation(const SpatialMatrix &m)
41 {
42  return Vector3d(-m(4, 2), m(3, 2), -m(3, 1));
43 }
44 
45 TEST(SpatialAlgebraTests, euler)
46 {
47  double th = 0.3;
48  RobotDynamics::Math::Quaternion qx(std::sin(th / 2.), 0., 0., std::cos(th / 2.)), qy(0., std::sin(th / 2.), 0., std::cos(th / 2.)),
49  qz(0., 0., std::sin(th / 2.), std::cos(th / 2.));
50 
51  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(intrinsicXYZAnglesToQuaternion(0.3, 0.3, 0.3), qx * qy * qz,
52  unit_test_utils::TEST_PREC));
53  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(RobotDynamics::Math::intrinsicZYXAnglesToQuaternion(0.3, 0.3, 0.3), qz * qy * qx,
54  unit_test_utils::TEST_PREC));
55  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(RobotDynamics::Math::intrinsicYXZAnglesToQuaternion(0.3, 0.3, 0.3), qy * qx * qz,
56  unit_test_utils::TEST_PREC));
57 
58 
59  RobotDynamics::Math::Vector3d euler(-0.3, 0.112, -1.316);
60 
61  Vector3d euler_from_matrix = toIntrinsicZYXAngles(RobotDynamics::Math::XeulerZYX(euler).E);
62  Vector3d euler_from_quaternion = toIntrinsicZYXAngles(toQuaternion(RobotDynamics::Math::XeulerZYX(euler).E));
63 
64  EXPECT_NEAR(euler[0], euler_from_quaternion[0], 1.e-10);
65  EXPECT_NEAR(euler[1], euler_from_quaternion[1], 1.e-10);
66  EXPECT_NEAR(euler[2], euler_from_quaternion[2], 1.e-10);
67 
68  EXPECT_NEAR(euler[0], euler_from_matrix[0], 1.e-10);
69  EXPECT_NEAR(euler[1], euler_from_matrix[1], 1.e-10);
70  EXPECT_NEAR(euler[2], euler_from_matrix[2], 1.e-10);
71 
72  // should be at a singularity for yaw-pitch-roll angles
73  euler = RobotDynamics::Math::Vector3d(-0.3, M_PI_2, -1.316);
74 
75  euler_from_matrix = toIntrinsicZYXAngles(RobotDynamics::Math::XeulerZYX(euler).E, euler[0]);
76  euler_from_quaternion = toIntrinsicZYXAngles(toQuaternion(RobotDynamics::Math::XeulerZYX(euler).E), euler[0]);
77 
78  EXPECT_NEAR(euler[0], euler_from_quaternion[0], 1.e-10);
79  EXPECT_NEAR(euler[1], euler_from_quaternion[1], 1.e-10);
80  EXPECT_NEAR(euler[2], euler_from_quaternion[2], 1.e-10);
81 
82  EXPECT_NEAR(euler[0], euler_from_matrix[0], 1.e-10);
83  EXPECT_NEAR(euler[1], euler_from_matrix[1], 1.e-10);
84  EXPECT_NEAR(euler[2], euler_from_matrix[2], 1.e-10);
85 
86  // should be at a singularity for yaw-pitch-roll angles
87  euler = RobotDynamics::Math::Vector3d(-0.5, -M_PI_2, -1.16);
88 
89  euler_from_matrix = toIntrinsicZYXAngles(RobotDynamics::Math::XeulerZYX(euler).E, euler[0]);
90  euler_from_quaternion = toIntrinsicZYXAngles(toQuaternion(RobotDynamics::Math::XeulerZYX(euler).E), euler[0]);
91 
92  EXPECT_NEAR(euler[0], euler_from_quaternion[0], 1.e-10);
93  EXPECT_NEAR(euler[1], euler_from_quaternion[1], 1.e-10);
94  EXPECT_NEAR(euler[2], euler_from_quaternion[2], 1.e-10);
95 
96  EXPECT_NEAR(euler[0], euler_from_matrix[0], 1.e-10);
97  EXPECT_NEAR(euler[1], euler_from_matrix[1], 1.e-10);
98  EXPECT_NEAR(euler[2], euler_from_matrix[2], 1.e-10);
99 }
100 
101 TEST(SpatialAlgebraTests, fromAxisAngleToQuaternion)
102 {
103  RobotDynamics::Math::Vector3d v(0.5, 0.11, -0.3);
104  v.normalize();
105  Eigen::AngleAxisd aa(0.5, v);
106  Eigen::Quaterniond q2(aa);
107 
108  RobotDynamics::Math::Quaternion q_exp(0.2084700340099281, 0.04586340748218418, -0.1250820204059569, 0.9689124217106447);
109  RobotDynamics::Math::Quaternion q = toQuaternion(v, 0.5);
110 
111  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(q_exp, q, unit_test_utils::TEST_PREC));
112 }
113 
114 TEST(SpatialAlgebraTests, fromMatrixToQuaternion)
115 {
116  RobotDynamics::Math::Matrix3d m = (RobotDynamics::Math::Xrotx(0.1) * RobotDynamics::Math::Xroty(0.1) * RobotDynamics::Math::Xrotz(-0.1)).E;
117 
118  Eigen::Quaterniond q2(m);
119 
120  RobotDynamics::Math::Quaternion q = toQuaternion(m);
121  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m, toMatrix(q), unit_test_utils::TEST_PREC));
122 
123  m << 0., 1., 0., 1., 0., 0., 0., 0., -1.;
124 
125  q = toQuaternion(m);
126  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m, toMatrix(q), unit_test_utils::TEST_PREC));
127 
128  m << -1., 0., 0., 0., 1., 0., 0., 0., -1.;
129 
130  q = toQuaternion(m);
131  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m, toMatrix(q), unit_test_utils::TEST_PREC));
132 
133  m << 1., 0., 0., 0., -1., 0., 0., 0., -1.;
134 
135  q = toQuaternion(m);
136  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m, toMatrix(q), unit_test_utils::TEST_PREC));
137 
138  m << -1., 0., 0., 0., -1., 0., 0., 0., 1.;
139 
140  q = toQuaternion(m);
141  EXPECT_TRUE(unit_test_utils::checkMatrix3dEpsilonClose(m, toMatrix(q), unit_test_utils::TEST_PREC));
142 
143  m = (RobotDynamics::Math::Xrotx(0.1144) * RobotDynamics::Math::Xroty(-1.99331) * RobotDynamics::Math::Xrotz(-0.98011)).E;
144  RobotDynamics::Math::Quaternion q_exp(-0.367164735122942, -0.7542393999032798, -0.2128590176200619, 0.5010030174893784);
145  EXPECT_TRUE(unit_test_utils::checkVector4dEpsilonClose(q_exp, toQuaternion(m), unit_test_utils::TEST_PREC));
146 }
147 
148 TEST(SpatialAlgebraTests, AxisAngle)
149 {
150  RobotDynamics::Math::Vector3d v(-0.3, 0.1, 0.4);
151  v.normalize();
152 
153  double angle = 0.5;
154 
155  RobotDynamics::Math::AxisAngle a(angle, v);
156 
157  RobotDynamics::Math::Quaternion q = toQuaternion(a);
158 
159  a = toAxisAngle(q);
160 
161  EXPECT_TRUE(a.axis().isApprox(v, 1e-12));
162 
163  q = RobotDynamics::Math::Quaternion(0.1, 0.2, 0.3, 0.4);
164 
165  a = toAxisAngle(q);
166 
167  RobotDynamics::Math::Quaternion q2 = q;
168  q.normalize();
169 
170  RobotDynamics::Math::AxisAngle a2;
171 
172  a2 = toAxisAngle(q2);
173 
174  EXPECT_TRUE(a.axis().isApprox(a2.axis(), 1e-12));
175  EXPECT_NEAR(a.angle(), a2.angle(), 1e-12);
176 
177  v = RobotDynamics::Math::Vector3d(0.1, 0.2, 0.3);
178  v.normalize();
179  q = RobotDynamics::Math::Quaternion(v.x(), v.y(), v.z(), 0.0);
180  q.normalize();
181 
182  a = toAxisAngle(q);
183 
184  EXPECT_TRUE(a.axis().isApprox(q.vec(), 1e-12));
185  EXPECT_NEAR(a.angle(), M_PI, 1e-12);
186 
187  q = RobotDynamics::Math::Quaternion(-0.000194225, 0.000545134, 0.000810327, 1.);
188 
189  a = toAxisAngle(q);
190 
191  EXPECT_NEAR(a.axis().x(), -0.195053, 1.0e-6);
192  EXPECT_NEAR(a.axis().y(), 0.547459, 1.0e-6);
193  EXPECT_NEAR(a.axis().z(), 0.813783, 1.0e-6);
194 }
195 
197 TEST(SpatialAlgebraTests, TestSpatialMatrixTimesSpatialVector)
198 {
199  SpatialMatrix s_matrix(1., 0., 0., 0., 0., 7., 0., 2., 0., 0., 8., 0., 0., 0., 3., 9., 0., 0., 0., 0., 6., 4., 0., 0., 0., 5., 0., 0., 5., 0., 4., 0., 0., 0., 0., 6.);
200  SpatialVector s_vector(1., 2., 3., 4., 5., 6.);
201 
202  SpatialVector result;
203  result = s_matrix * s_vector;
204 
205  SpatialVector test_result(43., 44., 45., 34., 35., 40.);
206  EXPECT_EQ (test_result, result);
207 }
208 
209 TEST(SpatialAlgebraTests, TestSpatialTransformInverse)
210 {
211  Matrix3d m(cos(0.4), sin(0.4), 0, -sin(0.4), cos(0.4), 0, 0, 0, 1);
212  Vector3d v(1, 2, 3);
213  SpatialTransform t1(m, v);
214  SpatialTransform t2(m, v);
215 
216  SpatialTransform t3 = t1.inverse();
217  t2.invert();
218 
219  EXPECT_EQ(t3.toMatrix(), t2.toMatrix());
220 }
221 
223 TEST(SpatialAlgebraTests, TestScalarTimesSpatialVector)
224 {
225  SpatialVector s_vector(1., 2., 3., 4., 5., 6.);
226 
227  SpatialVector result;
228  result = 3. * s_vector;
229 
230  SpatialVector test_result(3., 6., 9., 12., 15., 18.);
231 
232  EXPECT_EQ (test_result, result);
233 }
234 
236 TEST(SpatialAlgebraTests, TestScalarTimesSpatialMatrix)
237 {
238  SpatialMatrix s_matrix(1., 0., 0., 0., 0., 7., 0., 2., 0., 0., 8., 0., 0., 0., 3., 9., 0., 0., 0., 0., 6., 4., 0., 0., 0., 5., 0., 0., 5., 0., 4., 0., 0., 0., 0., 6.);
239 
240  SpatialMatrix result;
241  result = 3. * s_matrix;
242 
243  SpatialMatrix test_result(3., 0., 0., 0., 0., 21., 0., 6., 0., 0., 24., 0., 0., 0., 9., 27., 0., 0., 0., 0., 18., 12., 0., 0., 0., 15., 0., 0., 15., 0., 12., 0., 0., 0., 0., 18.);
244 
245  EXPECT_EQ (test_result, result);
246 }
247 
249 TEST(SpatialAlgebraTests, TestSpatialMatrixTimesSpatialMatrix)
250 {
251  SpatialMatrix s_matrix(1., 0., 0., 0., 0., 7., 0., 2., 0., 0., 8., 0., 0., 0., 3., 9., 0., 0., 0., 0., 6., 4., 0., 0., 0., 5., 0., 0., 5., 0., 4., 0., 0., 0., 0., 6.);
252 
253  SpatialMatrix result;
254  result = s_matrix * s_matrix;
255 
256  SpatialMatrix test_result(29., 0., 0., 0., 0., 49., 0., 44., 0., 0., 56., 0., 0., 0., 63., 63., 0., 0., 0., 0., 42., 70., 0., 0., 0., 35., 0., 0., 65., 0., 28., 0., 0., 0., 0., 64.);
257 
258  EXPECT_EQ (test_result, result);
259 }
260 
262 //
263 // This method computes a spatial force transformation from a spatial
264 // motion transformation and vice versa
265 TEST(SpatialAlgebraTests, TestSpatialMatrixTransformAdjoint)
266 {
267  SpatialMatrix s_matrix(1., 2., 3., 4., 5., 6., 7., 8., 9., 10., 11., 12., 13., 14., 15., 16., 17., 18., 19., 20., 21., 22., 23., 24., 25., 26., 27., 28., 29., 30., 31., 32., 33., 34., 35., 36.);
268 
269  SpatialMatrix result = spatial_adjoint(s_matrix);
270 
271  SpatialMatrix test_result_matrix(1., 2., 3., 19., 20., 21., 7., 8., 9., 25., 26., 27., 13., 14., 15., 31., 32., 33., 4., 5., 6., 22., 23., 24., 10., 11., 12., 28., 29., 30., 16., 17., 18., 34., 35., 36.);
272 
273  EXPECT_EQ (test_result_matrix, result);
274 }
275 
276 TEST(SpatialAlgebraTests, TestSpatialMatrixInverse)
277 {
278  SpatialMatrix s_matrix(0, 1, 2, 0, 1, 2, 3, 4, 5, 3, 4, 5, 6, 7, 8, 6, 7, 8, 0, 1, 2, 0, 1, 2, 3, 4, 5, 3, 4, 5, 6, 7, 8, 6, 7, 8);
279 
280  SpatialMatrix test_inv(0, 3, 6, 0, 3, 6, 1, 4, 7, 1, 4, 7, 2, 5, 8, 2, 5, 8, 0, 3, 6, 0, 3, 6, 1, 4, 7, 1, 4, 7, 2, 5, 8, 2, 5, 8);
281 
282  EXPECT_EQ (test_inv, spatial_inverse(s_matrix));
283 }
284 
285 TEST(SpatialAlgebraTests, TestSpatialMatrixGetRotation)
286 {
287  SpatialMatrix spatial_transform(1., 2., 3., 0., 0., 0., 4., 5., 6., 0., 0., 0., 7., 8., 9., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.);
288 
289  // Matrix3d rotation = spatial_transform.block<3,3>(0,0);
290  Matrix3d rotation = get_rotation(spatial_transform);
291  Matrix3d test_result(1., 2., 3., 4., 5., 6., 7., 8., 9.);
292 
293  EXPECT_EQ(test_result, rotation);
294 }
295 
296 TEST(SpatialAlgebraTests, TestSpatialMatrixGetTranslation)
297 {
298  SpatialMatrix spatial_transform(0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., -3., 2., 0., 0., 0., 0., 0., -1., 0., 0., 0., 0., 0., 0., 0., 0., 0.);
299 
300  Vector3d translation = get_translation(spatial_transform);
301  Vector3d test_result(1., 2., 3.);
302 
303  EXPECT_EQ(test_result, translation);
304 }
305 
306 TEST(SpatialAlgebraTests, TestSpatialVectorCross)
307 {
308  SpatialVector s_vec(1., 2., 3., 4., 5., 6.);
309 
310  SpatialMatrix test_cross(0., -3., 2., 0., 0., 0., 3., 0., -1., 0., 0., 0., -2., 1., 0., 0., 0., 0., 0., -6., 5., 0., -3., 2., 6., 0., -4., 3., 0., -1., -5., 4., 0., -2., 1., 0.);
311 
312  SpatialMatrix s_vec_cross(crossm(s_vec));
313  EXPECT_EQ (test_cross, s_vec_cross);
314 
315  SpatialMatrix s_vec_crossf(crossf(s_vec));
316  SpatialMatrix test_crossf = -1. * crossm(s_vec).transpose();
317 
318  EXPECT_EQ (test_crossf, s_vec_crossf);
319 }
320 
321 TEST(SpatialAlgebraTests, TestSpatialVectorCrossmCrossf)
322 {
323  SpatialVector s_vec(1., 2., 3., 4., 5., 6.);
324  SpatialVector t_vec(9., 8., 7., 6., 5., 4.);
325 
326  // by explicitly building the matrices (crossm/f with only one vector)
327  SpatialVector crossm_s_x_t = crossm(s_vec) * t_vec;
328  SpatialVector crossf_s_x_t = crossf(s_vec) * t_vec;
329 
330  // by using direct computation that avoids building of the matrix
331  SpatialVector crossm_s_t = crossm(s_vec, t_vec);
332  SpatialVector crossf_s_t = crossf(s_vec, t_vec);
333 
334  /*
335  cout << crossm_s_x_t << endl;
336  cout << "---" << endl;
337  cout << crossf_s_x_t << endl;
338  cout << "---" << endl;
339  cout << crossf_s_t << endl;
340  */
341 
342  EXPECT_EQ (crossm_s_x_t, crossm_s_t);
343  EXPECT_EQ (crossf_s_x_t, crossf_s_t);
344 }
345 
346 TEST(SpatialAlgebraTests, TestSpatialTransformApply)
347 {
348  Vector3d rot(1.1, 1.2, 1.3);
349  Vector3d trans(1.1, 1.2, 1.3);
350 
351  SpatialTransform X_st;
352  X_st.r = trans;
353 
354  SpatialMatrix X_66_matrix(SpatialMatrix::Zero(6, 6));
355  X_66_matrix = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
356  X_st.E = X_66_matrix.block<3, 3>(0, 0);
357 
358  // cout << X_66_matrix << endl;
359  // cout << X_st.E << endl;
360  // cout << X_st.r.transpose() << endl;
361 
362  SpatialVector v(1.1, 2.1, 3.1, 4.1, 5.1, 6.1);
363  SpatialVector v_66_res = X_66_matrix * v;
364  SpatialVector v_st_res = X_st.apply(v);
365 
366  // cout << (v_66_res - v_st_res).transpose() << endl;
367 
368  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(v_66_res, v_st_res, TEST_PREC));
369 }
370 
371 TEST(SpatialAlgebraTests, TestSpatialTransformApplyTranspose)
372 {
373  Vector3d rot(1.1, 1.2, 1.3);
374  Vector3d trans(1.1, 1.2, 1.3);
375 
376  SpatialTransform X_st;
377  X_st.r = trans;
378 
379  SpatialMatrix X_66_matrix(SpatialMatrix::Zero(6, 6));
380  X_66_matrix = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
381  X_st.E = X_66_matrix.block<3, 3>(0, 0);
382 
383  // cout << X_66_matrix << endl;
384  // cout << X_st.E << endl;
385  // cout << X_st.r.transpose() << endl;
386 
387  SpatialVector v(1.1, 2.1, 3.1, 4.1, 5.1, 6.1);
388  SpatialVector v_66_res = X_66_matrix.transpose() * v;
389  SpatialVector v_st_res = X_st.applyTranspose(v);
390 
391  // cout << (v_66_res - v_st_res).transpose() << endl;
392 
393  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(v_66_res, v_st_res, TEST_PREC));
394 }
395 
396 TEST(SpatialAlgebraTests, TestSpatialTransformApplyAdjoint)
397 {
398  SpatialTransform X(Xrotz(0.5) * Xroty(0.9) * Xrotx(0.2) * Xtrans(Vector3d(1.1, 1.2, 1.3)));
399 
400  SpatialMatrix X_adjoint = X.toMatrixAdjoint();
401 
402  SpatialVector f(1.1, 2.1, 4.1, 9.2, 3.3, 0.8);
403  SpatialVector f_apply = X.applyAdjoint(f);
404  SpatialVector f_matrix = X_adjoint * f;
405 
406  EXPECT_TRUE(unit_test_utils::checkSpatialVectorsEpsilonClose(f_matrix, f_apply, TEST_PREC));
407 }
408 
409 TEST(SpatialAlgebraTests, TestSpatialTransformToMatrix)
410 {
411  Vector3d rot(1.1, 1.2, 1.3);
412  Vector3d trans(1.1, 1.2, 1.3);
413 
414  SpatialMatrix X_matrix(SpatialMatrix::Zero(6, 6));
415  X_matrix = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
416 
417  SpatialTransform X_st;
418  X_st.E = X_matrix.block<3, 3>(0, 0);
419  X_st.r = trans;
420 
421  // SpatialMatrix X_diff = X_st.toMatrix() - X_matrix;
422  // cout << "Error: " << endl << X_diff << endl;
423 
424  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(X_matrix, X_st.toMatrix(), TEST_PREC));
425 }
426 
427 TEST(SpatialAlgebraTests, TestSpatialTransformToMatrixAdjoint)
428 {
429  Vector3d rot(1.1, 1.2, 1.3);
430  Vector3d trans(1.1, 1.2, 1.3);
431 
432  SpatialMatrix X_matrix(SpatialMatrix::Zero(6, 6));
433  X_matrix = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
434 
435  SpatialTransform X_st;
436  X_st.E = X_matrix.block<3, 3>(0, 0);
437  X_st.r = trans;
438 
439  // SpatialMatrix X_diff = X_st.toMatrixAdjoint() - spatial_adjoint(X_matrix);
440  // cout << "Error: " << endl << X_diff << endl;
441 
442  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(spatial_adjoint(X_matrix), X_st.toMatrixAdjoint(), TEST_PREC));
443 }
444 
445 TEST(SpatialAlgebraTests, TestSpatialTransformToMatrixTranspose)
446 {
447  Vector3d rot(1.1, 1.2, 1.3);
448  Vector3d trans(1.1, 1.2, 1.3);
449 
450  SpatialMatrix X_matrix(SpatialMatrix::Zero(6, 6));
451  X_matrix = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
452 
453  SpatialTransform X_st;
454  X_st.E = X_matrix.block<3, 3>(0, 0);
455  X_st.r = trans;
456 
457  // we have to copy the matrix as it is only transposed via a flag and
458  // thus data() does not return the proper data.
459  SpatialMatrix X_matrix_transposed = X_matrix.transpose();
460  // SpatialMatrix X_diff = X_st.toMatrixTranspose() - X_matrix_transposed;
461  // cout << "Error: " << endl << X_diff << endl;
462  // cout << "X_st: " << endl << X_st.toMatrixTranspose() << endl;
463  // cout << "X: " << endl << X_matrix_transposed() << endl;
464 
465  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(X_matrix_transposed, X_st.toMatrixTranspose(), TEST_PREC));
466 }
467 
468 TEST(SpatialAlgebraTests, TestSpatialTransformMultiply)
469 {
470  Vector3d rot(1.1, 1.2, 1.3);
471  Vector3d trans(1.1, 1.2, 1.3);
472 
473  SpatialMatrix X_matrix_1(SpatialMatrix::Zero(6, 6));
474  SpatialMatrix X_matrix_2(SpatialMatrix::Zero(6, 6));
475 
476  X_matrix_1 = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
477  X_matrix_2 = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
478 
479  SpatialTransform X_st_1;
480  SpatialTransform X_st_2;
481 
482  X_st_1.E = X_matrix_1.block<3, 3>(0, 0);
483  X_st_1.r = trans;
484  X_st_2.E = X_matrix_2.block<3, 3>(0, 0);
485  X_st_2.r = trans;
486 
487  SpatialTransform X_st_res = X_st_1 * X_st_2;
488  SpatialMatrix X_matrix_res = X_matrix_1 * X_matrix_2;
489 
490  // SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res;
491  // cout << "Error: " << endl << X_diff << endl;
492 
493  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(X_matrix_res, X_st_res.toMatrix(), TEST_PREC));
494 }
495 
496 TEST(SpatialAlgebraTests, TestSpatialTransformMultiplyEqual)
497 {
498  Vector3d rot(1.1, 1.2, 1.3);
499  Vector3d trans(1.1, 1.2, 1.3);
500 
501  SpatialMatrix X_matrix_1(SpatialMatrix::Zero(6, 6));
502  SpatialMatrix X_matrix_2(SpatialMatrix::Zero(6, 6));
503 
504  X_matrix_1 = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
505  X_matrix_2 = Xrotz_mat(rot[2]) * Xroty_mat(rot[1]) * Xrotx_mat(rot[0]) * Xtrans_mat(trans);
506 
507  SpatialTransform X_st_1;
508  SpatialTransform X_st_2;
509 
510  X_st_1.E = X_matrix_1.block<3, 3>(0, 0);
511  X_st_1.r = trans;
512  X_st_2.E = X_matrix_2.block<3, 3>(0, 0);
513  X_st_2.r = trans;
514 
515  SpatialTransform X_st_res = X_st_1;
516  X_st_res *= X_st_2;
517  SpatialMatrix X_matrix_res = X_matrix_1 * X_matrix_2;
518 
519  // SpatialMatrix X_diff = X_st_res.toMatrix() - X_matrix_res;
520  // cout << "Error: " << endl << X_diff << endl;
521 
522  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(X_st_res.toMatrix(), X_matrix_res, unit_test_utils::TEST_PREC));
523 }
524 
525 TEST(SpatialAlgebraTests, TestXrotAxis)
526 {
527  SpatialTransform X_rotX = Xrotx(M_PI * 0.15);
528  SpatialTransform X_rotX_axis = Xrot(M_PI * 0.15, Vector3d(1., 0., 0.));
529 
530  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(X_rotX.toMatrix(), X_rotX_axis.toMatrix(), unit_test_utils::TEST_PREC));
531 
532  // all the other axes
533  SpatialTransform X_rotX_90 = Xrotx(M_PI * 0.5);
534  SpatialTransform X_rotX_90_axis = Xrot(M_PI * 0.5, Vector3d(1., 0., 0.));
535 
536  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(X_rotX_90.toMatrix(), X_rotX_90_axis.toMatrix(), TEST_PREC));
537 
538  SpatialTransform X_rotY_90 = Xroty(M_PI * 0.5);
539  SpatialTransform X_rotY_90_axis = Xrot(M_PI * 0.5, Vector3d(0., 1., 0.));
540 
541  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(X_rotY_90.toMatrix(), X_rotY_90_axis.toMatrix(), TEST_PREC));
542 
543  SpatialTransform X_rotZ_90 = Xrotz(M_PI * 0.5);
544  SpatialTransform X_rotZ_90_axis = Xrot(M_PI * 0.5, Vector3d(0., 0., 1.));
545 
546  EXPECT_TRUE(unit_test_utils::checkSpatialMatrixEpsilonClose(X_rotZ_90.toMatrix(), X_rotZ_90_axis.toMatrix(), TEST_PREC));
547 }
548 
549 int main(int argc, char **argv)
550 {
551  ::testing::InitGoogleTest(&argc, argv);
552  return RUN_ALL_TESTS();
553 }
RobotDynamics::Math::toIntrinsicZYXAngles
static Vector3d toIntrinsicZYXAngles(const Quaternion &q, double yaw_at_pitch_singularity=0.)
Converte quaternion to intrinsic ZYX euler angles.
Definition: SpatialAlgebraOperators.h:34
spatial_adjoint
SpatialMatrix spatial_adjoint(const SpatialMatrix &m)
Definition: SpatialAlgebraTests.cc:17
RobotDynamics::Math::SpatialTransform::toMatrixTranspose
SpatialMatrix toMatrixTranspose() const
Returns spatial force transform transposed.
Definition: rdl_eigenmath.h:470
RobotDynamics::Math::Xrotx_mat
SpatialMatrix Xrotx_mat(const double &xrot)
Creates a rotational transformation around the X-axis.
Definition: rdl_mathutils.cc:138
RobotDynamics::Math::intrinsicXYZAnglesToQuaternion
static Quaternion intrinsicXYZAnglesToQuaternion(double roll, double pitch, double yaw)
Convert RPY angles to quaternion.
Definition: SpatialAlgebraOperators.h:114
RobotDynamics::Math::Xroty
SpatialTransform Xroty(const double &yrot)
Get transform with zero translation and pure rotation about y axis.
Definition: SpatialAlgebraOperators.h:286
RobotDynamics::Math::crossf
SpatialMatrix crossf(const SpatialVector &v)
Get the spatial force cross matrix.
Definition: SpatialAlgebraOperators.h:403
Body.h
unit_test_utils::TEST_PREC
const double TEST_PREC
Definition: UnitTestUtils.hpp:22
RobotDynamics::Math::SpatialTransform::applyTranspose
SpatialVector applyTranspose(const SpatialVector &f_sp) const
Applies .
Definition: rdl_eigenmath.h:405
RobotDynamics::Math::SpatialTransform::toMatrix
SpatialMatrix toMatrix() const
Return transform as 6x6 spatial matrix.
Definition: rdl_eigenmath.h:434
RobotDynamics::Math::Xtrans
SpatialTransform Xtrans(const Vector3d &r)
Get pure translation transform.
Definition: SpatialAlgebraOperators.h:369
spatial_inverse
SpatialMatrix spatial_inverse(const SpatialMatrix &m)
Definition: SpatialAlgebraTests.cc:25
RobotDynamics::Math::Xtrans_mat
SpatialMatrix Xtrans_mat(const Vector3d &displacement)
Creates a transformation of a linear displacement.
Definition: rdl_mathutils.cc:133
RobotDynamics::Math::toQuaternion
static Quaternion toQuaternion(const Vector3d &axis, double angle_rad)
Get quaternion representation of axis and angle.
Definition: SpatialAlgebraOperators.h:63
TEST
TEST(SpatialAlgebraTests, euler)
Definition: SpatialAlgebraTests.cc:45
unit_test_utils::checkVector4dEpsilonClose
static bool checkVector4dEpsilonClose(const RobotDynamics::Math::Vector4d &v1, const RobotDynamics::Math::Vector4d &v2, const double epsilon)
Definition: UnitTestUtils.hpp:275
RobotDynamics::Math::intrinsicYXZAnglesToQuaternion
static Quaternion intrinsicYXZAnglesToQuaternion(double pitch, double roll, double yaw)
Convert PRY angles to quaternion.
Definition: SpatialAlgebraOperators.h:98
RobotDynamics::Math::Xrotz
SpatialTransform Xrotz(const double &zrot)
Get transform with zero translation and pure rotation about z axis.
Definition: SpatialAlgebraOperators.h:300
TEST_PREC
const double TEST_PREC
Definition: SpatialAlgebraTests.cc:15
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::Math::SpatialTransform::apply
SpatialVector apply(const SpatialVector &v_sp) const
Transform a spatial vector. Same as .
Definition: rdl_eigenmath.h:390
RobotDynamics::Math::SpatialTransform::inverse
SpatialTransform inverse() const
Returns inverse of transform.
Definition: rdl_eigenmath.h:488
RobotDynamics::Math::Quaternion
Quaternion that are used for singularity free joints.
Definition: Quaternion.h:25
RobotDynamics::Math::Xrotx
SpatialTransform Xrotx(const double &xrot)
Get transform with zero translation and pure rotation about x axis.
Definition: SpatialAlgebraOperators.h:272
RobotDynamics::Math::SpatialTransform::toMatrixAdjoint
SpatialMatrix toMatrixAdjoint() const
Returns Spatial transform that transforms spatial force vectors.
Definition: rdl_eigenmath.h:452
get_rotation
Matrix3d get_rotation(const SpatialMatrix &m)
Definition: SpatialAlgebraTests.cc:35
RobotDynamics::Math::Xroty_mat
SpatialMatrix Xroty_mat(const double &yrot)
Creates a rotational transformation around the Y-axis.
Definition: rdl_mathutils.cc:147
unit_test_utils::checkSpatialMatrixEpsilonClose
static bool checkSpatialMatrixEpsilonClose(const RobotDynamics::Math::SpatialMatrix &t1, const RobotDynamics::Math::SpatialMatrix &t2, const double epsilon)
Definition: UnitTestUtils.hpp:122
RobotDynamics::Math
Math types such as vectors and matrices and utility functions.
Definition: ForceVector.hpp:21
unit_test_utils::checkSpatialVectorsEpsilonClose
static bool checkSpatialVectorsEpsilonClose(const RobotDynamics::Math::SpatialVector &v1, const RobotDynamics::Math::SpatialVector &v2, const double epsilon)
Definition: UnitTestUtils.hpp:306
get_translation
Vector3d get_translation(const SpatialMatrix &m)
Definition: SpatialAlgebraTests.cc:40
main
int main(int argc, char **argv)
Definition: SpatialAlgebraTests.cc:549
RobotDynamics::Math::SpatialTransform::invert
void invert()
Inverts in place. .
Definition: rdl_eigenmath.h:496
RobotDynamics::Math::XeulerZYX
SpatialTransform XeulerZYX(double yaw, double pitch, double roll)
Get transform with zero translation and intrinsic euler z/y/x rotation.
Definition: SpatialAlgebraOperators.h:316
RobotDynamics::Math::SpatialTransform::applyAdjoint
SpatialVector applyAdjoint(const SpatialVector &f_sp) const
Applies where is a spatial force.
Definition: rdl_eigenmath.h:421
RobotDynamics::Math::crossm
SpatialMatrix crossm(const SpatialVector &v)
Get the spatial motion cross matrix.
Definition: SpatialAlgebraOperators.h:379
RobotDynamics::Math::Xrotz_mat
SpatialMatrix Xrotz_mat(const double &zrot)
Creates a rotational transformation around the Z-axis.
Definition: rdl_mathutils.cc:156
RobotDynamics::Math::AxisAngle
Eigen::AngleAxisd AxisAngle
Definition: rdl_eigenmath.h:27
RobotDynamics::Math::intrinsicZYXAnglesToQuaternion
static Quaternion intrinsicZYXAnglesToQuaternion(double yaw, double pitch, double roll)
Convert YPR angles to quaternion.
Definition: SpatialAlgebraOperators.h:82
RobotDynamics::Math::toMatrix
static Matrix3d toMatrix(const Quaternion &q)
Convert quaternion to rotation matrix.
Definition: SpatialAlgebraOperators.h:187
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21
unit_test_utils::checkMatrix3dEpsilonClose
static bool checkMatrix3dEpsilonClose(const RobotDynamics::Math::Matrix3d &t1, const RobotDynamics::Math::Matrix3d &t2, const double epsilon)
Definition: UnitTestUtils.hpp:138
RobotDynamics::Math::Xrot
SpatialTransform Xrot(double angle_rad, const Vector3d &axis)
Get spatial transform from angle and axis.
Definition: SpatialAlgebraOperators.h:250
RobotDynamics::Math::toAxisAngle
static AxisAngle toAxisAngle(Quaternion q)
Get axis angle representation of a quaternion.
Definition: SpatialAlgebraOperators.h:132

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:28