ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/robots/e-puck/simulator/dynamics2d_epuck_model.cpp
Go to the documentation of this file.
00001 
00007 #include "dynamics2d_epuck_model.h"
00008 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_gripping.h>
00009 #include <argos3/plugins/simulator/physics_engines/dynamics2d/dynamics2d_engine.h>
00010 
00011 namespace argos {
00012 
00013    /****************************************/
00014    /****************************************/
00015 
00016    static const Real EPUCK_RADIUS                   = 0.035f;
00017    static const Real EPUCK_INTERWHEEL_DISTANCE      = 0.053f;
00018    static const Real EPUCK_HEIGHT                   = 0.086f;
00019 
00020    static const Real EPUCK_MAX_FORCE                = 1.5f;
00021    static const Real EPUCK_MAX_TORQUE               = 1.5f;
00022 
00023    enum EPUCK_WHEELS {
00024       EPUCK_LEFT_WHEEL = 0,
00025       EPUCK_RIGHT_WHEEL = 1
00026    };
00027 
00028    /****************************************/
00029    /****************************************/
00030 
00031    CDynamics2DEPuckModel::CDynamics2DEPuckModel(CDynamics2DEngine& c_engine,
00032                                                     CEPuckEntity& c_entity) :
00033       CDynamics2DModel(c_engine, c_entity.GetEmbodiedEntity()),
00034       m_cEPuckEntity(c_entity),
00035       m_cWheeledEntity(m_cEPuckEntity.GetWheeledEntity()),
00036       m_cDiffSteering(c_engine,
00037                       EPUCK_MAX_FORCE,
00038                       EPUCK_MAX_TORQUE,
00039                       EPUCK_INTERWHEEL_DISTANCE),
00040       m_fMass(0.4f),
00041       m_fCurrentWheelVelocity(m_cWheeledEntity.GetWheelVelocities()) {
00042       /* Create the actual body with initial position and orientation */
00043       m_ptActualBaseBody =
00044          cpSpaceAddBody(m_cDyn2DEngine.GetPhysicsSpace(),
00045                         cpBodyNew(m_fMass,
00046                                   cpMomentForCircle(m_fMass,
00047                                                     0.0f,
00048                                                     EPUCK_RADIUS + EPUCK_RADIUS,
00049                                                     cpvzero)));
00050       const CVector3& cPosition = GetEmbodiedEntity().GetPosition();
00051       m_ptActualBaseBody->p = cpv(cPosition.GetX(), cPosition.GetY());
00052       CRadians cXAngle, cYAngle, cZAngle;
00053       GetEmbodiedEntity().GetOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
00054       cpBodySetAngle(m_ptActualBaseBody, cZAngle.GetValue());
00055       /* Create the actual body shape */
00056       m_ptBaseShape =
00057          cpSpaceAddShape(m_cDyn2DEngine.GetPhysicsSpace(),
00058                          cpCircleShapeNew(m_ptActualBaseBody,
00059                                           EPUCK_RADIUS,
00060                                           cpvzero));
00061       m_ptBaseShape->e = 0.0; // No elasticity
00062       m_ptBaseShape->u = 0.7; // Lots of friction
00063       /* Constrain the actual base body to follow the diff steering control */
00064       m_cDiffSteering.AttachTo(m_ptActualBaseBody);
00065       /* Calculate bounding box */
00066       GetBoundingBox().MinCorner.SetZ(GetEmbodiedEntity().GetPosition().GetZ());
00067       GetBoundingBox().MaxCorner.SetZ(GetEmbodiedEntity().GetPosition().GetZ() + EPUCK_HEIGHT);
00068       CalculateBoundingBox();
00069    }
00070 
00071    /****************************************/
00072    /****************************************/
00073 
00074    CDynamics2DEPuckModel::~CDynamics2DEPuckModel() {
00075       m_cDiffSteering.Detach();
00076       cpSpaceRemoveBody(m_cDyn2DEngine.GetPhysicsSpace(), m_ptActualBaseBody);
00077       cpSpaceRemoveShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseShape);
00078       cpShapeFree(m_ptBaseShape);
00079       cpBodyFree(m_ptActualBaseBody);
00080    }
00081 
00082    /****************************************/
00083    /****************************************/
00084 
00085    bool CDynamics2DEPuckModel::CheckIntersectionWithRay(Real& f_t_on_ray,
00086                                                            const CRay3& c_ray) const {
00087       cpSegmentQueryInfo tInfo;
00088       if(cpShapeSegmentQuery(m_ptBaseShape,
00089                              cpv(c_ray.GetStart().GetX(), c_ray.GetStart().GetY()),
00090                              cpv(c_ray.GetEnd().GetX()  , c_ray.GetEnd().GetY()  ),
00091                              &tInfo)) {
00092          CVector3 cIntersectionPoint;
00093          c_ray.GetPoint(cIntersectionPoint, tInfo.t);
00094          if((cIntersectionPoint.GetZ() >= GetEmbodiedEntity().GetPosition().GetZ()                 ) &&
00095                         (cIntersectionPoint.GetZ() <= GetEmbodiedEntity().GetPosition().GetZ() + EPUCK_HEIGHT) ) {
00096             f_t_on_ray = tInfo.t;
00097             return true;
00098          }
00099          else {
00100             return false;
00101          }
00102       }
00103       else {
00104          return false;
00105       }
00106    }
00107 
00108    /****************************************/
00109    /****************************************/
00110 
00111    bool CDynamics2DEPuckModel::MoveTo(const CVector3& c_position,
00112                                          const CQuaternion& c_orientation,
00113                                          bool b_check_only) {
00114       /* Save body position and orientation */
00115       cpVect tOldPos = m_ptActualBaseBody->p;
00116       cpFloat fOldA = m_ptActualBaseBody->a;
00117       /* Move the body to the desired position */
00118       m_ptActualBaseBody->p = cpv(c_position.GetX(), c_position.GetY());
00119       CRadians cXAngle, cYAngle, cZAngle;
00120       c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
00121       cpBodySetAngle(m_ptActualBaseBody, cZAngle.GetValue());
00122       /* Create a shape sensor to test the movement */
00123       cpShape* ptTestShape = cpCircleShapeNew(m_ptActualBaseBody,
00124                                               EPUCK_RADIUS,
00125                                               cpvzero);
00126       /* Check if there is a collision */
00127       SInt32 nCollision = cpSpaceShapeQuery(m_cDyn2DEngine.GetPhysicsSpace(), ptTestShape, NULL, NULL);
00128       /* Dispose of the sensor shape */
00129       cpShapeFree(ptTestShape);
00130       /* Should we keep this movement? */
00131       if(b_check_only || nCollision) {
00132          /*
00133           * No, because it was only a check or there was a collision
00134           * Restore old body state
00135           */
00136          m_ptActualBaseBody->p = tOldPos;
00137          cpBodySetAngle(m_ptActualBaseBody, fOldA);
00138       }
00139       else {
00140          /*
00141           * It wasn't a check and there were no collisions
00142           */
00143          /* Update the active space hash */
00144          cpSpaceReindexShape(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseShape);
00145          /* Update bounding box */
00146          CalculateBoundingBox();
00147       }
00148       /* The movement is allowed if there is no collision */
00149       return !nCollision;
00150    }
00151 
00152    /****************************************/
00153    /****************************************/
00154 
00155    void CDynamics2DEPuckModel::Reset() {
00156       /* Reset body position */
00157       const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition();
00158       m_ptActualBaseBody->p = cpv(cPosition.GetX(), cPosition.GetY());
00159       /* Reset body orientation */
00160       CRadians cXAngle, cYAngle, cZAngle;
00161       GetEmbodiedEntity().GetInitOrientation().ToEulerAngles(cZAngle, cYAngle, cXAngle);
00162       cpBodySetAngle(m_ptActualBaseBody, cZAngle.GetValue());
00163       /* Zero speed and applied forces of actual base body */
00164       m_ptActualBaseBody->v = cpvzero;
00165       m_ptActualBaseBody->w = 0.0f;
00166       cpBodyResetForces(m_ptActualBaseBody);
00167       /* Zero speed and applied forces of base control body */
00168       m_cDiffSteering.Reset();
00169       /* Update bounding box */
00170       cpShapeCacheBB(m_ptBaseShape);
00171       CalculateBoundingBox();
00172    }
00173 
00174    /****************************************/
00175    /****************************************/
00176 
00177    void CDynamics2DEPuckModel::CalculateBoundingBox() {
00178       GetBoundingBox().MinCorner.SetX(m_ptBaseShape->bb.l);
00179       GetBoundingBox().MinCorner.SetY(m_ptBaseShape->bb.b);
00180       GetBoundingBox().MaxCorner.SetX(m_ptBaseShape->bb.r);
00181       GetBoundingBox().MaxCorner.SetY(m_ptBaseShape->bb.t);
00182    }
00183 
00184    /****************************************/
00185    /****************************************/
00186 
00187    void CDynamics2DEPuckModel::UpdateEntityStatus() {
00188       /* Update bounding box */
00189       CalculateBoundingBox();
00190       /* Update e-puck body position */
00191       m_cDyn2DEngine.PositionPhysicsToSpace(m_cSpacePosition, GetEmbodiedEntity().GetPosition(), m_ptActualBaseBody);
00192       GetEmbodiedEntity().SetPosition(m_cSpacePosition);
00193       /* Update e-puck body orientation */
00194       m_cDyn2DEngine.OrientationPhysicsToSpace(m_cSpaceOrientation, m_ptActualBaseBody);
00195       GetEmbodiedEntity().SetOrientation(m_cSpaceOrientation);
00196       /* Update e-puck components */
00197       m_cEPuckEntity.UpdateComponents();
00198       /* Check whether a transfer is necessary */
00199       if(m_cDyn2DEngine.IsEntityTransferActive()) {
00200          std::string strEngineId;
00201          if(m_cDyn2DEngine.CalculateTransfer(GetEmbodiedEntity().GetPosition().GetX(),
00202                                         GetEmbodiedEntity().GetPosition().GetY(),
00203                                         strEngineId)) {
00204             m_cDyn2DEngine.ScheduleEntityForTransfer(m_cEPuckEntity, strEngineId);
00205          }
00206       }
00207    }
00208 
00209    /****************************************/
00210    /****************************************/
00211 
00212    void CDynamics2DEPuckModel::UpdateFromEntityStatus() {
00213       /* Do we want to move? */
00214       if((m_fCurrentWheelVelocity[EPUCK_LEFT_WHEEL] != 0.0f) ||
00215          (m_fCurrentWheelVelocity[EPUCK_RIGHT_WHEEL] != 0.0f)) {
00216          m_cDiffSteering.SetWheelVelocity(m_fCurrentWheelVelocity[EPUCK_LEFT_WHEEL],
00217                                           m_fCurrentWheelVelocity[EPUCK_RIGHT_WHEEL]);
00218       }
00219       else {
00220          /* No, we don't want to move - zero all speeds */
00221          m_cDiffSteering.Reset();
00222       }
00223    }
00224 
00225    /****************************************/
00226    /****************************************/
00227 
00228    bool CDynamics2DEPuckModel::IsCollidingWithSomething() const {
00229       return cpSpaceShapeQuery(m_cDyn2DEngine.GetPhysicsSpace(), m_ptBaseShape, NULL, NULL) > 0;
00230    }
00231 
00232    /****************************************/
00233    /****************************************/
00234 
00235    REGISTER_STANDARD_DYNAMICS2D_OPERATIONS_ON_ENTITY(CEPuckEntity, CDynamics2DEPuckModel);
00236 
00237    /****************************************/
00238    /****************************************/
00239 
00240 }