dynamics2d_footbot_model.cpp
Go to the documentation of this file.
1 
7 #include "dynamics2d_footbot_model.h"
8 #include "footbot_turret_entity.h"
9 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_gripping.h>
10 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
11 
12 namespace argos {
13 
14  /****************************************/
15  /****************************************/
16 
17  /* P and D constants of the PD controller used for the turret position control. */
18  static const Real PD_P_CONSTANT = 0.4;
19  static const Real PD_D_CONSTANT = 0.2;
20 
21  static const Real FOOTBOT_RADIUS = 0.085036758f;
22  static const Real FOOTBOT_INTERWHEEL_DISTANCE = 0.14f;
23  static const Real FOOTBOT_HEIGHT = 0.146899733f;
24 
25  static const Real FOOTBOT_MAX_FORCE = 15.f;
26  static const Real FOOTBOT_MAX_TORQUE = 150.f;
27 
28  enum FOOTBOT_WHEELS {
29  FOOTBOT_LEFT_WHEEL = 0,
30  FOOTBOT_RIGHT_WHEEL = 1
31  };
32 
33  enum ETurretModes {
34  MODE_OFF,
35  MODE_PASSIVE,
36  MODE_SPEED_CONTROL,
37  MODE_POSITION_CONTROL,
38  };
39 
40  /****************************************/
41  /****************************************/
42 
43  CDynamics2DFootBotModel::CDynamics2DFootBotModel(CDynamics2DEngine& c_engine,
44  CFootBotEntity& c_entity) :
45  CDynamics2DMultiBodyObjectModel(c_engine, c_entity),
46  m_cFootBotEntity(c_entity),
47  m_cWheeledEntity(m_cFootBotEntity.GetWheeledEntity()),
48  m_cGripperEntity(c_entity.GetGripperEquippedEntity()),
49  m_cDiffSteering(c_engine,
50  FOOTBOT_MAX_FORCE,
51  FOOTBOT_MAX_TORQUE,
52  FOOTBOT_INTERWHEEL_DISTANCE,
53  c_entity.GetConfigurationNode()),
54  m_pcGripper(nullptr),
55  m_pcGrippable(nullptr),
56  m_fMass(1.6f),
57  m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()),
58  m_unLastTurretMode(m_cFootBotEntity.GetTurretEntity().GetMode()) {
59  RegisterAnchorMethod<CDynamics2DFootBotModel>(
60  GetEmbodiedEntity().GetOriginAnchor(),
61  &CDynamics2DFootBotModel::UpdateOriginAnchor);
62  RegisterAnchorMethod<CDynamics2DFootBotModel>(
63  GetEmbodiedEntity().GetAnchor("turret"),
64  &CDynamics2DFootBotModel::UpdateTurretAnchor);
65  RegisterAnchorMethod<CDynamics2DFootBotModel>(
66  GetEmbodiedEntity().GetAnchor("perspective_camera"),
67  &CDynamics2DFootBotModel::UpdatePerspectiveCameraAnchor);
68  /* Create the actual body with initial position and orientation */
69  m_ptActualBaseBody =
70  cpSpaceAddBody(GetDynamics2DEngine().GetPhysicsSpace(),
71  cpBodyNew(m_fMass,
72  cpMomentForCircle(m_fMass,
73  0.0f,
74  FOOTBOT_RADIUS + FOOTBOT_RADIUS,
75  cpvzero)));
76  const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position;
77  m_ptActualBaseBody->p = cpv(cPosition.GetX(), cPosition.GetY());
78  CRadians cXAngle, cYAngle, cZAngle;
79  GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
80  cpBodySetAngle(m_ptActualBaseBody, cZAngle.GetValue());
81  /* Create the actual body shape */
82  m_ptBaseShape =
83  cpSpaceAddShape(GetDynamics2DEngine().GetPhysicsSpace(),
84  cpCircleShapeNew(m_ptActualBaseBody,
85  FOOTBOT_RADIUS,
86  cpvzero));
87  m_ptBaseShape->e = 0.0; // No elasticity
88  m_ptBaseShape->u = 0.7; // Lots of friction
89  /* This shape is grippable */
90  m_pcGrippable = new CDynamics2DGrippable(GetEmbodiedEntity(),
91  m_ptBaseShape);
92  /* Constrain the actual base body to follow the diff steering control */
93  m_cDiffSteering.AttachTo(m_ptActualBaseBody);
94  /* Add the body so that the default methods work as expected */
95  AddBody(m_ptActualBaseBody, cpvzero, 0, FOOTBOT_HEIGHT);
96  /* Create the gripper body */
97  m_ptActualGripperBody =
98  cpSpaceAddBody(GetDynamics2DEngine().GetPhysicsSpace(),
99  cpBodyNew(m_fMass / 20.0,
100  cpMomentForCircle(m_fMass,
101  0.0f,
102  FOOTBOT_RADIUS + FOOTBOT_RADIUS,
103  cpvzero)));
104  m_ptActualGripperBody->p = cpv(cPosition.GetX(), cPosition.GetY());
105  cpBodySetAngle(m_ptActualGripperBody,
106  cZAngle.GetValue() +
107  m_cFootBotEntity.GetTurretEntity().GetRotation().GetValue());
108  /* Create the gripper shape */
109  cpShape* ptGripperShape =
110  cpSpaceAddShape(GetDynamics2DEngine().GetPhysicsSpace(),
111  cpCircleShapeNew(m_ptActualGripperBody,
112  0.01f,
113  cpv(FOOTBOT_RADIUS, 0.0f)));
114  m_pcGripper = new CDynamics2DGripper(GetDynamics2DEngine(),
115  m_cGripperEntity,
116  ptGripperShape);
117  /* Constrain the actual gripper body to follow the actual base body */
118  m_ptBaseGripperLinearMotion =
119  cpSpaceAddConstraint(GetDynamics2DEngine().GetPhysicsSpace(),
120  cpPivotJointNew2(m_ptActualBaseBody,
121  m_ptActualGripperBody,
122  cpvzero,
123  cpvzero));
124  m_ptBaseGripperAngularMotion = cpSpaceAddConstraint(GetDynamics2DEngine().GetPhysicsSpace(),
125  cpGearJointNew(m_ptActualBaseBody,
126  m_ptActualGripperBody,
127  0.0f,
128  1.0f));
129  m_ptBaseGripperAngularMotion->maxBias = 0.0f; /* disable joint correction */
130  m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE; /* limit the dragging torque */
131  /* Add the gripper body so that the default methods work as expected */
132  AddBody(m_ptActualGripperBody, cpvzero, 0, FOOTBOT_HEIGHT);
133  /* Switch to active mode if necessary */
134  if(m_unLastTurretMode == MODE_SPEED_CONTROL ||
135  m_unLastTurretMode == MODE_POSITION_CONTROL) {
136  TurretActiveToPassive();
137  }
138  }
139 
140  /****************************************/
141  /****************************************/
142 
143  CDynamics2DFootBotModel::~CDynamics2DFootBotModel() {
144  delete m_pcGripper;
145  delete m_pcGrippable;
146  switch(m_unLastTurretMode) {
147  case MODE_OFF:
148  case MODE_PASSIVE:
149  cpSpaceRemoveConstraint(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperLinearMotion);
150  cpSpaceRemoveConstraint(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperAngularMotion);
151  cpConstraintFree(m_ptBaseGripperLinearMotion);
152  cpConstraintFree(m_ptBaseGripperAngularMotion);
153  break;
154  case MODE_POSITION_CONTROL:
155  case MODE_SPEED_CONTROL:
156  cpSpaceRemoveConstraint(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperLinearMotion);
157  cpSpaceRemoveConstraint(GetDynamics2DEngine().GetPhysicsSpace(), m_ptGripperControlAngularMotion);
158  cpConstraintFree(m_ptBaseGripperLinearMotion);
159  cpConstraintFree(m_ptGripperControlAngularMotion);
160  cpBodyFree(m_ptControlGripperBody);
161  break;
162  }
163  m_cDiffSteering.Detach();
164  }
165 
166  /****************************************/
167  /****************************************/
168 
169  void CDynamics2DFootBotModel::MoveTo(const CVector3& c_position,
170  const CQuaternion& c_orientation) {
171  /* Release grippers and grippees */
172  m_pcGripper->Release();
173  m_pcGrippable->ReleaseAll();
174  /* Move robot */
175  CDynamics2DMultiBodyObjectModel::MoveTo(c_position,
176  c_orientation);
177  }
178 
179  /****************************************/
180  /****************************************/
181 
182  void CDynamics2DFootBotModel::Reset() {
183  /* Zero speed and applied forces of base control body */
184  m_cDiffSteering.Reset();
185  /* Release grippers and gripees */
186  m_pcGripper->Release();
187  m_pcGrippable->ReleaseAll();
188  /* Switch to turret passive mode if needed */
189  if(m_unLastTurretMode == MODE_SPEED_CONTROL ||
190  m_unLastTurretMode == MODE_POSITION_CONTROL) {
191  TurretActiveToPassive();
192  m_unLastTurretMode = MODE_OFF;
193  GetEmbodiedEntity().DisableAnchor("turret");
194  }
195  /* Reset the rest */
196  CDynamics2DMultiBodyObjectModel::Reset();
197  }
198 
199  /****************************************/
200  /****************************************/
201 
202  void CDynamics2DFootBotModel::CalculateBoundingBox() {
203  GetBoundingBox().MinCorner.SetX(m_ptBaseShape->bb.l);
204  GetBoundingBox().MinCorner.SetY(m_ptBaseShape->bb.b);
205  GetBoundingBox().MinCorner.SetZ(GetDynamics2DEngine().GetElevation());
206  GetBoundingBox().MaxCorner.SetX(m_ptBaseShape->bb.r);
207  GetBoundingBox().MaxCorner.SetY(m_ptBaseShape->bb.t);
208  GetBoundingBox().MaxCorner.SetZ(GetDynamics2DEngine().GetElevation() + FOOTBOT_HEIGHT);
209  }
210 
211  /****************************************/
212  /****************************************/
213 
214  void CDynamics2DFootBotModel::UpdateFromEntityStatus() {
215  /* Do we want to move? */
216  if((m_fCurrentWheelVelocity[FOOTBOT_LEFT_WHEEL] != 0.0f) ||
217  (m_fCurrentWheelVelocity[FOOTBOT_RIGHT_WHEEL] != 0.0f)) {
218  m_cDiffSteering.SetWheelVelocity(m_fCurrentWheelVelocity[FOOTBOT_LEFT_WHEEL],
219  m_fCurrentWheelVelocity[FOOTBOT_RIGHT_WHEEL]);
220  }
221  else {
222  /* No, we don't want to move - zero all speeds */
223  m_cDiffSteering.Reset();
224  }
225  /* Update turret structures if the state changed state in the last step */
226  if(m_cFootBotEntity.GetTurretEntity().GetMode() != m_unLastTurretMode) {
227  /* Enable or disable the anchor */
228  if(m_cFootBotEntity.GetTurretEntity().GetMode() != MODE_OFF) {
229  GetEmbodiedEntity().EnableAnchor("turret");
230  }
231  else {
232  GetEmbodiedEntity().DisableAnchor("turret");
233  }
234  /* Manage the thing like a state machine */
235  switch(m_unLastTurretMode) {
236  case MODE_OFF:
237  case MODE_PASSIVE:
238  switch(m_cFootBotEntity.GetTurretEntity().GetMode()) {
239  case MODE_POSITION_CONTROL:
240  case MODE_SPEED_CONTROL:
241  TurretPassiveToActive();
242  break;
243  case MODE_OFF:
244  case MODE_PASSIVE:
245  break;
246  }
247  break;
248  case MODE_SPEED_CONTROL:
249  case MODE_POSITION_CONTROL:
250  switch(m_cFootBotEntity.GetTurretEntity().GetMode()) {
251  case MODE_OFF:
252  case MODE_PASSIVE:
253  TurretActiveToPassive();
254  break;
255  case MODE_POSITION_CONTROL:
256  case MODE_SPEED_CONTROL:
257  break;
258  }
259  break;
260  }
261  /* Save the current mode for the next time step */
262  m_unLastTurretMode = m_cFootBotEntity.GetTurretEntity().GetMode();
263  }
264  /* Update the turret data */
265  switch(m_unLastTurretMode) {
266  /* Position control mode is implemented using a PD controller */
267  case MODE_POSITION_CONTROL: {
268  Real fCurRotErr = NormalizedDifference(
269  m_cFootBotEntity.GetTurretEntity().GetDesiredRotation(),
270  NormalizedDifference(
271  CRadians(m_ptActualGripperBody->a),
272  CRadians(m_ptActualBaseBody->a))).GetValue();
273  m_ptControlGripperBody->w =
274  m_cDiffSteering.GetAngularVelocity() +
275  (PD_P_CONSTANT * fCurRotErr +
276  PD_D_CONSTANT * (fCurRotErr - m_fPreviousTurretAngleError) * GetDynamics2DEngine().GetInverseSimulationClockTick());
277  m_fPreviousTurretAngleError = fCurRotErr;
278  break;
279  }
280  case MODE_SPEED_CONTROL:
281  m_ptControlGripperBody->w =
282  m_cDiffSteering.GetAngularVelocity() +
283  m_cFootBotEntity.GetTurretEntity().GetDesiredRotationSpeed();
284  break;
285  case MODE_OFF:
286  case MODE_PASSIVE:
287  if(m_cGripperEntity.IsGripping() &&
288  m_cGripperEntity.IsLocked()) {
289  m_ptBaseGripperAngularMotion->maxForce = 0.0001f; /* limit the dragging torque */
290  }
291  else {
292  m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE; /* limit the dragging torque */
293  }
294  break;
295  }
296  }
297 
298  /****************************************/
299  /****************************************/
300 
301  void CDynamics2DFootBotModel::TurretPassiveToActive() {
302  /* Delete constraints to actual base body */
303  cpSpaceRemoveConstraint(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBaseGripperAngularMotion);
304  cpConstraintFree(m_ptBaseGripperAngularMotion);
305  /* Create gripper control body */
306  m_ptControlGripperBody = cpBodyNew(INFINITY, INFINITY);
307  /* Create angular constraint from gripper control body to gripper actual body */
308  m_ptGripperControlAngularMotion = cpSpaceAddConstraint(GetDynamics2DEngine().GetPhysicsSpace(),
309  cpGearJointNew(m_ptActualGripperBody,
310  m_ptControlGripperBody,
311  0.0f,
312  1.0f));
313  m_ptGripperControlAngularMotion->maxBias = 0.0f; /* disable joint correction */
314  m_ptGripperControlAngularMotion->maxForce = FOOTBOT_MAX_TORQUE; /* limit the dragging torque */
315  }
316 
317  /****************************************/
318  /****************************************/
319 
320  void CDynamics2DFootBotModel::TurretActiveToPassive() {
321  /* Delete constraint from actual gripper body to gripper control body */
322  cpSpaceRemoveConstraint(GetDynamics2DEngine().GetPhysicsSpace(), m_ptGripperControlAngularMotion);
323  cpConstraintFree(m_ptGripperControlAngularMotion);
324  /* Delete control body */
325  cpBodyFree(m_ptControlGripperBody);
326  /* Create constraints from actual gripper body to actual base body */
327  m_ptBaseGripperAngularMotion = cpSpaceAddConstraint(GetDynamics2DEngine().GetPhysicsSpace(),
328  cpGearJointNew(m_ptActualBaseBody,
329  m_ptActualGripperBody,
330  0.0f,
331  1.0f));
332  m_ptBaseGripperAngularMotion->maxBias = 0.0f; /* disable joint correction */
333  m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE; /* limit the dragging torque */
334  }
335 
336  /****************************************/
337  /****************************************/
338 
339  void CDynamics2DFootBotModel::UpdateOriginAnchor(SAnchor& s_anchor) {
340  s_anchor.Position.SetX(m_ptActualBaseBody->p.x);
341  s_anchor.Position.SetY(m_ptActualBaseBody->p.y);
342  s_anchor.Orientation.FromAngleAxis(CRadians(m_ptActualBaseBody->a), CVector3::Z);
343  }
344 
345  /****************************************/
346  /****************************************/
347 
348  void CDynamics2DFootBotModel::UpdateTurretAnchor(SAnchor& s_anchor) {
349  s_anchor.Position.SetX(m_ptActualGripperBody->p.x);
350  s_anchor.Position.SetY(m_ptActualGripperBody->p.y);
351  s_anchor.Orientation.FromAngleAxis(CRadians(m_ptActualGripperBody->a), CVector3::Z);
352  s_anchor.OffsetOrientation.FromAngleAxis(
353  NormalizedDifference(
354  CRadians(m_ptActualGripperBody->a),
355  CRadians(m_ptActualBaseBody->a)),
356  CVector3::Z);
357  }
358 
359  /****************************************/
360  /****************************************/
361 
362  void CDynamics2DFootBotModel::UpdatePerspectiveCameraAnchor(SAnchor& s_anchor) {
363  s_anchor.Position.SetX(m_ptActualBaseBody->p.x + s_anchor.OffsetPosition.GetX());
364  s_anchor.Position.SetY(m_ptActualBaseBody->p.y + s_anchor.OffsetPosition.GetY());
365  s_anchor.Orientation =
366  s_anchor.OffsetOrientation *
367  CQuaternion(CRadians(m_ptActualBaseBody->a), CVector3::Z);
368  }
369 
370  /****************************************/
371  /****************************************/
372 
373  REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CFootBotEntity, CDynamics2DFootBotModel);
374 
375  /****************************************/
376  /****************************************/
377 
378 }
Real
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::NormalizedDifference
CRadians NormalizedDifference(const CRadians &c_angle1, const CRadians &c_angle2)
Calculates the normalized difference between the given angles.
Definition: angles.h:510
argos::REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY
REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CEPuckEntity, CDynamics2DEPuckModel)
argos::CEmbodiedEntity::DisableAnchor
void DisableAnchor(const std::string &str_id)
Disables an anchor.
Definition: embodied_entity.cpp:169
argos::CEmbodiedEntity::EnableAnchor
void EnableAnchor(const std::string &str_id)
Enables an anchor.
Definition: embodied_entity.cpp:150
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::CPhysicsEngine::GetInverseSimulationClockTick
static Real GetInverseSimulationClockTick()
Returns the inverse of GetSimulationClockTick().
Definition: physics_engine.cpp:271
argos::SAnchor
An anchor related to the body of an entity.
Definition: physics_model.h:38
argos::SAnchor::Orientation
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
argos::SAnchor::OffsetOrientation
CQuaternion OffsetOrientation
The initial orientation of the anchor wrt the body coordinate system.
Definition: physics_model.h:49
argos::SAnchor::Position
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
argos::SAnchor::OffsetPosition
CVector3 OffsetPosition
The initial position of the anchor wrt the body coordinate system.
Definition: physics_model.h:47
argos::CPhysicsModel::GetEmbodiedEntity
CEmbodiedEntity & GetEmbodiedEntity()
Returns the embodied entity associated to this physics model.
Definition: physics_model.h:133
argos::CPhysicsModel::GetBoundingBox
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
Definition: physics_model.h:214
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
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:172
argos::CQuaternion::FromAngleAxis
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
Definition: quaternion.h:126
argos::CVector3
A 3D vector class.
Definition: vector3.h:31
argos::CVector3::SetY
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:129
argos::CVector3::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:105
argos::CVector3::SetX
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:113
argos::CVector3::SetZ
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:145
argos::CVector3::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:121
argos::CVector3::Z
static const CVector3 Z
The z axis.
Definition: vector3.h:42
argos::CDynamics2DFootBotModel::CalculateBoundingBox
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
Definition: dynamics2d_footbot_model.cpp:202
argos::CDynamics2DFootBotModel::MoveTo
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
Definition: dynamics2d_footbot_model.cpp:169
argos::CDynamics2DFootBotModel::UpdatePerspectiveCameraAnchor
void UpdatePerspectiveCameraAnchor(SAnchor &s_anchor)
Definition: dynamics2d_footbot_model.cpp:362
argos::CDynamics2DFootBotModel::UpdateFromEntityStatus
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
Definition: dynamics2d_footbot_model.cpp:214
argos::CDynamics2DFootBotModel::CDynamics2DFootBotModel
CDynamics2DFootBotModel(CDynamics2DEngine &c_engine, CFootBotEntity &c_entity)
Definition: dynamics2d_footbot_model.cpp:43
argos::CFootBotEntity::GetTurretEntity
CFootBotTurretEntity & GetTurretEntity()
Definition: footbot_entity.h:72
argos::CFootBotTurretEntity::GetDesiredRotation
const CRadians & GetDesiredRotation() const
Definition: footbot_turret_entity.cpp:89
argos::CGripperEquippedEntity::IsGripping
bool IsGripping() const
Returns true if this gripper is gripping something.
Definition: gripper_equipped_entity.h:195
argos::CGripperEquippedEntity::IsLocked
bool IsLocked() const
Returns true if the gripper is locked.
Definition: gripper_equipped_entity.h:187
argos::CDynamics2DModel::GetDynamics2DEngine
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
Definition: dynamics2d_model.h:48
argos::CDynamics2DMultiBodyObjectModel
Base class for object models with multiple bodies.
Definition: dynamics2d_multi_body_object_model.h:44
argos::CDynamics2DMultiBodyObjectModel::MoveTo
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
Definition: dynamics2d_multi_body_object_model.cpp:42
argos::CDynamics2DMultiBodyObjectModel::AddBody
virtual void AddBody(cpBody *pt_body, const cpVect &t_offset_pos, cpFloat t_offset_orient, Real f_height)
Adds a body.
Definition: dynamics2d_multi_body_object_model.cpp:136