ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00011 #include <argos3/core/utility/string_utilities.h> 00012 #include <argos3/core/utility/math/range.h> 00013 #include <argos3/core/utility/logging/argos_log.h> 00014 #include <argos3/core/utility/math/rng.h> 00015 #include <argos3/core/simulator/simulator.h> 00016 #include <argos3/core/simulator/entity/composable_entity.h> 00017 #include <cstring> 00018 #include "space.h" 00019 00020 namespace argos { 00021 00022 /****************************************/ 00023 /****************************************/ 00024 00025 CSpace::CSpace() : 00026 m_unSimulationClock(0), 00027 m_pcFloorEntity(NULL), 00028 m_ptPhysicsEngines(NULL), 00029 m_ptMedia(NULL) {} 00030 00031 /****************************************/ 00032 /****************************************/ 00033 00034 void CSpace::Init(TConfigurationNode& t_tree) { 00035 /* Get reference to physics engine and media vectors */ 00036 m_ptPhysicsEngines = &(CSimulator::GetInstance().GetPhysicsEngines()); 00037 m_ptMedia = &(CSimulator::GetInstance().GetMedia()); 00038 /* Get the arena center and size */ 00039 GetNodeAttributeOrDefault(t_tree, "center", m_cArenaCenter, m_cArenaCenter); 00040 GetNodeAttribute(t_tree, "size", m_cArenaSize); 00041 /* 00042 * Add and initialize all entities in XML 00043 */ 00044 /* Start from the entities placed manually */ 00045 TConfigurationNodeIterator itArenaItem; 00046 for(itArenaItem = itArenaItem.begin(&t_tree); 00047 itArenaItem != itArenaItem.end(); 00048 ++itArenaItem) { 00049 if(itArenaItem->Value() != "distribute") { 00050 CEntity* pcEntity = CFactory<CEntity>::New(itArenaItem->Value()); 00051 pcEntity->Init(*itArenaItem); 00052 CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*this, *pcEntity); 00053 } 00054 } 00055 /* Place the entities to distribute automatically */ 00056 for(itArenaItem = itArenaItem.begin(&t_tree); 00057 itArenaItem != itArenaItem.end(); 00058 ++itArenaItem) { 00059 if(itArenaItem->Value() == "distribute") { 00060 Distribute(*itArenaItem); 00061 } 00062 } 00063 } 00064 00065 /****************************************/ 00066 /****************************************/ 00067 00068 void CSpace::Reset() { 00069 /* Reset the simulation clock */ 00070 m_unSimulationClock = 0; 00071 /* Reset the entities */ 00072 for(UInt32 i = 0; i < m_vecEntities.size(); ++i) { 00073 m_vecEntities[i]->Reset(); 00074 } 00075 } 00076 00077 /****************************************/ 00078 /****************************************/ 00079 00080 void CSpace::Destroy() { 00081 /* Remove all entities */ 00082 while(!m_vecRootEntities.empty()) { 00083 CallEntityOperation<CSpaceOperationRemoveEntity, CSpace, void>(*this, *m_vecRootEntities.back()); 00084 } 00085 } 00086 00087 /****************************************/ 00088 /****************************************/ 00089 00090 void CSpace::GetEntitiesMatching(CEntity::TVector& t_buffer, 00091 const std::string& str_pattern) { 00092 for(CEntity::TVector::iterator it = m_vecEntities.begin(); 00093 it != m_vecEntities.end(); ++it) { 00094 if(MatchPattern((*it)->GetId(), str_pattern)) { 00095 t_buffer.push_back(*it); 00096 } 00097 } 00098 } 00099 00100 /****************************************/ 00101 /****************************************/ 00102 00103 CSpace::TMapPerType& CSpace::GetEntitiesByType(const std::string& str_type) { 00104 TMapPerTypePerId::iterator itEntities = m_mapEntitiesPerTypePerId.find(str_type); 00105 if (itEntities != m_mapEntitiesPerTypePerId.end()){ 00106 return itEntities->second; 00107 } 00108 else { 00109 THROW_ARGOSEXCEPTION("Entity map for type \"" << str_type << "\" not found."); 00110 } 00111 } 00112 00113 /****************************************/ 00114 /****************************************/ 00115 00116 void CSpace::Update() { 00117 /* Update the controllable entities */ 00118 UpdateControllableEntities(); 00119 /* Update the physics engines */ 00120 UpdatePhysics(); 00121 /* Update media */ 00122 UpdateMedia(); 00123 } 00124 00125 /****************************************/ 00126 /****************************************/ 00127 00128 void CSpace::AddControllableEntity(CControllableEntity& c_entity) { 00129 m_vecControllableEntities.push_back(&c_entity); 00130 } 00131 00132 /****************************************/ 00133 /****************************************/ 00134 00135 void CSpace::RemoveControllableEntity(CControllableEntity& c_entity) { 00136 CControllableEntity::TVector::iterator it = find(m_vecControllableEntities.begin(), 00137 m_vecControllableEntities.end(), 00138 &c_entity); 00139 if(it != m_vecControllableEntities.end()) { 00140 m_vecControllableEntities.erase(it); 00141 } 00142 } 00143 00144 /****************************************/ 00145 /****************************************/ 00146 00147 void CSpace::AddEntityToPhysicsEngine(CEmbodiedEntity& c_entity) { 00148 /* Get a reference to the root entity */ 00149 CEntity* pcToAdd = &c_entity; 00150 while(pcToAdd->HasParent()) { 00151 pcToAdd = &pcToAdd->GetParent(); 00152 } 00153 /* Get a reference to the position of the entity */ 00154 const CVector3& cPos = c_entity.GetPosition(); 00155 /* Go through engines and check which ones could house the entity */ 00156 CPhysicsEngine::TVector vecPotentialEngines; 00157 for(size_t i = 0; i < m_ptPhysicsEngines->size(); ++i) { 00158 if((*m_ptPhysicsEngines)[i]->IsPointContained(cPos)) { 00159 vecPotentialEngines.push_back((*m_ptPhysicsEngines)[i]); 00160 } 00161 } 00162 /* If no engine can house the entity, bomb out */ 00163 if(vecPotentialEngines.empty()) { 00164 THROW_ARGOSEXCEPTION("No physics engine can house entity \"" << pcToAdd->GetId() << "\"."); 00165 } 00166 /* If the entity is not movable, add the entity to all the matching engines */ 00167 if(! c_entity.IsMovable()) { 00168 for(size_t i = 0; i < vecPotentialEngines.size(); ++i) { 00169 vecPotentialEngines[i]->AddEntity(*pcToAdd); 00170 } 00171 } 00172 /* If the entity is movable, only one engine can be associated to the embodied entity */ 00173 else if(vecPotentialEngines.size() == 1) { 00174 /* Only one engine matches, bingo! */ 00175 vecPotentialEngines[0]->AddEntity(*pcToAdd); 00176 } 00177 else { 00178 /* More than one engine matches, bomb out */ 00179 std::ostringstream ossEngines; 00180 ossEngines << "\"" << vecPotentialEngines[0]->GetId() << "\""; 00181 for(size_t i = 1; i < vecPotentialEngines.size(); ++i) { 00182 ossEngines << ", \"" << vecPotentialEngines[i]->GetId() << "\""; 00183 } 00184 THROW_ARGOSEXCEPTION("Multiple engines can house \"" << c_entity.GetId() << "\", but a movable entity and can only be added to a single engine. Conflicting engines: " << ossEngines); 00185 } 00186 } 00187 00188 /****************************************/ 00189 /****************************************/ 00190 00191 class RealNumberGenerator { 00192 public: 00193 virtual ~RealNumberGenerator() {} 00194 virtual CVector3 operator()(bool b_is_retry) = 0; 00195 }; 00196 00197 class ConstantGenerator : public RealNumberGenerator { 00198 public: 00199 ConstantGenerator(const CVector3& c_value) : 00200 m_cValue(c_value) {} 00201 00202 inline virtual CVector3 operator()(bool b_is_retry) { 00203 return m_cValue; 00204 } 00205 private: 00206 CVector3 m_cValue; 00207 00208 }; 00209 00210 class UniformGenerator : public RealNumberGenerator { 00211 public: 00212 UniformGenerator(const CVector3& c_min, 00213 const CVector3& c_max) : 00214 m_cMin(c_min), 00215 m_cMax(c_max) {} 00216 inline virtual CVector3 operator()(bool b_is_retry) { 00217 Real fRandX = 00218 m_cMax.GetX() > m_cMin.GetX() ? 00219 CSimulator::GetInstance().GetRNG()->Uniform(CRange<Real>(m_cMin.GetX(), m_cMax.GetX())) : 00220 m_cMax.GetX(); 00221 Real fRandY = 00222 m_cMax.GetY() > m_cMin.GetY() ? 00223 CSimulator::GetInstance().GetRNG()->Uniform(CRange<Real>(m_cMin.GetY(), m_cMax.GetY())) : 00224 m_cMax.GetY(); 00225 Real fRandZ = 00226 m_cMax.GetZ() > m_cMin.GetZ() ? 00227 CSimulator::GetInstance().GetRNG()->Uniform(CRange<Real>(m_cMin.GetZ(), m_cMax.GetZ())) : 00228 m_cMax.GetZ(); 00229 return CVector3(fRandX, fRandY, fRandZ); 00230 } 00231 private: 00232 CVector3 m_cMin; 00233 CVector3 m_cMax; 00234 }; 00235 00236 class GaussianGenerator : public RealNumberGenerator { 00237 public: 00238 GaussianGenerator(const CVector3& c_mean, 00239 const CVector3& c_std_dev) : 00240 m_cMean(c_mean), 00241 m_cStdDev(c_std_dev) {} 00242 inline virtual CVector3 operator()(bool b_is_retry) { 00243 return CVector3(CSimulator::GetInstance().GetRNG()->Gaussian(m_cStdDev.GetX(), m_cMean.GetX()), 00244 CSimulator::GetInstance().GetRNG()->Gaussian(m_cStdDev.GetY(), m_cMean.GetY()), 00245 CSimulator::GetInstance().GetRNG()->Gaussian(m_cStdDev.GetZ(), m_cMean.GetZ())); 00246 } 00247 private: 00248 CVector3 m_cMean; 00249 CVector3 m_cStdDev; 00250 }; 00251 00252 class GridGenerator : public RealNumberGenerator { 00253 public: 00254 GridGenerator(const CVector3 c_center, 00255 const UInt32 un_layout[], 00256 const CVector3 c_distances): 00257 m_cCenter(c_center), 00258 m_cDistances(c_distances), 00259 m_unNumEntityPlaced(0) { 00260 m_unLayout[0] = un_layout[0]; 00261 m_unLayout[1] = un_layout[1]; 00262 m_unLayout[2] = un_layout[2]; 00263 /* Check if layout is sane */ 00264 if( m_unLayout[0] == 0 || m_unLayout[1] == 0 || m_unLayout[2] == 0 ) { 00265 THROW_ARGOSEXCEPTION("'layout' values (distribute position, method 'grid') must all be different than 0"); 00266 } 00267 } 00268 00269 virtual CVector3 operator()(bool b_is_retry) { 00270 if(b_is_retry) { 00271 THROW_ARGOSEXCEPTION("Impossible to place entity #" << m_unNumEntityPlaced << " in grid"); 00272 } 00273 CVector3 cReturn; 00274 if(m_unNumEntityPlaced < m_unLayout[0] * m_unLayout[1] * m_unLayout[2]) { 00275 cReturn.SetX(m_cCenter.GetX() + ( m_unLayout[0] - 1 ) * m_cDistances.GetX() * 0.5 - ( m_unNumEntityPlaced % m_unLayout[0] ) * m_cDistances.GetX()); 00276 cReturn.SetY(m_cCenter.GetY() + ( m_unLayout[1] - 1 ) * m_cDistances.GetY() * 0.5 - ( m_unNumEntityPlaced / m_unLayout[0] ) % m_unLayout[1] * m_cDistances.GetY()); 00277 cReturn.SetZ(m_cCenter.GetZ() + ( m_unLayout[2] - 1 ) * m_cDistances.GetZ() * 0.5 - ( m_unNumEntityPlaced / ( m_unLayout[0] * m_unLayout[1] ) ) * m_cDistances.GetZ()); 00278 ++m_unNumEntityPlaced; 00279 } 00280 else { 00281 THROW_ARGOSEXCEPTION("Distribute position, method 'grid': trying to place more entities than allowed " 00282 "by the 'layout', check your 'quantity' tag"); 00283 } 00284 return cReturn; 00285 } 00286 00287 private: 00288 CVector3 m_cCenter; 00289 UInt32 m_unLayout[3]; 00290 CVector3 m_cDistances; 00291 UInt32 m_unNumEntityPlaced; 00292 }; 00293 00294 /****************************************/ 00295 /****************************************/ 00296 00297 RealNumberGenerator* CreateGenerator(TConfigurationNode& t_tree) { 00298 std::string strMethod; 00299 GetNodeAttribute(t_tree, "method", strMethod); 00300 if(strMethod == "uniform") { 00301 CVector3 cMin, cMax; 00302 GetNodeAttribute(t_tree, "min", cMin); 00303 GetNodeAttribute(t_tree, "max", cMax); 00304 if(! (cMin <= cMax)) { 00305 THROW_ARGOSEXCEPTION("Uniform generator: the min is not less than or equal to max: " << cMin << " / " << cMax); 00306 } 00307 return new UniformGenerator(cMin, cMax); 00308 } 00309 else if(strMethod == "gaussian") { 00310 CVector3 cMean, cStdDev; 00311 GetNodeAttribute(t_tree, "mean", cMean); 00312 GetNodeAttribute(t_tree, "std_dev", cStdDev); 00313 return new GaussianGenerator(cMean, cStdDev); 00314 } 00315 else if(strMethod == "constant") { 00316 CVector3 cValues; 00317 GetNodeAttribute(t_tree, "values", cValues); 00318 return new ConstantGenerator(cValues); 00319 } 00320 else if(strMethod == "grid") { 00321 CVector3 cCenter,cDistances; 00322 GetNodeAttribute(t_tree, "center", cCenter); 00323 GetNodeAttribute(t_tree, "distances", cDistances); 00324 UInt32 unLayout[3]; 00325 std::string strLayout; 00326 GetNodeAttribute(t_tree, "layout", strLayout); 00327 ParseValues<UInt32> (strLayout, 3, unLayout, ','); 00328 return new GridGenerator(cCenter, unLayout, cDistances); 00329 } 00330 else { 00331 THROW_ARGOSEXCEPTION("Unknown distribution method \"" << strMethod << "\""); 00332 } 00333 } 00334 00335 /****************************************/ 00336 /****************************************/ 00337 00338 static CEmbodiedEntity* GetEmbodiedEntity(CEntity* pc_entity) { 00339 /* Is the entity embodied itself? */ 00340 CEmbodiedEntity* pcEmbodiedTest = dynamic_cast<CEmbodiedEntity*>(pc_entity); 00341 if(pcEmbodiedTest != NULL) { 00342 return pcEmbodiedTest; 00343 } 00344 /* Is the entity composable with an embodied component? */ 00345 CComposableEntity* pcComposableTest = dynamic_cast<CComposableEntity*>(pc_entity); 00346 if(pcComposableTest != NULL) { 00347 if(pcComposableTest->HasComponent("body")) { 00348 return &(pcComposableTest->GetComponent<CEmbodiedEntity>("body")); 00349 } 00350 } 00351 /* No embodied entity found */ 00352 return NULL; 00353 } 00354 00355 /****************************************/ 00356 /****************************************/ 00357 00358 static CPositionalEntity* GetPositionalEntity(CEntity* pc_entity) { 00359 /* Is the entity positional itself? */ 00360 CPositionalEntity* pcPositionalTest = dynamic_cast<CPositionalEntity*>(pc_entity); 00361 if(pcPositionalTest != NULL) { 00362 return pcPositionalTest; 00363 } 00364 /* Is the entity composable with a positional component? */ 00365 CComposableEntity* pcComposableTest = dynamic_cast<CComposableEntity*>(pc_entity); 00366 if(pcComposableTest != NULL) { 00367 if(pcComposableTest->HasComponent("position")) { 00368 return &(pcComposableTest->GetComponent<CPositionalEntity>("position")); 00369 } 00370 } 00371 /* No positional entity found */ 00372 return NULL; 00373 } 00374 00375 /****************************************/ 00376 /****************************************/ 00377 00378 void CSpace::Distribute(TConfigurationNode& t_tree) { 00379 try { 00380 /* Get the needed nodes */ 00381 TConfigurationNode cPositionNode; 00382 cPositionNode = GetNode(t_tree, "position"); 00383 TConfigurationNode cOrientationNode; 00384 cOrientationNode = GetNode(t_tree, "orientation"); 00385 TConfigurationNode cEntityNode; 00386 cEntityNode = GetNode(t_tree, "entity"); 00387 /* Create the real number generators */ 00388 RealNumberGenerator* pcPositionGenerator = CreateGenerator(cPositionNode); 00389 RealNumberGenerator* pcOrientationGenerator = CreateGenerator(cOrientationNode); 00390 /* How many entities? */ 00391 UInt32 unQuantity; 00392 GetNodeAttribute(cEntityNode, "quantity", unQuantity); 00393 /* How many trials before failing? */ 00394 UInt32 unMaxTrials; 00395 GetNodeAttribute(cEntityNode, "max_trials", unMaxTrials); 00396 /* Get the (optional) entity base numbering */ 00397 UInt64 unBaseNum = 0; 00398 GetNodeAttributeOrDefault(cEntityNode, "base_num", unBaseNum, unBaseNum); 00399 /* Get the entity type to add (take only the first, ignore additional if any) */ 00400 TConfigurationNodeIterator itEntity; 00401 itEntity = itEntity.begin(&cEntityNode); 00402 if(itEntity == itEntity.end()) { 00403 THROW_ARGOSEXCEPTION("No entity to distribute specified."); 00404 } 00405 /* Get the entity base ID */ 00406 std::string strBaseId; 00407 GetNodeAttribute(*itEntity, "id", strBaseId); 00408 /* Add the requested entities */ 00409 for(UInt32 i = 0; i < unQuantity; ++i) { 00410 /* Copy the entity XML tree */ 00411 TConfigurationNode tEntityTree = *itEntity; 00412 /* Set progressive ID */ 00413 SetNodeAttribute(tEntityTree, "id", strBaseId + ToString(i+unBaseNum)); 00414 /* Go on until the entity is placed with no collisions or 00415 the max number of trials has been exceeded */ 00416 UInt32 unTrials = 0; 00417 bool bDone = false; 00418 bool bRetry = false; 00419 CEntity* pcEntity; 00420 do { 00421 /* Create entity */ 00422 pcEntity = CFactory<CEntity>::New(tEntityTree.Value()); 00423 /* If the tree does not have a 'body' node, create a new one */ 00424 if(!NodeExists(tEntityTree, "body")) { 00425 TConfigurationNode tBodyNode("body"); 00426 AddChildNode(tEntityTree, tBodyNode); 00427 } 00428 /* Get 'body' node */ 00429 TConfigurationNode& tBodyNode = GetNode(tEntityTree, "body"); 00430 /* Set the position */ 00431 SetNodeAttribute(tBodyNode, "position", (*pcPositionGenerator)(bRetry)); 00432 /* Set the orientation */ 00433 SetNodeAttribute(tBodyNode, "orientation", (*pcOrientationGenerator)(bRetry)); 00434 /* Init the entity (this also creates the components, if pcEntity is a composable) */ 00435 pcEntity->Init(tEntityTree); 00436 /* 00437 * Now that you have the entity and its components, check whether the entity is positional or embodied 00438 * or has one such component. 00439 * In case the entity is positional but not embodied, there's no need to check for collisions 00440 * In case the entity is embodied, we must check for collisions 00441 * To check for collisions, we add the entity in the place where it's supposed to be, 00442 * then we ask the engine if that entity is colliding with something 00443 * In case of collision, we remove the entity and try a different position/orientation 00444 */ 00445 /* Check for embodied */ 00446 CEmbodiedEntity* pcEmbodiedEntity = GetEmbodiedEntity(pcEntity); 00447 if(pcEmbodiedEntity == NULL) { 00448 /* Check failed, then check for positional */ 00449 CPositionalEntity* pcPositionalEntity = GetPositionalEntity(pcEntity); 00450 if(pcPositionalEntity == NULL) { 00451 THROW_ARGOSEXCEPTION("Cannot distribute entities that are not positional nor embodied, and \"" << tEntityTree.Value() << "\" is neither."); 00452 } 00453 else { 00454 /* Wherever we want to put the entity, it's OK, add it */ 00455 CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*this, *pcEntity); 00456 } 00457 } 00458 else { 00459 /* The entity is embodied */ 00460 /* Add it to the space and to the designated physics engine */ 00461 CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*this, *pcEntity); 00462 /* Check if it's colliding with anything else */ 00463 if(pcEmbodiedEntity->IsCollidingWithSomething()) { 00464 /* Set retry to true */ 00465 bRetry = true; 00466 /* Get rid of the entity */ 00467 CallEntityOperation<CSpaceOperationRemoveEntity, CSpace, void>(*this, *pcEntity); 00468 /* Increase the trial count */ 00469 ++unTrials; 00470 /* Too many trials? */ 00471 if(unTrials > unMaxTrials) { 00472 /* Yes, bomb out */ 00473 THROW_ARGOSEXCEPTION("Exceeded max trials when trying to distribute objects of type " << 00474 tEntityTree.Value() << " with base id \"" << 00475 strBaseId << "\". I managed to place only " << i << " objects."); 00476 } 00477 /* Retry with a new position */ 00478 } 00479 else { 00480 /* No collision, we're done with this entity */ 00481 bDone = true; 00482 } 00483 } 00484 } 00485 while(!bDone); 00486 } 00487 /* Delete the generators, now unneeded */ 00488 delete pcPositionGenerator; 00489 delete pcOrientationGenerator; 00490 } 00491 catch(CARGoSException& ex) { 00492 THROW_ARGOSEXCEPTION_NESTED("Error while trying to distribute entities", ex); 00493 } 00494 } 00495 00496 /****************************************/ 00497 /****************************************/ 00498 00499 }