Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Quaternion.cpp
1#define BIORBD_API_EXPORTS
2#include "Utils/Quaternion.h"
3
4#include "Utils/Error.h"
5#include "Utils/Matrix3d.h"
6#include "Utils/Rotation.h"
7#include "Utils/RotoTrans.h"
8#include "Utils/Vector.h"
9#include "Utils/Vector3d.h"
10
11#ifdef USE_SMOOTH_IF_ELSE
12#include "Utils/CasadiExpand.h"
13#endif
14
15using namespace BIORBD_NAMESPACE;
16
17utils::Quaternion::Quaternion(const utils::Scalar& kStabilizer)
18 : RigidBodyDynamics::Math::Vector4d(1, 0, 0, 0), m_Kstab(kStabilizer) {}
19
21 : RigidBodyDynamics::Math::Vector4d(other), m_Kstab(other.m_Kstab) {}
22
24 const RigidBodyDynamics::Math::Vector4d& vec4,
25 const utils::Scalar& kStabilizer)
26 : RigidBodyDynamics::Math::Vector4d(vec4), m_Kstab(kStabilizer) {}
27
29 const utils::Scalar& w,
30 const utils::Scalar& x,
31 const utils::Scalar& y,
32 const utils::Scalar& z,
33 const utils::Scalar& kStabilizer)
34 : RigidBodyDynamics::Math::Vector4d(w, x, y, z), m_Kstab(kStabilizer) {}
35
37 const utils::Scalar& w,
38 const utils::Vector3d& vec3,
39 const utils::Scalar& kStabilizer)
40 : RigidBodyDynamics::Math::Vector4d(w, vec3[0], vec3[1], vec3[2]),
41 m_Kstab(kStabilizer) {}
42
43utils::Scalar utils::Quaternion::w() const { return (*this)(0); }
44utils::Scalar utils::Quaternion::x() const { return (*this)(1); }
45utils::Scalar utils::Quaternion::y() const { return (*this)(2); }
46utils::Scalar utils::Quaternion::z() const { return (*this)(3); }
47
48void utils::Quaternion::setKStab(const utils::Scalar& newKStab) {
49 m_Kstab = newKStab;
50}
51
52utils::Scalar utils::Quaternion::kStab() const { return m_Kstab; }
53
55 const utils::Quaternion& q) const {
56 return utils::Quaternion(
57 (*this)[0] * q[0] - (*this)[1] * q[1] - (*this)[2] * q[2] -
58 (*this)[3] * q[3],
59 (*this)[0] * q[1] + (*this)[1] * q[0] + (*this)[2] * q[3] -
60 (*this)[3] * q[2],
61 (*this)[0] * q[2] + (*this)[2] * q[0] + (*this)[3] * q[1] -
62 (*this)[1] * q[3],
63 (*this)[0] * q[3] + (*this)[3] * q[0] + (*this)[1] * q[2] -
64 (*this)[2] * q[1],
65 (this->m_Kstab + q.m_Kstab) / 2);
66}
67
69 const utils::Scalar& scalar) const {
70 return utils::Quaternion(
71 this->RigidBodyDynamics::Math::Vector4d::operator*(scalar),
72 this->m_Kstab);
73}
74
76 return utils::Quaternion(
77 this->RigidBodyDynamics::Math::Vector4d::operator*(
78 static_cast<double>(scalar)),
79 this->m_Kstab);
80}
81
82#ifdef BIORBD_USE_CASADI_MATH
84 return utils::Quaternion(
85 this->RigidBodyDynamics::Math::Vector4d::operator*(scalar),
86 this->m_Kstab);
87}
88#endif
89
91 const utils::Quaternion& other) const {
92 return utils::Quaternion(
93 this->RigidBodyDynamics::Math::Vector4d::operator+(other),
94 (this->m_Kstab + other.m_Kstab) / 2);
95}
96
98 const utils::Quaternion& other) const {
99 return utils::Quaternion(
100 this->RigidBodyDynamics::Math::Vector4d::operator-(other),
101 (this->m_Kstab + other.m_Kstab) / 2);
102}
103
105 const utils::Scalar& angle,
106 const utils::Scalar& x,
107 const utils::Scalar& y,
108 const utils::Scalar& z,
109 const utils::Scalar& kStab) {
110 utils::Scalar angle_copy(angle);
111 utils::Scalar st = std::sin(angle_copy * M_PI / 360.);
112 return utils::Quaternion(
113 std::cos(angle_copy * M_PI / 360.), st * x, st * y, st * z, kStab);
114}
115
117 const utils::Scalar& angle,
118 const utils::Vector3d& axis,
119 const utils::Scalar& kStab) {
120 utils::Scalar angle_copy(angle);
121 utils::Scalar d = axis.norm();
122 utils::Scalar s2 = std::sin(angle_copy * 0.5) / d;
123 return utils::Quaternion(
124 std::cos(angle_copy * 0.5),
125 axis[0] * s2,
126 axis[1] * s2,
127 axis[2] * s2,
128 kStab);
129}
130
132 const utils::RotoTrans& rt,
133 const utils::Scalar& kStab) {
134 return fromMatrix(rt.rot(), kStab);
135}
136
138 const utils::Rotation& mat,
139 const utils::Scalar& kStab) {
140 utils::Scalar w = std::sqrt(1. + mat(0, 0) + mat(1, 1) + mat(2, 2)) * 0.5;
141 return Quaternion(
142 w,
143 (mat(2, 1) - mat(1, 2)) / (w * 4.),
144 (mat(0, 2) - mat(2, 0)) / (w * 4.),
145 (mat(1, 0) - mat(0, 1)) / (w * 4.),
146 kStab);
147}
148
150 const utils::Vector3d& zyx_angles,
151 const utils::Scalar& kStab) {
152 return fromAxisAngle(zyx_angles[2], utils::Vector3d(0., 0., 1.), kStab) *
153 fromAxisAngle(zyx_angles[1], utils::Vector3d(0., 1., 0.), kStab) *
154 fromAxisAngle(zyx_angles[0], utils::Vector3d(1., 0., 0.), kStab);
155}
156
158 const utils::Vector3d& yxz_angles,
159 const utils::Scalar& kStab) {
160 return fromAxisAngle(yxz_angles[1], utils::Vector3d(0., 1., 0.), kStab) *
161 fromAxisAngle(yxz_angles[0], utils::Vector3d(1., 0., 0.), kStab) *
162 fromAxisAngle(yxz_angles[2], utils::Vector3d(0., 0., 1.), kStab);
163}
164
166 const utils::Vector3d& xyz_angles,
167 const utils::Scalar& kStab) {
168 return fromAxisAngle(xyz_angles[0], utils::Vector3d(1., 0., 0.), kStab) *
169 fromAxisAngle(xyz_angles[1], utils::Vector3d(0., 1., 0.), kStab) *
170 fromAxisAngle(xyz_angles[2], utils::Vector3d(0., 0., 1.), kStab);
171}
172
174#ifndef BIORBD_USE_CASADI_MATH
175 if (!skipAsserts) {
177 fabs(this->squaredNorm() - 1.) < 1e-10,
178 "The Quaternion norm is not equal to one");
179 }
180#endif
181
182 utils::Scalar w = (*this)[0];
183 utils::Scalar x = (*this)[1];
184 utils::Scalar y = (*this)[2];
185 utils::Scalar z = (*this)[3];
187 1 - 2 * y * y - 2 * z * z,
188 2 * x * y - 2 * w * z,
189 2 * x * z + 2 * w * y,
190 2 * x * y + 2 * w * z,
191 1 - 2 * x * x - 2 * z * z,
192 2 * y * z - 2 * w * x,
193 2 * x * z - 2 * w * y,
194 2 * y * z + 2 * w * x,
195 1 - 2 * x * x - 2 * y * y);
196 return out;
197}
198
199#ifndef BIORBD_USE_CASADI_MATH
201 double alpha,
202 const utils::Quaternion& quat) const {
203 // check whether one of the two has 0 length
204 utils::Scalar s = std::sqrt(squaredNorm() * quat.squaredNorm());
205
206 // division by 0.f is unhealthy!
207#ifndef BIORBD_USE_CASADI_MATH
208 assert(s != 0.);
209#endif
210
211 utils::Scalar angle = acos(dot(quat) / s);
212#ifdef BIORBD_USE_CASADI_MATH
213 if (true) {
214#else
215 if (angle == 0. || std::isnan(angle)) {
216#endif
217 return *this;
218 }
219
220 utils::Scalar d = 1. / std::sin(angle);
221 utils::Scalar p0 = std::sin((1. - alpha) * angle);
222 utils::Scalar p1 = std::sin(alpha * angle);
223
224#ifdef BIORBD_USE_CASADI_MATH
225 return Quaternion(
226 IF_ELSE_NAMESPACE::if_else(
227 IF_ELSE_NAMESPACE::lt(dot(quat), 0.),
228 RigidBodyDynamics::Math::Vector4d(((*this) * p0 - quat * p1) * d),
229 RigidBodyDynamics::Math::Vector4d(((*this) * p0 + quat * p1) * d)),
230 (this->m_Kstab + quat.m_Kstab) / 2);
231
232#else
233 if (dot(quat) < 0.) {
234 return Quaternion(((*this) * p0 - quat * p1) * d, this->m_Kstab);
235 }
236 return Quaternion(
237 ((*this) * p0 + quat * p1) * d, (this->m_Kstab + quat.m_Kstab) / 2);
238#endif
239}
240#endif
241
243 return utils::Quaternion(
244 (*this)[0], -(*this)[1], -(*this)[2], -(*this)[3], this->kStab());
245}
246
248 const utils::Vector3d& omega,
249 const utils::Scalar& dt) {
250 utils::Scalar omega_norm = omega.norm();
251 return fromAxisAngle(dt * omega_norm, omega / omega_norm, this->m_Kstab) *
252 (*this);
253}
254
256 utils::Quaternion vec_quat(0., vec);
257
258 utils::Quaternion res_quat(vec_quat * (*this));
259 res_quat = conjugate() * res_quat;
260
261 return utils::Vector3d(res_quat[1], res_quat[2], res_quat[3]);
262}
263
264#include <iostream>
266 const utils::Vector3d& omega) const {
267 RigidBodyDynamics::Math::MatrixNd m(4, 3);
268 m(0, 0) = -(*this)[1];
269 m(0, 1) = -(*this)[2];
270 m(0, 2) = -(*this)[3];
271 m(1, 0) = (*this)[0];
272 m(1, 1) = -(*this)[3];
273 m(1, 2) = (*this)[2];
274 m(2, 0) = (*this)[3];
275 m(2, 1) = (*this)[0];
276 m(2, 2) = -(*this)[1];
277 m(3, 0) = -(*this)[2];
278 m(3, 1) = (*this)[1];
279 m(3, 2) = (*this)[0];
280
281 return utils::Quaternion(0.5 * m * omega, this->m_Kstab);
282}
283
285 const utils::Vector3d& euler,
286 const utils::String& seq) {
288 utils::Vector3d rot1(euler[0], 0, 0);
289 utils::Vector3d rot2(euler[0], euler[1], 0);
292
293 utils::Matrix3d result;
294 result.block(0, 0, 3, 1) = baseMatrix.block(0, 0, 3, 1);
295 result.block(0, 1, 3, 1) = RotMat1 * baseMatrix.block(0, 1, 3, 1);
296 result.block(0, 2, 3, 1) = RotMat2 * baseMatrix.block(0, 2, 3, 1);
297 return result;
298}
299
301 const utils::Vector3d& euler,
302 const utils::Vector3d& eulerDot,
303 const utils::String& seq) {
304 utils::Matrix3d velocity_matrix = velocityMatrix(euler, seq);
305 utils::Vector3d w = velocity_matrix * eulerDot;
306
307 return w;
308}
309
311 const utils::Vector3d& euler,
312 const utils::Vector3d& w,
313 const utils::String& seq) {
314 utils::Matrix3d velocity_matrix = velocityMatrix(euler, seq).inverse();
315 utils::Vector3d eulerDot = velocity_matrix * w;
316 return eulerDot;
317}
318
320 // Création du quaternion de "préproduit vectoriel"
321#ifdef BIORBD_USE_CASADI_MATH
322 utils::Scalar qw = (*this)(0);
323 utils::Scalar qx = (*this)(1);
324 utils::Scalar qy = (*this)(2);
325 utils::Scalar qz = (*this)(3);
326#else
327 utils::Scalar& qw = (*this)(0);
328 utils::Scalar& qx = (*this)(1);
329 utils::Scalar& qy = (*this)(2);
330 utils::Scalar& qz = (*this)(3);
331#endif
332 RigidBodyDynamics::Math::Matrix4d Q = RigidBodyDynamics::Math::Matrix4d(
333 qw, -qx, -qy, -qz, qx, qw, -qz, qy, qy, qz, qw, -qx, qz, -qy, qx, qw);
334
335 // Ajout du paramètre de stabilisation
336 RigidBodyDynamics::Math::Vector4d w_tp(
337 m_Kstab * w.norm() * (1 - this->norm()), w(0), w(1), w(2));
338 RigidBodyDynamics::Math::Vector4d newQuat(0.5 * Q * w_tp);
339
340 // Assigning is slightly faster than create a new Quaternion
341 qw = newQuat[0];
342 qx = newQuat[1];
343 qy = newQuat[2];
344 qz = newQuat[3];
345#ifdef BIORBD_USE_CASADI_MATH
346 *this = utils::Quaternion(qw, qx, qy, qz, m_Kstab);
347#endif
348}
349
350void utils::Quaternion::normalize() { *this = *this / this->norm(); }
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
static utils::Matrix3d fromEulerSequence(const utils::String &seq)
Creates a base 3d matrix from a euler sequence of exactly 3 elements.
Definition Matrix3d.cpp:67
Quaternion slerp(double alpha, const Quaternion &quat) const
Interpolation of the quaternion between to position.
Quaternion conjugate() const
Return the conjugate of the quaternion.
Scalar y() const
Return the Y-Component of the imaginary part of the Quaternion.
Scalar x() const
Return the X-Component of the imaginary part of the Quaternion.
Quaternion omegaToQuatDot(const Vector3d &omega) const
Converts a 3d angular velocity vector into a 4d derivative of the components of the quaternion.
Scalar w() const
Return the real part (w) the Quaternion.
Matrix3d velocityMatrix(const Vector3d &euler, const String &seq)
Generate the velocity matrix which allows to go from/to euler angles to/from omega(body velocity).
Quaternion timeStep(const Vector3d &omega, const Scalar &dt)
Integrate the quaternion from its velocity.
static Quaternion fromGLRotate(const Scalar &angle, const Scalar &x, const Scalar &y, const Scalar &z, const Scalar &kStab=1)
Construct Quaternion from a GL.
Quaternion operator+(const Quaternion &other) const
Add the quaternion to another.
Scalar kStab() const
Return the k stabilizer.
Vector3d eulerDotToOmega(const Vector3d &eulerDot, const Vector3d &euler, const String &seq)
Converts a 3d angular velocity vector.
Vector3d omegaToEulerDot(const Vector3d &euler, const Vector3d &w, const String &seq)
converts a 3d vector of the body angular velocity (omega) into a 3d vector of the euler angles rate.
static Quaternion fromYXZAngles(const Vector3d &yxz_angles, const Scalar &kStab=1)
Construct Quaternion from Euler angles (sequece YXZ).
static Quaternion fromZYXAngles(const Vector3d &zyx_angles, const Scalar &kStab=1)
Construct Quaternion from Euler angles (sequece ZYX).
void normalize()
Force the normalization of the quaternion.
static Quaternion fromXYZAngles(const Vector3d &xyz_angles, const Scalar &kStab=1)
Construct Quaternion from Euler angles (sequece XYZ).
static Quaternion fromMatrix(const RotoTrans &rt, const Scalar &kStab=1)
Construct Quaternion from a RotoTrans matrix.
void setKStab(const Scalar &newKStab)
Set the k stabilizer.
Rotation toMatrix(bool skipAsserts=false) const
Convert the quaternion to a RotoTrans.
Scalar z() const
Return the Z-Component of the imaginary part of the Quaternion.
Quaternion operator-(const Quaternion &other) const
Subtract the quaternion to another.
utils::Scalar m_Kstab
Stabilization factor for the derivation.
Definition Quaternion.h:351
void derivate(const Vector &w)
Return the time derivative of the quaterion.
Quaternion operator*(const Quaternion &other) const
Quaternion multiplication.
Quaternion(const Scalar &kStabilizer=1)
Construct Quaternion.
Vector3d rotate(const Vector3d &vec) const
Return a rotated vector from the quaternion.
static Quaternion fromAxisAngle(const Scalar &angle, const Vector3d &axis, const Scalar &kStab=1)
Construct Quaternion from an axis angle.
static Rotation fromEulerAngles(const Vector &rot, const String &seq)
Create a Rotation from Euler angles.
Definition Rotation.cpp:57
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
Rotation rot() const
Return the rotation matrix.
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Wrapper of the Eigen VectorXd.
Definition Vector.h:21
Scalar norm(size_t p=2, bool skipRoot=false) const
Return the Euclidian p-norm of the vector.
Definition Vector.cpp:34