ARGoS  3
A parallel, multi-engine simulator for swarm robotics
dynamics2d_epuck_model.cpp
Go to the documentation of this file.
1 
8 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_gripping.h>
9 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
10 
11 namespace argos {
12 
13  /****************************************/
14  /****************************************/
15 
16  static const Real EPUCK_MASS = 0.4f;
17 
18  static const Real EPUCK_RADIUS = 0.035f;
19  static const Real EPUCK_INTERWHEEL_DISTANCE = 0.053f;
20  static const Real EPUCK_HEIGHT = 0.086f;
21 
22  static const Real EPUCK_MAX_FORCE = 1.5f;
23  static const Real EPUCK_MAX_TORQUE = 1.5f;
24 
25  enum EPUCK_WHEELS {
28  };
29 
30  /****************************************/
31  /****************************************/
32 
34  CEPuckEntity& c_entity) :
35  CDynamics2DSingleBodyObjectModel(c_engine, c_entity),
36  m_cEPuckEntity(c_entity),
37  m_cWheeledEntity(m_cEPuckEntity.GetWheeledEntity()),
38  m_cDiffSteering(c_engine,
39  EPUCK_MAX_FORCE,
40  EPUCK_MAX_TORQUE,
41  EPUCK_INTERWHEEL_DISTANCE),
42  m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()) {
43  /* Create the body with initial position and orientation */
44  cpBody* ptBody =
45  cpSpaceAddBody(GetDynamics2DEngine().GetPhysicsSpace(),
46  cpBodyNew(EPUCK_MASS,
47  cpMomentForCircle(EPUCK_MASS,
48  0.0f,
49  EPUCK_RADIUS + EPUCK_RADIUS,
50  cpvzero)));
51  const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
52  ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
53  CRadians cXAngle, cYAngle, cZAngle;
54  GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
55  cpBodySetAngle(ptBody, cZAngle.GetValue());
56  /* Create the body shape */
57  cpShape* ptShape =
58  cpSpaceAddShape(GetDynamics2DEngine().GetPhysicsSpace(),
59  cpCircleShapeNew(ptBody,
60  EPUCK_RADIUS,
61  cpvzero));
62  ptShape->e = 0.0; // No elasticity
63  ptShape->u = 0.7; // Lots of friction
64  /* Constrain the actual base body to follow the diff steering control */
65  m_cDiffSteering.AttachTo(ptBody);
66  /* Set the body so that the default methods work as expected */
67  SetBody(ptBody, EPUCK_HEIGHT);
68  }
69 
70  /****************************************/
71  /****************************************/
72 
74  m_cDiffSteering.Detach();
75  }
76 
77  /****************************************/
78  /****************************************/
79 
82  m_cDiffSteering.Reset();
83  }
84 
85  /****************************************/
86  /****************************************/
87 
89  /* Do we want to move? */
90  if((m_fCurrentWheelVelocity[EPUCK_LEFT_WHEEL] != 0.0f) ||
91  (m_fCurrentWheelVelocity[EPUCK_RIGHT_WHEEL] != 0.0f)) {
92  m_cDiffSteering.SetWheelVelocity(m_fCurrentWheelVelocity[EPUCK_LEFT_WHEEL],
93  m_fCurrentWheelVelocity[EPUCK_RIGHT_WHEEL]);
94  }
95  else {
96  /* No, we don't want to move - zero all speeds */
97  m_cDiffSteering.Reset();
98  }
99  }
100 
101  /****************************************/
102  /****************************************/
103 
105 
106  /****************************************/
107  /****************************************/
108 
109 }
argos::EPUCK_RIGHT_WHEEL
@ EPUCK_RIGHT_WHEEL
Definition: dynamics2d_epuck_model.cpp:27
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
dynamics2d_epuck_model.h
argos::CDynamics2DEPuckModel
Definition: dynamics2d_epuck_model.h:23
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::CDynamics2DModel::GetDynamics2DEngine
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
Definition: dynamics2d_model.h:48
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
argos::CDynamics2DSingleBodyObjectModel
Base class for object models with a single body.
Definition: dynamics2d_single_body_object_model.h:47
argos::CDynamics2DEPuckModel::UpdateFromEntityStatus
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
Definition: dynamics2d_epuck_model.cpp:88
argos::CDynamics2DEngine
Definition: dynamics2d_engine.h:42
argos::REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY
REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CEPuckEntity, CDynamics2DEPuckModel)
argos::EPUCK_LEFT_WHEEL
@ EPUCK_LEFT_WHEEL
Definition: dynamics2d_epuck_model.cpp:26
argos::CEPuckEntity
Definition: epuck_entity.h:26
argos::CVector3::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
argos::CRadians::GetValue
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
argos::CQuaternion::ToEulerAngles
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:171
argos::CDynamics2DVelocityControl::Reset
void Reset()
Definition: dynamics2d_velocity_control.cpp:84
argos::CDynamics2DVelocityControl::Detach
void Detach()
Definition: dynamics2d_velocity_control.cpp:68
argos::CDynamics2DSingleBodyObjectModel::SetBody
virtual void SetBody(cpBody *pt_body, Real f_height)
Sets the body and registers the default origin anchor method.
Definition: dynamics2d_single_body_object_model.cpp:132
argos::CDynamics2DEPuckModel::~CDynamics2DEPuckModel
virtual ~CDynamics2DEPuckModel()
Definition: dynamics2d_epuck_model.cpp:73
argos::CDynamics2DEPuckModel::CDynamics2DEPuckModel
CDynamics2DEPuckModel(CDynamics2DEngine &c_engine, CEPuckEntity &c_entity)
Definition: dynamics2d_epuck_model.cpp:33
argos::CDynamics2DSingleBodyObjectModel::Reset
virtual void Reset()
Definition: dynamics2d_single_body_object_model.cpp:63
argos::CVector3::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos::CDynamics2DDifferentialSteeringControl::SetWheelVelocity
void SetWheelVelocity(Real f_left_wheel, Real f_right_wheel)
Definition: dynamics2d_differentialsteering_control.cpp:24
argos::CDynamics2DEPuckModel::Reset
virtual void Reset()
Definition: dynamics2d_epuck_model.cpp:80
argos::EPUCK_WHEELS
EPUCK_WHEELS
Definition: dynamics2d_epuck_model.cpp:25
argos::CPhysicsModel::GetEmbodiedEntity
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
Definition: physics_model.h:133
argos::CDynamics2DVelocityControl::AttachTo
void AttachTo(cpBody *pt_body)
Definition: dynamics2d_velocity_control.cpp:38