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_MOMENTUM_HPP__
6 : : #define __RDL_MOMENTUM_HPP__
7 : :
8 : : /**
9 : : * @file Momentum.hpp
10 : : */
11 : :
12 : : #include "rdl_dynamics/ForceVector.hpp"
13 : : #include "rdl_dynamics/MotionVector.hpp"
14 : : #include "rdl_dynamics/RigidBodyInertia.hpp"
15 : :
16 : : namespace RobotDynamics
17 : : {
18 : : namespace Math
19 : : {
20 : : /**
21 : : * @class Momentum
22 : : * @brief Momentum is mass/inertia multiplied by velocity
23 : : */
24 : : class Momentum : public ForceVector
25 : : {
26 : : public:
27 : 6038 : Momentum() : ForceVector()
28 : : {
29 : 6038 : }
30 : :
31 : 14895 : Momentum(const double kx, const double ky, const double kz, const double lx, const double ly, const double lz) : ForceVector(kx, ky, kz, lx, ly, lz)
32 : : {
33 : 14895 : }
34 : :
35 : : Momentum(const Vector3d& k, const Vector3d l) : ForceVector(k.x(), k.y(), k.z(), l.x(), l.y(), l.z())
36 : : {
37 : : }
38 : :
39 : 20136 : Momentum(const Momentum& momentum) : ForceVector(momentum)
40 : : {
41 : 20136 : }
42 : :
43 : 95 : explicit Momentum(const ForceVector& forceVector) : ForceVector(forceVector)
44 : : {
45 : 95 : }
46 : :
47 : 1184 : Momentum& operator=(const Momentum& m)
48 : : {
49 : 1184 : SpatialVector::operator=(m);
50 : 1184 : return *this;
51 : : }
52 : :
53 : 5656 : Momentum(const RigidBodyInertia& inertia, const MotionVector& vector) : Momentum(computeMomentum(inertia, vector))
54 : : {
55 : 5656 : }
56 : :
57 : : /**
58 : : * @brief Copy then transform a ForceVector by \f[
59 : : * f_2 =
60 : : * \begin{bmatrix}
61 : : * X.E & -X.E(X.r\times) \\
62 : : * \mathbf{0} & X.E
63 : : * \end{bmatrix}
64 : : * f_1
65 : : * \f]
66 : : * @param X
67 : : * @return Copied, transformed ForceVector
68 : : */
69 : 4170 : Momentum transform_copy(const SpatialTransform& X) const
70 : : {
71 : 4170 : Momentum m = *this;
72 : 4170 : m.transform(X);
73 : 4170 : return m;
74 : : }
75 : :
76 : : EIGEN_STRONG_INLINE double& kx()
77 : : {
78 : : return this->operator[](0);
79 : : }
80 : :
81 : : EIGEN_STRONG_INLINE double& ky()
82 : : {
83 : : return this->operator[](1);
84 : : }
85 : :
86 : : EIGEN_STRONG_INLINE double& kz()
87 : : {
88 : : return this->operator[](2);
89 : : }
90 : :
91 : : EIGEN_STRONG_INLINE double kx() const
92 : : {
93 : : return this->operator[](0);
94 : : }
95 : :
96 : : EIGEN_STRONG_INLINE double ky() const
97 : : {
98 : : return this->operator[](1);
99 : : }
100 : :
101 : : EIGEN_STRONG_INLINE double kz() const
102 : : {
103 : : return this->operator[](2);
104 : : }
105 : :
106 : : EIGEN_STRONG_INLINE double& lx()
107 : : {
108 : : return this->operator[](3);
109 : : }
110 : :
111 : : EIGEN_STRONG_INLINE double& ly()
112 : : {
113 : : return this->operator[](4);
114 : : }
115 : :
116 : : EIGEN_STRONG_INLINE double& lz()
117 : : {
118 : : return this->operator[](5);
119 : : }
120 : :
121 : : EIGEN_STRONG_INLINE double lx() const
122 : : {
123 : : return this->operator[](3);
124 : : }
125 : :
126 : : EIGEN_STRONG_INLINE double ly() const
127 : : {
128 : : return this->operator[](4);
129 : : }
130 : :
131 : : EIGEN_STRONG_INLINE double lz() const
132 : : {
133 : : return this->operator[](5);
134 : : }
135 : :
136 : : /**
137 : : * @brief Overloaded plus-equals operator
138 : : * @return \f$ f_2=f_2+f_1 \f$
139 : : */
140 : 592 : inline Momentum operator+=(const Momentum& v)
141 : : {
142 : 592 : (*this) << (this->mx() += v.mx()), (this->my() += v.my()), (this->mz() += v.mz()), (this->fx() += v.fx()), (this->fy() += v.fy()), (this->fz() += v.fz());
143 : 592 : return *this;
144 : : }
145 : :
146 : : /**
147 : : * @brief Overloaded plus-equals operator
148 : : * @return \f$ f_2=f_2-f_1 \f$
149 : : */
150 : : inline Momentum operator-=(const Momentum& v)
151 : : {
152 : : (*this) << (this->mx() -= v.mx()), (this->my() -= v.my()), (this->mz() -= v.mz()), (this->fx() -= v.fx()), (this->fy() -= v.fy()), (this->fz() -= v.fz());
153 : : return *this;
154 : : }
155 : :
156 : : /**
157 : : * @brief Operator for computing kinetic energy. With momentum, \f$ m \f$ and Math::MotionVector, \f$ v \f$ this
158 : : **performs performs \f$ \frac{m\cdot v}{2} \f$
159 : : * @param vector
160 : : * @return kinetic energy
161 : : */
162 : 4 : EIGEN_STRONG_INLINE double operator*(const MotionVector& vector)
163 : : {
164 : 4 : return this->dot(vector) * 0.5;
165 : : }
166 : :
167 : : /**
168 : : * @brief Computes momentum from inertia and velocity
169 : : * @param I
170 : : * @param v
171 : : */
172 : 14893 : static inline Momentum computeMomentum(const RigidBodyInertia& I, const MotionVector& v)
173 : : {
174 : 14893 : return Momentum(I.Ixx * v[0] + I.Iyx * v[1] + I.Izx * v[2] + I.h[1] * v[5] - I.h[2] * v[4],
175 : 14893 : I.Iyx * v[0] + I.Iyy * v[1] + I.Izy * v[2] - I.h[0] * v[5] + I.h[2] * v[3],
176 : 29786 : I.Izx * v[0] + I.Izy * v[1] + I.Izz * v[2] + I.h[0] * v[4] - I.h[1] * v[3], -I.h[1] * v[2] + I.h[2] * v[1] + I.m * v[3],
177 : 74465 : I.h[0] * v[2] - I.h[2] * v[0] + I.m * v[4], -I.h[0] * v[1] + I.h[1] * v[0] + I.m * v[5]);
178 : : }
179 : : };
180 : :
181 : : /**
182 : : * @brief Multiplying a Math::RigidBodyInertia by a Math::MotionVector returns a Momentum
183 : : * @param I
184 : : * @param v
185 : : * @return I*V
186 : : */
187 : 9237 : static inline Momentum operator*(const RigidBodyInertia& I, const MotionVector& v)
188 : : {
189 : 9237 : return Momentum::computeMomentum(I, v);
190 : : }
191 : :
192 : : /**
193 : : * @brief Add two momentums
194 : : * @param m1
195 : : * @param m2
196 : : * @return \f$ m = m1+m2 /f$
197 : : */
198 : 592 : static inline Momentum operator+(Momentum m1, const Momentum& m2)
199 : : {
200 : 592 : m1 += m2;
201 : 592 : return m1;
202 : : }
203 : :
204 : : /**
205 : : * @brief subtract momentums two momentums
206 : : * @param m1
207 : : * @param m2
208 : : * @return \f$ m = m1-m2 /f$
209 : : */
210 : : static inline Momentum operator-(Momentum m1, const Momentum& m2)
211 : : {
212 : : m1 -= m2;
213 : : return m1;
214 : : }
215 : :
216 : : } // namespace Math
217 : : } // namespace RobotDynamics
218 : :
219 : : #endif // ifndef __RDL_MOMENTUM_HPP__
|
