1#define BIORBD_API_EXPORTS
2#include "Utils/Quaternion.h"
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"
11#ifdef USE_SMOOTH_IF_ELSE
12#include "Utils/CasadiExpand.h"
15using namespace BIORBD_NAMESPACE;
18 : RigidBodyDynamics::Math::Vector4d(1, 0, 0, 0),
m_Kstab(kStabilizer) {}
21 : RigidBodyDynamics::Math::Vector4d(other),
m_Kstab(other.
m_Kstab) {}
24 const RigidBodyDynamics::Math::Vector4d& vec4,
25 const utils::Scalar& kStabilizer)
26 : RigidBodyDynamics::Math::Vector4d(vec4),
m_Kstab(kStabilizer) {}
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) {}
37 const utils::Scalar&
w,
39 const utils::Scalar& kStabilizer)
40 : RigidBodyDynamics::Math::Vector4d(
w, vec3[0], vec3[1], vec3[2]),
57 (*
this)[0] * q[0] - (*
this)[1] * q[1] - (*
this)[2] * q[2] -
59 (*
this)[0] * q[1] + (*
this)[1] * q[0] + (*
this)[2] * q[3] -
61 (*
this)[0] * q[2] + (*
this)[2] * q[0] + (*
this)[3] * q[1] -
63 (*
this)[0] * q[3] + (*
this)[3] * q[0] + (*
this)[1] * q[2] -
69 const utils::Scalar& scalar)
const {
71 this->RigidBodyDynamics::Math::Vector4d::operator*(scalar),
77 this->RigidBodyDynamics::Math::Vector4d::operator*(
78 static_cast<double>(scalar)),
82#ifdef BIORBD_USE_CASADI_MATH
85 this->RigidBodyDynamics::Math::Vector4d::operator*(scalar),
93 this->RigidBodyDynamics::Math::Vector4d::operator+(other),
100 this->RigidBodyDynamics::Math::Vector4d::operator-(other),
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.);
113 std::cos(angle_copy * M_PI / 360.), st *
x, st *
y, st *
z,
kStab);
117 const utils::Scalar& angle,
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;
124 std::cos(angle_copy * 0.5),
133 const utils::Scalar&
kStab) {
139 const utils::Scalar&
kStab) {
140 utils::Scalar
w = std::sqrt(1. + mat(0, 0) + mat(1, 1) + mat(2, 2)) * 0.5;
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.),
151 const utils::Scalar&
kStab) {
159 const utils::Scalar&
kStab) {
167 const utils::Scalar&
kStab) {
174#ifndef BIORBD_USE_CASADI_MATH
177 fabs(this->squaredNorm() - 1.) < 1e-10,
178 "The Quaternion norm is not equal to one");
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);
199#ifndef BIORBD_USE_CASADI_MATH
204 utils::Scalar s = std::sqrt(squaredNorm() * quat.squaredNorm());
207#ifndef BIORBD_USE_CASADI_MATH
211 utils::Scalar angle = acos(dot(quat) / s);
212#ifdef BIORBD_USE_CASADI_MATH
215 if (angle == 0. || std::isnan(angle)) {
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);
224#ifdef BIORBD_USE_CASADI_MATH
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)),
233 if (dot(quat) < 0.) {
237 ((*
this) * p0 + quat * p1) * d, (this->
m_Kstab + quat.
m_Kstab) / 2);
244 (*
this)[0], -(*
this)[1], -(*
this)[2], -(*
this)[3], this->
kStab());
249 const utils::Scalar& dt) {
250 utils::Scalar omega_norm = omega.norm();
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];
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);
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);
327 utils::Scalar& qw = (*this)(0);
328 utils::Scalar& qx = (*this)(1);
329 utils::Scalar& qy = (*this)(2);
330 utils::Scalar& qz = (*this)(3);
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);
336 RigidBodyDynamics::Math::Vector4d w_tp(
338 RigidBodyDynamics::Math::Vector4d newQuat(0.5 * Q * w_tp);
345#ifdef BIORBD_USE_CASADI_MATH
static void check(bool cond, const String &message)
Assert that raises the error message if false.
A wrapper for the Eigen::MatrixXd.
static utils::Matrix3d fromEulerSequence(const utils::String &seq)
Creates a base 3d matrix from a euler sequence of exactly 3 elements.
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.
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.
Homogenous matrix to describe translations and rotations simultaneously.
Rotation rot() const
Return the rotation matrix.
Wrapper around the std::string class with augmented functionality.
Wrapper around Eigen Vector3d and attach it to a parent.
Wrapper of the Eigen VectorXd.
Scalar norm(size_t p=2, bool skipRoot=false) const
Return the Euclidian p-norm of the vector.