pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
motion-dense.hpp
1 //
2 // Copyright (c) 2017-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_motion_dense_hpp__
6 #define __pinocchio_motion_dense_hpp__
7 
8 #include "pinocchio/spatial/skew.hpp"
9 
10 namespace pinocchio
11 {
12 
13  template<typename Derived>
14  struct SE3GroupAction< MotionDense<Derived> >
15  {
16  typedef typename SE3GroupAction< Derived >::ReturnType ReturnType;
17  };
18 
19  template<typename Derived, typename MotionDerived>
20  struct MotionAlgebraAction< MotionDense<Derived>, MotionDerived >
21  {
22  typedef typename MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType;
23  };
24 
25  template<typename Derived>
26  class MotionDense : public MotionBase<Derived>
27  {
28  public:
29  typedef MotionBase<Derived> Base;
30  MOTION_TYPEDEF_TPL(Derived);
31  typedef typename traits<Derived>::MotionRefType MotionRefType;
32 
33  using Base::linear;
34  using Base::angular;
35  using Base::derived;
36  using Base::isApprox;
37  using Base::isZero;
38 
39  Derived & setZero() { linear().setZero(); angular().setZero(); return derived(); }
40  Derived & setRandom() { linear().setRandom(); angular().setRandom(); return derived(); }
41 
42  ActionMatrixType toActionMatrix_impl() const
43  {
44  ActionMatrixType X;
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();
48 
49  return X;
50  }
51 
52  ActionMatrixType toDualActionMatrix_impl() const
53  {
54  ActionMatrixType X;
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();
58 
59  return X;
60  }
61 
62  template<typename D2>
63  bool isEqual_impl(const MotionDense<D2> & other) const
64  { return linear() == other.linear() && angular() == other.angular(); }
65 
66  template<typename D2>
67  bool isEqual_impl(const MotionBase<D2> & other) const
68  { return other.derived() == derived(); }
69 
70  // Arithmetic operators
71  template<typename D2>
72  Derived & operator=(const MotionDense<D2> & other)
73  {
74  linear() = other.linear();
75  angular() = other.angular();
76  return derived();
77  }
78 
79  template<typename D2>
80  Derived & operator=(const MotionBase<D2> & other)
81  {
82  other.derived().setTo(derived());
83  return derived();
84  }
85 
86  template<typename V6>
87  Derived & operator=(const Eigen::MatrixBase<V6> & v)
88  {
89  EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6);
90  linear() = v.template segment<3>(LINEAR);
91  angular() = v.template segment<3>(ANGULAR);
92  return derived();
93  }
94 
95  MotionPlain operator-() const { return derived().__opposite__(); }
96  template<typename M1>
97  MotionPlain operator+(const MotionDense<M1> & v) const { return derived().__plus__(v.derived()); }
98  template<typename M1>
99  MotionPlain operator-(const MotionDense<M1> & v) const { return derived().__minus__(v.derived()); }
100 
101  template<typename M1>
102  Derived & operator+=(const MotionDense<M1> & v) { return derived().__pequ__(v.derived()); }
103  template<typename M1>
104  Derived & operator+=(const MotionBase<M1> & v)
105  { v.derived().addTo(derived()); return derived(); }
106 
107  template<typename M1>
108  Derived & operator-=(const MotionDense<M1> & v) { return derived().__mequ__(v.derived()); }
109 
110  MotionPlain __opposite__() const { return MotionPlain(-linear(),-angular()); }
111 
112  template<typename M1>
113  MotionPlain __plus__(const MotionDense<M1> & v) const
114  { return MotionPlain(linear()+v.linear(), angular()+v.angular()); }
115 
116  template<typename M1>
117  MotionPlain __minus__(const MotionDense<M1> & v) const
118  { return MotionPlain(linear()-v.linear(), angular()-v.angular()); }
119 
120  template<typename M1>
121  Derived & __pequ__(const MotionDense<M1> & v)
122  { linear() += v.linear(); angular() += v.angular(); return derived(); }
123 
124  template<typename M1>
125  Derived & __mequ__(const MotionDense<M1> & v)
126  { linear() -= v.linear(); angular() -= v.angular(); return derived(); }
127 
128  template<typename OtherScalar>
129  MotionPlain __mult__(const OtherScalar & alpha) const
130  { return MotionPlain(alpha*linear(),alpha*angular()); }
131 
132  template<typename OtherScalar>
133  MotionPlain __div__(const OtherScalar & alpha) const
134  { return derived().__mult__((OtherScalar)(1)/alpha); }
135 
136  template<typename F1>
137  Scalar dot(const ForceBase<F1> & phi) const
138  { return phi.linear().dot(linear()) + phi.angular().dot(angular()); }
139 
140  template<typename D>
141  typename MotionAlgebraAction<D,Derived>::ReturnType cross_impl(const D & d) const
142  {
143  return d.motionAction(derived());
144  }
145 
146  template<typename M1, typename M2>
147  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
148  {
149  mout.linear() = v.linear().cross(angular())+v.angular().cross(linear());
150  mout.angular() = v.angular().cross(angular());
151  }
152 
153  template<typename M1>
154  MotionPlain motionAction(const MotionDense<M1> & v) const
155  {
156  MotionPlain res;
157  motionAction(v,res);
158  return res;
159  }
160 
161  template<typename M2>
162  bool isApprox(const MotionDense<M2> & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
163  {
164  return derived().isApprox_impl(m2, prec);
165  }
166 
167  template<typename D2>
168  bool isApprox_impl(const MotionDense<D2> & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
169  {
170  return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
171  }
172 
173  bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
174  {
175  return linear().isZero(prec) && angular().isZero(prec);
176  }
177 
178  template<typename S2, int O2, typename D2>
179  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
180  {
181  v.angular().noalias() = m.rotation()*angular();
182  v.linear().noalias() = m.rotation()*linear() + m.translation().cross(v.angular());
183  }
184 
185  template<typename S2, int O2>
186  typename SE3GroupAction<Derived>::ReturnType
187  se3Action_impl(const SE3Tpl<S2,O2> & m) const
188  {
189  typename SE3GroupAction<Derived>::ReturnType res;
190  se3Action_impl(m,res);
191  return res;
192  }
193 
194  template<typename S2, int O2, typename D2>
195  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
196  {
197  v.linear().noalias() = m.rotation().transpose()*(linear()-m.translation().cross(angular()));
198  v.angular().noalias() = m.rotation().transpose()*angular();
199  }
200 
201  template<typename S2, int O2>
202  typename SE3GroupAction<Derived>::ReturnType
203  se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
204  {
205  typename SE3GroupAction<Derived>::ReturnType res;
206  se3ActionInverse_impl(m,res);
207  return res;
208  }
209 
210  void disp_impl(std::ostream & os) const
211  {
212  os
213  << " v = " << linear().transpose () << std::endl
214  << " w = " << angular().transpose () << std::endl;
215  }
216 
218  MotionRefType ref() { return derived().ref(); }
219 
220  }; // class MotionDense
221 
223  template<typename M1, typename M2>
224  typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1,
225  const MotionDense<M2> & v2)
226  { return v1.derived().cross(v2.derived()); }
227 
228  template<typename M1, typename F1>
229  typename traits<F1>::ForcePlain operator^(const MotionDense<M1> & v,
230  const ForceBase<F1> & f)
231  { return v.derived().cross(f.derived()); }
232 
233  template<typename M1>
234  typename traits<M1>::MotionPlain operator*(const typename traits<M1>::Scalar alpha,
235  const MotionDense<M1> & v)
236  { return v*alpha; }
237 
238 } // namespace pinocchio
239 
240 #endif // ifndef __pinocchio_motion_dense_hpp__
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:44
pinocchio::ForceBase::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
pinocchio::ForceBase
Base interface for forces representation.
Definition: force-base.hpp:22
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
pinocchio::skew
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...
Definition: skew.hpp:21
pinocchio::ForceBase::linear
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42
pinocchio::operator*
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .
Definition: constraint-base.hpp:112