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/spatial/cartesian-axis.hpp" 15 template<
typename _Scalar,
int _Options>
23 typedef _Scalar Scalar;
24 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
25 typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
26 typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
27 typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
28 typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
29 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
30 typedef Matrix3 AngularType;
31 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef;
32 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef;
33 typedef Vector3 LinearType;
34 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef;
35 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef;
36 typedef Matrix6 ActionMatrixType;
37 typedef Matrix4 HomogeneousMatrixType;
40 template<
typename _Scalar,
int _Options>
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 PINOCCHIO_SE3_TYPEDEF_TPL(
SE3Tpl);
46 typedef Eigen::Quaternion<Scalar,Options> Quaternion;
54 using Base::translation;
56 SE3Tpl(): rot(), trans() {};
58 template<
typename QuaternionLike,
typename Vector3Like>
59 SE3Tpl(
const Eigen::QuaternionBase<QuaternionLike> & quat,
60 const Eigen::MatrixBase<Vector3Like> & trans)
61 : rot(quat.matrix()), trans(trans)
63 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
66 template<
typename Matrix3Like,
typename Vector3Like>
67 SE3Tpl(
const Eigen::MatrixBase<Matrix3Like> & R,
68 const Eigen::MatrixBase<Vector3Like> & trans)
69 : rot(R), trans(trans)
71 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
72 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3)
75 template<
typename Matrix4Like>
76 explicit SE3Tpl(
const Eigen::MatrixBase<Matrix4Like> & m)
77 : rot(m.template block<3,3>(LINEAR,LINEAR))
78 , trans(m.template block<3,1>(LINEAR,ANGULAR))
80 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4);
84 : rot(AngularType::Identity())
85 , trans(LinearType::Zero())
90 : rot(clone.rotation()),trans(clone.translation()) {}
95 rot = other.rotation();
96 trans = other.translation();
100 static SE3Tpl Identity()
105 SE3Tpl & setIdentity()
106 { rot.setIdentity (); trans.setZero ();
return *
this;}
111 return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
114 static SE3Tpl Random()
116 return SE3Tpl().setRandom();
128 HomogeneousMatrixType toHomogeneousMatrix_impl()
const 130 HomogeneousMatrixType M;
131 M.template block<3,3>(LINEAR,LINEAR) = rot;
132 M.template block<3,1>(LINEAR,ANGULAR) = trans;
133 M.template block<1,3>(ANGULAR,LINEAR).setZero();
141 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
143 M.template block<3,3>(ANGULAR,ANGULAR)
144 = M.template block<3,3>(LINEAR,LINEAR) = rot;
145 M.template block<3,3>(ANGULAR,LINEAR).setZero();
146 Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
148 B.col(0) = trans.cross(rot.col(0));
149 B.col(1) = trans.cross(rot.col(1));
150 B.col(2) = trans.cross(rot.col(2));
154 ActionMatrixType toActionMatrixInverse_impl()
const 156 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
158 M.template block<3,3>(ANGULAR,ANGULAR)
159 = M.template block<3,3>(LINEAR,LINEAR) = rot.transpose();
160 Block3 C = M.template block<3,3>(ANGULAR,LINEAR);
161 Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
163 #define PINOCCHIO_INTERNAL_COMPUTATION(axis_id,v3_in,v3_out,R,res) \ 164 CartesianAxis<axis_id>::cross(v3_in,v3_out); \ 165 res.col(axis_id).noalias() = R.transpose() * v3_out; 167 PINOCCHIO_INTERNAL_COMPUTATION(0,trans,C.col(0),rot,B);
168 PINOCCHIO_INTERNAL_COMPUTATION(1,trans,C.col(0),rot,B);
169 PINOCCHIO_INTERNAL_COMPUTATION(2,trans,C.col(0),rot,B);
171 #undef PINOCCHIO_INTERNAL_COMPUTATION 177 ActionMatrixType toDualActionMatrix_impl()
const 179 typedef Eigen::Block<ActionMatrixType,3,3> Block3;
181 M.template block<3,3>(ANGULAR,ANGULAR)
182 = M.template block<3,3>(LINEAR,LINEAR) = rot;
183 M.template block<3,3>(LINEAR,ANGULAR).setZero();
184 Block3 B = M.template block<3,3>(ANGULAR,LINEAR);
186 B.col(0) = trans.cross(rot.col(0));
187 B.col(1) = trans.cross(rot.col(1));
188 B.col(2) = trans.cross(rot.col(2));
192 void disp_impl(std::ostream & os)
const 195 <<
" R =\n" << rot << std::endl
196 <<
" p = " << trans.transpose() << std::endl;
206 return d.se3Action(*
this);
213 return d.se3ActionInverse(*
this);
216 template<
typename EigenDerived>
217 typename EigenDerived::PlainObject
218 actOnEigenObject(
const Eigen::MatrixBase<EigenDerived> & p)
const 219 {
return (rotation()*p+translation()).eval(); }
221 template<
typename MapDerived>
222 Vector3 actOnEigenObject(
const Eigen::MapBase<MapDerived> & p)
const 223 {
return Vector3(rotation()*p+translation()); }
225 template<
typename EigenDerived>
226 typename EigenDerived::PlainObject
227 actInvOnEigenObject(
const Eigen::MatrixBase<EigenDerived> & p)
const 228 {
return (rotation().transpose()*(p-translation())).eval(); }
230 template<
typename MapDerived>
231 Vector3 actInvOnEigenObject(
const Eigen::MapBase<MapDerived> & p)
const 232 {
return Vector3(rotation().transpose()*(p-translation())); }
234 Vector3 act_impl(
const Vector3 & p)
const 235 {
return Vector3(rotation()*p+translation()); }
237 Vector3 actInv_impl(
const Vector3 & p)
const 238 {
return Vector3(rotation().transpose()*(p-translation())); }
242 {
return SE3Tpl(rot*m2.rotation()
243 ,translation()+rotation()*m2.translation());}
247 {
return SE3Tpl(rot.transpose()*m2.rotation(),
248 rot.transpose()*(m2.translation()-translation()));}
252 {
return this->act_impl(m2);}
257 return (rotation() == m2.rotation() && translation() == m2.translation());
262 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 264 return rotation().isApprox(m2.rotation(), prec)
265 && translation().isApprox(m2.translation(), prec);
268 bool isIdentity(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const 270 return rotation().isIdentity(prec) && translation().isZero(prec);
273 ConstAngularRef rotation_impl()
const {
return rot; }
274 AngularRef rotation_impl() {
return rot; }
275 void rotation_impl(
const AngularType & R) { rot = R; }
276 ConstLinearRef translation_impl()
const {
return trans;}
277 LinearRef translation_impl() {
return trans;}
278 void translation_impl(
const LinearType & p) { trans = p; }
281 template<
typename NewScalar>
285 ReturnType res(rot.template cast<NewScalar>(),
286 trans.template cast<NewScalar>());
301 template<
typename OtherScalar>
302 static SE3Tpl Interpolate(
const SE3Tpl & A,
const SE3Tpl & B,
const OtherScalar & alpha);
312 #endif // ifndef __pinocchio_se3_tpl_hpp__
SE3GroupAction< D >::ReturnType act_impl(const D &d) const
— GROUP ACTIONS ON M6, F6 and I6 —
SE3GroupAction< D >::ReturnType actInv_impl(const D &d) const
by = aXb.actInv(ay)
SE3Tpl< NewScalar, Options > cast() const
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Main pinocchio namespace.
ActionMatrixType toActionMatrix_impl() const
Vb.toVector() = bXa.toMatrix() * Va.toVector()
Common traits structure to fully define base classes for CRTP.
SE3Tpl inverse() const
aXb = bXa.inverse()