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_FORCE_HPP__
6 : : #define __RDL_SPATIAL_FORCE_HPP__
7 : :
8 : : /**
9 : : * @file SpatialForce.hpp
10 : : * @page spatial_force Spatial Force
11 : : *
12 : : * A RobotDynamics::Math::SpatialForce is a RobotDynamics::Math::ForceVector with an associated reference frame
13 : : */
14 : :
15 : : #include "rdl_dynamics/ForceVector.hpp"
16 : : #include "rdl_dynamics/FrameObject.hpp"
17 : : #include "rdl_dynamics/FrameVector.hpp"
18 : :
19 : : namespace RobotDynamics
20 : : {
21 : : namespace Math
22 : : {
23 : : /**
24 : : * @class SpatialForce
25 : : * @ingroup reference_frame
26 : : * @brief A SpatialForce is a spatial vector with the angular part being three moments and the linear part
27 : : * being 3 linear forces
28 : : */
29 : : class SpatialForce : public ForceVector, public FrameObject
30 : : {
31 : : public:
32 : : /**
33 : : * @brief Constructor. RobotDynamics::FrameObject::referenceFrame is initialized to nullptr
34 : : */
35 : 25434 : SpatialForce() : ForceVector(), FrameObject(nullptr)
36 : : {
37 : 25434 : }
38 : :
39 : : /**
40 : : * @brief Constructor. Force vector elements will be zero
41 : : * @param referenceFrame Frame spatial force will be expressed in
42 : : */
43 : : explicit SpatialForce(ReferenceFramePtr referenceFrame) : ForceVector(), FrameObject(referenceFrame)
44 : : {
45 : : }
46 : :
47 : : /**
48 : : * @brief Constructor
49 : : * @param referenceFrame ReferenceFrame this force vector is expressed in
50 : : * @param mx x-Momentum
51 : : * @param my y-Momentum
52 : : * @param mz z-Momentum
53 : : * @param fx x-Force
54 : : * @param fy y-Force
55 : : * @param fz z-Force
56 : : */
57 : 17982 : SpatialForce(ReferenceFramePtr referenceFrame, const double mx, const double my, const double mz, const double fx, const double fy, const double fz)
58 : 17982 : : ForceVector(mx, my, mz, fx, fy, fz), FrameObject(referenceFrame)
59 : : {
60 : 17982 : }
61 : :
62 : : /**
63 : : * @brief Constructor
64 : : * @param referenceFrame ReferenceFrame this force vector is expressed in
65 : : * @param m Vector containing the angular component
66 : : * @param f Vector containing the linear component
67 : : */
68 : : SpatialForce(ReferenceFramePtr referenceFrame, const Vector3d& m, const Vector3d f)
69 : : : ForceVector(m.x(), m.y(), m.z(), f.x(), f.y(), f.z()), FrameObject(referenceFrame)
70 : : {
71 : : }
72 : :
73 : 1189 : SpatialForce& operator=(const SpatialForce& f)
74 : : {
75 : 1189 : this->referenceFrame = f.getReferenceFrame();
76 : 1189 : SpatialVector::operator=(f);
77 : 1189 : return *this;
78 : : }
79 : :
80 : 8397 : Math::TransformableGeometricObject* getTransformableGeometricObject()
81 : : {
82 : 8397 : return this;
83 : : }
84 : :
85 : : /**
86 : : * @brief Copy and change frame
87 : : * @param referenceFrame
88 : : * @return Copied spatial transform with frame changed
89 : : */
90 : 8902 : SpatialForce changeFrameAndCopy(ReferenceFramePtr referenceFrame) const
91 : : {
92 : 8902 : SpatialForce ret = *this;
93 : 8902 : ret.changeFrame(referenceFrame);
94 : 8902 : return ret;
95 : 0 : }
96 : :
97 : : /**
98 : : * @brief Get linear part of spatial force as a frame vector
99 : : * @return FrameVector consisting of the reference frame and the linear portion
100 : : */
101 : 1 : FrameVector getFramedLinearPart() const
102 : : {
103 : 1 : return FrameVector(this->referenceFrame, this->getLinearPart());
104 : : }
105 : :
106 : : /**
107 : : * @brief Get angular part of spatial force as a frame vector
108 : : * @return FrameVector consisting of the reference frame and the angular portion
109 : : */
110 : 1 : FrameVector getFramedAngularPart() const
111 : : {
112 : 1 : return FrameVector(this->referenceFrame, this->getAngularPart());
113 : : }
114 : :
115 : : /**
116 : : * @brief Copy constructor
117 : : * @param spatialForce
118 : : */
119 : 62720 : SpatialForce(const SpatialForce& spatialForce) : ForceVector(spatialForce), FrameObject(spatialForce.referenceFrame)
120 : : {
121 : 62720 : }
122 : :
123 : : /**
124 : : * @brief Constructor
125 : : * @param referenceFrame ReferenceFrame this force vector is expressed in
126 : : * @param spatialVector
127 : : */
128 : 6865 : SpatialForce(ReferenceFramePtr referenceFrame, const SpatialVector& spatialVector) : ForceVector(spatialVector), FrameObject(referenceFrame)
129 : : {
130 : 6865 : }
131 : :
132 : : /**
133 : : * @brief Get copy of this SpatialForce as type ForceVector
134 : : * @return A SpatialForce as type ForceVector
135 : : */
136 : : inline ForceVector toForceVector() const
137 : : {
138 : : return ForceVector(this->mx(), this->my(), this->mz(), this->fx(), this->fy(), this->fz());
139 : : }
140 : :
141 : 3581 : void setIncludingFrame(ReferenceFramePtr referenceFrame, const SpatialVector& v)
142 : : {
143 : 3581 : this->set(v);
144 : 3581 : this->referenceFrame = referenceFrame;
145 : 3581 : }
146 : :
147 : 17563 : void setIncludingFrame(ReferenceFramePtr referenceFrame, double wx, double wy, double wz, double vx, double vy, double vz)
148 : : {
149 : 17563 : this->SpatialVector::set(wx, wy, wz, vx, vy, vz);
150 : 17563 : this->referenceFrame = referenceFrame;
151 : 17563 : }
152 : :
153 : 151 : void setIncludingFrame(ReferenceFramePtr referenceFrame, const Vector3d& m, const Vector3d& f)
154 : : {
155 : 151 : this->SpatialVector::set(m.x(), m.y(), m.z(), f.x(), f.y(), f.z());
156 : 151 : this->referenceFrame = referenceFrame;
157 : 151 : }
158 : :
159 : : /**
160 : : * @brief Operator for scaling a spatial force vector
161 : : * @param scale Each force element will be multiplied by this scale
162 : : * @return \f$ f_sp = f_sp * scale \f$
163 : : */
164 : 151 : inline SpatialForce operator*=(double scale)
165 : : {
166 : 151 : mx() *= scale;
167 : 151 : my() *= scale;
168 : 151 : mz() *= scale;
169 : :
170 : 151 : fx() *= scale;
171 : 151 : fy() *= scale;
172 : 151 : fz() *= scale;
173 : :
174 : 151 : return *this;
175 : : }
176 : :
177 : : /**
178 : : * @brief Overloaded += operator. Frame checks are performed
179 : : * @param f
180 : : * @return \f$ f_sp=f_sp+f \f$
181 : : */
182 : 8721 : inline SpatialForce operator+=(const SpatialForce& f)
183 : : {
184 : 8721 : this->checkReferenceFramesMatch(&f);
185 : :
186 : 8720 : this->mx() += f.mx();
187 : 8720 : this->my() += f.my();
188 : 8720 : this->mz() += f.mz();
189 : :
190 : 8720 : this->fx() += f.fx();
191 : 8720 : this->fy() += f.fy();
192 : 8720 : this->fz() += f.fz();
193 : :
194 : 8720 : return *this;
195 : : }
196 : :
197 : : /**
198 : : * @brief Overloaded -= operator. Frame checks are performed
199 : : * @param f
200 : : * @return \f$ f_sp=f_sp-f \f$
201 : : */
202 : 1116 : inline SpatialForce operator-=(const SpatialForce& f)
203 : : {
204 : 1116 : this->checkReferenceFramesMatch(&f);
205 : :
206 : 1116 : this->mx() -= f.mx();
207 : 1116 : this->my() -= f.my();
208 : 1116 : this->mz() -= f.mz();
209 : :
210 : 1116 : this->fx() -= f.fx();
211 : 1116 : this->fy() -= f.fy();
212 : 1116 : this->fz() -= f.fz();
213 : :
214 : 1116 : return *this;
215 : : }
216 : : };
217 : :
218 : : /**
219 : : * @brief Overloaded + operator to add two SpatialForce vectors and return the result in a new SpatialForce. Frame
220 : : **checks are performed
221 : : * @param f1
222 : : * @param f2
223 : : * @return \f$ f_sp=f_sp_1+f_sp_2 \f$
224 : : */
225 : 1077 : inline SpatialForce operator+(SpatialForce f1, const SpatialForce& f2)
226 : : {
227 : 1077 : return f1 += f2;
228 : : }
229 : :
230 : : /**
231 : : * @brief Overloaded - operator to add two SpatialForce vectors and return the result in a new SpatialForce. Frame
232 : : **checks are performed
233 : : * @param f1
234 : : * @param f2
235 : : * @return \f$ f_sp=f_sp_1-f_sp_2 \f$
236 : : */
237 : 474 : inline SpatialForce operator-(SpatialForce f1, const SpatialForce& f2)
238 : : {
239 : 474 : return f1 -= f2;
240 : : }
241 : :
242 : : /**
243 : : * @brief Overloaded * operator to scale a spatial force by a scalar value
244 : : * @param f1
245 : : * @param scale
246 : : * @return \f$ f_sp=f_sp_1*scalar \f$
247 : : */
248 : 151 : inline SpatialForce operator*(SpatialForce f1, double scale)
249 : : {
250 : 151 : return f1 *= scale;
251 : : }
252 : : typedef std::vector<SpatialForce, Eigen::aligned_allocator<SpatialForce>> SpatialForceV;
253 : : } // namespace Math
254 : : } // namespace RobotDynamics
255 : :
256 : : #endif // ifndef __RDL_SPATIAL_FORCE_HPP__
|
