Drake
quaternion.h
Go to the documentation of this file.
1 
4 #pragma once
5 
6 #include <cmath>
7 
8 #include <Eigen/Dense>
9 
11 
12 namespace drake {
13 namespace math {
14 
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);
21  return q_conj;
22 }
23 
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.");
30 
31  Eigen::Quaternion<typename Derived1::Scalar> q1_eigen(q1(0), q1(1), q1(2),
32  q1(3));
33  Eigen::Quaternion<typename Derived2::Scalar> q2_eigen(q2(0), q2(1), q2(2),
34  q2(3));
35  auto ret_eigen = q1_eigen * q2_eigen;
37  r << ret_eigen.w(), ret_eigen.x(), ret_eigen.y(), ret_eigen.z();
38 
39  return r;
40 }
41 
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.");
48 
50  v_quat << 0, v;
51  auto q_times_v = quatProduct(q, v_quat);
52  auto q_conj = quatConjugate(q);
53  auto v_rot = quatProduct(q_times_v, q_conj);
54  Vector3<typename DerivedV::Scalar> r = v_rot.template bottomRows<3>();
55  return r;
56 }
57 
58 template <typename Derived1, typename Derived2>
60  const Eigen::MatrixBase<Derived1>& q1,
61  const Eigen::MatrixBase<Derived2>& q2) {
62  return quatProduct(quatConjugate(q1), q2);
63 }
64 
65 template <typename Derived1, typename Derived2, typename DerivedU>
66 typename Derived1::Scalar quatDiffAxisInvar(
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.");
71  auto r = quatDiff(q1, q2);
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);
74 }
75 
76 template <typename Derived>
77 typename Derived::Scalar quatNorm(const Eigen::MatrixBase<Derived>& q) {
78  using std::acos;
79  return acos(q(0));
80 }
81 
97 template <typename Derived1, typename Derived2, typename Scalar>
98 Vector4<Scalar> Slerp(const Eigen::MatrixBase<Derived1>& q1,
99  const Eigen::MatrixBase<Derived2>& q2,
100  const Scalar& interpolation_parameter) {
101  using std::acos;
102  using std::sin;
103 
104  // Compute the quaternion inner product
105  auto lambda = (q1.transpose() * q2).value();
106  int q2_sign;
107  if (lambda < Scalar(0)) {
108  // The quaternions are pointing in opposite directions, so use the
109  // equivalent alternative representation for q2
110  lambda = -lambda;
111  q2_sign = -1;
112  } else {
113  q2_sign = 1;
114  }
115 
116  // Calculate interpolation factors
117  // TODO(tkoolen): do we really want an epsilon so small?
118  Scalar r, s;
119  if (std::abs(1.0 - lambda) < Eigen::NumTraits<Scalar>::epsilon()) {
120  // The quaternions are nearly parallel, so use linear interpolation
121  r = 1.0 - interpolation_parameter;
122  s = interpolation_parameter;
123  } else {
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;
128  }
129 
130  auto ret = (q1 * r).eval();
131  ret += q2 * (q2_sign * s);
132  return ret;
133 }
134 
135 template <typename Derived>
137  const Eigen::MatrixBase<Derived>& q) {
138  using std::sqrt;
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();
144 
145  a << q_normalized.template tail<3>() / s, 2.0 * std::acos(q_normalized(0));
146  return a;
147 }
148 
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);
158 
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;
166 
167  return M;
168 }
169 
170 template <typename Derived>
172  const Eigen::MatrixBase<Derived>& q) {
173  using std::asin;
174  using std::atan2;
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);
181 
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));
186  return ret;
187 }
188 
189 template <typename Derived>
190 Eigen::Quaternion<typename Derived::Scalar> quat2eigenQuaternion(
191  const Eigen::MatrixBase<Derived>& q) {
192  // The Eigen Quaterniond constructor when used with 4 arguments, uses the (w,
193  // x, y, z) ordering, just as we do.
194  // HOWEVER: when the constructor is called on a 4-element Vector, the elements
195  // must be in (x, y, z, w) order.
196  // So, the following two calls will give you the SAME quaternion:
197  // Quaternion<double>(q(0), q(1), q(2), q(3));
198  // Quaternion<double>(Vector4d(q(3), q(0), q(1), q(2)))
199  // which is gross and will cause you much pain.
200  // see:
201  // http://eigen.tuxfamily.org/dox/classEigen_1_1Quaternion.html#a91b6ea2cac13ab2d33b6e74818ee1490
202  //
203  // This method takes a nice, normal (w, x, y, z) order vector and gives you
204  // the Quaternion you expect.
205  return Eigen::Quaternion<typename Derived::Scalar>(q(0), q(1), q(2), q(3));
206 }
207 
208 } // namespace math
209 } // namespace drake
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