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