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 }
A 3D vector class.
Definition: vector3.h:29
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
virtual void SetBody(cpBody *pt_body, Real f_height)
Sets the body and registers the default origin anchor method.
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
Base class for object models with a single body.
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
CDynamics2DEPuckModel(CDynamics2DEngine &c_engine, CEPuckEntity &c_entity)
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.