pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
motion-base.hpp
1//
2// Copyright (c) 2015-2020 CNRS INRIA
3// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4//
5
6#ifndef __pinocchio_spatial_motion_base_hpp__
7#define __pinocchio_spatial_motion_base_hpp__
8
9namespace pinocchio
10{
11
12 template<class Derived>
13 class MotionBase : NumericalBase<Derived>
14 {
15 public:
16 MOTION_TYPEDEF_TPL(Derived);
17
18 Derived & derived()
19 {
20 return *static_cast<Derived *>(this);
21 }
22 const Derived & derived() const
23 {
24 return *static_cast<const Derived *>(this);
25 }
26
27 Derived & const_cast_derived() const
28 {
29 return *const_cast<Derived *>(&derived());
30 }
31
32 ConstAngularType angular() const
33 {
34 return derived().angular_impl();
35 }
36 ConstLinearType linear() const
37 {
38 return derived().linear_impl();
39 }
40 AngularType angular()
41 {
42 return derived().angular_impl();
43 }
44 LinearType linear()
45 {
46 return derived().linear_impl();
47 }
48
49 template<typename V3Like>
50 void angular(const Eigen::MatrixBase<V3Like> & w)
51 {
52 derived().angular_impl(w.derived());
53 }
54
55 template<typename V3Like>
56 void linear(const Eigen::MatrixBase<V3Like> & v)
57 {
58 derived().linear_impl(v.derived());
59 }
60
61 operator PlainReturnType() const
62 {
63 return derived().plain();
64 }
65 PlainReturnType plain() const
66 {
67 return derived().plain();
68 }
69
70 ToVectorConstReturnType toVector() const
71 {
72 return derived().toVector_impl();
73 }
74 ToVectorReturnType toVector()
75 {
76 return derived().toVector_impl();
77 }
78 operator Vector6() const
79 {
80 return toVector();
81 }
82
83 ActionMatrixType toActionMatrix() const
84 {
85 return derived().toActionMatrix_impl();
86 }
87 ActionMatrixType toDualActionMatrix() const
88 {
89 return derived().toDualActionMatrix_impl();
90 }
91 operator Matrix6() const
92 {
93 return toActionMatrix();
94 }
95
109 HomogeneousMatrixType toHomogeneousMatrix() const
110 {
111 return derived().toHomogeneousMatrix_impl();
112 }
113
114 void setZero()
115 {
116 derived().setZero();
117 }
118
119 template<typename M2>
120 bool operator==(const MotionBase<M2> & other) const
121 {
122 return derived().isEqual_impl(other.derived());
123 }
124
125 template<typename M2>
126 bool operator!=(const MotionBase<M2> & other) const
127 {
128 return !(derived() == other.derived());
129 }
130
131 Derived operator-() const
132 {
133 return derived().__opposite__();
134 }
135 Derived operator+(const MotionBase<Derived> & v) const
136 {
137 return derived().__plus__(v.derived());
138 }
139 Derived operator-(const MotionBase<Derived> & v) const
140 {
141 return derived().__minus__(v.derived());
142 }
143 Derived & operator+=(const MotionBase<Derived> & v)
144 {
145 return derived().__pequ__(v.derived());
146 }
147 Derived & operator-=(const MotionBase<Derived> & v)
148 {
149 return derived().__mequ__(v.derived());
150 }
151
152 template<typename OtherScalar>
153 typename internal::RHSScalarMultiplication<Derived, OtherScalar>::ReturnType
154 operator*(const OtherScalar & alpha) const
155 {
156 return derived().__mult__(alpha);
157 }
158
159 template<typename OtherScalar>
160 Derived operator/(const OtherScalar & alpha) const
161 {
162 return derived().__div__(alpha);
163 }
164
165 template<typename OtherSpatialType>
166 typename MotionAlgebraAction<OtherSpatialType, Derived>::ReturnType
167 cross(const OtherSpatialType & d) const
168 {
169 return derived().cross_impl(d);
170 }
171
172 bool isApprox(
173 const Derived & other,
174 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
175 {
176 return derived().isApprox_impl(other, prec);
177 }
178
179 bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
180 {
181 return derived().isZero_impl(prec);
182 }
183
184 template<typename S2, int O2>
185 typename SE3GroupAction<Derived>::ReturnType se3Action(const SE3Tpl<S2, O2> & m) const
186 {
187 return derived().se3Action_impl(m);
188 }
189
190 template<typename S2, int O2>
191 typename SE3GroupAction<Derived>::ReturnType se3ActionInverse(const SE3Tpl<S2, O2> & m) const
192 {
193 return derived().se3ActionInverse_impl(m);
194 }
195
196 template<typename ForceDerived>
197 Scalar dot(const ForceDense<ForceDerived> & f) const
198 {
199 return derived().dot(f.derived());
200 }
201
202 void disp(std::ostream & os) const
203 {
204 derived().disp_impl(os);
205 }
206 friend std::ostream & operator<<(std::ostream & os, const MotionBase<Derived> & v)
207 {
208 v.disp(os);
209 return os;
210 }
211
212 }; // class MotionBase
213
214 template<typename MotionDerived>
215 typename internal::RHSScalarMultiplication<
216 MotionDerived,
217 typename MotionDerived::Scalar>::ReturnType
218 operator*(const typename MotionDerived::Scalar & alpha, const MotionBase<MotionDerived> & motion)
219 {
220 return motion * alpha;
221 }
222
223} // namespace pinocchio
224
225#endif // ifndef __pinocchio_spatial_motion_base_hpp__
HomogeneousMatrixType toHomogeneousMatrix() const
The homogeneous representation of the motion vector .
Main pinocchio namespace.
Definition treeview.dox:11
MotionTpl< Scalar, Options > motion(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointMotionVisitor to get the joint internal motion as a dense motion.