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