ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
Go to the documentation of this file.
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>
16 static Real SymmetricClamp(
Real f_max,
18 if(f_value > f_max)
return f_max;
19 if(f_value < -f_max)
return -f_max;
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),
57 m_cMaxForce(c_max_force),
58 m_fMaxTorque(f_max_torque) {
73 m_pfLinearError[0] = 0.0f;
74 m_pfLinearError[1] = 0.0f;
75 m_pfLinearError[2] = 0.0f;
100 cLimits.
GetMin().GetX() + m_fArmLength),
101 cLimits.
GetMax().GetX() - m_fArmLength));
104 cLimits.
GetMin().GetY() + m_fArmLength),
105 cLimits.
GetMax().GetY() - m_fArmLength));
109 cLimits.
GetMax().GetZ() - m_fBodyHeight));
130 m_cTorque.
SetValue(m_fRotationalControl);
151 const CRay3& c_ray)
const {
156 return m_cShape.
Intersects(f_t_on_ray, c_ray);
162 void CPointMass3DQuadRotorModel::PositionalControl() {
164 m_cLinearControl.
Set(
165 SymmetricClamp(m_cMaxForce.
GetX(), PDControl(
169 m_pfLinearError[0])),
170 SymmetricClamp(m_cMaxForce.
GetY(), PDControl(
174 m_pfLinearError[1])),
175 SymmetricClamp(m_cMaxForce.
GetZ(), PDControl(
181 m_fRotationalControl =
182 SymmetricClamp(m_fMaxTorque, PDControl(
183 (m_sDesiredPositionData.
Yaw - m_cYaw).SignedNormalize().GetValue(),
192 void CPointMass3DQuadRotorModel::SpeedControl() {
194 m_cLinearControl.
Set(
195 SymmetricClamp(m_cMaxForce.
GetX(), PDControl(
199 m_pfLinearError[0])),
200 SymmetricClamp(m_cMaxForce.
GetY(), PDControl(
204 m_pfLinearError[1])),
205 SymmetricClamp(m_cMaxForce.
GetZ(), PDControl(
211 m_fRotationalControl =
212 SymmetricClamp(m_fMaxTorque, PDControl(
213 (m_sDesiredSpeedData.
RotSpeed - m_cRotSpeed).GetValue(),
222 Real CPointMass3DQuadRotorModel::PDControl(
Real f_cur_error,
227 f_k_p * f_cur_error +
229 f_old_error = f_cur_error;
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Real GetZ() const
Returns the z coordinate of this vector.
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
virtual bool CheckIntersectionWithRay(Real &f_t_on_ray, const CRay3 &c_ray) const
void SetX(const Real f_x)
Sets the x coordinate of this vector.
The namespace containing all the ARGoS related code.
T Min(const T &t_v1, const T &t_v2)
Returns the smaller of the two passed arguments.
It defines the basic type CRadians, used to store an angle value in radians.
virtual void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
static const CVector3 Z
The z axis.
This entity is a link to a body in the physics engine.
CSpace & GetSpace() const
Returns a reference to the simulated space.
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
Real GetX() const
Returns the x coordinate of this vector.
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
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)
An anchor related to the body of an entity.
bool Intersects(Real &f_t_on_ray, const CRay3 &c_ray)
void RegisterAnchorMethod(const SAnchor &s_anchor, void(MODEL::*pt_method)(SAnchor &))
Registers an anchor method.
const SPositionControlData & GetPositionControlData() const
void SetValue(Real f_value)
Sets the value in radians.
Real GetPhysicsClockTick() const
Returns the length of the physics engine tick.
CVector3 m_cAcceleration
The acceleration of this model in the engine.
CPointMass3DEngine & m_cPM3DEngine
Reference to the physics engine.
CVector3 m_cVelocity
The linear velocity of this model in the engine.
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
CVector3 m_cPosition
The position of the model in this engine.
Real GetY() const
Returns the y coordinate of this vector.
virtual void UpdateOriginAnchor(SAnchor &s_anchor)
Updates the origin anchor associated to the embodied entity.
const CRange< CVector3 > & GetArenaLimits() const
CVector3 Position
The position of the anchor wrt the global coordinate system.
EControlMethod GetControlMethod() const
float Real
Collects all ARGoS code.
T Max(const T &t_v1, const T &t_v2)
Returns the bigger of the two passed arguments.
CRadians & UnsignedNormalize()
Normalizes the value in the range [0:TWO_PI].
void SetY(const Real f_y)
Sets the y coordinate of this vector.
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.