motion-dense.hpp
Go to the documentation of this file.
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  {
17  };
18 
19  template<typename Derived, typename MotionDerived>
20  struct MotionAlgebraAction< MotionDense<Derived>, MotionDerived >
21  {
23  };
24 
25  template<typename Derived>
26  class MotionDense : public MotionBase<Derived>
27  {
28  public:
30  MOTION_TYPEDEF_TPL(Derived);
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>
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>
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>
196  se3Action_impl(const SE3Tpl<S2,O2> & m) const
197  {
199  se3Action_impl(m,res);
200  return res;
201  }
202 
203  template<typename S2, int O2, typename D2>
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>
213  {
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>
234  const MotionDense<M2> & v2)
235  { return v1.derived().cross(v2.derived()); }
236 
237  template<typename M1, typename F1>
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__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
MotionPlain operator+(const MotionDense< M1 > &v) const
Derived & operator-=(const MotionDense< M1 > &v)
bool isEqual_impl(const MotionBase< D2 > &other) const
Return type of the ation of a Motion onto an object of type D.
#define MOTION_TYPEDEF_TPL(Derived)
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Definition: math/matrix.hpp:56
SE3GroupAction< Derived >::ReturnType ReturnType
ActionMatrixType toActionMatrix_impl() const
MotionPlain operator-() const
MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType
Scalar dot(const ForceBase< F1 > &phi) const
ConstLinearType linear() const
Definition: motion-base.hpp:22
Derived & operator=(const Eigen::MatrixBase< V6 > &v)
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
ConstAngularType angular() const
Definition: motion-base.hpp:21
SE3::Scalar Scalar
Definition: conversions.cpp:13
SE3GroupAction< Derived >::ReturnType se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
MotionPlain __div__(const OtherScalar &alpha) const
MotionAlgebraAction< D, Derived >::ReturnType cross_impl(const D &d) const
MotionPlain motionAction(const MotionDense< M1 > &v) const
Derived & __mequ__(const MotionDense< M1 > &v)
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
SE3GroupAction< Derived >::ReturnType se3Action_impl(const SE3Tpl< S2, O2 > &m) const
Derived & derived()
Definition: force-base.hpp:27
MotionBase< Derived > Base
HomogeneousMatrixType toHomogeneousMatrix_impl() const
FCL_REAL d
Derived & operator=(const MotionBase< D2 > &other)
void disp_impl(std::ostream &os) const
bool isApprox_impl(const MotionDense< D2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
bool isEqual_impl(const MotionDense< D2 > &other) const
Base interface for forces representation.
Definition: force-base.hpp:22
Derived & operator=(const MotionDense< D2 > &other)
MotionPlain __opposite__() const
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42
traits< Derived >::MotionRefType MotionRefType
Derived & operator+=(const MotionBase< M1 > &v)
Derived & __pequ__(const MotionDense< M1 > &v)
Main pinocchio namespace.
Definition: timings.cpp:28
bool isZero_impl(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Derived & operator+=(const MotionDense< M1 > &v)
res
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
bool isApprox(const MotionDense< M2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
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
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
M
static void motionAction(const MotionDense< MotionDerived > &v, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion on a set of forces, represented by a 6xN matrix whose each column represent a spat...
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
MotionPlain __plus__(const MotionDense< M1 > &v) const
MotionPlain __mult__(const OtherScalar &alpha) const
MotionPlain operator-(const MotionDense< M1 > &v) const
ActionMatrixType toDualActionMatrix_impl() const
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .
MotionPlain __minus__(const MotionDense< M1 > &v) const


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32