ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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 }