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_MOMENTUM_HP__
6 : : #define __RDL_SPATIAL_MOMENTUM_HP__
7 : :
8 : : /**
9 : : * @file SpatialMomentum.hpp
10 : : * @page spatial_momentum Spatial Momentum
11 : : *
12 : : * A RobotDynamics::Math::SpatialMomentum represents a 6D momentum vector with an associated reference frame
13 : : */
14 : :
15 : : #include "rdl_dynamics/SpatialRigidBodyInertia.hpp"
16 : : #include "rdl_dynamics/Momentum.hpp"
17 : : #include "rdl_dynamics/SpatialMotion.hpp"
18 : :
19 : : namespace RobotDynamics
20 : : {
21 : : namespace Math
22 : : {
23 : : /**
24 : : * @class SpatialMomentum
25 : : * @ingroup reference_frame
26 : : * @brief A SpatialMomentum is a Momentum expressed in a RobotDynamics::ReferenceFrame. The angular portion of the
27 : : * vector is referred to as \f$k\f$ and the linear portion as \f$l\f$
28 : : */
29 : : class SpatialMomentum : public Momentum, public FrameObject
30 : : {
31 : : public:
32 : : /**
33 : : * @brief Constructor. RobotDynamics::ReferenceFrame is initialized to nullptr
34 : : */
35 : 2 : SpatialMomentum() : Momentum(), FrameObject(nullptr)
36 : : {
37 : 2 : }
38 : :
39 : : /**
40 : : * @brief Costructor
41 : : * @param referenceFrame RobotDynamics::ReferenceFrame the momentum is expressed in
42 : : * @param kx x-Angular
43 : : * @param ky y-Angular
44 : : * @param kz z-Angular
45 : : * @param lx x-Linear
46 : : * @param ly y-Linear
47 : : * @param lz z-Linear
48 : : */
49 : 1 : SpatialMomentum(ReferenceFramePtr referenceFrame, const double kx, const double ky, const double kz, const double lx, const double ly, const double lz)
50 : 1 : : Momentum(kx, ky, kz, lx, ly, lz), FrameObject(referenceFrame)
51 : : {
52 : 1 : }
53 : :
54 : : /**
55 : : * @brief Constructor
56 : : * @param referenceFrame RobotDynamics::ReferenceFrame the momentum is expressed in
57 : : * @param k Angular part
58 : : * @param l Linear part
59 : : */
60 : : SpatialMomentum(ReferenceFramePtr referenceFrame, const Vector3d& k, const Vector3d l)
61 : : : Momentum(k.x(), k.y(), k.z(), l.x(), l.y(), l.z()), FrameObject(referenceFrame)
62 : : {
63 : : }
64 : :
65 : : /**
66 : : * @brief Copy constructor
67 : : * @param SpatialMomentum
68 : : */
69 : : SpatialMomentum(const SpatialMomentum& SpatialMomentum) : Momentum(SpatialMomentum), FrameObject(SpatialMomentum.referenceFrame)
70 : : {
71 : : }
72 : :
73 : : /**
74 : : * @brief Constructor
75 : : * @param referenceFrame RobotDynamics::ReferenceFrame the momentum is expressed in
76 : : * @param forceVector
77 : : */
78 : 2 : SpatialMomentum(ReferenceFramePtr referenceFrame, const ForceVector& forceVector) : Momentum(forceVector), FrameObject(referenceFrame)
79 : : {
80 : 2 : }
81 : :
82 : : /**
83 : : * @brief Constructor
84 : : * @param referenceFrame RobotDynamics::ReferenceFrame the momentum is expressed in
85 : : */
86 : : explicit SpatialMomentum(ReferenceFramePtr referenceFrame) : Momentum(), FrameObject(referenceFrame)
87 : : {
88 : : }
89 : :
90 : : /**
91 : : * @brief Constructor. Momentum is computed from SpatialInertia and SpatialMotion by \f$ p=I*m \f$. Frame checks
92 : : * are performed
93 : : * @param inertia
94 : : * @param vector
95 : : */
96 : 3168 : SpatialMomentum(const SpatialInertia& inertia, const SpatialMotion& vector) : Momentum(inertia, vector), FrameObject(inertia.getReferenceFrame())
97 : : {
98 : 3168 : inertia.getReferenceFrame()->checkReferenceFramesMatch(vector.getReferenceFrame());
99 : 3168 : }
100 : :
101 : : /**
102 : : * @brief Construct from a reference frame and the product of inertia and motion vector
103 : : */
104 : : SpatialMomentum(ReferenceFramePtr referenceFrame, const RigidBodyInertia& inertia, const MotionVector& vector)
105 : : : Momentum(inertia, vector), FrameObject(referenceFrame)
106 : : {
107 : : }
108 : :
109 : : SpatialMomentum& operator=(const SpatialMomentum& m)
110 : : {
111 : : this->referenceFrame = m.getReferenceFrame();
112 : : SpatialVector::operator=(m);
113 : : return *this;
114 : : }
115 : :
116 : : /**
117 : : * @brief Set a momentum by multiplying the supplied motion vector by the supplied inertia. Set the stored
118 : : * RobotDynamics::FrameObject::referenceFrame to the supplied ReferenceFrame pointer. No frame checks will be
119 : : **performed
120 : : * @param referenceFrame
121 : : * @param inertia
122 : : * @param vector
123 : : */
124 : : void setIncludingFrame(ReferenceFramePtr referenceFrame, const RigidBodyInertia& inertia, const MotionVector& vector)
125 : : {
126 : : this->referenceFrame = referenceFrame;
127 : : SpatialVector::operator=(computeMomentum(inertia, vector));
128 : : }
129 : :
130 : : /**
131 : : * @brief Constructor
132 : : * @param referenceFrame
133 : : * @param f
134 : : */
135 : 2 : void setIncludingFrame(ReferenceFramePtr referenceFrame, const ForceVector& f)
136 : : {
137 : 2 : this->referenceFrame = referenceFrame;
138 : 2 : ForceVector::set(f);
139 : 2 : }
140 : :
141 : : /**
142 : : * @brief Operator that returns the kinetic energy. Given a momentum, \f$m\f$ and motion vector \f$v\f$,
143 : : * the resulting kinetic energy is computed by \f$ E_k = 0.5 m \cdot v \f$. Frame checks will be performed.
144 : : * @return double The kinetic energy
145 : : */
146 : 4 : inline double operator*(const SpatialMotion& vector)
147 : : {
148 : 4 : this->checkReferenceFramesMatch(&vector);
149 : 3 : return Momentum::operator*(vector);
150 : : }
151 : :
152 : : protected:
153 : 1 : TransformableGeometricObject* getTransformableGeometricObject()
154 : : {
155 : 1 : return this;
156 : : }
157 : : };
158 : :
159 : : /**
160 : : * Overloaded * operator for computing a SpatialMomentum from a SpatialInertia and SpatialMotion. Frame checks will
161 : : * be performed.
162 : : * @param inertia
163 : : * @param vector
164 : : * @return The resulting SpatialMomentum
165 : : */
166 : 3164 : inline SpatialMomentum operator*(const SpatialInertia& inertia, const SpatialMotion& vector)
167 : : {
168 : 3164 : return SpatialMomentum(inertia, vector);
169 : : }
170 : : typedef std::vector<SpatialMomentum, Eigen::aligned_allocator<SpatialMomentum>> SpatialMomentumV;
171 : : } // namespace Math
172 : : } // namespace RobotDynamics
173 : :
174 : : #endif //__RDL_SPATIAL_MOMENTUM_HP__
|
