force-dense.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_spatial_force_dense_hpp__
6 #define __pinocchio_spatial_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:
28  FORCE_TYPEDEF_TPL(Derived);
30 
31  using Base::angular;
32  using Base::derived;
33  using Base::isApprox;
34  using Base::isZero;
35  using Base::linear;
36  using Base::operator=;
37 
38  Derived & setZero()
39  {
40  linear().setZero();
41  angular().setZero();
42  return derived();
43  }
44  Derived & setRandom()
45  {
46  linear().setRandom();
47  angular().setRandom();
48  return derived();
49  }
50 
51  template<typename D2>
52  bool isEqual_impl(const ForceDense<D2> & other) const
53  {
54  return linear() == other.linear() && angular() == other.angular();
55  }
56 
57  template<typename D2>
58  bool isEqual_impl(const ForceBase<D2> & other) const
59  {
60  return other.derived() == derived();
61  }
62 
63  // Arithmetic operators
64  template<typename D2>
65  Derived & operator=(const ForceDense<D2> & other)
66  {
67  return derived().set(other.derived());
68  }
69 
70  Derived & operator=(const ForceDense & other)
71  {
72  return derived().set(other.derived());
73  }
74 
75  template<typename D2>
76  Derived & set(const ForceDense<D2> & other)
77  {
78  linear() = other.linear();
79  angular() = other.angular();
80  return derived();
81  }
82 
83  template<typename V6>
84  Derived & operator=(const Eigen::MatrixBase<V6> & v)
85  {
86  EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
87  assert(v.size() == 6);
88  linear() = v.template segment<3>(LINEAR);
89  angular() = v.template segment<3>(ANGULAR);
90  return derived();
91  }
92 
93  ForcePlain operator-() const
94  {
95  return derived().__opposite__();
96  }
97  template<typename F1>
98  ForcePlain operator+(const ForceDense<F1> & f) const
99  {
100  return derived().__plus__(f.derived());
101  }
102  template<typename F1>
103  ForcePlain operator-(const ForceDense<F1> & f) const
104  {
105  return derived().__minus__(f.derived());
106  }
107 
108  template<typename F1>
109  Derived & operator+=(const ForceDense<F1> & f)
110  {
111  return derived().__pequ__(f.derived());
112  }
113  template<typename F1>
114  Derived & operator+=(const ForceBase<F1> & f)
115  {
116  f.derived().addTo(derived());
117  return derived();
118  }
119 
120  template<typename M1>
121  Derived & operator-=(const ForceDense<M1> & v)
122  {
123  return derived().__mequ__(v.derived());
124  }
125 
126  ForcePlain __opposite__() const
127  {
128  return ForcePlain(-linear(), -angular());
129  }
130 
131  template<typename M1>
132  ForcePlain __plus__(const ForceDense<M1> & v) const
133  {
134  return ForcePlain(linear() + v.linear(), angular() + v.angular());
135  }
136 
137  template<typename M1>
138  ForcePlain __minus__(const ForceDense<M1> & v) const
139  {
140  return ForcePlain(linear() - v.linear(), angular() - v.angular());
141  }
142 
143  template<typename M1>
144  Derived & __pequ__(const ForceDense<M1> & v)
145  {
146  linear() += v.linear();
147  angular() += v.angular();
148  return derived();
149  }
150 
151  template<typename M1>
152  Derived & __mequ__(const ForceDense<M1> & v)
153  {
154  linear() -= v.linear();
155  angular() -= v.angular();
156  return derived();
157  }
158 
159  template<typename OtherScalar>
160  ForcePlain __mult__(const OtherScalar & alpha) const
161  {
162  return ForcePlain(alpha * linear(), alpha * angular());
163  }
164 
165  template<typename OtherScalar>
166  ForcePlain __div__(const OtherScalar & alpha) const
167  {
168  return derived().__mult__((OtherScalar)(1) / alpha);
169  }
170 
171  template<typename F1>
172  Scalar dot(const MotionDense<F1> & phi) const
173  {
174  return phi.linear().dot(linear()) + phi.angular().dot(angular());
175  }
176 
177  template<typename M1, typename M2>
178  void motionAction(const MotionDense<M1> & v, ForceDense<M2> & fout) const
179  {
180  fout.linear().noalias() = v.angular().cross(linear());
181  fout.angular().noalias() = v.angular().cross(angular()) + v.linear().cross(linear());
182  }
183 
184  template<typename M1>
185  ForcePlain motionAction(const MotionDense<M1> & v) const
186  {
187  ForcePlain res;
188  motionAction(v, res);
189  return res;
190  }
191 
192  template<typename M2>
193  bool isApprox(
194  const ForceDense<M2> & f,
195  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
196  {
197  return derived().isApprox_impl(f, prec);
198  }
199 
200  template<typename D2>
202  const ForceDense<D2> & f,
203  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
204  {
205  return linear().isApprox(f.linear(), prec) && angular().isApprox(f.angular(), prec);
206  }
207 
208  bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
209  {
210  return linear().isZero(prec) && angular().isZero(prec);
211  }
212 
213  template<typename S2, int O2, typename D2>
215  {
216  f.linear().noalias() = m.rotation() * linear();
217  f.angular().noalias() = m.rotation() * angular();
218  f.angular() += m.translation().cross(f.linear());
219  }
220 
221  template<typename S2, int O2>
222  ForcePlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
223  {
224  ForcePlain res;
225  se3Action_impl(m, res);
226  return res;
227  }
228 
229  template<typename S2, int O2, typename D2>
231  {
232  f.linear().noalias() = m.rotation().transpose() * linear();
233  f.angular().noalias() =
234  m.rotation().transpose() * (angular() - m.translation().cross(linear()));
235  }
236 
237  template<typename S2, int O2>
238  ForcePlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
239  {
240  ForcePlain res;
242  return res;
243  }
244 
245  void disp_impl(std::ostream & os) const
246  {
247  os << " f = " << linear().transpose() << std::endl
248  << "tau = " << angular().transpose() << std::endl;
249  }
250 
253  {
254  return derived().ref();
255  }
256 
257  protected:
259 
260  ForceDense(const ForceDense &) = delete;
261 
262  }; // class ForceDense
263 
265  template<typename F1>
266  typename traits<F1>::ForcePlain
268  {
269  return f.derived() * alpha;
270  }
271 
272 } // namespace pinocchio
273 
274 #endif // ifndef __pinocchio_spatial_force_dense_hpp__
pinocchio::ForceDense::isEqual_impl
bool isEqual_impl(const ForceBase< D2 > &other) const
Definition: force-dense.hpp:58
pinocchio::ForceDense::isZero_impl
bool isZero_impl(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: force-dense.hpp:208
pinocchio::ForceDense::linear
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:57
pinocchio::ForceDense::operator-
ForcePlain operator-() const
Definition: force-dense.hpp:93
pinocchio::ForceDense::__opposite__
ForcePlain __opposite__() const
Definition: force-dense.hpp:126
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:25
pinocchio::ForceBase::linear
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:57
pinocchio::ForceDense::FORCE_TYPEDEF_TPL
FORCE_TYPEDEF_TPL(Derived)
pinocchio::ForceDense::se3Action_impl
ForcePlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
Definition: force-dense.hpp:222
pinocchio::ForceBase::derived
Derived & derived()
Definition: force-base.hpp:28
pinocchio::ForceBase
Base interface for forces representation.
Definition: context/casadi.hpp:32
pinocchio::ForceDense::isApprox_impl
bool isApprox_impl(const ForceDense< D2 > &f, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: force-dense.hpp:201
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
pinocchio::SE3GroupAction::ReturnType
D ReturnType
Definition: spatial/se3.hpp:41
pinocchio::ForceDense::__minus__
ForcePlain __minus__(const ForceDense< M1 > &v) const
Definition: force-dense.hpp:138
pinocchio::ForceDense::isApprox
bool isApprox(const ForceDense< M2 > &f, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: force-dense.hpp:193
pinocchio::ForceDense::operator+=
Derived & operator+=(const ForceBase< F1 > &f)
Definition: force-dense.hpp:114
pinocchio::MotionDense
Definition: context/casadi.hpp:36
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::ForceDense::se3ActionInverse_impl
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, ForceDense< D2 > &f) const
Definition: force-dense.hpp:230
pinocchio::SE3GroupAction< ForceDense< Derived > >::ReturnType
SE3GroupAction< Derived >::ReturnType ReturnType
Definition: force-dense.hpp:14
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: spatial/motion.hpp:45
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::ForceDense::ref
ForceRefType ref()
Definition: force-dense.hpp:252
pinocchio::MotionDense::linear
ConstLinearType linear() const
Definition: motion-base.hpp:36
pinocchio::SE3GroupAction
Definition: spatial/se3.hpp:39
pinocchio::MotionAlgebraAction::ReturnType
D ReturnType
Definition: spatial/motion.hpp:47
pinocchio::ForceDense::ForceRefType
traits< Derived >::ForceRefType ForceRefType
Definition: force-dense.hpp:29
pinocchio::ForceDense::Base
ForceBase< Derived > Base
Definition: force-dense.hpp:27
pinocchio::ForceDense::operator+
ForcePlain operator+(const ForceDense< F1 > &f) const
Definition: force-dense.hpp:98
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
pinocchio::ForceDense::__plus__
ForcePlain __plus__(const ForceDense< M1 > &v) const
Definition: force-dense.hpp:132
pinocchio::ForceDense::derived
Derived & derived()
Definition: force-base.hpp:28
pinocchio::ForceDense::motionAction
ForcePlain motionAction(const MotionDense< M1 > &v) const
Definition: force-dense.hpp:185
pinocchio::ForceDense::__pequ__
Derived & __pequ__(const ForceDense< M1 > &v)
Definition: force-dense.hpp:144
pinocchio::ForceDense::operator=
Derived & operator=(const Eigen::MatrixBase< V6 > &v)
Definition: force-dense.hpp:84
traits
pinocchio::ForceDense::operator-
ForcePlain operator-(const ForceDense< F1 > &f) const
Definition: force-dense.hpp:103
pinocchio::ForceDense::se3Action_impl
void se3Action_impl(const SE3Tpl< S2, O2 > &m, ForceDense< D2 > &f) const
Definition: force-dense.hpp:214
pinocchio::ForceDense::ForceDense
ForceDense()
Definition: force-dense.hpp:258
pinocchio::ForceDense::disp_impl
void disp_impl(std::ostream &os) const
Definition: force-dense.hpp:245
pinocchio::ForceDense::operator-=
Derived & operator-=(const ForceDense< M1 > &v)
Definition: force-dense.hpp:121
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::ForceBase::isZero
bool isZero(const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: force-base.hpp:158
pinocchio::ForceDense::__mequ__
Derived & __mequ__(const ForceDense< M1 > &v)
Definition: force-dense.hpp:152
pinocchio::ForceDense::setZero
Derived & setZero()
Definition: force-dense.hpp:38
pinocchio::ForceDense::__mult__
ForcePlain __mult__(const OtherScalar &alpha) const
Definition: force-dense.hpp:160
pinocchio::ForceDense::dot
Scalar dot(const MotionDense< F1 > &phi) const
Definition: force-dense.hpp:172
pinocchio::ForceDense::operator+=
Derived & operator+=(const ForceDense< F1 > &f)
Definition: force-dense.hpp:109
pinocchio::ForceDense
Definition: context/casadi.hpp:34
pinocchio::ForceDense::setRandom
Derived & setRandom()
Definition: force-dense.hpp:44
pinocchio::ForceDense::__div__
ForcePlain __div__(const OtherScalar &alpha) const
Definition: force-dense.hpp:166
pinocchio::MotionAlgebraAction< ForceDense< Derived >, MotionDerived >::ReturnType
MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType
Definition: force-dense.hpp:20
pinocchio::ForceDense::se3ActionInverse_impl
ForcePlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
Definition: force-dense.hpp:238
pinocchio::ForceDense::set
Derived & set(const ForceDense< D2 > &other)
Definition: force-dense.hpp:76
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:71
pinocchio::ForceBase::isApprox
bool isApprox(const Derived &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: force-base.hpp:148
pinocchio::ForceBase::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:47
pinocchio::ForceDense::motionAction
void motionAction(const MotionDense< M1 > &v, ForceDense< M2 > &fout) const
Definition: force-dense.hpp:178
pinocchio::ForceDense::operator=
Derived & operator=(const ForceDense< D2 > &other)
Definition: force-dense.hpp:65
pinocchio::ForceDense::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:47
pinocchio::ForceDense::operator=
Derived & operator=(const ForceDense &other)
Definition: force-dense.hpp:70
pinocchio::MotionDense::angular
ConstAngularType angular() const
Definition: motion-base.hpp:32
pinocchio::ForceDense::isEqual_impl
bool isEqual_impl(const ForceDense< D2 > &other) const
Definition: force-dense.hpp:52
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


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