5 #ifndef __pinocchio_force_dense_hpp__
6 #define __pinocchio_force_dense_hpp__
11 template<
typename Derived>
14 typedef typename SE3GroupAction< Derived >::ReturnType ReturnType;
17 template<
typename Derived,
typename MotionDerived>
20 typedef typename MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType;
23 template<
typename Derived>
28 FORCE_TYPEDEF_TPL(Derived);
36 using Base::operator=;
38 Derived & setZero() {
linear().setZero();
angular().setZero();
return derived(); }
39 Derived & setRandom() {
linear().setRandom();
angular().setRandom();
return derived(); }
47 {
return other.derived() == derived(); }
61 return derived().setFrom(other.derived());
65 Derived & operator=(
const Eigen::MatrixBase<V6> & v)
67 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
68 linear() = v.template segment<3>(LINEAR);
69 angular() = v.template segment<3>(ANGULAR);
73 ForcePlain operator-()
const {
return derived().__opposite__(); }
75 ForcePlain operator+(
const ForceDense<F1> & f)
const {
return derived().__plus__(f.derived()); }
77 ForcePlain operator-(
const ForceDense<F1> & f)
const {
return derived().__minus__(f.derived()); }
80 Derived & operator+=(
const ForceDense<F1> & f) {
return derived().__pequ__(f.derived()); }
83 { f.derived().addTo(derived());
return derived(); }
86 Derived & operator-=(
const ForceDense<M1> & v) {
return derived().__mequ__(v.derived()); }
88 ForcePlain __opposite__()
const {
return ForcePlain(-
linear(),-
angular()); }
102 template<
typename M1>
106 template<
typename OtherScalar>
107 ForcePlain __mult__(
const OtherScalar & alpha)
const
110 template<
typename OtherScalar>
111 ForcePlain __div__(
const OtherScalar & alpha)
const
112 {
return derived().__mult__((OtherScalar)(1)/alpha); }
114 template<
typename F1>
116 {
return phi.linear().dot(
linear()) + phi.angular().dot(
angular()); }
118 template<
typename M1,
typename M2>
125 template<
typename M1>
133 template<
typename M2>
134 bool isApprox(
const ForceDense<M2> & f,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
135 {
return derived().isApprox_impl(f, prec);}
137 template<
typename D2>
138 bool isApprox_impl(
const ForceDense<D2> & f,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
143 bool isZero_impl(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
148 template<
typename S2,
int O2,
typename D2>
156 template<
typename S2,
int O2>
160 se3Action_impl(m,res);
164 template<
typename S2,
int O2,
typename D2>
167 f.
linear().noalias() = m.rotation().transpose()*
linear();
171 template<
typename S2,
int O2>
172 ForcePlain se3ActionInverse_impl(
const SE3Tpl<S2,O2> & m)
const
175 se3ActionInverse_impl(m,res);
179 void disp_impl(std::ostream & os)
const
182 <<
" f = " <<
linear().transpose () << std::endl
183 <<
"tau = " <<
angular().transpose () << std::endl;
187 ForceRefType
ref() {
return derived().ref(); }
192 template<
typename F1>
195 {
return f.derived()*alpha; }
199 #endif // ifndef __pinocchio_force_dense_hpp__