6 #ifndef __pinocchio_se3_tpl_hpp__
7 #define __pinocchio_se3_tpl_hpp__
16 #include <Eigen/Geometry>
20 template<
typename _Scalar,
int _Options>
29 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
30 typedef Eigen::Matrix<Scalar,4,1,Options>
Vector4;
31 typedef Eigen::Matrix<Scalar,6,1,Options>
Vector6;
32 typedef Eigen::Matrix<Scalar,3,3,Options>
Matrix3;
33 typedef Eigen::Matrix<Scalar,4,4,Options>
Matrix4;
34 typedef Eigen::Matrix<Scalar,6,6,Options>
Matrix6;
46 template<
typename _Scalar,
int _Options>
49 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
64 template<
typename QuaternionLike,
typename Vector3Like>
65 SE3Tpl(
const Eigen::QuaternionBase<QuaternionLike> & quat,
66 const Eigen::MatrixBase<Vector3Like> &
trans)
69 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
72 template<
typename Matrix3Like,
typename Vector3Like>
73 SE3Tpl(
const Eigen::MatrixBase<Matrix3Like> & R,
74 const Eigen::MatrixBase<Vector3Like> &
trans)
77 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
78 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3)
81 template<
typename Matrix4Like>
82 explicit SE3Tpl(
const Eigen::MatrixBase<Matrix4Like> & m)
83 :
rot(
m.template block<3,3>(LINEAR,LINEAR))
84 ,
trans(
m.template block<3,1>(LINEAR,ANGULAR))
86 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4);
91 ,
trans(LinearType::Zero())
112 {
rot.setIdentity ();
trans.setZero ();
return *
this;}
122 return SE3Tpl().setRandom();
136 HomogeneousMatrixType
M;
137 M.template block<3,3>(LINEAR,LINEAR) =
rot;
138 M.template block<3,1>(LINEAR,ANGULAR) =
trans;
139 M.template block<1,3>(ANGULAR,LINEAR).setZero();
147 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
149 M.template block<3,3>(ANGULAR,ANGULAR)
150 =
M.template block<3,3>(LINEAR,LINEAR) =
rot;
151 M.template block<3,3>(ANGULAR,LINEAR).setZero();
152 Block3
B =
M.template block<3,3>(LINEAR,ANGULAR);
162 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
164 M.template block<3,3>(ANGULAR,ANGULAR)
165 =
M.template block<3,3>(LINEAR,LINEAR) =
rot.transpose();
166 Block3
C =
M.template block<3,3>(ANGULAR,LINEAR);
167 Block3
B =
M.template block<3,3>(LINEAR,ANGULAR);
169 #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id,v3_in,v3_out,R,res) \
170 CartesianAxis<axis_id>::cross(v3_in,v3_out); \
171 res.col(axis_id).noalias() = R.transpose() * v3_out;
177 #undef PINOCCHIO_INTERNAL_COMPUTATION
185 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
187 M.template block<3,3>(ANGULAR,ANGULAR)
188 =
M.template block<3,3>(LINEAR,LINEAR) =
rot;
189 M.template block<3,3>(LINEAR,ANGULAR).setZero();
190 Block3
B =
M.template block<3,3>(ANGULAR,LINEAR);
201 <<
" R =\n" <<
rot << std::endl
202 <<
" p = " <<
trans.transpose() << std::endl;
212 return d.se3Action(*
this);
219 return d.se3ActionInverse(*
this);
222 template<
typename EigenDerived>
223 typename EigenDerived::PlainObject
227 template<
typename MapDerived>
231 template<
typename EigenDerived>
232 typename EigenDerived::PlainObject
236 template<
typename MapDerived>
268 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
287 template<
typename NewScalar>
291 ReturnType
res(
rot.template cast<NewScalar>(),
292 trans.template cast<NewScalar>());
307 rot = orthogonalProjection(
rot);
312 PlainType
res(*
this);
res.normalize();
327 template<
typename OtherScalar>
338 template<
typename Scalar,
int Options>
345 template<
typename Scalar,
int Options,
typename NewScalar>
351 if(pinocchio::cast<NewScalar>(Eigen::NumTraits<Scalar>::epsilon()) > Eigen::NumTraits<NewScalar>::epsilon())
360 #endif // ifndef __pinocchio_se3_tpl_hpp__