5 #ifndef __pinocchio_motion_dense_hpp__
6 #define __pinocchio_motion_dense_hpp__
8 #include "pinocchio/spatial/skew.hpp"
13 template<
typename Derived>
16 typedef typename SE3GroupAction< Derived >::ReturnType ReturnType;
19 template<
typename Derived,
typename MotionDerived>
22 typedef typename MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType;
25 template<
typename Derived>
30 MOTION_TYPEDEF_TPL(Derived);
39 Derived & setZero() { linear().setZero(); angular().setZero();
return derived(); }
40 Derived & setRandom() { linear().setRandom(); angular().setRandom();
return derived(); }
42 ActionMatrixType toActionMatrix_impl()
const
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();
52 ActionMatrixType toDualActionMatrix_impl()
const
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();
62 HomogeneousMatrixType toHomogeneousMatrix_impl()
const
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();
72 bool isEqual_impl(
const MotionDense<D2> & other)
const
73 {
return linear() == other.linear() && angular() == other.angular(); }
76 bool isEqual_impl(
const MotionBase<D2> & other)
const
77 {
return other.derived() == derived(); }
81 Derived & operator=(
const MotionDense<D2> & other)
83 linear() = other.linear();
84 angular() = other.angular();
89 Derived & operator=(
const MotionBase<D2> & other)
91 other.derived().setTo(derived());
96 Derived & operator=(
const Eigen::MatrixBase<V6> & v)
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>
106 MotionPlain operator+(
const MotionDense<M1> & v)
const {
return derived().__plus__(v.derived()); }
107 template<
typename M1>
108 MotionPlain operator-(
const MotionDense<M1> & v)
const {
return derived().__minus__(v.derived()); }
110 template<
typename M1>
111 Derived & operator+=(
const MotionDense<M1> & v) {
return derived().__pequ__(v.derived()); }
112 template<
typename M1>
113 Derived & operator+=(
const MotionBase<M1> & v)
114 { v.derived().addTo(derived());
return derived(); }
116 template<
typename M1>
117 Derived & operator-=(
const MotionDense<M1> & v) {
return derived().__mequ__(v.derived()); }
119 MotionPlain __opposite__()
const {
return MotionPlain(-linear(),-angular()); }
121 template<
typename M1>
122 MotionPlain __plus__(
const MotionDense<M1> & v)
const
123 {
return MotionPlain(linear()+v.linear(), angular()+v.angular()); }
125 template<
typename M1>
126 MotionPlain __minus__(
const MotionDense<M1> & v)
const
127 {
return MotionPlain(linear()-v.linear(), angular()-v.angular()); }
129 template<
typename M1>
130 Derived & __pequ__(
const MotionDense<M1> & v)
131 { linear() += v.linear(); angular() += v.angular();
return derived(); }
133 template<
typename M1>
134 Derived & __mequ__(
const MotionDense<M1> & v)
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>
146 Scalar dot(
const ForceBase<F1> & phi)
const
147 {
return phi.linear().dot(linear()) + phi.angular().dot(angular()); }
150 typename MotionAlgebraAction<D,Derived>::ReturnType cross_impl(
const D & d)
const
152 return d.motionAction(derived());
155 template<
typename M1,
typename M2>
156 void motionAction(
const MotionDense<M1> & v, MotionDense<M2> & mout)
const
158 mout.linear() = v.linear().cross(angular())+v.angular().cross(linear());
159 mout.angular() = v.angular().cross(angular());
162 template<
typename M1>
163 MotionPlain motionAction(
const MotionDense<M1> & v)
const
170 template<
typename M2>
171 bool isApprox(
const MotionDense<M2> & m2,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
173 return derived().isApprox_impl(m2, prec);
176 template<
typename D2>
177 bool isApprox_impl(
const MotionDense<D2> & m2,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
179 return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
182 bool isZero_impl(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
184 return linear().isZero(prec) && angular().isZero(prec);
187 template<
typename S2,
int O2,
typename D2>
188 void se3Action_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
190 v.angular().noalias() = m.rotation()*angular();
191 v.linear().noalias() = m.rotation()*linear() + m.translation().cross(v.angular());
194 template<
typename S2,
int O2>
195 typename SE3GroupAction<Derived>::ReturnType
196 se3Action_impl(
const SE3Tpl<S2,O2> & m)
const
198 typename SE3GroupAction<Derived>::ReturnType res;
199 se3Action_impl(m,res);
203 template<
typename S2,
int O2,
typename D2>
204 void se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m, MotionDense<D2> & v)
const
206 v.linear().noalias() = m.rotation().transpose()*(linear()-m.translation().cross(angular()));
207 v.angular().noalias() = m.rotation().transpose()*angular();
210 template<
typename S2,
int O2>
211 typename SE3GroupAction<Derived>::ReturnType
212 se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const
214 typename SE3GroupAction<Derived>::ReturnType res;
215 se3ActionInverse_impl(m,res);
219 void disp_impl(std::ostream & os)
const
222 <<
" v = " << linear().transpose () << std::endl
223 <<
" w = " << angular().transpose () << std::endl;
227 MotionRefType
ref() {
return derived().ref(); }
232 template<
typename M1,
typename M2>
235 {
return v1.derived().cross(v2.derived()); }
237 template<
typename M1,
typename F1>
238 typename traits<F1>::ForcePlain operator^(
const MotionDense<M1> & v,
239 const ForceBase<F1> & f)
240 {
return v.derived().cross(f.derived()); }
242 template<
typename M1>
243 typename traits<M1>::MotionPlain
operator*(
const typename traits<M1>::Scalar alpha,
244 const MotionDense<M1> & v)
249 #endif // ifndef __pinocchio_motion_dense_hpp__