pointmass3d_quadrotor_model.cpp
Go to the documentation of this file.
1 
7 #include "pointmass3d_quadrotor_model.h"
8 #include <argos3/core/utility/logging/argos_log.h>
9 #include <argos3/core/utility/math/cylinder.h>
10 #include <argos3/core/simulator/simulator.h>
11 #include <argos3/core/simulator/space/space.h>
12 #include <argos3/plugins/simulator/physics_engines/pointmass3d/pointmass3d_engine.h>
13 
14 namespace argos {
15 
16  /****************************************/
17  /****************************************/
18 
19  static Real SymmetricClamp(Real f_max,
20  Real f_value) {
21  if(f_value > f_max) return f_max;
22  if(f_value < -f_max) return -f_max;
23  return f_value;
24  }
25 
26  /****************************************/
27  /****************************************/
28 
29  CPointMass3DQuadRotorModel::CPointMass3DQuadRotorModel(CPointMass3DEngine& c_engine,
30  CEmbodiedEntity& c_body,
31  CQuadRotorEntity& c_quadrotor,
32  Real f_body_height,
33  Real f_arm_length,
34  Real f_body_mass,
35  Real f_body_inertia,
36  const CVector3& c_pos_kp,
37  const CVector3& c_pos_kd,
38  Real f_yaw_kp,
39  Real f_yaw_kd,
40  const CVector3& c_vel_kp,
41  const CVector3& c_vel_kd,
42  Real f_rot_kp,
43  Real f_rot_kd,
44  const CVector3& c_max_force,
45  Real f_max_torque) :
46  CPointMass3DModel(c_engine, c_body),
47  m_cQuadRotorEntity(c_quadrotor),
48  m_fBodyHeight(f_body_height),
49  m_fArmLength(f_arm_length),
50  m_fBodyMass(f_body_mass),
51  m_fBodyInertia(f_body_inertia),
52  m_cPosKP(c_pos_kp),
53  m_cPosKD(c_pos_kd),
54  m_fYawKP(f_yaw_kp),
55  m_fYawKD(f_yaw_kd),
56  m_cVelKP(c_vel_kp),
57  m_cVelKD(c_vel_kd),
58  m_fRotKP(f_rot_kp),
59  m_fRotKD(f_rot_kd),
60  m_cMaxForce(c_max_force),
61  m_fMaxTorque(f_max_torque) {
62  Reset();
63  /* Register the origin anchor update method */
64  RegisterAnchorMethod(GetEmbodiedEntity().GetOriginAnchor(),
65  &CPointMass3DModel::UpdateOriginAnchor);
66  /* Get initial rotation */
67  CRadians cTmp1, cTmp2;
68  GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(m_cYaw, cTmp1, cTmp2);
69  }
70 
71  /****************************************/
72  /****************************************/
73 
74  void CPointMass3DQuadRotorModel::Reset() {
75  CPointMass3DModel::Reset();
76  m_pfLinearError[0] = 0.0f;
77  m_pfLinearError[1] = 0.0f;
78  m_pfLinearError[2] = 0.0f;
79  m_fRotError = 0.0f;
80  }
81 
82  /****************************************/
83  /****************************************/
84 
85  void CPointMass3DQuadRotorModel::UpdateFromEntityStatus() {
86  m_sDesiredPositionData = m_cQuadRotorEntity.GetPositionControlData();
87  }
88 
89  /****************************************/
90  /****************************************/
91 
92  void CPointMass3DQuadRotorModel::Step() {
93  /*
94  * Update positional information
95  */
96  /* Integration step */
97  m_cPosition += m_cVelocity * m_cPM3DEngine.GetPhysicsClockTick();
98  m_cYaw += m_cRotSpeed * m_cPM3DEngine.GetPhysicsClockTick();
99  /* Limit the quad-rotor position within the arena limits */
100  const CRange<CVector3>& cLimits = CSimulator::GetInstance().GetSpace().GetArenaLimits();
101  m_cPosition.SetX(
102  Min(Max(m_cPosition.GetX(),
103  cLimits.GetMin().GetX() + m_fArmLength),
104  cLimits.GetMax().GetX() - m_fArmLength));
105  m_cPosition.SetY(
106  Min(Max(m_cPosition.GetY(),
107  cLimits.GetMin().GetY() + m_fArmLength),
108  cLimits.GetMax().GetY() - m_fArmLength));
109  m_cPosition.SetZ(
110  Min(Max(m_cPosition.GetZ(),
111  cLimits.GetMin().GetZ()),
112  cLimits.GetMax().GetZ() - m_fBodyHeight));
113  /* Normalize the yaw */
114  m_cYaw.UnsignedNormalize();
115  /*
116  * Update velocity information
117  */
118  m_cVelocity += (m_cPM3DEngine.GetPhysicsClockTick() / m_fBodyMass) * m_cAcceleration;
119  m_cRotSpeed += (m_cPM3DEngine.GetPhysicsClockTick() / m_fBodyInertia) * m_cTorque;
120  /*
121  * Update control information
122  */
123  if(m_cQuadRotorEntity.GetControlMethod() == CQuadRotorEntity::POSITION_CONTROL)
124  PositionalControl();
125  else
126  SpeedControl();
127  /*
128  * Update force/torque information
129  */
130  m_cAcceleration.SetX(m_cLinearControl.GetX());
131  m_cAcceleration.SetY(m_cLinearControl.GetY());
132  m_cAcceleration.SetZ(m_cLinearControl.GetZ() + m_fBodyMass * m_cPM3DEngine.GetGravity());
133  m_cTorque.SetValue(m_fRotationalControl);
134  }
135 
136  /****************************************/
137  /****************************************/
138 
139  void CPointMass3DQuadRotorModel::CalculateBoundingBox() {
140  GetBoundingBox().MinCorner.Set(
141  GetEmbodiedEntity().GetOriginAnchor().Position.GetX() - m_fArmLength,
142  GetEmbodiedEntity().GetOriginAnchor().Position.GetY() - m_fArmLength,
143  GetEmbodiedEntity().GetOriginAnchor().Position.GetZ());
144  GetBoundingBox().MaxCorner.Set(
145  GetEmbodiedEntity().GetOriginAnchor().Position.GetX() + m_fArmLength,
146  GetEmbodiedEntity().GetOriginAnchor().Position.GetY() + m_fArmLength,
147  GetEmbodiedEntity().GetOriginAnchor().Position.GetZ() + m_fBodyHeight);
148  }
149 
150  /****************************************/
151  /****************************************/
152 
153  bool CPointMass3DQuadRotorModel::CheckIntersectionWithRay(Real& f_t_on_ray,
154  const CRay3& c_ray) const {
155  CCylinder m_cShape(m_fArmLength,
156  m_fBodyHeight,
157  m_cPosition,
158  CVector3::Z);
159  return m_cShape.Intersects(f_t_on_ray, c_ray);
160  }
161 
162  /****************************************/
163  /****************************************/
164 
165  void CPointMass3DQuadRotorModel::PositionalControl() {
166  /* Linear control */
167  m_cLinearControl.Set(
168  SymmetricClamp(m_cMaxForce.GetX(), PDControl(
169  m_sDesiredPositionData.Position.GetX() - m_cPosition.GetX(),
170  m_cPosKP.GetX(),
171  m_cPosKD.GetX(),
172  m_pfLinearError[0])),
173  SymmetricClamp(m_cMaxForce.GetY(), PDControl(
174  m_sDesiredPositionData.Position.GetY() - m_cPosition.GetY(),
175  m_cPosKP.GetY(),
176  m_cPosKD.GetY(),
177  m_pfLinearError[1])),
178  SymmetricClamp(m_cMaxForce.GetZ(), PDControl(
179  m_sDesiredPositionData.Position.GetZ() - m_cPosition.GetZ(),
180  m_cPosKP.GetZ(),
181  m_cPosKD.GetZ(),
182  m_pfLinearError[2]) - m_fBodyMass * m_cPM3DEngine.GetGravity()));
183  /* Rotational control */
184  m_fRotationalControl =
185  SymmetricClamp(m_fMaxTorque, PDControl(
186  (m_sDesiredPositionData.Yaw - m_cYaw).SignedNormalize().GetValue(),
187  m_fYawKP,
188  m_fYawKD,
189  m_fRotError));
190  }
191 
192  /****************************************/
193  /****************************************/
194 
195  void CPointMass3DQuadRotorModel::SpeedControl() {
196  /* Linear control */
197  m_cLinearControl.Set(
198  SymmetricClamp(m_cMaxForce.GetX(), PDControl(
199  m_sDesiredSpeedData.Velocity.GetX() - m_cVelocity.GetX(),
200  m_cVelKP.GetX(),
201  m_cVelKD.GetX(),
202  m_pfLinearError[0])),
203  SymmetricClamp(m_cMaxForce.GetY(), PDControl(
204  m_sDesiredSpeedData.Velocity.GetY() - m_cVelocity.GetY(),
205  m_cVelKP.GetY(),
206  m_cVelKD.GetY(),
207  m_pfLinearError[1])),
208  SymmetricClamp(m_cMaxForce.GetZ(), PDControl(
209  m_sDesiredSpeedData.Velocity.GetZ() - m_cVelocity.GetZ(),
210  m_cVelKP.GetZ(),
211  m_cVelKD.GetZ(),
212  m_pfLinearError[2]) - m_fBodyMass * m_cPM3DEngine.GetGravity()));
213  /* Rotational control */
214  m_fRotationalControl =
215  SymmetricClamp(m_fMaxTorque, PDControl(
216  (m_sDesiredSpeedData.RotSpeed - m_cRotSpeed).GetValue(),
217  m_fRotKP,
218  m_fRotKD,
219  m_fRotError));
220  }
221 
222  /****************************************/
223  /****************************************/
224 
225  Real CPointMass3DQuadRotorModel::PDControl(Real f_cur_error,
226  Real f_k_p,
227  Real f_k_d,
228  Real& f_old_error) {
229  Real fOutput =
230  f_k_p * f_cur_error + /* proportional term */
231  f_k_d * (f_cur_error - f_old_error) / m_cPM3DEngine.GetPhysicsClockTick(); /* derivative term */
232  f_old_error = f_cur_error;
233  return fOutput;
234  }
235 
236  /****************************************/
237  /****************************************/
238 
239  void CPointMass3DQuadRotorModel::UpdateOriginAnchor(SAnchor& s_anchor) {
240  s_anchor.Position = m_cPosition;
241  s_anchor.Orientation = CQuaternion(m_cYaw, CVector3::Z);
242  }
243 
244  /****************************************/
245  /****************************************/
246 
247 }
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::Max
T Max(const T &t_v1, const T &t_v2)
Returns the bigger of the two passed arguments.
Definition: general.h:95
argos::Min
T Min(const T &t_v1, const T &t_v2)
Returns the smaller of the two passed arguments.
Definition: general.h:77
argos::CEmbodiedEntity
This entity is a link to a body in the physics engine.
Definition: embodied_entity.h:48
argos::CEmbodiedEntity::GetOriginAnchor
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
Definition: embodied_entity.h:119
argos::CPhysicsEngine::GetPhysicsClockTick
Real GetPhysicsClockTick() const
Returns the length of the physics engine tick.
Definition: physics_engine.h:280
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::SAnchor::Orientation
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
argos::CPhysicsModel::RegisterAnchorMethod
void RegisterAnchorMethod(const SAnchor &s_anchor, void(MODEL::*pt_method)(SAnchor &))
Registers an anchor method.
Definition: physics_model.h:341
argos::CPhysicsModel::GetEmbodiedEntity
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
Definition: physics_model.h:133
argos::CPhysicsModel::GetBoundingBox
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
Definition: physics_model.h:214
argos::CSimulator::GetSpace
CSpace & GetSpace() const
Returns a reference to the simulated space.
Definition: simulator.h:104
argos::CSimulator::GetInstance
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
argos::CSpace::GetArenaLimits
const CRange< CVector3 > & GetArenaLimits() const
Returns the arena limits.
Definition: space.h:409
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
argos::CRadians::UnsignedNormalize
CRadians & UnsignedNormalize()
Normalizes the value in the range [0:TWO_PI].
Definition: angles.h:148
argos::CRadians::SetValue
void SetValue(Real f_value)
Sets the value in radians.
Definition: angles.h:127
argos::CCylinder::Intersects
bool Intersects(Real &f_t_on_ray, const CRay3 &c_ray)
Definition: cylinder.cpp:9
argos::CQuaternion::ToEulerAngles
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
argos::CRange::GetMax
T GetMax() const
Definition: range.h:48
argos::CRange::GetMin
T GetMin() const
Definition: range.h:32
argos::CVector3
A 3D vector class.
Definition: vector3.h:31
argos::CVector3::SetY
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:129
argos::CVector3::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:105
argos::CVector3::SetX
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:113
argos::CVector3::Set
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:155
argos::CVector3::SetZ
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:145
argos::CVector3::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:121
argos::CVector3::Z
static const CVector3 Z
The z axis.
Definition: vector3.h:42
argos::CVector3::GetZ
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:137
argos::CQuadRotorEntity::GetControlMethod
EControlMethod GetControlMethod() const
Definition: quadrotor_entity.h:58
argos::CQuadRotorEntity::GetPositionControlData
const SPositionControlData & GetPositionControlData() const
Definition: quadrotor_entity.h:66
argos::CPointMass3DModel::m_cVelocity
CVector3 m_cVelocity
The linear velocity of this model in the engine.
Definition: pointmass3d_model.h:77
argos::CPointMass3DModel::UpdateOriginAnchor
virtual void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
Definition: pointmass3d_model.cpp:56
argos::CPointMass3DModel::m_cPM3DEngine
CPointMass3DEngine & m_cPM3DEngine
Reference to the physics engine.
Definition: pointmass3d_model.h:71
argos::CPointMass3DModel::m_cAcceleration
CVector3 m_cAcceleration
The acceleration of this model in the engine.
Definition: pointmass3d_model.h:80
argos::CPointMass3DModel::m_cPosition
CVector3 m_cPosition
The position of the model in this engine.
Definition: pointmass3d_model.h:74
argos::CPointMass3DQuadRotorModel::CalculateBoundingBox
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
Definition: pointmass3d_quadrotor_model.cpp:139
argos::CPointMass3DQuadRotorModel::CPointMass3DQuadRotorModel
CPointMass3DQuadRotorModel(CPointMass3DEngine &c_engine, CEmbodiedEntity &c_body, CQuadRotorEntity &c_quadrotor, Real f_body_height, Real f_arm_length, Real f_body_mass, Real f_body_inertia, const CVector3 &c_pos_kp, const CVector3 &c_pos_kd, Real f_yaw_kp, Real f_yaw_kd, const CVector3 &c_vel_kp, const CVector3 &c_vel_kd, Real f_rot_kp, Real f_rot_kd, const CVector3 &c_max_force=CVector3(1000.0f, 1000.0f, 1000.0f), Real f_max_torque=1000.0f)
Definition: pointmass3d_quadrotor_model.cpp:29
argos::CPointMass3DQuadRotorModel::CheckIntersectionWithRay
virtual bool CheckIntersectionWithRay(Real &f_t_on_ray, const CRay3 &c_ray) const
Definition: pointmass3d_quadrotor_model.cpp:153
argos::CPointMass3DQuadRotorModel::UpdateFromEntityStatus
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
Definition: pointmass3d_quadrotor_model.cpp:85
argos::CPointMass3DQuadRotorModel::UpdateOriginAnchor
virtual void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
Definition: pointmass3d_quadrotor_model.cpp:239