dynamics2d_footbot_model.cpp
Go to the documentation of this file.
1 
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 
31  };
32 
33  enum ETurretModes {
38  };
39 
40  /****************************************/
41  /****************************************/
42 
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(NULL),
55  m_pcGrippable(NULL),
56  m_fMass(1.6f),
57  m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()),
58  m_unLastTurretMode(m_cFootBotEntity.GetTurretEntity().GetMode()) {
59  RegisterAnchorMethod<CDynamics2DFootBotModel>(
62  RegisterAnchorMethod<CDynamics2DFootBotModel>(
63  GetEmbodiedEntity().GetAnchor("turret"),
65  RegisterAnchorMethod<CDynamics2DFootBotModel>(
66  GetEmbodiedEntity().GetAnchor("perspective_camera"),
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 
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;
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 
170  const CQuaternion& c_orientation) {
171  /* Release grippers and grippees */
172  m_pcGripper->Release();
173  m_pcGrippable->ReleaseAll();
174  /* Move robot */
176  c_orientation);
177  }
178 
179  /****************************************/
180  /****************************************/
181 
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 */
197  }
198 
199  /****************************************/
200  /****************************************/
201 
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 
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()) {
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:
250  switch(m_cFootBotEntity.GetTurretEntity().GetMode()) {
251  case MODE_OFF:
252  case MODE_PASSIVE:
253  TurretActiveToPassive();
254  break;
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(),
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 
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 
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);
354  CRadians(m_ptActualGripperBody->a),
355  CRadians(m_ptActualBaseBody->a)),
356  CVector3::Z);
357  }
358 
359  /****************************************/
360  /****************************************/
361 
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 
374 
375  /****************************************/
376  /****************************************/
377 
378 }
A 3D vector class.
Definition: vector3.h:29
Base class for object models with multiple bodies.
static Real GetInverseSimulationClockTick()
Returns the inverse of GetSimulationClockTick().
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
CQuaternion OffsetOrientation
The initial orientation of the anchor wrt the body coordinate system.
Definition: physics_model.h:49
bool IsGripping() const
Returns true if this gripper is gripping something.
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
CDynamics2DFootBotModel(CDynamics2DEngine &c_engine, CFootBotEntity &c_entity)
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
virtual void AddBody(cpBody *pt_body, const cpVect &t_offset_pos, cpFloat t_offset_orient, Real f_height)
Adds a body.
bool IsLocked() const
Returns true if the gripper is locked.
CFootBotTurretEntity & GetTurretEntity()
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
Definition: quaternion.h:126
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:101
An anchor related to the body of an entity.
Definition: physics_model.h:38
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
void UpdatePerspectiveCameraAnchor(SAnchor &s_anchor)
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.
static const CVector3 Z
The z axis.
Definition: vector3.h:40
CRadians NormalizedDifference(const CRadians &c_angle1, const CRadians &c_angle2)
Calculates the normalized difference between the given angles.
Definition: angles.h:510
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:117
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
CVector3 OffsetPosition
The initial position of the anchor wrt the body coordinate system.
Definition: physics_model.h:47
const SAnchor & GetAnchor(const std::string &str_id) const
Returns the wanted anchor as a const reference.
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:133
const CRadians & GetDesiredRotation() const
REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CEPuckEntity, CDynamics2DEPuckModel)
void DisableAnchor(const std::string &str_id)
Disables an anchor.
void EnableAnchor(const std::string &str_id)
Enables an anchor.
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.