15 template <
typename Derived>
17 const Eigen::MatrixBase<Derived>& a) {
20 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
21 auto axis = a.template head<3>();
23 auto arg = 0.5 * angle;
31 template <
typename Derived>
33 const Eigen::MatrixBase<Derived>& a) {
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);
42 auto ctheta =
cos(theta);
43 auto stheta =
sin(theta);
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;
53 template <
typename Derived>
55 const Eigen::MatrixBase<Derived>& a) {
56 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 4);
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