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 }
CVector3 OffsetPosition
The initial position of the anchor wrt the body coordinate system.
Definition: physics_model.h:47
const SBoundingBox & GetBoundingBox() const
Returns an axis-aligned box that contains the physics model.
An anchor related to the body of an entity.
Definition: physics_model.h:38
virtual void AddBody(cpBody *pt_body, const cpVect &t_offset_pos, cpFloat t_offset_orient, Real f_height)
Adds a body.
bool IsGripping() const
Returns true if this gripper is gripping something.
A 3D vector class.
Definition: vector3.h:29
const CRadians & GetDesiredRotation() const
void ToEulerAngles(CRadians &c_z_angle, CRadians &c_y_angle, CRadians &c_x_angle) const
Definition: quaternion.h:172
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:133
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
void UpdatePerspectiveCameraAnchor(SAnchor &s_anchor)
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
Definition: quaternion.h:126
Base class for object models with multiple bodies.
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:117
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
static Real GetInverseSimulationClockTick()
Returns the inverse of GetSimulationClockTick().
CDynamics2DEngine & GetDynamics2DEngine()
Returns the dynamics 2D engine state.
virtual void UpdateFromEntityStatus()
Updates the state of this model from the status of the associated entity.
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
void EnableAnchor(const std::string &str_id)
Enables an anchor.
virtual void CalculateBoundingBox()
Calculates the axis-aligned box that contains the entire physics model.
REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CEPuckEntity, CDynamics2DEPuckModel)
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:101
CRadians NormalizedDifference(const CRadians &c_angle1, const CRadians &c_angle2)
Calculates the normalized difference between the given angles.
Definition: angles.h:510
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
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
const SAnchor & GetAnchor(const std::string &str_id) const
Returns the wanted anchor as a const reference.
static const CVector3 Z
The z axis.
Definition: vector3.h:40
CFootBotTurretEntity & GetTurretEntity()
virtual void MoveTo(const CVector3 &c_position, const CQuaternion &c_orientation)
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
CQuaternion OffsetOrientation
The initial orientation of the anchor wrt the body coordinate system.
Definition: physics_model.h:49
bool IsLocked() const
Returns true if the gripper is locked.
void DisableAnchor(const std::string &str_id)
Disables an anchor.
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
CDynamics2DFootBotModel(CDynamics2DEngine &c_engine, CFootBotEntity &c_entity)