15 template <
typename Derived>
17 const Eigen::MatrixBase<Derived>& q) {
18 static_assert(Derived::SizeAtCompileTime == 4,
"Wrong size.");
20 q_conj << q(0), -q(1), -q(2), -q(3);
24 template <
typename Derived1,
typename Derived2>
26 const Eigen::MatrixBase<Derived1>& q1,
27 const Eigen::MatrixBase<Derived2>& q2) {
28 static_assert(Derived1::SizeAtCompileTime == 4,
"Wrong size.");
29 static_assert(Derived2::SizeAtCompileTime == 4,
"Wrong size.");
31 Eigen::Quaternion<typename Derived1::Scalar> q1_eigen(q1(0), q1(1), q1(2),
33 Eigen::Quaternion<typename Derived2::Scalar> q2_eigen(q2(0), q2(1), q2(2),
35 auto ret_eigen = q1_eigen * q2_eigen;
37 r << ret_eigen.w(), ret_eigen.x(), ret_eigen.y(), ret_eigen.z();
42 template <
typename DerivedQ,
typename DerivedV>
44 const Eigen::MatrixBase<DerivedQ>& q,
45 const Eigen::MatrixBase<DerivedV>& v) {
46 static_assert(DerivedQ::SizeAtCompileTime == 4,
"Wrong size.");
47 static_assert(DerivedV::SizeAtCompileTime == 3,
"Wrong size.");
58 template <
typename Derived1,
typename Derived2>
60 const Eigen::MatrixBase<Derived1>& q1,
61 const Eigen::MatrixBase<Derived2>& q2) {
65 template <
typename Derived1,
typename Derived2,
typename DerivedU>
67 const Eigen::MatrixBase<Derived1>& q1,
68 const Eigen::MatrixBase<Derived2>& q2,
69 const Eigen::MatrixBase<DerivedU>& u) {
70 static_assert(DerivedU::SizeAtCompileTime == 3,
"Wrong size.");
72 return -2.0 + 2 * r(0) * r(0) +
73 2 * pow(u(0) * r(1) + u(1) * r(2) + u(2) * r(3), 2);
76 template <
typename Derived>
77 typename Derived::Scalar
quatNorm(
const Eigen::MatrixBase<Derived>& q) {
97 template <
typename Derived1,
typename Derived2,
typename Scalar>
99 const Eigen::MatrixBase<Derived2>& q2,
100 const Scalar& interpolation_parameter) {
105 auto lambda = (q1.transpose() * q2).value();
107 if (lambda < Scalar(0)) {
119 if (
std::abs(1.0 - lambda) < Eigen::NumTraits<Scalar>::epsilon()) {
121 r = 1.0 - interpolation_parameter;
122 s = interpolation_parameter;
124 Scalar alpha = acos(lambda);
125 Scalar gamma = 1.0 /
sin(alpha);
126 r =
std::sin((1.0 - interpolation_parameter) * alpha) * gamma;
127 s =
std::sin(interpolation_parameter * alpha) * gamma;
130 auto ret = (q1 * r).eval();
131 ret += q2 * (q2_sign * s);
135 template <
typename Derived>
137 const Eigen::MatrixBase<Derived>& q) {
139 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
140 auto q_normalized = q.normalized();
141 auto s =
sqrt(1.0 - q_normalized(0) * q_normalized(0)) +
142 std::numeric_limits<typename Derived::Scalar>::epsilon();
145 a << q_normalized.template tail<3>() / s, 2.0 * std::acos(q_normalized(0));
149 template <
typename Derived>
151 const Eigen::MatrixBase<Derived>& q) {
152 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
153 auto q_normalized = q.normalized();
154 auto w = q_normalized(0);
155 auto x = q_normalized(1);
156 auto y = q_normalized(2);
157 auto z = q_normalized(3);
160 M.row(0) << w * w +
x *
x - y * y - z * z, 2.0 *
x * y - 2.0 * w * z,
161 2.0 *
x * z + 2.0 * w * y;
162 M.row(1) << 2.0 *
x * y + 2.0 * w * z, w * w + y * y -
x *
x - z * z,
163 2.0 * y * z - 2.0 * w *
x;
164 M.row(2) << 2.0 * x * z - 2.0 * w * y, 2.0 * y * z + 2.0 * w *
x,
165 w * w + z * z - x * x - y * y;
170 template <
typename Derived>
172 const Eigen::MatrixBase<Derived>& q) {
175 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
176 auto q_normalized = q.normalized();
177 auto w = q_normalized(0);
178 auto x = q_normalized(1);
179 auto y = q_normalized(2);
180 auto z = q_normalized(3);
183 ret << atan2(2.0 * (w *
x + y * z), w * w + z * z - (
x *
x + y * y)),
184 asin(2.0 * (w * y - z *
x)),
185 atan2(2.0 * (w * z + x * y), w * w + x * x - (y * y + z * z));
189 template <
typename Derived>
191 const Eigen::MatrixBase<Derived>& q) {
205 return Eigen::Quaternion<typename Derived::Scalar>(q(0), q(1), q(2), q(3));
std::vector< Number > x
Definition: IpoptSolver.cpp:169
This file contains abbreviated definitions for certain specializations of Eigen::Matrix that are comm...
Eigen::Matrix< Scalar, 4, 1 > Vector4
A column vector of size 4, templated on scalar type.
Definition: eigen_types.h:28
Definition: constants.h:3
Vector3< typename Derived::Scalar > quat2rpy(const Eigen::MatrixBase< Derived > &q)
Definition: quaternion.h:171
FunctionalForm abs(FunctionalForm const &x)
Definition: functional_form.cc:183
Vector4< typename Derived1::Scalar > quatProduct(const Eigen::MatrixBase< Derived1 > &q1, const Eigen::MatrixBase< Derived2 > &q2)
Definition: quaternion.h:25
FunctionalForm sqrt(FunctionalForm const &x)
Definition: functional_form.cc:208
Derived::Scalar quatNorm(const Eigen::MatrixBase< Derived > &q)
Definition: quaternion.h:77
Vector4< typename Derived1::Scalar > quatDiff(const Eigen::MatrixBase< Derived1 > &q1, const Eigen::MatrixBase< Derived2 > &q2)
Definition: quaternion.h:59
Eigen::Matrix< Scalar, 3, 1 > Vector3
A column vector of size 3, templated on scalar type.
Definition: eigen_types.h:24
Eigen::Quaternion< typename Derived::Scalar > quat2eigenQuaternion(const Eigen::MatrixBase< Derived > &q)
Definition: quaternion.h:190
Vector3< typename DerivedV::Scalar > quatRotateVec(const Eigen::MatrixBase< DerivedQ > &q, const Eigen::MatrixBase< DerivedV > &v)
Definition: quaternion.h:43
Vector4< typename Derived::Scalar > quatConjugate(const Eigen::MatrixBase< Derived > &q)
Definition: quaternion.h:16
Derived1::Scalar quatDiffAxisInvar(const Eigen::MatrixBase< Derived1 > &q1, const Eigen::MatrixBase< Derived2 > &q2, const Eigen::MatrixBase< DerivedU > &u)
Definition: quaternion.h:66
Vector4< typename Derived::Scalar > quat2axis(const Eigen::MatrixBase< Derived > &q)
Definition: quaternion.h:136
Eigen::Matrix< Scalar, 3, 3 > Matrix3
A matrix of 3 rows and 3 columns, templated on scalar type.
Definition: eigen_types.h:40
Vector4< Scalar > Slerp(const Eigen::MatrixBase< Derived1 > &q1, const Eigen::MatrixBase< Derived2 > &q2, const Scalar &interpolation_parameter)
Q = Slerp(q1, q2, f) Spherical linear interpolation between two quaternions This function uses the im...
Definition: quaternion.h:98
Matrix3< typename Derived::Scalar > quat2rotmat(const Eigen::MatrixBase< Derived > &q)
Definition: quaternion.h:150
FunctionalForm sin(FunctionalForm const &x)
Definition: functional_form.cc:203