6 #ifndef __pinocchio_python_explog_hpp__ 7 #define __pinocchio_python_explog_hpp__ 9 #include "pinocchio/spatial/explog.hpp" 16 template<
typename Vector3Like>
17 Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
23 template<
typename Vector3Like>
24 Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
27 typedef Eigen::Matrix<typename Vector3Like::Scalar,3,3,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
32 template<
typename Matrix3Like>
34 Jlog3_proxy(
const Matrix3Like & M)
41 template<
typename Matrix3Like,
typename Vector3Like>
43 Hlog3_proxy(
const Matrix3Like &
M,
const Vector3Like &
v)
50 template<
typename Scalar,
int Options>
56 template<
typename Vector6Like>
63 template<
typename Scalar,
int Options>
71 template<
typename Scalar,
int Options>
80 template<
typename Vector6Like>
81 Eigen::Matrix<typename Vector6Like::Scalar,6,6,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
86 typedef typename Motion::Matrix6 ReturnType;
91 template<
typename Matrix3Like>
92 Eigen::Matrix<typename Matrix3Like::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options>
98 template<
typename Matrix4Like>
102 return log6(homegenous_matrix);
108 #endif // ifndef __pinocchio_python_explog_hpp__ Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > log3(const Eigen::MatrixBase< Matrix3Like > &R, typename Matrix3Like::Scalar &theta)
Same as log3.
SE3Tpl< Scalar, Options >::Matrix6 Jlog6_proxy(const SE3Tpl< Scalar, Options > &M)
void Jexp3(const Eigen::MatrixBase< Vector3Like > &r, const Eigen::MatrixBase< Matrix3Like > &Jexp)
Derivative of .
void Jlog3(const Scalar &theta, const Eigen::MatrixBase< Vector3Like > &log, const Eigen::MatrixBase< Matrix3Like > &Jlog)
Derivative of log3.
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > log3_proxy(const Matrix3Like &R)
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3_proxy(const Vector3Like &v)
void Jexp6(const MotionDense< MotionDerived > &nu, const Eigen::MatrixBase< Matrix6Like > &Jexp)
Derivative of exp6 Computed as the inverse of Jlog6.
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > exp3(const Eigen::MatrixBase< Vector3Like > &v)
Exp: so3 -> SO3.
SE3Tpl< Scalar, Options > exp6_proxy(const MotionTpl< Scalar, Options > &v)
MotionTpl< Scalar, Options >::Matrix6 Jexp6_proxy(const MotionTpl< Scalar, Options > &v)
MotionTpl< double, 0 > Motion
Main pinocchio namespace.
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > Jexp3_proxy(const Vector3Like &v)
PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like) Jlog3_proxy(const Matrix3Like &M)
SE3Tpl< typename MotionDerived::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options > exp6(const MotionDense< MotionDerived > &nu)
Exp: se3 -> SE3.
MotionTpl< typename Matrix4Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix4Like)::Options > log6_proxy(const Matrix4Like &homegenous_matrix)