6 #ifndef __pinocchio_se3_tpl_hpp__ 7 #define __pinocchio_se3_tpl_hpp__ 9 #include <Eigen/Geometry> 10 #include "pinocchio/math/quaternion.hpp" 11 #include "pinocchio/math/rotation.hpp" 12 #include "pinocchio/spatial/cartesian-axis.hpp" 16 template<
typename _Scalar,
int _Options>
24 typedef _Scalar Scalar;
25 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
26 typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
27 typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
28 typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
29 typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
30 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
31 typedef Matrix3 AngularType;
32 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef;
33 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef;
34 typedef Vector3 LinearType;
35 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef;
36 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef;
37 typedef Matrix6 ActionMatrixType;
38 typedef Matrix4 HomogeneousMatrixType;
42 template<
typename _Scalar,
int _Options>
45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 PINOCCHIO_SE3_TYPEDEF_TPL(
SE3Tpl);
48 typedef Eigen::Quaternion<Scalar,Options> Quaternion;
56 using Base::translation;
58 SE3Tpl(): rot(), trans() {};
60 template<
typename QuaternionLike,
typename Vector3Like>
61 SE3Tpl(
const Eigen::QuaternionBase<QuaternionLike> & quat,
62 const Eigen::MatrixBase<Vector3Like> & trans)
63 : rot(quat.matrix()), trans(trans)
65 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
68 template<
typename Matrix3Like,
typename Vector3Like>
69 SE3Tpl(
const Eigen::MatrixBase<Matrix3Like> & R,
70 const Eigen::MatrixBase<Vector3Like> & trans)
71 : rot(R), trans(trans)
73 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
74 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3)
77 template<
typename Matrix4Like>
78 explicit SE3Tpl(
const Eigen::MatrixBase<Matrix4Like> & m)
79 : rot(m.template block<3,3>(LINEAR,LINEAR))
80 , trans(m.template block<3,1>(LINEAR,ANGULAR))
82 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4);
86 : rot(AngularType::Identity())
87 , trans(LinearType::Zero())
92 : rot(clone.rotation()),trans(clone.translation()) {}
97 rot = other.rotation();
98 trans = other.translation();
102 static SE3Tpl Identity()
107 SE3Tpl & setIdentity()
108 { rot.setIdentity (); trans.setZero ();
return *
this;}
113 return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
116 static SE3Tpl Random()
118 return SE3Tpl().setRandom();
130 HomogeneousMatrixType toHomogeneousMatrix_impl()
const 132 HomogeneousMatrixType M;
133 M.template block<3,3>(LINEAR,LINEAR) = rot;
134 M.template block<3,1>(LINEAR,ANGULAR) = trans;
135 M.template block<1,3>(ANGULAR,LINEAR).setZero();
143 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
145 M.template block<3,3>(ANGULAR,ANGULAR)
146 = M.template block<3,3>(LINEAR,LINEAR) = rot;
147 M.template block<3,3>(ANGULAR,LINEAR).setZero();
148 Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
150 B.col(0) = trans.cross(rot.col(0));
151 B.col(1) = trans.cross(rot.col(1));
152 B.col(2) = trans.cross(rot.col(2));
156 ActionMatrixType toActionMatrixInverse_impl()
const 158 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
160 M.template block<3,3>(ANGULAR,ANGULAR)
161 = M.template block<3,3>(LINEAR,LINEAR) = rot.transpose();
162 Block3 C = M.template block<3,3>(ANGULAR,LINEAR);
163 Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
165 #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id,v3_in,v3_out,R,res) \ 166 CartesianAxis<axis_id>::cross(v3_in,v3_out); \ 167 res.col(axis_id).noalias() = R.transpose() * v3_out; 169 PINOCCHIO_INTERNAL_COMPUTATION(0,trans,C.col(0),rot,B);
170 PINOCCHIO_INTERNAL_COMPUTATION(1,trans,C.col(0),rot,B);
171 PINOCCHIO_INTERNAL_COMPUTATION(2,trans,C.col(0),rot,B);
173 #undef PINOCCHIO_INTERNAL_COMPUTATION 179 ActionMatrixType toDualActionMatrix_impl()
const 181 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
183 M.template block<3,3>(ANGULAR,ANGULAR)
184 = M.template block<3,3>(LINEAR,LINEAR) = rot;
185 M.template block<3,3>(LINEAR,ANGULAR).setZero();
186 Block3 B = M.template block<3,3>(ANGULAR,LINEAR);
188 B.col(0) = trans.cross(rot.col(0));
189 B.col(1) = trans.cross(rot.col(1));
190 B.col(2) = trans.cross(rot.col(2));
194 void disp_impl(std::ostream & os)
const 197 <<
" R =\n" << rot << std::endl
198 <<
" p = " << trans.transpose() << std::endl;
208 return d.se3Action(*
this);
215 return d.se3ActionInverse(*
this);
218 template<
typename EigenDerived>
219 typename EigenDerived::PlainObject
220 actOnEigenObject(
const Eigen::MatrixBase<EigenDerived> & p)
const 221 {
return (rotation()*p+translation()).eval(); }
223 template<
typename MapDerived>
224 Vector3 actOnEigenObject(
const Eigen::MapBase<MapDerived> & p)
const 225 {
return Vector3(rotation()*p+translation()); }
227 template<
typename EigenDerived>
228 typename EigenDerived::PlainObject
229 actInvOnEigenObject(
const Eigen::MatrixBase<EigenDerived> & p)
const 230 {
return (rotation().transpose()*(p-translation())).eval(); }
232 template<
typename MapDerived>
233 Vector3 actInvOnEigenObject(
const Eigen::MapBase<MapDerived> & p)
const 234 {
return Vector3(rotation().transpose()*(p-translation())); }
236 Vector3 act_impl(
const Vector3 & p)
const 237 {
return Vector3(rotation()*p+translation()); }
239 Vector3 actInv_impl(
const Vector3 & p)
const 240 {
return Vector3(rotation().transpose()*(p-translation())); }
244 {
return SE3Tpl(rot*m2.rotation()
245 ,translation()+rotation()*m2.translation());}
249 {
return SE3Tpl(rot.transpose()*m2.rotation(),
250 rot.transpose()*(m2.translation()-translation()));}
254 {
return this->act_impl(m2);}
259 return (rotation() == m2.rotation() && translation() == m2.translation());
264 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 266 return rotation().isApprox(m2.rotation(), prec)
267 && translation().isApprox(m2.translation(), prec);
270 bool isIdentity(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 272 return rotation().isIdentity(prec) && translation().isZero(prec);
275 ConstAngularRef rotation_impl()
const {
return rot; }
276 AngularRef rotation_impl() {
return rot; }
277 void rotation_impl(
const AngularType & R) { rot = R; }
278 ConstLinearRef translation_impl()
const {
return trans;}
279 LinearRef translation_impl() {
return trans;}
280 void translation_impl(
const LinearType & p) { trans = p; }
283 template<
typename NewScalar>
287 ReturnType res(rot.template cast<NewScalar>(),
288 trans.template cast<NewScalar>());
292 if(pinocchio::cast<NewScalar>(Eigen::NumTraits<Scalar>::epsilon()) > Eigen::NumTraits<NewScalar>::epsilon())
297 bool isNormalized(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 299 return (rot.transpose()*rot).eval().isIdentity(prec);
307 PlainType normalized()
const 309 PlainType res(*
this); res.normalize();
324 template<
typename OtherScalar>
325 static SE3Tpl Interpolate(
const SE3Tpl & A,
const SE3Tpl & B,
const OtherScalar & alpha);
335 #endif // ifndef __pinocchio_se3_tpl_hpp__
SE3Tpl inverse() const
aXb = bXa.inverse()
SE3GroupAction< D >::ReturnType actInv_impl(const D &d) const
by = aXb.actInv(ay)
Matrix3 orthogonalProjection(const Eigen::MatrixBase< Matrix3 > &mat)
Orthogonal projection of a matrix on the SO(3) manifold.
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Main pinocchio namespace.
Base class for rigid transformation.
SE3Tpl< NewScalar, Options > cast() const
Common traits structure to fully define base classes for CRTP.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
ActionMatrixType toActionMatrix_impl() const
Vb.toVector() = bXa.toMatrix() * Va.toVector()
SE3GroupAction< D >::ReturnType act_impl(const D &d) const
— GROUP ACTIONS ON M6, F6 and I6 —