5 #ifndef __pinocchio_motion_dense_hpp__ 6 #define __pinocchio_motion_dense_hpp__ 8 #include "pinocchio/spatial/skew.hpp" 13 template<
typename Derived>
19 template<
typename Derived,
typename MotionDerived>
25 template<
typename Derived>
39 Derived &
setZero() { linear().setZero(); angular().setZero();
return derived(); }
40 Derived &
setRandom() { linear().setRandom(); angular().setRandom();
return derived(); }
45 X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) =
skew(angular());
46 X.template block <3,3> (LINEAR, ANGULAR) =
skew(linear());
47 X.template block <3,3> (ANGULAR, LINEAR).
setZero();
55 X.template block <3,3> (ANGULAR, ANGULAR) = X.template block <3,3> (LINEAR, LINEAR) =
skew(angular());
56 X.template block <3,3> (ANGULAR, LINEAR) =
skew(linear());
57 X.template block <3,3> (LINEAR, ANGULAR).
setZero();
64 HomogeneousMatrixType
M;
65 M.template block<3,3>(0, 0) =
skew(angular());
66 M.template block<3,1>(0, 3) = linear();
67 M.template block<1,4>(3, 0).
setZero();
73 {
return linear() == other.
linear() && angular() == other.
angular(); }
77 {
return other.
derived() == derived(); }
91 other.
derived().setTo(derived());
98 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
99 linear() = v.template segment<3>(LINEAR);
100 angular() = v.template segment<3>(ANGULAR);
104 MotionPlain
operator-()
const {
return derived().__opposite__(); }
105 template<
typename M1>
107 template<
typename M1>
110 template<
typename M1>
112 template<
typename M1>
114 { v.
derived().addTo(derived());
return derived(); }
116 template<
typename M1>
119 MotionPlain
__opposite__()
const {
return MotionPlain(-linear(),-angular()); }
121 template<
typename M1>
123 {
return MotionPlain(linear()+v.
linear(), angular()+v.
angular()); }
125 template<
typename M1>
127 {
return MotionPlain(linear()-v.
linear(), angular()-v.
angular()); }
129 template<
typename M1>
131 { linear() += v.
linear(); angular() += v.
angular();
return derived(); }
133 template<
typename M1>
135 { linear() -= v.
linear(); angular() -= v.
angular();
return derived(); }
137 template<
typename OtherScalar>
138 MotionPlain
__mult__(
const OtherScalar & alpha)
const 139 {
return MotionPlain(alpha*linear(),alpha*angular()); }
141 template<
typename OtherScalar>
142 MotionPlain
__div__(
const OtherScalar & alpha)
const 143 {
return derived().__mult__((OtherScalar)(1)/alpha); }
145 template<
typename F1>
147 {
return phi.
linear().dot(linear()) + phi.
angular().dot(angular()); }
152 return d.motionAction(derived());
155 template<
typename M1,
typename M2>
162 template<
typename M1>
170 template<
typename M2>
173 return derived().isApprox_impl(m2, prec);
176 template<
typename D2>
179 return linear().isApprox(m2.
linear(), prec) && angular().isApprox(m2.
angular(), prec);
184 return linear().isZero(prec) && angular().isZero(prec);
187 template<
typename S2,
int O2,
typename D2>
194 template<
typename S2,
int O2>
199 se3Action_impl(m,res);
203 template<
typename S2,
int O2,
typename D2>
210 template<
typename S2,
int O2>
215 se3ActionInverse_impl(m,res);
222 <<
" v = " << linear().transpose () << std::endl
223 <<
" w = " << angular().transpose () << std::endl;
227 MotionRefType
ref() {
return derived().ref(); }
232 template<
typename M1,
typename M2>
237 template<
typename M1,
typename F1>
242 template<
typename M1>
249 #endif // ifndef __pinocchio_motion_dense_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
MotionPlain operator+(const MotionDense< M1 > &v) const
Derived & operator-=(const MotionDense< M1 > &v)
bool isEqual_impl(const MotionBase< D2 > &other) const
Return type of the ation of a Motion onto an object of type D.
#define MOTION_TYPEDEF_TPL(Derived)
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
SE3GroupAction< Derived >::ReturnType ReturnType
ActionMatrixType toActionMatrix_impl() const
MotionPlain operator-() const
MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType
Scalar dot(const ForceBase< F1 > &phi) const
ConstLinearType linear() const
Derived & operator=(const Eigen::MatrixBase< V6 > &v)
ConstAngularType angular() const
Return the angular part of the force vector.
ConstAngularType angular() const
SE3GroupAction< Derived >::ReturnType se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
MotionPlain __div__(const OtherScalar &alpha) const
MotionAlgebraAction< D, Derived >::ReturnType cross_impl(const D &d) const
MotionPlain motionAction(const MotionDense< M1 > &v) const
Derived & __mequ__(const MotionDense< M1 > &v)
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
SE3GroupAction< Derived >::ReturnType se3Action_impl(const SE3Tpl< S2, O2 > &m) const
MotionBase< Derived > Base
HomogeneousMatrixType toHomogeneousMatrix_impl() const
Derived & operator=(const MotionBase< D2 > &other)
void disp_impl(std::ostream &os) const
bool isApprox_impl(const MotionDense< D2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
bool isEqual_impl(const MotionDense< D2 > &other) const
Base interface for forces representation.
Derived & operator=(const MotionDense< D2 > &other)
MotionPlain __opposite__() const
ConstLinearType linear() const
Return the linear part of the force vector.
traits< Derived >::MotionRefType MotionRefType
Derived & operator+=(const MotionBase< M1 > &v)
Derived & __pequ__(const MotionDense< M1 > &v)
ConstAngularRef rotation() const
Main pinocchio namespace.
bool isZero_impl(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Derived & operator+=(const MotionDense< M1 > &v)
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
Common traits structure to fully define base classes for CRTP.
bool isApprox(const MotionDense< M2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
static void motionAction(const MotionDense< MotionDerived > &v, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion on a set of forces, represented by a 6xN matrix whose each column represent a spat...
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
MotionPlain __plus__(const MotionDense< M1 > &v) const
MotionPlain __mult__(const OtherScalar &alpha) const
MotionPlain operator-(const MotionDense< M1 > &v) const
ActionMatrixType toDualActionMatrix_impl() const
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
.
ConstLinearRef translation() const
MotionPlain __minus__(const MotionDense< M1 > &v) const