Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Rotation.cpp
1#define BIORBD_API_EXPORTS
2#include "Utils/Rotation.h"
3
4#include "RigidBody/NodeSegment.h"
5#include "Utils/Error.h"
6#include "Utils/SpatialTransform.h"
7#include "Utils/String.h"
8#include "Utils/Vector.h"
9#include "Utils/Vector3d.h"
10
11using namespace BIORBD_NAMESPACE;
12
14 : utils::Matrix3d(matrix) {
16}
17
18#ifdef BIORBD_USE_CASADI_MATH
19
20utils::Rotation::Rotation(const RBDLCasadiMath::MX_Xd_static<3, 3>& other)
21 : utils::Matrix3d(other) {}
22
23utils::Rotation::Rotation(const RBDLCasadiMath::MX_Xd_SubMatrix& other)
24 : utils::Matrix3d(other) {}
25#endif
26
28 const utils::Scalar& v00,
29 const utils::Scalar& v01,
30 const utils::Scalar& v02,
31 const utils::Scalar& v10,
32 const utils::Scalar& v11,
33 const utils::Scalar& v12,
34 const utils::Scalar& v20,
35 const utils::Scalar& v21,
36 const utils::Scalar& v22)
37 : utils::Matrix3d(v00, v01, v02, v10, v11, v12, v20, v21, v22) {}
38
40 const utils::Vector& rotation,
41 const utils::String& rotationSequence)
42 : utils::Matrix3d(fromEulerAngles(rotation, rotationSequence)) {}
43
46
48 utils::Error::check(idx <= 2, "Axis must be between 0 and 2 included");
49 return this->block(0, static_cast<unsigned int>(idx), 3, 1);
50}
51
56
58 const utils::Vector& rot,
59 const utils::String& seq) {
60 // Check for size consistency
62 seq.length() == static_cast<size_t>(rot.rows()),
63 "Rotation and sequence of rotation must be the same length");
64
66 out.setIdentity();
67 // Set the actual rotation matrix to this
69 for (size_t i = 0; i < seq.length(); ++i) {
70 utils::Scalar cosVi(std::cos(rot[static_cast<unsigned int>(i)]));
71 utils::Scalar sinVi(std::sin(rot[static_cast<unsigned int>(i)]));
72 if (seq.tolower()[i] == 'x')
73 tp = utils::Matrix3d(1, 0, 0, 0, cosVi, -sinVi, 0, sinVi, cosVi);
74
75 else if (seq.tolower()[i] == 'y')
76 tp = utils::Matrix3d(cosVi, 0, sinVi, 0, 1, 0, -sinVi, 0, cosVi);
77
78 else if (seq.tolower()[i] == 'z')
79 tp = utils::Matrix3d(cosVi, -sinVi, 0, sinVi, cosVi, 0, 0, 0, 1);
80 else {
81 utils::Error::raise("Rotation sequence not recognized");
82 }
83
84 out.block(0, 0, 3, 3) = out.block(0, 0, 3, 3) * tp;
85 }
86 return out;
87}
88
90 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
91 axis1markers,
92 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
93 axis2markers,
94 const std::pair<utils::String, utils::String>& axesNames,
95 const utils::String& axisToRecalculate) {
96 if (!axesNames.first.compare("") || !axesNames.second.compare("")) {
98 "axesNames must be defined with a pair of \"x\", \"y\" or \"z\"");
99 }
100
101 // Figure out where to put the axes
102 std::vector<size_t> map(3);
103 std::vector<size_t> toMultiply(2);
104 if (!axesNames.first.tolower().compare("x")) {
105 map[0] = 0;
106 if (!axesNames.second.tolower().compare("y")) {
107 map[1] = 1;
108 map[2] = 2;
109 toMultiply[0] = 0;
110 toMultiply[1] = 1;
111 } else {
112 map[1] = 2;
113 map[2] = 1;
114 toMultiply[0] = 2;
115 toMultiply[1] = 0;
116 }
117 } else if (!axesNames.first.tolower().compare("y")) {
118 map[0] = 1;
119 if (!axesNames.second.tolower().compare("x")) {
120 map[1] = 0;
121 map[2] = 2;
122 toMultiply[0] = 0;
123 toMultiply[1] = 1;
124 } else {
125 map[1] = 2;
126 map[2] = 0;
127 toMultiply[0] = 1;
128 toMultiply[1] = 2;
129 }
130 } else if (!axesNames.first.tolower().compare("z")) {
131 map[0] = 2;
132 if (!axesNames.second.tolower().compare("x")) {
133 map[1] = 0;
134 map[2] = 1;
135 toMultiply[0] = 2;
136 toMultiply[1] = 0;
137 } else {
138 map[1] = 1;
139 map[2] = 0;
140 toMultiply[0] = 1;
141 toMultiply[1] = 2;
142 }
143 }
144
145 // Get the system of axis XYZ
146 std::vector<utils::Vector3d> axes(3);
147 axes[map[0]] = axis1markers.second - axis1markers.first;
148 axes[map[1]] = axis2markers.second - axis2markers.first;
149 axes[map[2]] = axes[toMultiply[0]].cross(axes[toMultiply[1]]);
150
151 // Recalculate one axis
152 if (!axisToRecalculate.tolower().compare("x")) {
153 axes[0] = axes[1].cross(axes[2]);
154 } else if (!axisToRecalculate.tolower().compare("y")) {
155 axes[1] = axes[2].cross(axes[0]);
156 } else if (!axisToRecalculate.tolower().compare("z")) {
157 axes[2] = axes[0].cross(axes[1]);
158 }
159
160 // Organize them in a non-normalized matrix
161 utils::Rotation r_out;
162 for (size_t i = 0; i < 3; ++i) {
163 r_out.block(0, static_cast<unsigned int>(i), 3, 1) = axes[i];
164 }
165 return r_out;
166}
167
169 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
170 axis1markers,
171 const std::pair<rigidbody::NodeSegment, rigidbody::NodeSegment>&
172 axis2markers,
173 const std::pair<utils::String, utils::String>& axesNames,
174 const utils::String& axisToRecalculate) {
176 axis1markers, axis2markers, axesNames, axisToRecalculate));
177
178 // Organize them in a normalized matrix
179 for (size_t i = 0; i < 3; ++i) {
180 // Normalize axes
181 r_out.block<3, 1>(0, static_cast<unsigned int>(i)).normalize();
182 }
183
184 return r_out;
185}
186
188 const utils::Rotation& r,
189 const utils::String& seq) {
191 if (!seq.compare("zyzz")) {
192 v = utils::Vector(3);
193 } else {
194 v = utils::Vector(seq.length());
195 }
196
197 // sequences taken from
198 // https://pdfslide.net/documents/euler-angles-58fcebd3620f7.html
199 if (!seq.compare("x")) {
200 v[0] = std::asin(r(2, 1)); // x
201 } else if (!seq.compare("y")) {
202 v[0] = std::asin(r(0, 2)); // y
203 } else if (!seq.compare("z")) {
204 v[0] = std::asin(r(1, 0)); // z
205 } else if (!seq.compare("xy")) {
206 v[0] = std::asin(r(2, 1)); // x
207 v[1] = std::asin(r(0, 2)); // y
208 } else if (!seq.compare("xz")) {
209 v[0] = -std::asin(r(1, 2)); // x
210 v[1] = -std::asin(r(0, 1)); // z
211 } else if (!seq.compare("yx")) {
212 v[0] = -std::asin(r(2, 0)); // y
213 v[1] = -std::asin(r(1, 2)); // x
214 } else if (!seq.compare("yz")) {
215 v[0] = std::asin(r(0, 2)); // y
216 v[1] = std::asin(r(1, 0)); // z
217 } else if (!seq.compare("zx")) {
218 v[0] = std::asin(r(1, 0)); // z
219 v[1] = std::asin(r(2, 1)); // x
220 } else if (!seq.compare("zy")) {
221 v[0] = -std::asin(r(0, 1)); // z
222 v[1] = -std::asin(r(2, 0)); // y
223 } else if (!seq.compare("xyz")) {
224 v[0] = std::atan2(-r(1, 2), r(2, 2)); // x
225 v[1] = std::asin(r(0, 2)); // y
226 v[2] = std::atan2(-r(0, 1), r(0, 0)); // z
227 } else if (!seq.compare("xzy")) {
228 v[0] = std::atan2(r(2, 1), r(1, 1)); // x
229 v[1] = std::asin(-r(0, 1)); // z
230 v[2] = std::atan2(r(0, 2), r(0, 0)); // y
231 } else if (!seq.compare("yxy")) {
232 v[0] = std::atan2(r(0, 1), r(2, 1)); // y
233 v[1] = std::acos(r(1, 1)); // x
234 v[2] = std::atan2(r(1, 0), -r(1, 2)); // y
235 } else if (!seq.compare("yxz")) {
236 v[0] = std::atan2(r(0, 2), r(2, 2)); // y
237 v[1] = std::asin(-r(1, 2)); // x
238 v[2] = std::atan2(r(1, 0), r(1, 1)); // z
239 } else if (!seq.compare("yzx")) {
240 v[0] = std::atan2(-r(2, 0), r(0, 0)); // y
241 v[1] = std::asin(r(1, 0)); // z
242 v[2] = std::atan2(-r(1, 2), r(1, 1)); // x
243 } else if (!seq.compare("yzy")) {
244 v[0] = std::atan2(r(2, 1), -r(0, 1)); // y
245 v[1] = std::acos(r(1, 1)); // z
246 v[2] = std::atan2(r(1, 2), r(1, 0)); // y
247 } else if (!seq.compare("zxy")) {
248 v[0] = std::atan2(-r(0, 1), r(1, 1)); // z
249 v[1] = std::asin(r(2, 1)); // x
250 v[2] = std::atan2(-r(2, 0), r(2, 2)); // y
251 } else if (!seq.compare("zyx")) {
252 v[0] = std::atan2(r(1, 0), r(0, 0)); // z
253 v[1] = std::asin(-r(2, 0)); // y
254 v[2] = std::atan2(r(2, 1), r(2, 2)); // x
255 } else if (!seq.compare("zxz")) {
256 v[0] = std::atan2(r(0, 2), -r(1, 2)); // z
257 v[1] = std::acos(r(2, 2)); // x
258 v[2] = std::atan2(r(2, 0), r(2, 1)); // z
259 } else if (!seq.compare("zyz")) {
260 v[0] = std::atan2(r(1, 2), r(0, 2)); // z
261 v[1] = std::acos(r(2, 2)); // y
262 v[2] = std::atan2(r(2, 1), -r(2, 0)); // z
263 } else if (!seq.compare("zyzz")) {
264 v[0] = std::atan2(r(1, 2), r(0, 2)); // z
265 v[1] = std::acos(r(2, 2)); // y
266 v[2] = std::atan2(r(2, 1), -r(2, 0)) + v[0]; // z+z
267 } else {
268 utils::Error::raise("Angle sequence is not recognized");
269 }
270
271 return v;
272}
273
274#ifndef BIORBD_USE_CASADI_MATH
276 const std::vector<utils::Rotation>& mToMean) {
277 utils::Matrix3d m_tp;
278 m_tp.setZero();
279
280 // Initial guess being the actual arithmetic mean
281 for (size_t i = 0; i < mToMean.size(); ++i) {
282 m_tp += mToMean[i];
283 }
284 m_tp = m_tp / mToMean.size();
285
286 // SVD decomposition
287 Eigen::JacobiSVD<Eigen::Matrix3d> svd(
288 m_tp, Eigen::ComputeFullU | Eigen::ComputeFullV);
289
290 // Normalize the matrix
291 utils::Rotation m_out(svd.matrixU() * svd.matrixV().transpose());
292
293 return m_out;
294}
295#endif
296
298#ifndef BIORBD_USE_CASADI_MATH
299#ifndef SKIP_ASSERT
300 double sqrtNorm = static_cast<double>(this->squaredNorm());
301 bool checkUnit(fabs(sqrtNorm - 3.) < 1e-4);
302 if (!checkUnit) {
305 utils::String("The Rotation matrix square norm is equal to ") +
306 sqrtNorm +
307 ", but should be equal to 3. Consider replacing the RT matrix by this "
308 "closest orthonormal matrix \n[" +
309 R(0, 0) + ", " + R(0, 1) + ", " + R(0, 2) + "\n" + R(1, 0) + ", " +
310 R(1, 1) + ", " + R(1, 2) + "\n" + R(2, 0) + ", " + R(2, 1) + ", " +
311 R(2, 2) + "]\n" +
312 "Alternatively, you can compile with SKIP_ASSERT set to ON to turn off "
313 "this error message");
314 }
315
316#endif
317#endif
318}
319
320std::ostream& operator<<(std::ostream& os, const utils::Rotation& a) {
321 os << a.block(0, 0, 3, 3);
322 return os;
323}
324
325#ifdef BIORBD_USE_CASADI_MATH
326void utils::Rotation::operator=(
327 const RBDLCasadiMath::MX_Xd_static<3, 3>& other) {
328 this->Matrix3d::operator=(other);
329}
330
331void utils::Rotation::operator=(const RBDLCasadiMath::MX_Xd_SubMatrix& other) {
332 this->Matrix3d::operator=(other);
333}
334#endif
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
A wrapper for the Eigen::MatrixXd.
Definition Matrix3d.h:21
utils::Matrix3d orthoNormalize() const
Produce an orthonormal matrix from the current matrix.
Definition Matrix3d.cpp:56
Matrix3d()
Construct matrix.
Definition Matrix3d.cpp:9
static utils::Rotation mean(const std::vector< utils::Rotation > &mToMean)
Get the mean of the Rotation matrices.
Definition Rotation.cpp:275
static Rotation fromMarkersNonNormalized(const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis1markers, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis2markers, const std::pair< String, String > &axesNames, const String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes defined by markers
Definition Rotation.cpp:89
static Rotation fromEulerAngles(const Vector &rot, const String &seq)
Create a Rotation from Euler angles.
Definition Rotation.cpp:57
void checkUnitary()
Check if the Rotation is a unitary matrix of rotation.
Definition Rotation.cpp:297
Vector3d axe(size_t idx) const
Get a particular axis of the Rotation matrix.
Definition Rotation.cpp:47
static Rotation fromSpatialTransform(const utils::SpatialTransform &st)
set the Rotation from a spatial transform
Definition Rotation.cpp:52
static Vector toEulerAngles(const Rotation &r, const String &seq)
Return extracted angles from the rotation matrix into Euler angles using the provided sequence.
Definition Rotation.cpp:187
static Rotation fromMarkers(const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis1markers, const std::pair< rigidbody::NodeSegment, rigidbody::NodeSegment > &axis2markers, const std::pair< String, String > &axesNames, const String &axisToRecalculate)
fromMarkers Creates a system of axes from two axes defined by markers
Definition Rotation.cpp:168
Rotation(const Matrix3d &matrix=Matrix3d::Identity())
Construct Rotation matrix.
Definition Rotation.cpp:13
Wrapper of the SpatialTransform (rotation and translation).
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
static String tolower(const String &str)
Convert a string to a lower case string.
Definition String.cpp:76
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Wrapper of the Eigen VectorXd.
Definition Vector.h:21