pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
force-dense.hpp
1 //
2 // Copyright (c) 2017-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_force_dense_hpp__
6 #define __pinocchio_force_dense_hpp__
7 
8 namespace pinocchio
9 {
10 
11  template<typename Derived>
12  struct SE3GroupAction< ForceDense<Derived> >
13  {
15  };
16 
17  template<typename Derived, typename MotionDerived>
18  struct MotionAlgebraAction< ForceDense<Derived>, MotionDerived >
19  {
21  };
22 
23  template<typename Derived>
24  class ForceDense : public ForceBase<Derived>
25  {
26  public:
27  typedef ForceBase<Derived> Base;
28  FORCE_TYPEDEF_TPL(Derived);
29  typedef typename traits<Derived>::ForceRefType ForceRefType;
30 
31  using Base::linear;
32  using Base::angular;
33  using Base::derived;
34  using Base::isApprox;
35  using Base::isZero;
36  using Base::operator=;
37 
38  Derived & setZero() { linear().setZero(); angular().setZero(); return derived(); }
39  Derived & setRandom() { linear().setRandom(); angular().setRandom(); return derived(); }
40 
41  template<typename D2>
42  bool isEqual_impl(const ForceDense<D2> & other) const
43  { return linear() == other.linear() && angular() == other.angular(); }
44 
45  template<typename D2>
46  bool isEqual_impl(const ForceBase<D2> & other) const
47  { return other.derived() == derived(); }
48 
49  // Arithmetic operators
50  template<typename D2>
51  Derived & setFrom(const ForceDense<D2> & other)
52  {
53  linear() = other.linear();
54  angular() = other.angular();
55  return derived();
56  }
57 
58  template<typename D2>
59  Derived & operator=(const ForceDense<D2> & other)
60  {
61  return derived().setFrom(other.derived());
62  }
63 
64  template<typename V6>
65  Derived & operator=(const Eigen::MatrixBase<V6> & v)
66  {
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);
70  return derived();
71  }
72 
73  ForcePlain operator-() const { return derived().__opposite__(); }
74  template<typename F1>
75  ForcePlain operator+(const ForceDense<F1> & f) const { return derived().__plus__(f.derived()); }
76  template<typename F1>
77  ForcePlain operator-(const ForceDense<F1> & f) const { return derived().__minus__(f.derived()); }
78 
79  template<typename F1>
80  Derived & operator+=(const ForceDense<F1> & f) { return derived().__pequ__(f.derived()); }
81  template<typename F1>
82  Derived & operator+=(const ForceBase<F1> & f)
83  { f.derived().addTo(derived()); return derived(); }
84 
85  template<typename M1>
86  Derived & operator-=(const ForceDense<M1> & v) { return derived().__mequ__(v.derived()); }
87 
88  ForcePlain __opposite__() const { return ForcePlain(-linear(),-angular()); }
89 
90  template<typename M1>
91  ForcePlain __plus__(const ForceDense<M1> & v) const
92  { return ForcePlain(linear()+v.linear(), angular()+v.angular()); }
93 
94  template<typename M1>
95  ForcePlain __minus__(const ForceDense<M1> & v) const
96  { return ForcePlain(linear()-v.linear(), angular()-v.angular()); }
97 
98  template<typename M1>
99  Derived & __pequ__(const ForceDense<M1> & v)
100  { linear() += v.linear(); angular() += v.angular(); return derived(); }
101 
102  template<typename M1>
103  Derived & __mequ__(const ForceDense<M1> & v)
104  { linear() -= v.linear(); angular() -= v.angular(); return derived(); }
105 
106  template<typename OtherScalar>
107  ForcePlain __mult__(const OtherScalar & alpha) const
108  { return ForcePlain(alpha*linear(),alpha*angular()); }
109 
110  template<typename OtherScalar>
111  ForcePlain __div__(const OtherScalar & alpha) const
112  { return derived().__mult__((OtherScalar)(1)/alpha); }
113 
114  template<typename F1>
115  Scalar dot(const MotionDense<F1> & phi) const
116  { return phi.linear().dot(linear()) + phi.angular().dot(angular()); }
117 
118  template<typename M1, typename M2>
119  void motionAction(const MotionDense<M1> & v, ForceDense<M2> & fout) const
120  {
121  fout.linear().noalias() = v.angular().cross(linear());
122  fout.angular().noalias() = v.angular().cross(angular())+v.linear().cross(linear());
123  }
124 
125  template<typename M1>
126  ForcePlain motionAction(const MotionDense<M1> & v) const
127  {
128  ForcePlain res;
129  motionAction(v,res);
130  return res;
131  }
132 
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);}
136 
137  template<typename D2>
138  bool isApprox_impl(const ForceDense<D2> & f, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
139  {
140  return linear().isApprox(f.linear(), prec) && angular().isApprox(f.angular(), prec);
141  }
142 
143  bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
144  {
145  return linear().isZero(prec) && angular().isZero(prec);
146  }
147 
148  template<typename S2, int O2, typename D2>
149  void se3Action_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const
150  {
151  f.linear().noalias() = m.rotation()*linear();
152  f.angular().noalias() = m.rotation()*angular();
153  f.angular() += m.translation().cross(f.linear());
154  }
155 
156  template<typename S2, int O2>
157  ForcePlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
158  {
159  ForcePlain res;
160  se3Action_impl(m,res);
161  return res;
162  }
163 
164  template<typename S2, int O2, typename D2>
165  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const
166  {
167  f.linear().noalias() = m.rotation().transpose()*linear();
168  f.angular().noalias() = m.rotation().transpose()*(angular()-m.translation().cross(linear()));
169  }
170 
171  template<typename S2, int O2>
172  ForcePlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
173  {
174  ForcePlain res;
175  se3ActionInverse_impl(m,res);
176  return res;
177  }
178 
179  void disp_impl(std::ostream & os) const
180  {
181  os
182  << " f = " << linear().transpose () << std::endl
183  << "tau = " << angular().transpose () << std::endl;
184  }
185 
187  ForceRefType ref() { return derived().ref(); }
188 
189  }; // class ForceDense
190 
192  template<typename F1>
193  typename traits<F1>::ForcePlain operator*(const typename traits<F1>::Scalar alpha,
194  const ForceDense<F1> & f)
195  { return f.derived()*alpha; }
196 
197 } // namespace pinocchio
198 
199 #endif // ifndef __pinocchio_force_dense_hpp__
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:44
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
Base interface for forces representation.
Definition: force-base.hpp:22
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42
ForceRefType ref()
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .