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_HPP__
6 : : #define __RDL_FRAME_VECTOR_HPP__
7 : :
8 : : /**
9 : : * @file FrameVector.hpp
10 : : * @page frame_vector Frame Vector
11 : : *
12 : : * A RobotDynamics::Math::FrameVector is a 3d vector with an associated reference frame
13 : : */
14 : :
15 : : #include "rdl_dynamics/FrameObject.hpp"
16 : :
17 : : namespace RobotDynamics
18 : : {
19 : : namespace Math
20 : : {
21 : : /**
22 : : * @class FrameVector
23 : : * @ingroup reference_frame
24 : : * @brief A FrameVector is a 3D vector with a ReferenceFrame, and all operations between FrameVectors and other frame
25 : : **objects
26 : : * will perform runtime checks that objects are expressed in the same frames.
27 : : * This class and its implementation are an adaptation of FrameVector.java by <a href="http://robots.ihmc.us/">Jerry
28 : : **Pratt and the IHMC Robotics Group</a>.
29 : : */
30 : : class FrameVector : public FrameObject, public Math::Vector3d
31 : : {
32 : : public:
33 : : /**
34 : : * @brief Default constructor. Initializes its ReferenceFrame to nullptr
35 : : */
36 : 218 : FrameVector() : FrameObject(nullptr), Math::Vector3d(0., 0., 0.)
37 : : {
38 : 218 : }
39 : :
40 : : /**
41 : : * @brief Constructor
42 : : * @param referenceFrame A pointer to a ReferenceFrame
43 : : */
44 : : explicit FrameVector(ReferenceFramePtr referenceFrame) : FrameObject(referenceFrame), Math::Vector3d()
45 : : {
46 : : }
47 : :
48 : : /**
49 : : * @brief Constructor
50 : : * @param referenceFrame A pointer to a ReferenceFrame
51 : : * @param x Value of the x-coordinate
52 : : * @param y Value of the y-coordinate
53 : : * @param z Value of the z-coordinate
54 : : */
55 : 2711 : FrameVector(ReferenceFramePtr referenceFrame, const double& x, const double& y, const double& z) : FrameObject(referenceFrame), Math::Vector3d(x, y, z)
56 : : {
57 : 2711 : }
58 : :
59 : : /**
60 : : * @brief Constructor
61 : : * @param referenceFrame Pointer to a ReferenceFrame
62 : : * @param vector A Vector3d used to set the x,y, and z coordinates
63 : : */
64 : 4139 : FrameVector(ReferenceFramePtr referenceFrame, const Eigen::Vector3d& vector) : FrameObject(referenceFrame), Math::Vector3d(vector[0], vector[1], vector[2])
65 : : {
66 : 4139 : }
67 : :
68 : : /**
69 : : * @brief Destructor
70 : : */
71 : 7979 : virtual ~FrameVector()
72 : 7979 : {
73 : 7979 : }
74 : :
75 : : /**
76 : : * @brief Return pointer to this object as type TransformableGeometricObject. See FrameObject::changeFrame for
77 : : * an example of where this method is used.
78 : : * @return Pointer to this object as type TransformableGeometricObject
79 : : */
80 : 242 : Math::TransformableGeometricObject* getTransformableGeometricObject()
81 : : {
82 : 242 : return this;
83 : : }
84 : :
85 : : /**
86 : : * @brief copy into new frame vector and change the frame of that
87 : : * @param referenceFrame
88 : : * @return
89 : : */
90 : 2 : FrameVector changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
91 : : {
92 : 2 : FrameVector p = *this;
93 : 2 : p.changeFrame(referenceFrame);
94 : 2 : return p;
95 : 0 : }
96 : :
97 : : /**
98 : : * @brief Set x, y, and z components to 0
99 : : */
100 : 6 : inline void setToZero()
101 : : {
102 : 6 : set(0., 0., 0.);
103 : 6 : }
104 : :
105 : : /**
106 : : * @brief Set the x, y, and z components and the ReferenceFrame these components are expressed in
107 : : * @param x The x-component
108 : : * @param y y-component
109 : : * @param z z-component
110 : : * @throws ReferenceFrameException If *referenceFrame=nullptr
111 : : * @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
112 : : */
113 : 2 : inline void setIncludingFrame(const double x, const double y, const double z, ReferenceFramePtr referenceFrame)
114 : : {
115 : 2 : if (!referenceFrame)
116 : : {
117 : 1 : throw ReferenceFrameException("Reference frame cannot be nullptr!");
118 : : }
119 : :
120 : 1 : set(x, y, z);
121 : 1 : this->referenceFrame = referenceFrame;
122 : 1 : }
123 : :
124 : : /**
125 : : * @brief Set the x, y, and z components and the ReferenceFrame these components are expressed in
126 : : * @param vector Used to set the x,y, and z components of this point
127 : : * @throws ReferenceFrameException If *referenceFrame=nullptr
128 : : * @param referenceFrame Pointer to a ReferenceFrame this point is expressed in
129 : : */
130 : 199 : inline void setIncludingFrame(const Eigen::Vector3d& vector, ReferenceFramePtr referenceFrame)
131 : : {
132 : 199 : if (!referenceFrame)
133 : : {
134 : 1 : throw ReferenceFrameException("Reference frame cannot be nullptr!");
135 : : }
136 : :
137 : 198 : set(vector[0], vector[1], vector[2]);
138 : 198 : this->referenceFrame = referenceFrame;
139 : 198 : }
140 : :
141 : : /**
142 : : * @brief Dot product between two FrameVectors, i.e. \f$ v_1 \cdot v_2 \f$
143 : : * @param frameVector
144 : : * @throws ReferenceFrameException If the FrameVectors aren't expressed in the same ReferenceFrame
145 : : * @return The dot product
146 : : */
147 : 6 : inline double dot(const FrameVector& frameVector) const
148 : : {
149 : 6 : checkReferenceFramesMatch(&frameVector);
150 : :
151 : 5 : return this->x() * frameVector.x() + this->y() * frameVector.y() + this->z() * frameVector.z();
152 : : }
153 : :
154 : : /**
155 : : * @brief Cross product between two FrameVectors, i.e. \f$ v_1 \times v_2 \f$
156 : : * @param vector
157 : : * @throws ReferenceFrameException If the FrameVectors aren't expressed in the same ReferenceFrame
158 : : * @return The cross product
159 : : */
160 : 2028 : inline FrameVector cross(const FrameVector& vector) const
161 : : {
162 : 2028 : checkReferenceFramesMatch(&vector);
163 : 1028 : return FrameVector(this->referenceFrame, this->y() * vector.z() - this->z() * vector.y(), vector.x() * this->z() - vector.z() * this->x(),
164 : 2056 : this->x() * vector.y() - this->y() * vector.x());
165 : : }
166 : :
167 : : /**
168 : : * @brief Cross product, i.e. \f$ v_1 \times v_2 \f$
169 : : * @param vector
170 : : * @return The cross product
171 : : */
172 : 1151 : inline Vector3d cross(const Vector3d& vector) const
173 : : {
174 : 2302 : return RobotDynamics::Math::Vector3d::cross(vector);
175 : : }
176 : :
177 : : /**
178 : : * @brief Computer the angle between two FrameVectors, \f$ \arccos{\frac{v_1 \cdot v_2}{|v_1||v_2|}} \f$
179 : : * @param frameVector
180 : : * @return The angle between the vectors
181 : : */
182 : 2 : inline double getAngleBetweenVectors(const FrameVector& frameVector) const
183 : : {
184 : 2 : checkReferenceFramesMatch(&frameVector);
185 : :
186 : 2 : return acos(std::max(-1., std::min(this->dot(frameVector) / (this->norm() * frameVector.norm()), 1.)));
187 : : }
188 : :
189 : 7 : inline Vector3d vec() const
190 : : {
191 : 7 : return Math::Vector3d(x(), y(), z());
192 : : }
193 : :
194 : 15 : void operator+=(const Vector3d& v)
195 : : {
196 : 15 : this->x() += v.x();
197 : 15 : this->y() += v.y();
198 : 15 : this->z() += v.z();
199 : 15 : }
200 : :
201 : 1 : void operator-=(const Vector3d& v)
202 : : {
203 : 1 : this->x() -= v.x();
204 : 1 : this->y() -= v.y();
205 : 1 : this->z() -= v.z();
206 : 1 : }
207 : :
208 : : /**
209 : : * @brief Plus euals operator that performs runtime frame checks, \f$ v_1=v_1-v \f$
210 : : * @param v
211 : : */
212 : 14 : void operator+=(const FrameVector& v)
213 : : {
214 : 14 : this->checkReferenceFramesMatch(&v);
215 : 14 : this->x() += v.x();
216 : 14 : this->y() += v.y();
217 : 14 : this->z() += v.z();
218 : 14 : }
219 : :
220 : : /**
221 : : * @brief Minus euals operator that performs runtime frame checks, \f$ v_1=v_1-v \f$
222 : : * @param v
223 : : */
224 : 76 : void operator-=(const FrameVector& v)
225 : : {
226 : 76 : this->checkReferenceFramesMatch(&v);
227 : 76 : this->x() -= v.x();
228 : 76 : this->y() -= v.y();
229 : 76 : this->z() -= v.z();
230 : 76 : }
231 : :
232 : : /**
233 : : * @brief Times euals operator that performs runtime frame checks, \f$ v=v*scale \f$
234 : : * @tparam typename of the scale parameter
235 : : * @param scale
236 : : */
237 : : template <typename T>
238 : 68 : void operator*=(const T scale)
239 : : {
240 : 68 : this->x() *= scale;
241 : 68 : this->y() *= scale;
242 : 68 : this->z() *= scale;
243 : 68 : }
244 : : };
245 : :
246 : : template <typename T>
247 : 1 : inline FrameVector operator*(FrameVector v1, const T scale)
248 : : {
249 : 1 : v1 *= scale;
250 : 1 : return v1;
251 : : }
252 : :
253 : : template <typename T>
254 : 1 : inline FrameVector operator*(const T scale, FrameVector v1)
255 : : {
256 : 1 : v1 *= scale;
257 : 1 : return v1;
258 : : }
259 : :
260 : 15 : inline FrameVector operator+(FrameVector v1, const Vector3d v2)
261 : : {
262 : 15 : v1 += v2;
263 : 15 : return v1;
264 : : }
265 : :
266 : 1 : inline FrameVector operator-(FrameVector v1, const Vector3d v2)
267 : : {
268 : 1 : v1 -= v2;
269 : 1 : return v1;
270 : : }
271 : :
272 : : /**
273 : : * @brief Add two FrameVectors together
274 : : * @param v1
275 : : * @param v2
276 : : * @return THe result of \f$ v_1 + v_2 \f$
277 : : */
278 : : // cppcheck-suppress passedByValue
279 : 14 : inline FrameVector operator+(FrameVector v1, const FrameVector v2)
280 : : {
281 : 14 : v1 += v2;
282 : 14 : return v1;
283 : : }
284 : :
285 : : /**
286 : : * @brief Add two FrameVectors together
287 : : * @param v1
288 : : * @param v2
289 : : * @return THe result of \f$ v_1 - v_2 \f$
290 : : */
291 : : // cppcheck-suppress passedByValue
292 : 16 : inline FrameVector operator-(FrameVector v1, const FrameVector v2)
293 : : {
294 : 16 : v1 -= v2;
295 : 16 : return v1;
296 : : }
297 : : typedef std::vector<FrameVector, Eigen::aligned_allocator<FrameVector>> FrameVectorV;
298 : : } // namespace Math
299 : : } // namespace RobotDynamics
300 : :
301 : : #endif // ifndef __RDL_FRAME_VECTOR_HPP__
|
