se3.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_SE3_HPP_
#define ROBOTOC_SE3_HPP_
 
#include <Eigen/Core>
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/explog.hpp"
 
 
namespace robotoc {
 
using SE3 = pinocchio::SE3;
 
template <typename SE3Type>
const Eigen::Matrix<double, 6, 1> Log6Map(const SE3Type& SE3_obj) {
  return pinocchio::log6(SE3_obj).toVector();
}
 
template <typename SE3Type, typename MatrixType>
void computeJLog6Map(const SE3Type& SE3_obj, 
                     const Eigen::MatrixBase<MatrixType>& J) {
  assert(J.rows() == 6);
  assert(J.cols() == 6);
  pinocchio::Jlog6(SE3_obj, const_cast<Eigen::MatrixBase<MatrixType>&>(J));
}
 
} // namespace robotoc
 
#endif // ROBOTOC_SE3_HPP_