pointmass3d_quadrotor_model.h

Go to the documentation of this file.

 
#ifndef POINTMASS3D_QUADROTOR_H
#define POINTMASS3D_QUADROTOR_H
 
namespace argos {
   class CPointMass3DEngine;
   class CPointMass3DQuadRotorModel;
   class CQuadRotorEntity;
}
 
#include <argos3/plugins/simulator/physics_engines/pointmass3d/pointmass3d_model.h>
#include <argos3/core/simulator/entity/embodied_entity.h>
#include <argos3/plugins/simulator/entities/quadrotor_entity.h>
 
namespace argos {
 
   class CPointMass3DQuadRotorModel : public CPointMass3DModel {
 
   public:
      
      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 ~CPointMass3DQuadRotorModel() {}
      
      virtual void Reset();
 
      virtual void UpdateFromEntityStatus();
      virtual void Step();
 
      virtual void CalculateBoundingBox();
 
      virtual bool CheckIntersectionWithRay(Real& f_t_on_ray,
                                            const CRay3& c_ray) const;
 
      virtual void UpdateOriginAnchor(SAnchor& s_anchor);
 
   private:
 
      void PositionalControl();
      void SpeedControl();
 
      Real PDControl(Real f_cur_error,
                     Real f_k_p,
                     Real f_k_d,
                     Real& f_old_error);
   private:
 
      CQuadRotorEntity& m_cQuadRotorEntity;
 
      Real m_fBodyHeight;
 
      Real m_fArmLength;
 
      Real m_fBodyMass;
 
      Real m_fBodyInertia;
 
      CVector3 m_cPosKP;
 
      CVector3 m_cPosKD;
      
      Real m_fYawKP;
 
      Real m_fYawKD;
 
      CVector3 m_cVelKP;
 
      CVector3 m_cVelKD;
 
      Real m_fRotKP;
 
      Real m_fRotKD;
 
      CVector3 m_cMaxForce;
 
      Real m_fMaxTorque;
 
      CQuadRotorEntity::SPositionControlData m_sDesiredPositionData;
 
      CQuadRotorEntity::SSpeedControlData m_sDesiredSpeedData;
 
      CRadians m_cYaw;
 
      CRadians m_cRotSpeed;
 
      CRadians m_cTorque;
 
      CVector3 m_cLinearControl;
 
      Real m_fRotationalControl;
 
      Real m_pfLinearError[3];
 
      Real m_fRotError;
   };
 
}
 
#endif