pinocchio  2.7.1
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  HomogeneousMatrixType toHomogeneousMatrix_impl() const
63  {
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();
68  return M;
69  }
70 
71  template<typename D2>
72  bool isEqual_impl(const MotionDense<D2> & other) const
73  { return linear() == other.linear() && angular() == other.angular(); }
74 
75  template<typename D2>
76  bool isEqual_impl(const MotionBase<D2> & other) const
77  { return other.derived() == derived(); }
78 
79  // Arithmetic operators
80  template<typename D2>
81  Derived & operator=(const MotionDense<D2> & other)
82  {
83  linear() = other.linear();
84  angular() = other.angular();
85  return derived();
86  }
87 
88  template<typename D2>
89  Derived & operator=(const MotionBase<D2> & other)
90  {
91  other.derived().setTo(derived());
92  return derived();
93  }
94 
95  template<typename V6>
96  Derived & operator=(const Eigen::MatrixBase<V6> & v)
97  {
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);
101  return derived();
102  }
103 
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()); }
109 
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(); }
115 
116  template<typename M1>
117  Derived & operator-=(const MotionDense<M1> & v) { return derived().__mequ__(v.derived()); }
118 
119  MotionPlain __opposite__() const { return MotionPlain(-linear(),-angular()); }
120 
121  template<typename M1>
122  MotionPlain __plus__(const MotionDense<M1> & v) const
123  { return MotionPlain(linear()+v.linear(), angular()+v.angular()); }
124 
125  template<typename M1>
126  MotionPlain __minus__(const MotionDense<M1> & v) const
127  { return MotionPlain(linear()-v.linear(), angular()-v.angular()); }
128 
129  template<typename M1>
130  Derived & __pequ__(const MotionDense<M1> & v)
131  { linear() += v.linear(); angular() += v.angular(); return derived(); }
132 
133  template<typename M1>
134  Derived & __mequ__(const MotionDense<M1> & v)
135  { linear() -= v.linear(); angular() -= v.angular(); return derived(); }
136 
137  template<typename OtherScalar>
138  MotionPlain __mult__(const OtherScalar & alpha) const
139  { return MotionPlain(alpha*linear(),alpha*angular()); }
140 
141  template<typename OtherScalar>
142  MotionPlain __div__(const OtherScalar & alpha) const
143  { return derived().__mult__((OtherScalar)(1)/alpha); }
144 
145  template<typename F1>
146  Scalar dot(const ForceBase<F1> & phi) const
147  { return phi.linear().dot(linear()) + phi.angular().dot(angular()); }
148 
149  template<typename D>
150  typename MotionAlgebraAction<D,Derived>::ReturnType cross_impl(const D & d) const
151  {
152  return d.motionAction(derived());
153  }
154 
155  template<typename M1, typename M2>
156  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
157  {
158  mout.linear() = v.linear().cross(angular())+v.angular().cross(linear());
159  mout.angular() = v.angular().cross(angular());
160  }
161 
162  template<typename M1>
163  MotionPlain motionAction(const MotionDense<M1> & v) const
164  {
165  MotionPlain res;
166  motionAction(v,res);
167  return res;
168  }
169 
170  template<typename M2>
171  bool isApprox(const MotionDense<M2> & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
172  {
173  return derived().isApprox_impl(m2, prec);
174  }
175 
176  template<typename D2>
177  bool isApprox_impl(const MotionDense<D2> & m2, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
178  {
179  return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
180  }
181 
182  bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
183  {
184  return linear().isZero(prec) && angular().isZero(prec);
185  }
186 
187  template<typename S2, int O2, typename D2>
188  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
189  {
190  v.angular().noalias() = m.rotation()*angular();
191  v.linear().noalias() = m.rotation()*linear() + m.translation().cross(v.angular());
192  }
193 
194  template<typename S2, int O2>
195  typename SE3GroupAction<Derived>::ReturnType
196  se3Action_impl(const SE3Tpl<S2,O2> & m) const
197  {
198  typename SE3GroupAction<Derived>::ReturnType res;
199  se3Action_impl(m,res);
200  return res;
201  }
202 
203  template<typename S2, int O2, typename D2>
204  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
205  {
206  v.linear().noalias() = m.rotation().transpose()*(linear()-m.translation().cross(angular()));
207  v.angular().noalias() = m.rotation().transpose()*angular();
208  }
209 
210  template<typename S2, int O2>
211  typename SE3GroupAction<Derived>::ReturnType
212  se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
213  {
214  typename SE3GroupAction<Derived>::ReturnType res;
215  se3ActionInverse_impl(m,res);
216  return res;
217  }
218 
219  void disp_impl(std::ostream & os) const
220  {
221  os
222  << " v = " << linear().transpose () << std::endl
223  << " w = " << angular().transpose () << std::endl;
224  }
225 
227  MotionRefType ref() { return derived().ref(); }
228 
229  }; // class MotionDense
230 
232  template<typename M1, typename M2>
233  typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1,
234  const MotionDense<M2> & v2)
235  { return v1.derived().cross(v2.derived()); }
236 
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()); }
241 
242  template<typename M1>
243  typename traits<M1>::MotionPlain operator*(const typename traits<M1>::Scalar alpha,
244  const MotionDense<M1> & v)
245  { return v*alpha; }
246 
247 } // namespace pinocchio
248 
249 #endif // ifndef __pinocchio_motion_dense_hpp__
pinocchio::operator*
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
&#160;
Definition: constraint-base.hpp:112
pinocchio::MotionDense
Definition: fwd.hpp:41
pinocchio::MotionDense::ref
MotionRefType ref()
Definition: motion-dense.hpp:227
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
pinocchio::MotionBase
Definition: fwd.hpp:40
pinocchio::SE3GroupAction
Definition: se3.hpp:39
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::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11