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  c_entity.GetConfigurationNode()),
43  m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()) {
44  /* Create the body with initial position and orientation */
45  cpBody* ptBody =
46  cpSpaceAddBody(GetDynamics2DEngine().GetPhysicsSpace(),
47  cpBodyNew(EPUCK_MASS,
48  cpMomentForCircle(EPUCK_MASS,
49  0.0f,
50  EPUCK_RADIUS + EPUCK_RADIUS,
51  cpvzero)));
52  const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
53  ptBody->p = cpv(cPosition.GetX(), cPosition.GetY());
54  CRadians cXAngle, cYAngle, cZAngle;
55  GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
56  cpBodySetAngle(ptBody, cZAngle.GetValue());
57  /* Create the body shape */
58  cpShape* ptShape =
59  cpSpaceAddShape(GetDynamics2DEngine().GetPhysicsSpace(),
60  cpCircleShapeNew(ptBody,
61  EPUCK_RADIUS,
62  cpvzero));
63  ptShape->e = 0.0; // No elasticity
64  ptShape->u = 0.7; // Lots of friction
65  /* Constrain the actual base body to follow the diff steering control */
66  m_cDiffSteering.AttachTo(ptBody);
67  /* Set the body so that the default methods work as expected */
68  SetBody(ptBody, EPUCK_HEIGHT);
69  }
70 
71  /****************************************/
72  /****************************************/
73 
75  m_cDiffSteering.Detach();
76  }
77 
78  /****************************************/
79  /****************************************/
80 
83  m_cDiffSteering.Reset();
84  }
85 
86  /****************************************/
87  /****************************************/
88 
90  /* Do we want to move? */
91  if((m_fCurrentWheelVelocity[EPUCK_LEFT_WHEEL] != 0.0f) ||
92  (m_fCurrentWheelVelocity[EPUCK_RIGHT_WHEEL] != 0.0f)) {
93  m_cDiffSteering.SetWheelVelocity(m_fCurrentWheelVelocity[EPUCK_LEFT_WHEEL],
94  m_fCurrentWheelVelocity[EPUCK_RIGHT_WHEEL]);
95  }
96  else {
97  /* No, we don't want to move - zero all speeds */
98  m_cDiffSteering.Reset();
99  }
100  }
101 
102  /****************************************/
103  /****************************************/
104 
106 
107  /****************************************/
108  /****************************************/
109 
110 }
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CEPuckEntity, CDynamics2DEPuckModel)
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.
Definition: physics_model.h:53
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
A 3D vector class.
Definition: vector3.h:31
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:105
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:121
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
CDynamics2DEPuckModel(CDynamics2DEngine &c_engine, CEPuckEntity &c_entity)
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
Base class for object models with a single body.
virtual void SetBody(cpBody *pt_body, Real f_height)
Sets the body and registers the default origin anchor method.