pointmass3d_quadrotor_model.h
Go to the documentation of this file.
1 
7 #ifndef POINTMASS3D_QUADROTOR_H
8 #define POINTMASS3D_QUADROTOR_H
9 
10 namespace argos {
11  class CPointMass3DEngine;
12  class CPointMass3DQuadRotorModel;
13  class CQuadRotorEntity;
14 }
15 
16 #include <argos3/plugins/simulator/physics_engines/pointmass3d/pointmass3d_model.h>
17 #include <argos3/core/simulator/entity/embodied_entity.h>
18 #include <argos3/plugins/simulator/entities/quadrotor_entity.h>
19 
20 namespace argos {
21 
23 
24  public:
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 = CVector3(1000.0f, 1000.0f, 1000.0f),
42  Real f_max_torque = 1000.0f);
43 
45 
46  virtual void Reset();
47 
48  virtual void UpdateFromEntityStatus();
49  virtual void Step();
50 
51  virtual void CalculateBoundingBox();
52 
53  virtual bool CheckIntersectionWithRay(Real& f_t_on_ray,
54  const CRay3& c_ray) const;
55 
56  virtual void UpdateOriginAnchor(SAnchor& s_anchor);
57 
58  private:
59 
60  void PositionalControl();
61  void SpeedControl();
62 
63  Real PDControl(Real f_cur_error,
64  Real f_k_p,
65  Real f_k_d,
66  Real& f_old_error);
67  private:
68 
70  CQuadRotorEntity& m_cQuadRotorEntity;
71 
73  Real m_fBodyHeight;
74 
76  Real m_fArmLength;
77 
79  Real m_fBodyMass;
80 
82  Real m_fBodyInertia;
83 
85  CVector3 m_cPosKP;
86 
88  CVector3 m_cPosKD;
89 
91  Real m_fYawKP;
92 
94  Real m_fYawKD;
95 
97  CVector3 m_cVelKP;
98 
100  CVector3 m_cVelKD;
101 
103  Real m_fRotKP;
104 
106  Real m_fRotKD;
107 
109  CVector3 m_cMaxForce;
110 
112  Real m_fMaxTorque;
113 
115  CQuadRotorEntity::SPositionControlData m_sDesiredPositionData;
116 
118  CQuadRotorEntity::SSpeedControlData m_sDesiredSpeedData;
119 
121  CRadians m_cYaw;
122 
124  CRadians m_cRotSpeed;
125 
127  CRadians m_cTorque;
128 
130  CVector3 m_cLinearControl;
131 
133  Real m_fRotationalControl;
134 
136  Real m_pfLinearError[3];
137 
139  Real m_fRotError;
140  };
141 
142 }
143 
144 #endif
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
This entity is a link to a body in the physics engine.
An anchor related to the body of an entity.
Definition: physics_model.h:38
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
A 3D vector class.
Definition: vector3.h:31
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
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)
virtual bool CheckIntersectionWithRay(Real &f_t_on_ray, const CRay3 &c_ray) const
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
virtual void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.