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