Drake
axis_angle.h
Go to the documentation of this file.
1 
4 #pragma once
5 
6 #include <cmath>
7 
8 #include <Eigen/Dense>
9 
11 #include "drake/math/quaternion.h"
12 
13 namespace drake {
14 namespace math {
15 template <typename Derived>
17  const Eigen::MatrixBase<Derived>& a) {
18  using std::cos;
19  using std::sin;
20  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
21  auto axis = a.template head<3>();
22  auto angle = a(3);
23  auto arg = 0.5 * angle;
24  auto c = cos(arg);
25  auto s = sin(arg);
27  ret << c, s * axis;
28  return ret;
29 }
30 
31 template <typename Derived>
33  const Eigen::MatrixBase<Derived>& a) {
34  using std::cos;
35  using std::sin;
36  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
37  const auto& axis = (a.template head<3>()) / (a.template head<3>()).norm();
38  const auto& theta = a(3);
39  auto x = axis(0);
40  auto y = axis(1);
41  auto z = axis(2);
42  auto ctheta = cos(theta);
43  auto stheta = sin(theta);
44  auto c = 1 - ctheta;
46  R << ctheta + x * x * c, x * y * c - z * stheta, x * z * c + y * stheta,
47  y * x * c + z * stheta, ctheta + y * y * c, y * z * c - x * stheta,
48  z * x * c - y * stheta, z * y * c + x * stheta, ctheta + z * z * c;
49 
50  return R;
51 }
52 
53 template <typename Derived>
55  const Eigen::MatrixBase<Derived>& a) {
56  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
57  return quat2rpy(axis2quat(a));
58 }
59 
60 } // namespace math
61 } // 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 cos(FunctionalForm const &x)
Definition: functional_form.cc:188
Eigen::Matrix< Scalar, 3, 1 > Vector3
A column vector of size 3, templated on scalar type.
Definition: eigen_types.h:24
Vector3< typename Derived::Scalar > axis2rpy(const Eigen::MatrixBase< Derived > &a)
Definition: axis_angle.h:54
Utilities for arithmetic on quaternions.
Eigen::Matrix< Scalar, 3, 3 > Matrix3
A matrix of 3 rows and 3 columns, templated on scalar type.
Definition: eigen_types.h:40
Matrix3< typename Derived::Scalar > axis2rotmat(const Eigen::MatrixBase< Derived > &a)
Definition: axis_angle.h:32
Vector4< typename Derived::Scalar > axis2quat(const Eigen::MatrixBase< Derived > &a)
Definition: axis_angle.h:16
FunctionalForm sin(FunctionalForm const &x)
Definition: functional_form.cc:203