matrix-twist.cpp
Go to the documentation of this file.
1 // Copyright 2011 Thomas Moulard.
2 
3 #include <sstream>
4 
5 #define BOOST_TEST_MODULE matrix_twist
6 
7 #include <boost/test/floating_point_comparison.hpp>
8 #include <boost/test/output_test_stream.hpp>
9 #include <boost/test/unit_test.hpp>
10 #include <sot/core/matrix-geometry.hh>
11 
12 using boost::test_tools::output_test_stream;
13 
14 #define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
15  for (unsigned i = 0; i < N; ++i) \
16  for (unsigned j = 0; j < M; ++j) \
17  BOOST_REQUIRE_CLOSE(LEFT(i, j), RIGHT(i, j), TOLERANCE)
18 
19 #define MATRIX_6x6_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
20  MATRIX_BOOST_REQUIRE_CLOSE(6, 6, LEFT, RIGHT, TOLERANCE)
21 
22 #define MATRIX_4x4_INIT(M, A00, A01, A02, A03, A10, A11, A12, A13, A20, A21, \
23  A22, A23, A30, A31, A32, A33) \
24  M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03; \
25  M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13; \
26  M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23; \
27  M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33
28 
29 #define MATRIX_6x6_INIT(M, A00, A01, A02, A03, A04, A05, A10, A11, A12, A13, \
30  A14, A15, A20, A21, A22, A23, A24, A25, A30, A31, A32, \
31  A33, A34, A35, A40, A41, A42, A43, A44, A45, A50, A51, \
32  A52, A53, A54, A55) \
33  M(0, 0) = A00, M(0, 1) = A01, M(0, 2) = A02, M(0, 3) = A03, M(0, 4) = A04, \
34  M(0, 5) = A05; \
35  M(1, 0) = A10, M(1, 1) = A11, M(1, 2) = A12, M(1, 3) = A13, M(1, 4) = A14, \
36  M(1, 5) = A15; \
37  M(2, 0) = A20, M(2, 1) = A21, M(2, 2) = A22, M(2, 3) = A23, M(2, 4) = A24, \
38  M(2, 5) = A25; \
39  M(3, 0) = A30, M(3, 1) = A31, M(3, 2) = A32, M(3, 3) = A33, M(3, 4) = A34, \
40  M(3, 5) = A35; \
41  M(4, 0) = A40, M(4, 1) = A41, M(4, 2) = A42, M(4, 3) = A43, M(4, 4) = A44, \
42  M(4, 5) = A45; \
43  M(5, 0) = A50, M(5, 1) = A51, M(5, 2) = A52, M(5, 3) = A53, M(5, 4) = A54, \
44  M(5, 5) = A55
45 
46 // For reminder:
47 //
48 // R \in R^3 x R^3
49 // M = [R ; t] \in R^4 x R^4
50 // [0 ; 1]
51 //
52 // Twist(M) = [R ; t^ R] \in R^6 x R^6
53 // [0 ; R]
54 //
55 // t^ R \in R^3xR^3
56 //
57 //
58 // Twist(M)^{-1} = [R^T ; -R^T t^]
59 // [0 ; R^T ]
60 
61 BOOST_AUTO_TEST_CASE(constructor_trivial_identity) {
62  dynamicgraph::sot::MatrixHomogeneous M(Eigen::Matrix4d::Identity());
63  dynamicgraph::sot::MatrixTwist twist;
64  dynamicgraph::sot::buildFrom(M, twist);
65 
66  dynamicgraph::Matrix twistRef(6, 6);
67 
68  for (unsigned i = 0; i < 6; ++i)
69  for (unsigned j = 0; j < 6; ++j) twistRef(i, j) = (i == j) ? 1. : 0.;
70 
71  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
72 }
73 
74 BOOST_AUTO_TEST_CASE(constructor_rotation_only) {
75  dynamicgraph::sot::MatrixHomogeneous M;
76 
77  MATRIX_4x4_INIT(M, 0., 0., 1., 0., 1., 0., 0., 0., 0., -1., 0., 0., 0., 0.,
78  0., 1.);
79  dynamicgraph::sot::MatrixTwist twist;
80  dynamicgraph::sot::buildFrom(M, twist);
81  dynamicgraph::Matrix twistRef(6, 6);
82  MATRIX_6x6_INIT(twistRef, 0., 0., 1., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.,
83  -1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 1.,
84  0., 0., 0., 0., 0., 0., -1., 0.);
85 
86  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
87 }
88 
89 BOOST_AUTO_TEST_CASE(constructor_translation_only) {
90  dynamicgraph::sot::MatrixHomogeneous M;
91 
92  double tx = 11.;
93  double ty = 22.;
94  double tz = 33.;
95 
96  MATRIX_4x4_INIT(M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
97  1.);
98  dynamicgraph::sot::MatrixTwist twist;
99  dynamicgraph::sot::buildFrom(M, twist);
100 
101  dynamicgraph::Matrix twistRef(6, 6);
102  MATRIX_6x6_INIT(twistRef, 1., 0., 0., 0., -tz, ty, 0., 1., 0., tz, 0., -tx,
103  0., 0., 1., -ty, tx, 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
104  0., 1., 0., 0., 0., 0., 0., 0., 1.);
105 
106  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
107 }
108 
109 BOOST_AUTO_TEST_CASE(constructor_rotation_translation) {
110  dynamicgraph::sot::MatrixHomogeneous M;
111 
112  double tx = 11.;
113  double ty = 22.;
114  double tz = 33.;
115 
116  MATRIX_4x4_INIT(M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
117  0., 1.);
118  dynamicgraph::sot::MatrixTwist twist;
119  dynamicgraph::sot::buildFrom(M, twist);
120 
121  dynamicgraph::Matrix twistRef(6, 6);
122  MATRIX_6x6_INIT(twistRef, 0., 0., 1., ty, tz, 0., 0., -1., 0., -tx, 0., tz,
123  1., 0., 0., 0., -tx, -ty, 0., 0., 0., 0., 0., 1., 0., 0., 0.,
124  0., -1., 0., 0., 0., 0., 1., 0., 0.);
125 
126  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twist, twistRef, 0.001);
127 }
128 
129 BOOST_AUTO_TEST_CASE(inverse_translation_only) {
130  dynamicgraph::sot::MatrixHomogeneous M;
131 
132  double tx = 11.;
133  double ty = 22.;
134  double tz = 33.;
135 
136  MATRIX_4x4_INIT(M, 1., 0., 0., tx, 0., 1., 0., ty, 0., 0., 1., tz, 0., 0., 0.,
137  1.);
138  dynamicgraph::sot::MatrixTwist twist;
139  dynamicgraph::sot::buildFrom(M, twist);
140 
141  dynamicgraph::sot::MatrixTwist twistInv(twist.inverse());
142 
143  dynamicgraph::sot::MatrixTwist twistInv_;
144  twistInv_ = twist.inverse();
145 
146  dynamicgraph::Matrix twistRef(6, 6);
147  MATRIX_6x6_INIT(twistRef, 1., 0., 0., 0., tz, -ty, 0., 1., 0., -tz, 0., tx,
148  0., 0., 1., ty, -tx, 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
149  0., 1., 0., 0., 0., 0., 0., 0., 1.);
150 
151  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv, twistRef, 0.001);
152  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv_, twistRef, 0.001);
153 }
154 
155 BOOST_AUTO_TEST_CASE(inverse_translation_rotation) {
156  dynamicgraph::sot::MatrixHomogeneous M;
157 
158  double tx = 11.;
159  double ty = 22.;
160  double tz = 33.;
161 
162  MATRIX_4x4_INIT(M, 0., 0., 1., tx, 0., -1., 0., ty, 1., 0., 0., tz, 0., 0.,
163  0., 1.);
164  dynamicgraph::sot::MatrixTwist twist;
165  dynamicgraph::sot::buildFrom(M, twist);
166 
167  dynamicgraph::sot::MatrixTwist twistInv(twist.inverse());
168 
169  dynamicgraph::sot::MatrixTwist twistInv_;
170  twistInv_ = twist.inverse();
171 
172  dynamicgraph::Matrix twistRef(6, 6);
173  MATRIX_6x6_INIT(twistRef, 0., 0., 1., ty, -tx, -0., 0., -1., 0., tz, -0., -tx,
174  1., 0., 0., -0., tz, -ty, 0., 0., 0., 0., 0., 1., 0., 0., 0.,
175  0., -1., 0., 0., 0., 0., 1., 0., 0.);
176 
177  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv, twistRef, 0.001);
178  MATRIX_6x6_BOOST_REQUIRE_CLOSE(twistInv_, twistRef, 0.001);
179 }
dynamicgraph::sot::MatrixHomogeneous
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:75
i
int i
MATRIX_4x4_INIT
#define MATRIX_4x4_INIT(M, A00, A01, A02, A03, A10, A11, A12, A13, A20, A21, A22, A23, A30, A31, A32, A33)
Definition: matrix-twist.cpp:22
MATRIX_6x6_BOOST_REQUIRE_CLOSE
#define MATRIX_6x6_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE)
Definition: matrix-twist.cpp:19
dynamicgraph::sot::buildFrom
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
Definition: matrix-geometry.hh:88
MATRIX_6x6_INIT
#define MATRIX_6x6_INIT(M, A00, A01, A02, A03, A04, A05, A10, A11, A12, A13, A14, A15, A20, A21, A22, A23, A24, A25, A30, A31, A32, A33, A34, A35, A40, A41, A42, A43, A44, A45, A50, A51, A52, A53, A54, A55)
Definition: matrix-twist.cpp:29
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(constructor_trivial_identity)
Definition: matrix-twist.cpp:61
dynamicgraph::sot::MatrixTwist
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
Definition: matrix-geometry.hh:82
dynamicgraph::Matrix
Eigen::MatrixXd Matrix
M
M

sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26