ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/robots/foot-bot/simulator/dynamics2d_footbot_model.cpp
Go to the documentation of this file.
00001 
00007 #include "dynamics2d_footbot_model.h"
00008 #include "footbot_turret_entity.h"
00009 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_gripping.h>
00010 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
00011 
00012 namespace argos {
00013 
00014    /****************************************/
00015    /****************************************/
00016 
00017    /* P and D constants of the PD controller used for the turret position control. */
00018    static const Real PD_P_CONSTANT = 0.4;
00019    static const Real PD_D_CONSTANT = 0.2;
00020 
00021    static const Real FOOTBOT_RADIUS                   = 0.085036758f;
00022    static const Real FOOTBOT_INTERWHEEL_DISTANCE      = 0.14f;
00023    static const Real FOOTBOT_HEIGHT                   = 0.146899733f;
00024 
00025    static const Real FOOTBOT_MAX_FORCE                = 1.5f;
00026    static const Real FOOTBOT_MAX_TORQUE               = 1.5f;
00027 
00028    enum FOOTBOT_WHEELS {
00029       FOOTBOT_LEFT_WHEEL = 0,
00030       FOOTBOT_RIGHT_WHEEL = 1
00031    };
00032 
00033    enum ETurretModes {
00034       MODE_OFF,
00035       MODE_PASSIVE,
00036       MODE_SPEED_CONTROL,
00037       MODE_POSITION_CONTROL,
00038    };
00039 
00040    /****************************************/
00041    /****************************************/
00042 
00043    CDynamics2DFootBotModel::CDynamics2DFootBotModel(CDynamics2DEngine& c_engine,
00044                                                     CFootBotEntity& c_entity) :
00045       CDynamics2DModel(c_engine, c_entity.GetEmbodiedEntity()),
00046       m_cFootBotEntity(c_entity),
00047       m_cWheeledEntity(m_cFootBotEntity.GetWheeledEntity()),
00048       m_cGripperEntity(c_entity.GetGripperEquippedEntity()),
00049       m_cDiffSteering(c_engine,
00050                       FOOTBOT_MAX_FORCE,
00051                       FOOTBOT_MAX_TORQUE,
00052                       FOOTBOT_INTERWHEEL_DISTANCE),
00053       m_pcGripper(NULL),
00054       m_pcGrippable(NULL),
00055       m_fMass(1.6f),
00056       m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()),
00057       m_unLastTurretMode(m_cFootBotEntity.GetTurretEntity().GetMode()) {
00058       /* Create the actual body with initial position and orientation */
00059       m_ptActualBaseBody =
00060          cpSpaceAddBody(m_cDyn2DEngine.GetPhysicsSpace(),
00061                         cpBodyNew(m_fMass,
00062                                   cpMomentForCircle(m_fMass,
00063                                                     0.0f,
00064                                                     FOOTBOT_RADIUS + FOOTBOT_RADIUS,
00065                                                     cpvzero)));
00066       const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
00067       m_ptActualBaseBody->p = cpv(cPosition.GetX(), cPosition.GetY());
00068       CRadians cXAngle, cYAngle, cZAngle;
00069       GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
00070       cpBodySetAngle(m_ptActualBaseBody, cZAngle.GetValue());
00071       /* Create the actual body shape */
00072       m_ptBaseShape =
00073          cpSpaceAddShape(m_cDyn2DEngine.GetPhysicsSpace(),
00074                          cpCircleShapeNew(m_ptActualBaseBody,
00075                                           FOOTBOT_RADIUS,
00076                                           cpvzero));
00077       m_ptBaseShape->e = 0.0; // No elasticity
00078       m_ptBaseShape->u = 0.7; // Lots of friction
00079       /* This shape is grippable */
00080       m_pcGrippable = new CDynamics2DGrippable(GetEmbodiedEntity(),
00081                                                m_ptBaseShape);
00082       /* Constrain the actual base body to follow the diff steering control */
00083       m_cDiffSteering.AttachTo(m_ptActualBaseBody);
00084       /* Create the gripper body */      
00085       m_ptActualGripperBody =
00086          cpSpaceAddBody(m_cDyn2DEngine.GetPhysicsSpace(),
00087                         cpBodyNew(m_fMass / 20.0,
00088                                   cpMomentForCircle(m_fMass,
00089                                                     0.0f,
00090                                                     FOOTBOT_RADIUS + FOOTBOT_RADIUS,
00091                                                     cpvzero)));
00092       m_ptActualGripperBody->p = cpv(cPosition.GetX(), cPosition.GetY());
00093       cpBodySetAngle(m_ptActualGripperBody,
00094                      cZAngle.GetValue() +
00095                      m_cFootBotEntity.GetTurretEntity().GetRotation().GetValue());
00096       /* Create the gripper shape */
00097       m_ptGripperShape = 
00098          cpSpaceAddShape(m_cDyn2DEngine.GetPhysicsSpace(),
00099                          cpCircleShapeNew(m_ptActualGripperBody,
00100                                           0.01f,
00101                                           cpv(FOOTBOT_RADIUS, 0.0f)));
00102       m_pcGripper = new CDynamics2DGripper(m_cDyn2DEngine,
00103                                            m_cGripperEntity,
00104                                            m_ptGripperShape);
00105       /* Constrain the actual gripper body to follow the actual base body */
00106       m_ptBaseGripperLinearMotion =
00107          cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
00108                               cpPivotJointNew2(m_ptActualBaseBody,
00109                                                m_ptActualGripperBody,
00110                                                cpvzero,
00111                                                cpvzero));
00112       m_ptBaseGripperAngularMotion = cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
00113                                                           cpGearJointNew(m_ptActualBaseBody,
00114                                                                          m_ptActualGripperBody,
00115                                                                          0.0f,
00116                                                                          1.0f));
00117       m_ptBaseGripperAngularMotion->maxBias = 0.0f; /* disable joint correction */
00118       m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE; /* limit the dragging torque */
00119       /* Associate this model to the body data for ray queries */
00120       m_ptActualBaseBody->data = this;
00121       /* Switch to active mode if necessary */
00122       if(m_unLastTurretMode == MODE_SPEED_CONTROL ||
00123          m_unLastTurretMode == MODE_POSITION_CONTROL) {
00124          TurretActiveToPassive();
00125       }
00126       /* Calculate bounding box */
00127       GetBoundingBox().MinCorner.SetZ(GetEmbodiedEntity().GetPosition().GetZ());
00128       GetBoundingBox().MaxCorner.SetZ(GetEmbodiedEntity().GetPosition().GetZ() + FOOTBOT_HEIGHT);
00129       CalculateBoundingBox();
00130    }
00131 
00132    /****************************************/
00133    /****************************************/
00134 
00135    CDynamics2DFootBotModel::~CDynamics2DFootBotModel() {
00136       delete m_pcGripper;
00137       delete m_pcGrippable;
00138       switch(m_unLastTurretMode) {
00139          case MODE_OFF:
00140          case MODE_PASSIVE:
00141             cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseGripperLinearMotion);
00142             cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseGripperAngularMotion);
00143             cpSpaceRemoveBody(m_cDyn2DEngine.GetPhysicsSpace(), m_ptActualGripperBody);
00144             cpSpaceRemoveShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptGripperShape);
00145             cpConstraintFree(m_ptBaseGripperLinearMotion);
00146             cpConstraintFree(m_ptBaseGripperAngularMotion);
00147             cpShapeFree(m_ptGripperShape);
00148             cpBodyFree(m_ptActualGripperBody);
00149             break;
00150          case MODE_POSITION_CONTROL:
00151          case MODE_SPEED_CONTROL:
00152             cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseGripperLinearMotion);
00153             cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptGripperControlAngularMotion);
00154             cpSpaceRemoveBody(m_cDyn2DEngine.GetPhysicsSpace(), m_ptActualGripperBody);
00155             cpSpaceRemoveShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptGripperShape);
00156             cpConstraintFree(m_ptBaseGripperLinearMotion);
00157             cpConstraintFree(m_ptGripperControlAngularMotion);
00158             cpShapeFree(m_ptGripperShape);
00159             cpBodyFree(m_ptActualGripperBody);
00160             cpBodyFree(m_ptControlGripperBody);
00161             break;
00162       }
00163       m_cDiffSteering.Detach();
00164       cpSpaceRemoveBody(m_cDyn2DEngine.GetPhysicsSpace(), m_ptActualBaseBody);
00165       cpSpaceRemoveShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseShape);
00166       cpShapeFree(m_ptBaseShape);
00167       cpBodyFree(m_ptActualBaseBody);
00168    }
00169 
00170    /****************************************/
00171    /****************************************/
00172 
00173    bool CDynamics2DFootBotModel::MoveTo(const CVector3& c_position,
00174                                          const CQuaternion& c_orientation,
00175                                          bool b_check_only) {
00176       /* Save body position and orientation */
00177       cpVect tOldPos = m_ptActualBaseBody->p;
00178       cpFloat fOldA = m_ptActualBaseBody->a;
00179       /* Move the body to the desired position */
00180       m_ptActualBaseBody->p = cpv(c_position.GetX(), c_position.GetY());
00181       CRadians cXAngle, cYAngle, cZAngle;
00182       c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
00183       cpBodySetAngle(m_ptActualBaseBody, cZAngle.GetValue());
00184       /* Create a shape sensor to test the movement */
00185       cpShape* ptTestShape = cpCircleShapeNew(m_ptActualBaseBody,
00186                                               FOOTBOT_RADIUS,
00187                                               cpvzero);
00188       /* Check if there is a collision */
00189       SInt32 nCollision = cpSpaceShapeQuery(m_cDyn2DEngine.GetPhysicsSpace(), ptTestShape, NULL, NULL);
00190       /* Dispose of the sensor shape */
00191       cpShapeFree(ptTestShape);
00192       /* Should we keep this movement? */
00193       if(b_check_only || nCollision) {
00194          /*
00195           * No, because it was only a check or there was a collision
00196           * Restore old body state
00197           */
00198          m_ptActualBaseBody->p = tOldPos;
00199          cpBodySetAngle(m_ptActualBaseBody, fOldA);
00200       }
00201       else {
00202          /*
00203           * It wasn't a check and there were no collisions
00204           * Keep the movement and move the gripper body too
00205           */
00206          m_ptActualGripperBody->p = cpv(c_position.GetX(), c_position.GetY());
00207          cpBodySetAngle(m_ptActualGripperBody,
00208                         cZAngle.GetValue() + m_cFootBotEntity.GetTurretEntity().GetRotation().GetValue());
00209          /* Release grippers and gripees */
00210          m_pcGripper->Release();
00211          m_pcGrippable->ReleaseAll();
00212          /* Update the active space hash */
00213          cpSpaceReindexShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseShape);
00214          /* Update bounding box */
00215          CalculateBoundingBox();
00216       }
00217       /* The movement is allowed if there is no collision */
00218       return !nCollision;
00219    }
00220 
00221    /****************************************/
00222    /****************************************/
00223 
00224    void CDynamics2DFootBotModel::Reset() {
00225       /* Reset body position */
00226       const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
00227       m_ptActualBaseBody->p = cpv(cPosition.GetX(), cPosition.GetY());
00228       m_ptActualGripperBody->p = cpv(cPosition.GetX(), cPosition.GetY());
00229       /* Reset body orientation */
00230       CRadians cXAngle, cYAngle, cZAngle;
00231       GetEmbodiedEntity().GetInitOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
00232       cpBodySetAngle(m_ptActualBaseBody, cZAngle.GetValue());
00233       cpBodySetAngle(m_ptActualGripperBody, cZAngle.GetValue());
00234       /* Zero speed and applied forces of actual base body */
00235       m_ptActualBaseBody->v = cpvzero;
00236       m_ptActualBaseBody->w = 0.0f;
00237       cpBodyResetForces(m_ptActualBaseBody);
00238       /* Zero speed and applied forces of base control body */
00239       m_cDiffSteering.Reset();
00240       /* Release grippers and gripees */
00241       m_pcGripper->Release();
00242       m_pcGrippable->ReleaseAll();
00243       /* Zero speed and applied forces of actual gripper body */
00244       m_ptActualGripperBody->v = cpvzero;
00245       m_ptActualGripperBody->w = 0.0f;
00246       cpBodyResetForces(m_ptActualGripperBody);
00247       /* Switch to turret passive mode if needed */
00248       if(m_unLastTurretMode == MODE_SPEED_CONTROL ||
00249          m_unLastTurretMode == MODE_POSITION_CONTROL) {
00250          TurretActiveToPassive();
00251          m_unLastTurretMode = MODE_OFF;
00252       }
00253       /* Update bounding box */
00254       cpShapeCacheBB(m_ptBaseShape);
00255       CalculateBoundingBox();
00256    }
00257 
00258    /****************************************/
00259    /****************************************/
00260 
00261    void CDynamics2DFootBotModel::CalculateBoundingBox() {
00262       GetBoundingBox().MinCorner.SetX(m_ptBaseShape->bb.l);
00263       GetBoundingBox().MinCorner.SetY(m_ptBaseShape->bb.b);
00264       GetBoundingBox().MaxCorner.SetX(m_ptBaseShape->bb.r);
00265       GetBoundingBox().MaxCorner.SetY(m_ptBaseShape->bb.t);
00266    }
00267 
00268    /****************************************/
00269    /****************************************/
00270 
00271    void CDynamics2DFootBotModel::UpdateEntityStatus() {
00272       /* Update bounding box */
00273       CalculateBoundingBox();
00274       /* Update foot-bot body position */
00275       m_cDyn2DEngine.PositionPhysicsToSpace(m_cSpacePosition, GetEmbodiedEntity().GetPosition(), m_ptActualBaseBody);
00276       GetEmbodiedEntity().SetPosition(m_cSpacePosition);
00277       /* Update foot-bot body orientation */
00278       m_cDyn2DEngine.OrientationPhysicsToSpace(m_cSpaceOrientation, m_ptActualBaseBody);
00279       GetEmbodiedEntity().SetOrientation(m_cSpaceOrientation);
00280       /* Update foot-bot turret rotation */
00281       m_cFootBotEntity.GetTurretEntity().SetRotation(CRadians(m_ptActualGripperBody->a - m_ptActualBaseBody->a));
00282       /* Update foot-bot components */
00283       m_cFootBotEntity.UpdateComponents();
00284       /* Check whether a transfer is necessary */
00285       if(m_cDyn2DEngine.IsEntityTransferActive()) {
00286          std::string strEngineId;
00287          if(m_cDyn2DEngine.CalculateTransfer(GetEmbodiedEntity().GetPosition().GetX(),
00288                                         GetEmbodiedEntity().GetPosition().GetY(),
00289                                         strEngineId)) {
00290             m_cDyn2DEngine.ScheduleEntityForTransfer(m_cFootBotEntity, strEngineId);
00291          }
00292       }
00293    }
00294 
00295    /****************************************/
00296    /****************************************/
00297 
00298    void CDynamics2DFootBotModel::UpdateFromEntityStatus() {
00299       /* Do we want to move? */
00300       if((m_fCurrentWheelVelocity[FOOTBOT_LEFT_WHEEL] != 0.0f) ||
00301          (m_fCurrentWheelVelocity[FOOTBOT_RIGHT_WHEEL] != 0.0f)) {
00302          m_cDiffSteering.SetWheelVelocity(m_fCurrentWheelVelocity[FOOTBOT_LEFT_WHEEL],
00303                                           m_fCurrentWheelVelocity[FOOTBOT_RIGHT_WHEEL]);
00304       }
00305       else {
00306          /* No, we don't want to move - zero all speeds */
00307          m_cDiffSteering.Reset();
00308       }
00309       /* Update turret structures if the state changed state in the last step */
00310       if(m_cFootBotEntity.GetTurretEntity().GetMode() != m_unLastTurretMode) {
00311          /* Manage the thing like a state machine */
00312          switch(m_unLastTurretMode) {
00313             case MODE_OFF:
00314             case MODE_PASSIVE:
00315                switch(m_cFootBotEntity.GetTurretEntity().GetMode()) {
00316                   case MODE_POSITION_CONTROL:
00317                   case MODE_SPEED_CONTROL:
00318                      TurretPassiveToActive();
00319                      break;
00320                   case MODE_OFF:
00321                   case MODE_PASSIVE:
00322                      break;
00323                }
00324                break;
00325             case MODE_SPEED_CONTROL:
00326             case MODE_POSITION_CONTROL:
00327                switch(m_cFootBotEntity.GetTurretEntity().GetMode()) {
00328                   case MODE_OFF:
00329                   case MODE_PASSIVE:
00330                      TurretActiveToPassive();
00331                      break;
00332                   case MODE_POSITION_CONTROL:
00333                   case MODE_SPEED_CONTROL:
00334                      break;
00335                }
00336                break;
00337          }
00338          /* Save the current mode for the next time step */
00339          m_unLastTurretMode = m_cFootBotEntity.GetTurretEntity().GetMode();
00340       }
00341       /* Update the turret data */
00342       switch(m_unLastTurretMode) {
00343 
00344          /* Position control mode is implemented using a PD controller */
00345          case MODE_POSITION_CONTROL:
00346             m_ptControlGripperBody->w =
00347                m_cDiffSteering.GetAngularVelocity() +
00348                (PD_P_CONSTANT * (m_cFootBotEntity.GetTurretEntity().GetRotation().GetValue() - (m_ptActualGripperBody->a - m_ptActualBaseBody->a))
00349                 + PD_D_CONSTANT * (m_cFootBotEntity.GetTurretEntity().GetRotation().GetValue() - (m_ptActualGripperBody->a - m_ptActualBaseBody->a) - m_fPreviousTurretAngleError) )*
00350                m_cDyn2DEngine.GetInverseSimulationClockTick();
00351             m_fPreviousTurretAngleError = m_cFootBotEntity.GetTurretEntity().GetRotation().GetValue() - (m_ptActualGripperBody->a - m_ptActualBaseBody->a);
00352             break;
00353          case MODE_SPEED_CONTROL:
00354             m_ptControlGripperBody->w =
00355                m_cDiffSteering.GetAngularVelocity() +
00356                m_cFootBotEntity.GetTurretEntity().GetRotationSpeed();
00357             break;
00358          case MODE_OFF:
00359          case MODE_PASSIVE:
00360             if(m_cGripperEntity.IsGripping() &&
00361                m_cGripperEntity.IsLocked()) {
00362                m_ptBaseGripperAngularMotion->maxForce = 0.0001f; /* limit the dragging torque */
00363             }
00364             else {
00365                m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE; /* limit the dragging torque */
00366             }
00367             break;
00368       }
00369    }
00370 
00371    /****************************************/
00372    /****************************************/
00373 
00374    void CDynamics2DFootBotModel::TurretPassiveToActive() {
00375       /* Delete constraints to actual base body */
00376       cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseGripperAngularMotion);
00377       cpConstraintFree(m_ptBaseGripperAngularMotion);
00378       /* Create gripper control body */
00379       m_ptControlGripperBody = cpBodyNew(INFINITY, INFINITY);
00380       /* Create angular constraint from gripper control body to gripper actual body */
00381       m_ptGripperControlAngularMotion = cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
00382                                                              cpGearJointNew(m_ptActualGripperBody,
00383                                                                             m_ptControlGripperBody,
00384                                                                             0.0f,
00385                                                                             1.0f));
00386       m_ptGripperControlAngularMotion->maxBias = 0.0f; /* disable joint correction */
00387       m_ptGripperControlAngularMotion->maxForce = FOOTBOT_MAX_TORQUE; /* limit the dragging torque */
00388    }
00389 
00390    /****************************************/
00391    /****************************************/
00392 
00393    void CDynamics2DFootBotModel::TurretActiveToPassive() {
00394       /* Delete constraint from actual gripper body to gripper control body */
00395       cpSpaceRemoveConstraint(m_cDyn2DEngine.GetPhysicsSpace(), m_ptGripperControlAngularMotion);
00396       cpConstraintFree(m_ptGripperControlAngularMotion);
00397       /* Delete control body */
00398       cpBodyFree(m_ptControlGripperBody);
00399       /* Create constraints from actual gripper body to actual base body */
00400       m_ptBaseGripperAngularMotion = cpSpaceAddConstraint(m_cDyn2DEngine.GetPhysicsSpace(),
00401                                                           cpGearJointNew(m_ptActualBaseBody,
00402                                                                          m_ptActualGripperBody,
00403                                                                          0.0f,
00404                                                                          1.0f));
00405       m_ptBaseGripperAngularMotion->maxBias = 0.0f; /* disable joint correction */
00406       m_ptBaseGripperAngularMotion->maxForce = FOOTBOT_MAX_TORQUE; /* limit the dragging torque */
00407    }
00408 
00409    /****************************************/
00410    /****************************************/
00411 
00412    bool CDynamics2DFootBotModel::IsCollidingWithSomething() const {
00413       return cpSpaceShapeQuery(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseShape, NULL, NULL) > 0;
00414    }
00415 
00416    /****************************************/
00417    /****************************************/
00418 
00419    REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CFootBotEntity, CDynamics2DFootBotModel);
00420 
00421    /****************************************/
00422    /****************************************/
00423 
00424 }