Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // RDL - Robot Dynamics Library
3 : : // Licensed under the zlib license. See LICENSE for more details.
4 : :
5 : : #ifndef __RDL_FRAME_VECTOR_PAIR_HPP__
6 : : #define __RDL_FRAME_VECTOR_PAIR_HPP__
7 : :
8 : : /**
9 : : * @file FrameVectorPair.hpp
10 : : * @page frame_vector_pair Frame Vector Pair
11 : : *
12 : : * A RobotDynamics::Math::FrameVectorPair is a data type containing two 3d vectors with an associated
13 : : * reference frame. The prime motivation for this is for ensuring spatial transforms aren't performed on
14 : : * 3d vectors and separate the storage of vectors of, for example, point velocities from spatial velocities
15 : : */
16 : :
17 : : #include "rdl_dynamics/FrameObject.hpp"
18 : : #include "rdl_dynamics/FrameVector.hpp"
19 : : #include "rdl_dynamics/SpatialMotion.hpp"
20 : :
21 : : namespace RobotDynamics
22 : : {
23 : : namespace Math
24 : : {
25 : : /**
26 : : * @class FrameVectorPair
27 : : * @ingroup reference_frame
28 : : * @brief A FrameVector is a pair of 3D vector with a ReferenceFrame
29 : : */
30 : : class FrameVectorPair
31 : : {
32 : : public:
33 : : /**
34 : : * @brief Default constructor. Initializes its ReferenceFrame to nullptr
35 : : */
36 : 2 : FrameVectorPair() : lin(nullptr, 0., 0., 0.), ang(nullptr, 0., 0., 0.)
37 : : {
38 : 2 : }
39 : :
40 : : /*
41 : : * @brief constructor
42 : : * @param linear Linear part
43 : : * @param angular Angular part
44 : : */
45 : 1 : FrameVectorPair(const FrameVector& linear, const FrameVector& angular) : lin(linear), ang(angular)
46 : : {
47 : 1 : linear.checkReferenceFramesMatch(&angular);
48 : 1 : }
49 : :
50 : : /*
51 : : * @brief constructor
52 : : * @param referenceFrame pointer to a reference frame
53 : : */
54 : : explicit FrameVectorPair(ReferenceFramePtr referenceFrame) : lin(referenceFrame), ang(referenceFrame)
55 : : {
56 : : }
57 : :
58 : : /**
59 : : * @brief Constructor
60 : : * @param v Spatial motion
61 : : */
62 : 95 : explicit FrameVectorPair(const SpatialMotion& v)
63 : 95 : {
64 : 95 : lin.setIncludingFrame(v.getLinearPart(), v.getReferenceFrame());
65 : 95 : ang.setIncludingFrame(v.getAngularPart(), v.getReferenceFrame());
66 : 95 : }
67 : :
68 : : /**
69 : : * @brief Constructor
70 : : * @param referenceFrame A pointer to a ReferenceFrame
71 : : * @param linear linear component
72 : : * @param angular angular component
73 : : */
74 : 1 : FrameVectorPair(ReferenceFramePtr referenceFrame, const Vector3d& linear, const Vector3d& angular) : lin(referenceFrame, linear), ang(referenceFrame, angular)
75 : : {
76 : 1 : }
77 : :
78 : : /**
79 : : * @brief Constructor
80 : : * @param referenceFrame
81 : : * @param v
82 : : */
83 : 42 : FrameVectorPair(ReferenceFramePtr referenceFrame, const SpatialVector& v) : lin(referenceFrame, v.getLinearPart()), ang(referenceFrame, v.getAngularPart())
84 : : {
85 : 42 : }
86 : :
87 : : /**
88 : : * @brief Destructor
89 : : */
90 : 207 : virtual ~FrameVectorPair()
91 : 207 : {
92 : 207 : }
93 : :
94 : : /**
95 : : * @brief Change the frame of the two 3d vectors. Equivalent to the following math expression
96 : : * \f[
97 : : * \begin{bmatrix}
98 : : * E & 0 \\
99 : : * 0 & E
100 : : * \end{bmatrix}
101 : : * \begin{bmatrix}
102 : : * angular \\
103 : : * linear
104 : : * \end{bmatrix}
105 : : * \f]
106 : : *
107 : : * @param referenceFrame
108 : : */
109 : 125 : void changeFrame(ReferenceFramePtr referenceFrame)
110 : : {
111 : 125 : assert(referenceFrame);
112 : 125 : lin.changeFrame(referenceFrame);
113 : 125 : ang.changeFrame(referenceFrame);
114 : 125 : }
115 : :
116 : : /**
117 : : * @brief copy into new frame vector and change the frame of that
118 : : * @param referenceFrame
119 : : * @return Copy of frame vector pair expressed in new frame
120 : : */
121 : 18 : FrameVectorPair changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
122 : : {
123 : 18 : FrameVectorPair p = *this;
124 : 18 : p.changeFrame(referenceFrame);
125 : 18 : return p;
126 : 0 : }
127 : :
128 : : /**
129 : : * @brief Set x, y, and z components of linear and angular to 0
130 : : */
131 : : inline void setToZero()
132 : : {
133 : : lin.setZero();
134 : : ang.setZero();
135 : : }
136 : :
137 : : /**
138 : : * @brief Set the reference frame
139 : : * @param referenceFrame
140 : : */
141 : 2 : void setReferenceFrame(ReferenceFramePtr referenceFrame)
142 : : {
143 : 2 : lin.setReferenceFrame(referenceFrame);
144 : 2 : ang.setReferenceFrame(referenceFrame);
145 : 2 : }
146 : :
147 : : /**
148 : : * @brief Set the components and the ReferenceFrame these components are expressed in
149 : : * @param linear Linear component
150 : : * @param angular Angular component
151 : : * @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
152 : : */
153 : 2 : inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const Vector3d& linear, const Vector3d& angular)
154 : : {
155 : 2 : assert(referenceFrame);
156 : :
157 : 2 : lin.setIncludingFrame(linear, referenceFrame);
158 : 2 : ang.setIncludingFrame(angular, referenceFrame);
159 : 2 : }
160 : :
161 : : /**
162 : : * @brief Set the components and the ReferenceFrame these components are expressed in
163 : : * @param v spatial vector
164 : : * @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
165 : : */
166 : : inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const SpatialVector& v)
167 : : {
168 : : assert(referenceFrame);
169 : :
170 : : lin.setIncludingFrame(v.getLinearPart(), referenceFrame);
171 : : ang.setIncludingFrame(v.getAngularPart(), referenceFrame);
172 : : }
173 : :
174 : : /**
175 : : * @brief Set the components and the ReferenceFrame these components are expressed in
176 : : * @param v Motion vector
177 : : * @param referenceFrame pointer to a reference frame
178 : : */
179 : : inline void setIncludingFrame(ReferenceFramePtr referenceFrame, const MotionVector& v)
180 : : {
181 : : assert(referenceFrame);
182 : :
183 : : lin.setIncludingFrame(v.getLinearPart(), referenceFrame);
184 : : ang.setIncludingFrame(v.getAngularPart(), referenceFrame);
185 : : }
186 : :
187 : : /**
188 : : * @brief Set the components and the ReferenceFrame these components are expressed in
189 : : * @param v spatial motion
190 : : */
191 : : inline void setIncludingFrame(const SpatialMotion& v)
192 : : {
193 : : lin.setIncludingFrame(v.getLinearPart(), v.getReferenceFrame());
194 : : ang.setIncludingFrame(v.getAngularPart(), v.getReferenceFrame());
195 : : }
196 : :
197 : : /**
198 : : * @brief Set the linear vector
199 : : * @param v
200 : : */
201 : 139 : inline void setLinearPart(const Vector3d& v)
202 : : {
203 : 139 : lin.set(v);
204 : 139 : }
205 : :
206 : : /**
207 : : * @brief Set the angular vector
208 : : * @param v
209 : : */
210 : 2 : inline void setAngularPart(const Vector3d& v)
211 : : {
212 : 2 : ang.set(v);
213 : 2 : }
214 : :
215 : : /**
216 : : * @brief Get copy of linear component
217 : : * @return Copy of linear part
218 : : */
219 : 319 : inline FrameVector linear() const
220 : : {
221 : 319 : return lin;
222 : : }
223 : :
224 : : /**
225 : : * @brief Get copy of angular component
226 : : * @return Copy of angular component
227 : : */
228 : 402 : inline FrameVector angular() const
229 : : {
230 : 402 : return ang;
231 : : }
232 : :
233 : : template <typename T>
234 : 33 : void operator*=(const T scale)
235 : : {
236 : 33 : lin *= scale;
237 : 33 : ang *= scale;
238 : 33 : }
239 : :
240 : : void operator+=(const FrameVectorPair& v)
241 : : {
242 : : lin += v.linear();
243 : : ang += v.angular();
244 : : }
245 : :
246 : 30 : void operator-=(const FrameVectorPair& v)
247 : : {
248 : 30 : lin -= v.linear();
249 : 30 : ang -= v.angular();
250 : 30 : }
251 : :
252 : : /**
253 : : * @brief Get pointer to linear vector
254 : : * @return pointer
255 : : */
256 : 1 : FrameVector* linearPtr()
257 : : {
258 : 1 : return &lin;
259 : : }
260 : :
261 : : /**
262 : : * @brief Get pointer to angular vector
263 : : * @return pointer
264 : : */
265 : 1 : FrameVector* angularPtr()
266 : : {
267 : 1 : return ∠
268 : : }
269 : :
270 : : protected:
271 : : FrameVector lin; /**< Linear component */
272 : : FrameVector ang; /**< Angular component */
273 : : };
274 : :
275 : : template <typename T>
276 : : inline FrameVectorPair operator*(FrameVectorPair pair, const T scalar)
277 : : {
278 : : pair *= scalar;
279 : : return pair;
280 : : }
281 : :
282 : : template <typename T>
283 : 33 : inline FrameVectorPair operator*(const T scalar, FrameVectorPair pair)
284 : : {
285 : 33 : pair *= scalar;
286 : 33 : return pair;
287 : : }
288 : :
289 : : // cppcheck-suppress passedByValue
290 : : inline FrameVectorPair operator+(FrameVectorPair v1, const FrameVectorPair v2)
291 : : {
292 : : v1 += v2;
293 : : return v1;
294 : : }
295 : :
296 : : // cppcheck-suppress passedByValue
297 : 7 : inline FrameVectorPair operator-(FrameVectorPair v1, const FrameVectorPair v2)
298 : : {
299 : 7 : v1 -= v2;
300 : 7 : return v1;
301 : : }
302 : : } // namespace Math
303 : : } // namespace RobotDynamics
304 : :
305 : : #endif // ifndef __RDL_FRAME_VECTOR_HPP__
|
