motion-dense.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_spatial_motion_dense_hpp__
6 #define __pinocchio_spatial_motion_dense_hpp__
7 
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::angular;
34  using Base::derived;
35  using Base::isApprox;
36  using Base::isZero;
37  using Base::linear;
38 
39  Derived & setZero()
40  {
41  linear().setZero();
42  angular().setZero();
43  return derived();
44  }
45  Derived & setRandom()
46  {
47  linear().setRandom();
48  angular().setRandom();
49  return derived();
50  }
51 
52  ActionMatrixType toActionMatrix_impl() const
53  {
54  ActionMatrixType X;
55  X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
56  skew(angular());
57  X.template block<3, 3>(LINEAR, ANGULAR) = skew(linear());
58  X.template block<3, 3>(ANGULAR, LINEAR).setZero();
59 
60  return X;
61  }
62 
63  ActionMatrixType toDualActionMatrix_impl() const
64  {
65  ActionMatrixType X;
66  X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
67  skew(angular());
68  X.template block<3, 3>(ANGULAR, LINEAR) = skew(linear());
69  X.template block<3, 3>(LINEAR, ANGULAR).setZero();
70 
71  return X;
72  }
73 
74  HomogeneousMatrixType toHomogeneousMatrix_impl() const
75  {
76  HomogeneousMatrixType M;
77  M.template block<3, 3>(0, 0) = skew(angular());
78  M.template block<3, 1>(0, 3) = linear();
79  M.template block<1, 4>(3, 0).setZero();
80  return M;
81  }
82 
83  template<typename D2>
84  bool isEqual_impl(const MotionDense<D2> & other) const
85  {
86  return linear() == other.linear() && angular() == other.angular();
87  }
88 
89  template<typename D2>
90  bool isEqual_impl(const MotionBase<D2> & other) const
91  {
92  return other.derived() == derived();
93  }
94 
95  // Arithmetic operators
96  template<typename D2>
97  Derived & operator=(const MotionDense<D2> & other)
98  {
99  return derived().set(other.derived());
100  }
101 
102  Derived & operator=(const MotionDense & other)
103  {
104  return derived().set(other.derived());
105  }
106 
107  template<typename D2>
108  Derived & set(const MotionDense<D2> & other)
109  {
110  linear() = other.linear();
111  angular() = other.angular();
112  return derived();
113  }
114 
115  template<typename D2>
116  Derived & operator=(const MotionBase<D2> & other)
117  {
118  other.derived().setTo(derived());
119  return derived();
120  }
121 
122  template<typename V6>
123  Derived & operator=(const Eigen::MatrixBase<V6> & v)
124  {
125  EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
126  assert(v.size() == 6);
127  linear() = v.template segment<3>(LINEAR);
128  angular() = v.template segment<3>(ANGULAR);
129  return derived();
130  }
131 
132  MotionPlain operator-() const
133  {
134  return derived().__opposite__();
135  }
136  template<typename M1>
137  MotionPlain operator+(const MotionDense<M1> & v) const
138  {
139  return derived().__plus__(v.derived());
140  }
141  template<typename M1>
142  MotionPlain operator-(const MotionDense<M1> & v) const
143  {
144  return derived().__minus__(v.derived());
145  }
146 
147  template<typename M1>
148  Derived & operator+=(const MotionDense<M1> & v)
149  {
150  return derived().__pequ__(v.derived());
151  }
152  template<typename M1>
153  Derived & operator+=(const MotionBase<M1> & v)
154  {
155  v.derived().addTo(derived());
156  return derived();
157  }
158 
159  template<typename M1>
160  Derived & operator-=(const MotionDense<M1> & v)
161  {
162  return derived().__mequ__(v.derived());
163  }
164 
165  MotionPlain __opposite__() const
166  {
167  return MotionPlain(-linear(), -angular());
168  }
169 
170  template<typename M1>
171  MotionPlain __plus__(const MotionDense<M1> & v) const
172  {
173  return MotionPlain(linear() + v.linear(), angular() + v.angular());
174  }
175 
176  template<typename M1>
177  MotionPlain __minus__(const MotionDense<M1> & v) const
178  {
179  return MotionPlain(linear() - v.linear(), angular() - v.angular());
180  }
181 
182  template<typename M1>
183  Derived & __pequ__(const MotionDense<M1> & v)
184  {
185  linear() += v.linear();
186  angular() += v.angular();
187  return derived();
188  }
189 
190  template<typename M1>
191  Derived & __mequ__(const MotionDense<M1> & v)
192  {
193  linear() -= v.linear();
194  angular() -= v.angular();
195  return derived();
196  }
197 
198  template<typename OtherScalar>
199  MotionPlain __mult__(const OtherScalar & alpha) const
200  {
201  return MotionPlain(alpha * linear(), alpha * angular());
202  }
203 
204  template<typename OtherScalar>
205  MotionPlain __div__(const OtherScalar & alpha) const
206  {
207  return derived().__mult__((OtherScalar)(1) / alpha);
208  }
209 
210  template<typename F1>
211  Scalar dot(const ForceBase<F1> & phi) const
212  {
213  return phi.linear().dot(linear()) + phi.angular().dot(angular());
214  }
215 
216  template<typename D>
218  {
219  return d.motionAction(derived());
220  }
221 
222  template<typename M1, typename M2>
223  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
224  {
225  mout.linear() = v.linear().cross(angular()) + v.angular().cross(linear());
226  mout.angular() = v.angular().cross(angular());
227  }
228 
229  template<typename M1>
230  MotionPlain motionAction(const MotionDense<M1> & v) const
231  {
232  MotionPlain res;
233  motionAction(v, res);
234  return res;
235  }
236 
237  template<typename M2>
238  bool isApprox(
239  const MotionDense<M2> & m2,
240  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
241  {
242  return derived().isApprox_impl(m2, prec);
243  }
244 
245  template<typename D2>
247  const MotionDense<D2> & m2,
248  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
249  {
250  return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
251  }
252 
253  bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
254  {
255  return linear().isZero(prec) && angular().isZero(prec);
256  }
257 
258  template<typename S2, int O2, typename D2>
260  {
261  v.angular().noalias() = m.rotation() * angular();
262  v.linear().noalias() = m.rotation() * linear() + m.translation().cross(v.angular());
263  }
264 
265  template<typename S2, int O2>
267  {
269  se3Action_impl(m, res);
270  return res;
271  }
272 
273  template<typename S2, int O2, typename D2>
275  {
276  v.linear().noalias() =
277  m.rotation().transpose() * (linear() - m.translation().cross(angular()));
278  v.angular().noalias() = m.rotation().transpose() * angular();
279  }
280 
281  template<typename S2, int O2>
284  {
287  return res;
288  }
289 
290  void disp_impl(std::ostream & os) const
291  {
292  os << " v = " << linear().transpose() << std::endl
293  << " w = " << angular().transpose() << std::endl;
294  }
295 
298  {
299  return derived().ref();
300  }
301 
302  protected:
304 
305  MotionDense(const MotionDense &) = delete;
306 
307  }; // class MotionDense
308 
310  template<typename M1, typename M2>
312  {
313  return v1.derived().cross(v2.derived());
314  }
315 
316  template<typename M1, typename F1>
318  {
319  return v.derived().cross(f.derived());
320  }
321 
322  template<typename M1>
323  typename traits<M1>::MotionPlain
325  {
326  return v * alpha;
327  }
328 
329 } // namespace pinocchio
330 
331 #endif // ifndef __pinocchio_spatial_motion_dense_hpp__
pinocchio::MotionDense::__opposite__
MotionPlain __opposite__() const
Definition: motion-dense.hpp:165
pinocchio::SE3GroupAction< MotionDense< Derived > >::ReturnType
SE3GroupAction< Derived >::ReturnType ReturnType
Definition: motion-dense.hpp:16
pinocchio::MotionDense::operator-
MotionPlain operator-(const MotionDense< M1 > &v) const
Definition: motion-dense.hpp:142
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:25
pinocchio::MotionDense::setZero
Derived & setZero()
Definition: motion-dense.hpp:39
pinocchio::MotionDense::operator=
Derived & operator=(const Eigen::MatrixBase< V6 > &v)
Definition: motion-dense.hpp:123
pinocchio::ForceBase::linear
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:57
pinocchio::MotionDense::ref
MotionRefType ref()
Definition: motion-dense.hpp:297
pinocchio::MotionDense::MotionRefType
traits< Derived >::MotionRefType MotionRefType
Definition: motion-dense.hpp:31
pinocchio::MotionDense::se3ActionInverse_impl
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Definition: motion-dense.hpp:274
pinocchio::ForceBase
Base interface for forces representation.
Definition: context/casadi.hpp:32
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
pinocchio::SE3GroupAction::ReturnType
D ReturnType
Definition: spatial/se3.hpp:41
pinocchio::MotionDense::MOTION_TYPEDEF_TPL
MOTION_TYPEDEF_TPL(Derived)
pinocchio::MotionBase::angular
ConstAngularType angular() const
Definition: motion-base.hpp:32
pinocchio::MotionBase::derived
Derived & derived()
Definition: motion-base.hpp:18
pinocchio::MotionDense::operator-=
Derived & operator-=(const MotionDense< M1 > &v)
Definition: motion-dense.hpp:160
pinocchio::MotionDense::operator+=
Derived & operator+=(const MotionBase< M1 > &v)
Definition: motion-dense.hpp:153
pinocchio::MotionDense
Definition: context/casadi.hpp:36
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::MotionDense::isEqual_impl
bool isEqual_impl(const MotionBase< D2 > &other) const
Definition: motion-dense.hpp:90
pinocchio::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:36
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::MotionBase::isApprox
bool isApprox(const Derived &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: motion-base.hpp:172
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: spatial/motion.hpp:45
pinocchio::MotionBase::isZero
bool isZero(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: motion-base.hpp:179
pinocchio::operator*
TridiagonalSymmetricMatrixApplyOnTheLeftReturnType< LhsMatrixType, TridiagonalSymmetricMatrixTpl< S, O > > operator*(const Eigen::MatrixBase< LhsMatrixType > &lhs, const TridiagonalSymmetricMatrixTpl< S, O > &rhs)
Definition: math/tridiagonal-matrix.hpp:319
autodiff-rnea.f
f
Definition: autodiff-rnea.py:24
pinocchio::MotionDense::linear
ConstLinearType linear() const
Definition: motion-base.hpp:36
pinocchio::MotionBase
Definition: spatial/fwd.hpp:42
pinocchio::SE3GroupAction
Definition: spatial/se3.hpp:39
pinocchio::MotionDense::operator-
MotionPlain operator-() const
Definition: motion-dense.hpp:132
skew.hpp
pinocchio::MotionAlgebraAction::ReturnType
D ReturnType
Definition: spatial/motion.hpp:47
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
pinocchio::MotionDense::MotionDense
MotionDense()
Definition: motion-dense.hpp:303
pinocchio::MotionDense::se3Action_impl
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Definition: motion-dense.hpp:259
pinocchio::MotionDense::operator+=
Derived & operator+=(const MotionDense< M1 > &v)
Definition: motion-dense.hpp:148
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:22
collision-with-point-clouds.X
X
Definition: collision-with-point-clouds.py:36
traits
pinocchio::MotionDense::__plus__
MotionPlain __plus__(const MotionDense< M1 > &v) const
Definition: motion-dense.hpp:171
pinocchio::MotionDense::operator+
MotionPlain operator+(const MotionDense< M1 > &v) const
Definition: motion-dense.hpp:137
pinocchio::MotionDense::isApprox_impl
bool isApprox_impl(const MotionDense< D2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: motion-dense.hpp:246
pinocchio::MotionDense::operator=
Derived & operator=(const MotionDense< D2 > &other)
Definition: motion-dense.hpp:97
pinocchio::MotionDense::__mult__
MotionPlain __mult__(const OtherScalar &alpha) const
Definition: motion-dense.hpp:199
M
M
pinocchio::MotionDense::se3Action_impl
SE3GroupAction< Derived >::ReturnType se3Action_impl(const SE3Tpl< S2, O2 > &m) const
Definition: motion-dense.hpp:266
pinocchio::MotionAlgebraAction< MotionDense< Derived >, MotionDerived >::ReturnType
MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType
Definition: motion-dense.hpp:22
pinocchio::MotionDense::motionAction
MotionPlain motionAction(const MotionDense< M1 > &v) const
Definition: motion-dense.hpp:230
pinocchio::MotionDense::__mequ__
Derived & __mequ__(const MotionDense< M1 > &v)
Definition: motion-dense.hpp:191
pinocchio::MotionDense::toActionMatrix_impl
ActionMatrixType toActionMatrix_impl() const
Definition: motion-dense.hpp:52
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::MotionDense::dot
Scalar dot(const ForceBase< F1 > &phi) const
Definition: motion-dense.hpp:211
pinocchio::MotionDense::operator=
Derived & operator=(const MotionDense &other)
Definition: motion-dense.hpp:102
pinocchio::MotionDense::__div__
MotionPlain __div__(const OtherScalar &alpha) const
Definition: motion-dense.hpp:205
pinocchio::MotionDense::toDualActionMatrix_impl
ActionMatrixType toDualActionMatrix_impl() const
Definition: motion-dense.hpp:63
pinocchio::MotionDense::cross_impl
MotionAlgebraAction< D, Derived >::ReturnType cross_impl(const D &d) const
Definition: motion-dense.hpp:217
pinocchio::MotionDense::motionAction
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
Definition: motion-dense.hpp:223
pinocchio::MotionDense::__pequ__
Derived & __pequ__(const MotionDense< M1 > &v)
Definition: motion-dense.hpp:183
pinocchio::MotionDense::se3ActionInverse_impl
SE3GroupAction< Derived >::ReturnType se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
Definition: motion-dense.hpp:283
pinocchio::operator^
EIGEN_STRONG_INLINE MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionHelicalUnalignedTpl< S2, O2 > &m2)
Definition: joint-helical-unaligned.hpp:220
pinocchio::MotionDense::toHomogeneousMatrix_impl
HomogeneousMatrixType toHomogeneousMatrix_impl() const
Definition: motion-dense.hpp:74
pinocchio::MotionDense::isZero_impl
bool isZero_impl(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: motion-dense.hpp:253
pinocchio::MotionDense::set
Derived & set(const MotionDense< D2 > &other)
Definition: motion-dense.hpp:108
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
pinocchio::MotionDense::__minus__
MotionPlain __minus__(const MotionDense< M1 > &v) const
Definition: motion-dense.hpp:177
pinocchio::ForceBase::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:47
pinocchio::MotionDense::derived
Derived & derived()
Definition: motion-base.hpp:18
pinocchio::MotionDense::Base
MotionBase< Derived > Base
Definition: motion-dense.hpp:29
d
FCL_REAL d
pinocchio::MotionDense::operator=
Derived & operator=(const MotionBase< D2 > &other)
Definition: motion-dense.hpp:116
pinocchio::MotionDense::disp_impl
void disp_impl(std::ostream &os) const
Definition: motion-dense.hpp:290
X
pinocchio::MotionDense::isEqual_impl
bool isEqual_impl(const MotionDense< D2 > &other) const
Definition: motion-dense.hpp:84
pinocchio::MotionDense::angular
ConstAngularType angular() const
Definition: motion-base.hpp:32
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::MotionDense::isApprox
bool isApprox(const MotionDense< M2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: motion-dense.hpp:238
pinocchio::MotionDense::setRandom
Derived & setRandom()
Definition: motion-dense.hpp:45


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:11