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_SPATIAL_MOTION_HPP__
6 : : #define __RDL_SPATIAL_MOTION_HPP__
7 : :
8 : : /**
9 : : * @file SpatialMotion.hpp
10 : : * @page spatial_motion Spatial Motion
11 : : *
12 : : * A RobotDynamics::Math::SpatialMotion is a 6D RobotDynamics::Math::MotionVector with an associated reference frame
13 : : */
14 : :
15 : : #include "rdl_dynamics/FrameObject.hpp"
16 : : #include "rdl_dynamics/FrameVector.hpp"
17 : : #include "rdl_dynamics/MotionVector.hpp"
18 : :
19 : : namespace RobotDynamics
20 : : {
21 : : namespace Math
22 : : {
23 : : /**
24 : : * @class SpatialMotion
25 : : * @ingroup reference_frame
26 : : * @brief A SpatialMotion vector is a MotionVector with a RobotDynamics::ReferenceFrame it is expressed in. This allows
27 : : * for runtime checks that frame rules are obeyed and makes it easy to change the frame the metion vector is
28 : : * expressed in. As with a SpatialAcceleration, a SpatialMotion vector is the spatial velocity of a
29 : : **SpatialMotion::bodyFrame
30 : : * relative to a SpatialMotion::baseFrame and is expressed in RobotDynamics::FrameObject::referenceFrame
31 : : */
32 : : class SpatialMotion : public MotionVector, public FrameObject
33 : : {
34 : : public:
35 : : /**
36 : : * @brief Constructor. SpatialMotion::bodyFrame, SpatialMotion::baseFrame, and
37 : : **RobotDynamics::FrameObject::referenceFrame initialized to nullptr
38 : : */
39 : 32 : SpatialMotion() : MotionVector(), FrameObject(nullptr)
40 : : {
41 : 32 : bodyFrame = nullptr;
42 : 32 : baseFrame = nullptr;
43 : 32 : }
44 : :
45 : : /**
46 : : * @brief Constructor
47 : : * @param bodyFrame RobotDynamics::ReferenceFrame in motion
48 : : * @param baseFrame RobotDynamics::ReferenceFrame the motion is relative to
49 : : * @param expressedInFrame RobotDynamics::ReferenceFrame the motion vector is expressed in
50 : : * @param wx x-Angular part
51 : : * @param wy y-Angular part
52 : : * @param wz z-Angular part
53 : : * @param vx x-Linear part
54 : : * @param vy y-Linear part
55 : : * @param vz z-Linear part
56 : : */
57 : 21 : SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const double wx, const double wy, const double wz,
58 : : const double vx, const double vy, const double vz)
59 : 21 : : MotionVector(wx, wy, wz, vx, vy, vz), FrameObject(expressedInFrame)
60 : : {
61 : 21 : this->bodyFrame = bodyFrame;
62 : 21 : this->baseFrame = baseFrame;
63 : 21 : }
64 : :
65 : : /**
66 : : * @brief Constructor
67 : : * @param bodyFrame RobotDynamics::ReferenceFrame in motion
68 : : * @param baseFrame RobotDynamics::ReferenceFrame the motion is relative to
69 : : * @param expressedInFrame RobotDynamics::ReferenceFrame the motion vector is expressed in
70 : : * @param w Angular part
71 : : * @param v Linear part
72 : : */
73 : : SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const Vector3d& w, const Vector3d v)
74 : : : MotionVector(w.x(), w.y(), w.z(), v.x(), v.y(), v.z()), FrameObject(expressedInFrame)
75 : : {
76 : : this->bodyFrame = bodyFrame;
77 : : this->baseFrame = baseFrame;
78 : : }
79 : :
80 : : /**
81 : : * @brief Constructor
82 : : * @param bodyFrame RobotDynamics::ReferenceFrame in motion
83 : : * @param baseFrame RobotDynamics::ReferenceFrame the motion is relative to
84 : : * @param expressedInFrame RobotDynamics::ReferenceFrame the motion vector is expressed in
85 : : * @param v
86 : : */
87 : 32208 : SpatialMotion(ReferenceFramePtr bodyFrame, ReferenceFramePtr baseFrame, ReferenceFramePtr expressedInFrame, const SpatialVector& v)
88 : 32208 : : MotionVector(v), FrameObject(expressedInFrame)
89 : : {
90 : 32208 : this->bodyFrame = bodyFrame;
91 : 32208 : this->baseFrame = baseFrame;
92 : 32208 : }
93 : :
94 : : /**
95 : : * @brief Copy constructor
96 : : * @param spatialMotion
97 : : */
98 : 44088 : SpatialMotion(const SpatialMotion& spatialMotion) : MotionVector(spatialMotion), FrameObject(spatialMotion.referenceFrame)
99 : : {
100 : 44086 : this->bodyFrame = spatialMotion.bodyFrame;
101 : 44087 : this->baseFrame = spatialMotion.baseFrame;
102 : 44088 : }
103 : :
104 : : /**
105 : : * @brief Get linear part of spatial motion as a frame vector
106 : : * @return FrameVector consisting of the reference frame and the linear portion
107 : : */
108 : 1 : FrameVector getFramedLinearPart() const
109 : : {
110 : 1 : return FrameVector(this->referenceFrame, this->getLinearPart());
111 : : }
112 : :
113 : : /**
114 : : * @brief Get angular part of spatial motion as a frame vector
115 : : * @return FrameVector consisting of the reference frame and the angular portion
116 : : */
117 : 1 : FrameVector getFramedAngularPart() const
118 : : {
119 : 1 : return FrameVector(this->referenceFrame, this->getAngularPart());
120 : : }
121 : :
122 : 250 : Math::TransformableGeometricObject* getTransformableGeometricObject()
123 : : {
124 : 250 : return this;
125 : : }
126 : :
127 : : /**
128 : : * @brief Copy and change frame
129 : : * @param referenceFrame
130 : : * @return Copied spatial transform with frame changed
131 : : */
132 : 2 : SpatialMotion changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
133 : : {
134 : 2 : SpatialMotion ret = *this;
135 : 2 : ret.changeFrame(referenceFrame);
136 : 2 : return ret;
137 : 0 : }
138 : :
139 : : /**
140 : : * @brief Get a SpatialMotions SpatialMotion::bodyFrame
141 : : * @return The body frame, i.e. the principal moving frame
142 : : */
143 : 14512 : inline ReferenceFramePtr getBodyFrame() const
144 : : {
145 : 14512 : return bodyFrame;
146 : : }
147 : :
148 : : /**
149 : : * @brief Get a SpatialMotions SpatialMotion::baseFrame
150 : : * @return The base frame, i.e. the frame the SpatialMotion is relative to
151 : : */
152 : 14658 : inline ReferenceFramePtr getBaseFrame() const
153 : : {
154 : 14658 : return baseFrame;
155 : : }
156 : :
157 : : /**
158 : : * @brief Get a copy of this SpatialMotion as type MotionVector
159 : : * @return MotionVector that is a copy of a SpatialMotion
160 : : */
161 : 1 : inline MotionVector toMotionVector() const
162 : : {
163 : 1 : return *this;
164 : : }
165 : :
166 : 1 : void setIncludingFrame(ReferenceFramePtr referenceFrame, const SpatialVector& v)
167 : : {
168 : 1 : this->set(v);
169 : 1 : this->referenceFrame = referenceFrame;
170 : 1 : }
171 : :
172 : 1 : void setIncludingFrame(ReferenceFramePtr referenceFrame, double wx, double wy, double wz, double vx, double vy, double vz)
173 : : {
174 : 1 : this->SpatialVector::set(wx, wy, wz, vx, vy, vz);
175 : 1 : this->referenceFrame = referenceFrame;
176 : 1 : }
177 : :
178 : : void setIncludingFrame(ReferenceFramePtr referenceFrame, Vector3d angularPart, Vector3d linearPart)
179 : : {
180 : : this->SpatialVector::set(angularPart, linearPart);
181 : : this->referenceFrame = referenceFrame;
182 : : }
183 : :
184 : 226 : SpatialMotion& operator=(const SpatialMotion& v)
185 : : {
186 : 226 : SpatialVector::operator=(v);
187 : 226 : this->referenceFrame = v.getReferenceFrame();
188 : 226 : this->bodyFrame = v.getBodyFrame();
189 : 226 : this->baseFrame = v.getBaseFrame();
190 : 226 : return *this;
191 : : }
192 : :
193 : : /**
194 : : * @brief Overloaded += operator. Frame checks are performed.
195 : : * @param v
196 : : */
197 : : void operator+=(const SpatialMotion& v);
198 : :
199 : : /**
200 : : * @brief Overloaded -= operator. Frame checks are performed
201 : : * @param v
202 : : */
203 : : void operator-=(const SpatialMotion& v);
204 : :
205 : : /**
206 : : * @brief This is an operator for performing what is referred to in Featherstone as the spatial vector
207 : : **cross(\f$\times\f$) operator times a MotionVector. In
208 : : * VDuindam the spatial vector cross operator is referred to as the adjoint of a twist, denoted \f$ad_T\f$. This
209 : : **method performs,
210 : : * \f[
211 : : * m =(v\times) m = ad_{v}m =
212 : : * \begin{bmatrix}
213 : : * \omega\times & \mathbf{0} \\
214 : : * v\times & \omega\times
215 : : * \end{bmatrix} m
216 : : * \f]
217 : : * The 3d vector \f$\times\f$ operator is equivalent to the \f$\sim\f$ operator. See Math::toTildeForm.
218 : : * @param v
219 : : */
220 : : void operator%=(const SpatialMotion& v);
221 : :
222 : : /**
223 : : * @brief Sets the body frame of a spatial motion
224 : : *
225 : : * @param referenceFrame
226 : : */
227 : 41 : void setBodyFrame(ReferenceFramePtr referenceFrame)
228 : : {
229 : 41 : this->bodyFrame = referenceFrame;
230 : 41 : }
231 : :
232 : : /**
233 : : * @brief Sets the base frame of a spatial motion
234 : : * @param referenceFrame
235 : : */
236 : 1 : void setBaseFrame(ReferenceFramePtr referenceFrame)
237 : : {
238 : 1 : this->baseFrame = referenceFrame;
239 : 1 : }
240 : :
241 : : protected:
242 : : ReferenceFramePtr bodyFrame;
243 : : ReferenceFramePtr baseFrame;
244 : : };
245 : :
246 : 3 : EIGEN_STRONG_INLINE SpatialMotion operator+(SpatialMotion v1, const SpatialMotion& v2)
247 : : {
248 : 3 : v1 += v2;
249 : 2 : return v1;
250 : : }
251 : :
252 : 121 : EIGEN_STRONG_INLINE SpatialMotion operator-(SpatialMotion v1, const SpatialMotion& v2)
253 : : {
254 : 121 : v1 -= v2;
255 : 121 : return v1;
256 : : }
257 : :
258 : 7584 : EIGEN_STRONG_INLINE SpatialMotion operator%(SpatialMotion v1, const SpatialMotion& v2)
259 : : {
260 : 7584 : v1.SpatialMotion::operator%=(v2);
261 : 7584 : return v1;
262 : : }
263 : : typedef std::vector<SpatialMotion, Eigen::aligned_allocator<SpatialMotion>> SpatialMotionV;
264 : : } // namespace Math
265 : : } // namespace RobotDynamics
266 : :
267 : : #endif //__RDL_SPATIAL_MOTION_HPP__
|
