ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "dynamics2d_engine.h" 00008 #include "dynamics2d_model.h" 00009 #include "dynamics2d_gripping.h" 00010 00011 #include <argos3/core/simulator/simulator.h> 00012 00013 #include <cmath> 00014 00015 namespace argos { 00016 00017 /****************************************/ 00018 /****************************************/ 00019 00020 CDynamics2DEngine::CDynamics2DEngine() : 00021 m_fStaticHashCellSize(0.1f), 00022 m_fActiveHashCellSize(2.0f * 0.085036758f), 00023 m_nStaticHashCells(1000), 00024 m_nActiveHashCells(1000), 00025 m_ptSpace(NULL), 00026 m_ptGroundBody(NULL), 00027 m_fElevation(0.0f), 00028 m_bEntityTransferActive(false) { 00029 } 00030 00031 /****************************************/ 00032 /****************************************/ 00033 00034 void CDynamics2DEngine::Init(TConfigurationNode& t_tree) { 00035 /* Init parent */ 00036 CPhysicsEngine::Init(t_tree); 00037 /* Parse XML */ 00038 GetNodeAttributeOrDefault(t_tree, "static_cell_size", m_fStaticHashCellSize, m_fStaticHashCellSize); 00039 GetNodeAttributeOrDefault(t_tree, "active_cell_size", m_fActiveHashCellSize, m_fActiveHashCellSize); 00040 GetNodeAttributeOrDefault(t_tree, "static_cells", m_nStaticHashCells, m_nStaticHashCells); 00041 GetNodeAttributeOrDefault(t_tree, "active_cells", m_nActiveHashCells, m_nActiveHashCells); 00042 GetNodeAttributeOrDefault(t_tree, "elevation", m_fElevation, m_fElevation); 00043 if(NodeExists(t_tree, "boundaries")) { 00044 /* Parse the boundary definition */ 00045 TConfigurationNode& tBoundaries = GetNode(t_tree, "boundaries"); 00046 SBoundarySegment sBoundSegment; 00047 CVector2 cLastPoint, cCurPoint; 00048 std::string strConnectWith; 00049 TConfigurationNodeIterator tVertexIt("vertex"); 00050 /* Get the first vertex */ 00051 tVertexIt = tVertexIt.begin(&tBoundaries); 00052 if(tVertexIt == tVertexIt.end()) { 00053 THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": you didn't specify any <vertex>!"); 00054 } 00055 GetNodeAttribute(*tVertexIt, "point", cLastPoint); 00056 m_vecVertices.push_back(cLastPoint); 00057 /* Go through the other vertices */ 00058 ++tVertexIt; 00059 while(tVertexIt != tVertexIt.end()) { 00060 /* Read vertex data and fill in segment struct */ 00061 GetNodeAttribute(*tVertexIt, "point", cCurPoint); 00062 m_vecVertices.push_back(cCurPoint); 00063 sBoundSegment.Segment.SetStart(cLastPoint); 00064 sBoundSegment.Segment.SetEnd(cCurPoint); 00065 GetNodeAttribute(*tVertexIt, "connect_with", strConnectWith); 00066 if(strConnectWith == "gate") { 00067 /* Connect to previous vertex with a gate */ 00068 sBoundSegment.Type = SBoundarySegment::SEGMENT_TYPE_GATE; 00069 GetNodeAttribute(*tVertexIt, "to_engine", sBoundSegment.EngineId); 00070 } 00071 else if(strConnectWith == "wall") { 00072 /* Connect to previous vertex with a wall */ 00073 sBoundSegment.Type = SBoundarySegment::SEGMENT_TYPE_WALL; 00074 sBoundSegment.EngineId = ""; 00075 } 00076 else { 00077 /* Parse error */ 00078 THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": unknown vertex connection method \"" << strConnectWith << "\". Allowed methods are \"wall\" and \"gate\"."); 00079 } 00080 m_vecSegments.push_back(sBoundSegment); 00081 /* Next vertex */ 00082 cLastPoint = cCurPoint; 00083 ++tVertexIt; 00084 } 00085 /* Check that the boundary is a closed path */ 00086 if(m_vecVertices.front() != m_vecVertices.back()) { 00087 THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": the specified path is not closed. The first and last points of the boundaries MUST be the same."); 00088 } 00089 } 00090 /* Initialize physics */ 00091 cpInitChipmunk(); 00092 cpResetShapeIdCounter(); 00093 /* Used to attach static geometries so that they won't move and to simulate friction */ 00094 m_ptGroundBody = cpBodyNew(INFINITY, INFINITY); 00095 /* Create the space to contain the movable objects */ 00096 m_ptSpace = cpSpaceNew(); 00097 /* Subiterations to solve constraints. 00098 The more, the better for precision but the worse for speed 00099 */ 00100 m_ptSpace->iterations = GetIterations(); 00101 /* Resize the space hash. 00102 This has dramatic effects on performance. 00103 TODO: - find optimal parameters automatically (average entity size) 00104 cpSpaceReindexStaticHash(m_ptSpace, m_fStaticHashCellSize, m_nStaticHashCells); 00105 cpSpaceResizeActiveHash(m_ptSpace, m_fActiveHashCellSize, m_nActiveHashCells); 00106 */ 00107 /* Gripper-Gripped callback functions */ 00108 cpSpaceAddCollisionHandler( 00109 m_ptSpace, 00110 SHAPE_GRIPPER, 00111 SHAPE_GRIPPABLE, 00112 BeginCollisionBetweenGripperAndGrippable, 00113 ManageCollisionBetweenGripperAndGrippable, 00114 NULL, 00115 NULL, 00116 NULL); 00117 /* Add boundaries, if specified */ 00118 if(! m_vecSegments.empty()) { 00119 cpShape* ptSegment; 00120 for(size_t i = 0; i < m_vecSegments.size(); ++i) { 00121 if(m_vecSegments[i].Type == SBoundarySegment::SEGMENT_TYPE_WALL) { 00122 ptSegment = 00123 cpSpaceAddShape( 00124 m_ptSpace, 00125 cpSegmentShapeNew( 00126 m_ptGroundBody, 00127 cpv(m_vecSegments[i].Segment.GetStart().GetX(), 00128 m_vecSegments[i].Segment.GetStart().GetY()), 00129 cpv(m_vecSegments[i].Segment.GetEnd().GetX(), 00130 m_vecSegments[i].Segment.GetEnd().GetY()), 00131 0.0f)); 00132 ptSegment->e = 0.0f; // no elasticity 00133 ptSegment->u = 1.0f; // max friction 00134 } 00135 else { 00136 /* There is at least a gate, transfer is activated */ 00137 m_bEntityTransferActive = true; 00138 } 00139 } 00140 } 00141 } 00142 00143 /****************************************/ 00144 /****************************************/ 00145 00146 void CDynamics2DEngine::Reset() { 00147 for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin(); 00148 it != m_tPhysicsModels.end(); ++it) { 00149 it->second->Reset(); 00150 } 00151 cpSpaceReindexStatic(m_ptSpace); 00152 } 00153 00154 /****************************************/ 00155 /****************************************/ 00156 00157 void CDynamics2DEngine::Update() { 00158 /* Update the physics state from the entities */ 00159 for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin(); 00160 it != m_tPhysicsModels.end(); ++it) { 00161 it->second->UpdateFromEntityStatus(); 00162 } 00163 /* Perform the step */ 00164 cpSpaceStep(m_ptSpace, GetSimulationClockTick()); 00165 /* Update the simulated space */ 00166 for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin(); 00167 it != m_tPhysicsModels.end(); ++it) { 00168 it->second->UpdateEntityStatus(); 00169 } 00170 } 00171 00172 /****************************************/ 00173 /****************************************/ 00174 00175 void CDynamics2DEngine::Destroy() { 00176 /* Empty the physics model map */ 00177 for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin(); 00178 it != m_tPhysicsModels.end(); ++it) { 00179 delete it->second; 00180 } 00181 m_tPhysicsModels.clear(); 00182 /* Get rid of the physics space */ 00183 cpSpaceFree(m_ptSpace); 00184 cpBodyFree(m_ptGroundBody); 00185 } 00186 00187 /****************************************/ 00188 /****************************************/ 00189 00190 bool CDynamics2DEngine::IsPointContained(const CVector3& c_point) { 00191 /* Check that Z-coordinate is within elevation */ 00192 if(c_point.GetZ() > m_fElevation || c_point.GetZ() < m_fElevation) { 00193 return false; 00194 } 00195 if(! IsEntityTransferActive()) { 00196 /* The engine has no boundaries on XY, so the wanted point is in for sure */ 00197 return true; 00198 } 00199 else { 00200 /* Check the boundaries */ 00201 for(size_t i = 0; i < m_vecSegments.size(); ++i) { 00202 const CVector2& cP0 = m_vecSegments[i].Segment.GetStart(); 00203 const CVector2& cP1 = m_vecSegments[i].Segment.GetEnd(); 00204 Real fCriterion = 00205 (c_point.GetY() - cP0.GetY()) * (cP1.GetX() - cP0.GetX()) - 00206 (c_point.GetX() - cP0.GetX()) * (cP1.GetY() - cP0.GetY()); 00207 if(fCriterion > 0.0f) { 00208 return false; 00209 } 00210 } 00211 return true; 00212 } 00213 } 00214 00215 /****************************************/ 00216 /****************************************/ 00217 00218 UInt32 CDynamics2DEngine::GetNumPhysicsEngineEntities() { 00219 return m_tPhysicsModels.size(); 00220 } 00221 00222 /****************************************/ 00223 /****************************************/ 00224 00225 void CDynamics2DEngine::AddEntity(CEntity& c_entity) { 00226 CallEntityOperation<CDynamics2DOperationAddEntity, CDynamics2DEngine, void>(*this, c_entity); 00227 cpResetShapeIdCounter(); 00228 } 00229 00230 /****************************************/ 00231 /****************************************/ 00232 00233 void CDynamics2DEngine::RemoveEntity(CEntity& c_entity) { 00234 CallEntityOperation<CDynamics2DOperationRemoveEntity, CDynamics2DEngine, void>(*this, c_entity); 00235 } 00236 00237 /****************************************/ 00238 /****************************************/ 00239 00240 void CDynamics2DEngine::TransferEntities() { 00241 for(size_t i = 0; i < m_vecTransferData.size(); ++i) { 00242 CPhysicsEngine& cToEngine = CSimulator::GetInstance().GetPhysicsEngine(m_vecTransferData[i].EngineId); 00243 cToEngine.AddEntity(*m_vecTransferData[i].Entity); 00244 RemoveEntity(*m_vecTransferData[i].Entity); 00245 } 00246 m_vecTransferData.clear(); 00247 } 00248 00249 /****************************************/ 00250 /****************************************/ 00251 00252 CEmbodiedEntity* CDynamics2DEngine::CheckIntersectionWithRay(Real& f_t_on_ray, 00253 const CRay3& c_ray) const { 00254 /* Structure to store the query data */ 00255 cpSegmentQueryInfo tInfo; 00256 /* Query the first hit along the ray */ 00257 cpShape* ptHit = cpSpaceSegmentQueryFirst( 00258 m_ptSpace, 00259 cpv(c_ray.GetStart().GetX(), c_ray.GetStart().GetY()), 00260 cpv(c_ray.GetEnd().GetX() , c_ray.GetEnd().GetY() ), 00261 CP_ALL_LAYERS, 00262 CP_NO_GROUP, 00263 &tInfo); 00264 /* Check whether we have a hit */ 00265 if(ptHit) { 00266 /* Hit found, is it within the limits? */ 00267 CDynamics2DModel& cModel = *reinterpret_cast<CDynamics2DModel*>(ptHit->body->data); 00268 CVector3 cIntersectionPoint; 00269 c_ray.GetPoint(cIntersectionPoint, tInfo.t); 00270 if((cIntersectionPoint.GetZ() >= cModel.GetBoundingBox().MinCorner.GetZ()) && 00271 (cIntersectionPoint.GetZ() <= cModel.GetBoundingBox().MaxCorner.GetZ()) ) { 00272 /* Yes, a real hit */ 00273 f_t_on_ray = tInfo.t; 00274 return &cModel.GetEmbodiedEntity(); 00275 } 00276 else { 00277 return NULL; 00278 } 00279 } 00280 else { 00281 /* Hit not found */ 00282 return NULL; 00283 } 00284 } 00285 00286 /****************************************/ 00287 /****************************************/ 00288 00289 bool CDynamics2DEngine::CalculateTransfer(Real f_x, Real f_y, 00290 std::string& str_engine_id) { 00291 /* 00292 * TODO: this method makes the assumption that only one gate is trespassed at any time. 00293 * This assumption may be false in some ill-shaped polygons or when the gate isn't just a 00294 * segment, but is a sequence of segments. 00295 */ 00296 for(size_t i = 0; i < m_vecSegments.size(); ++i) { 00297 if(m_vecSegments[i].Type == SBoundarySegment::SEGMENT_TYPE_GATE) { 00298 const CVector2& cP0 = m_vecSegments[i].Segment.GetStart(); 00299 const CVector2& cP1 = m_vecSegments[i].Segment.GetEnd(); 00300 Real fCriterion = 00301 (f_y - cP0.GetY()) * (cP1.GetX() - cP0.GetX()) - 00302 (f_x - cP0.GetX()) * (cP1.GetY() - cP0.GetY()); 00303 if(fCriterion < 0.0f) { 00304 str_engine_id = m_vecSegments[i].EngineId; 00305 return true; 00306 } 00307 } 00308 } 00309 return false; 00310 } 00311 00312 /****************************************/ 00313 /****************************************/ 00314 00315 void CDynamics2DEngine::PositionPhysicsToSpace(CVector3& c_new_pos, 00316 const CVector3& c_original_pos, 00317 const cpBody* pt_body) { 00318 c_new_pos.SetX(pt_body->p.x); 00319 c_new_pos.SetY(pt_body->p.y); 00320 c_new_pos.SetZ(c_original_pos.GetZ()); 00321 } 00322 00323 /****************************************/ 00324 /****************************************/ 00325 00326 void CDynamics2DEngine::OrientationPhysicsToSpace(CQuaternion& c_new_orient, 00327 cpBody* pt_body) { 00328 c_new_orient.FromAngleAxis(CRadians(pt_body->a), CVector3::Z); 00329 } 00330 00331 /****************************************/ 00332 /****************************************/ 00333 00334 void CDynamics2DEngine::AddPhysicsModel(const std::string& str_id, 00335 CDynamics2DModel& c_model) { 00336 m_tPhysicsModels[str_id] = &c_model; 00337 } 00338 00339 /****************************************/ 00340 /****************************************/ 00341 00342 void CDynamics2DEngine::RemovePhysicsModel(const std::string& str_id) { 00343 CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.find(str_id); 00344 if(it != m_tPhysicsModels.end()) { 00345 delete it->second; 00346 m_tPhysicsModels.erase(it); 00347 } 00348 else { 00349 THROW_ARGOSEXCEPTION("Dynamics2D model id \"" << str_id << "\" not found in dynamics 2D engine \"" << GetId() << "\""); 00350 } 00351 } 00352 00353 /****************************************/ 00354 /****************************************/ 00355 00356 REGISTER_PHYSICS_ENGINE(CDynamics2DEngine, 00357 "dynamics2d", 00358 "Carlo Pinciroli [ilpincy@gmail.com]", 00359 "1.0", 00360 "A 2D dynamics physics engine.", 00361 "This physics engine is a 2D dynamics engine based on the Chipmunk library\n" 00362 "(http://code.google.com/p/chipmunk-physics).\n\n" 00363 "REQUIRED XML CONFIGURATION\n\n" 00364 " <physics_engines>\n" 00365 " ...\n" 00366 " <dynamics2d id=\"dyn2d\" />\n" 00367 " ...\n" 00368 " </physics_engines>\n\n" 00369 "The 'id' attribute is necessary and must be unique among the physics engines.\n" 00370 "It is used in the subsequent section <arena_physics> to assign entities to\n" 00371 "physics engines. If two engines share the same id, initialization aborts.\n\n" 00372 "OPTIONAL XML CONFIGURATION\n\n" 00373 "The plane of the physics engine can be translated on the Z axis, to simulate\n" 00374 "for example hovering objects, such as flying robots. To translate the plane\n" 00375 "2m up the Z axis, use the 'elevation' attribute as follows:\n\n" 00376 " <physics_engines>\n" 00377 " ...\n" 00378 " <dynamics2d id=\"dyn2d\"\n" 00379 " elevation=\"2.0\" />\n" 00380 " ...\n" 00381 " </physics_engines>\n\n" 00382 "When not specified, the elevation is zero, which means that the plane\n" 00383 "corresponds to the XY plane.\n", 00384 "Under development" 00385 ); 00386 00387 }