pointmass3d_engine.cpp
Go to the documentation of this file.
1 
7 #include "pointmass3d_engine.h"
8 #include "pointmass3d_model.h"
9 #include <argos3/core/utility/logging/argos_log.h>
10 #include <argos3/core/utility/configuration/argos_configuration.h>
11 
12 namespace argos {
13 
14  /****************************************/
15  /****************************************/
16 
18  m_fGravity(-9.81f) {
19  }
20 
21  /****************************************/
22  /****************************************/
23 
25  }
26 
27  /****************************************/
28  /****************************************/
29 
31  /* Init parent */
32  CPhysicsEngine::Init(t_tree);
33  /* Set gravity */
34  GetNodeAttributeOrDefault(t_tree, "gravity", m_fGravity, m_fGravity);
35  }
36 
37  /****************************************/
38  /****************************************/
39 
41  for(auto it = m_tPhysicsModels.begin();
42  it != m_tPhysicsModels.end(); ++it) {
43  it->second->Reset();
44  }
45  }
46 
47  /****************************************/
48  /****************************************/
49 
51  /* Empty the physics entity map */
52  for(auto it = m_tPhysicsModels.begin();
53  it != m_tPhysicsModels.end(); ++it) {
54  delete it->second;
55  }
56  m_tPhysicsModels.clear();
57  }
58 
59  /****************************************/
60  /****************************************/
61 
63  /* Update the physics state from the entities */
64  for(auto it = m_tPhysicsModels.begin();
65  it != m_tPhysicsModels.end(); ++it) {
66  it->second->UpdateFromEntityStatus();
67  }
68  for(size_t i = 0; i < GetIterations(); ++i) {
69  /* Perform the step */
70  for(auto it = m_tPhysicsModels.begin();
71  it != m_tPhysicsModels.end(); ++it) {
72  it->second->UpdatePhysics();
73  }
74  }
75  for(auto it = m_tPhysicsModels.begin();
76  it != m_tPhysicsModels.end(); ++it) {
77  it->second->Step();
78  }
79  /* Update the simulated space */
80  for(auto it = m_tPhysicsModels.begin();
81  it != m_tPhysicsModels.end(); ++it) {
82  it->second->UpdateEntityStatus();
83  }
84  }
85 
86  /****************************************/
87  /****************************************/
88 
90  return m_tPhysicsModels.size();
91  }
92 
93  /****************************************/
94  /****************************************/
95 
97  SOperationOutcome cOutcome =
98  CallEntityOperation<CPointMass3DOperationAddEntity, CPointMass3DEngine, SOperationOutcome>
99  (*this, c_entity);
100  return cOutcome.Value;
101  }
102 
103  /****************************************/
104  /****************************************/
105 
107  SOperationOutcome cOutcome =
108  CallEntityOperation<CPointMass3DOperationRemoveEntity, CPointMass3DEngine, SOperationOutcome>
109  (*this, c_entity);
110  return cOutcome.Value;
111  }
112 
113  /****************************************/
114  /****************************************/
115 
117  return true;
118  }
119 
120  /****************************************/
121  /****************************************/
122 
124  return false;
125  }
126 
127  /****************************************/
128  /****************************************/
129 
131  }
132 
133  /****************************************/
134  /****************************************/
135 
137  const CRay3& c_ray) const {
138  Real fTOnRay;
139  for(auto it = m_tPhysicsModels.begin();
140  it != m_tPhysicsModels.end();
141  ++it) {
142  if(it->second->CheckIntersectionWithRay(fTOnRay, c_ray)) {
143  t_data.push_back(
145  &it->second->GetEmbodiedEntity(),
146  fTOnRay));
147  }
148  }
149  }
150 
151  /****************************************/
152  /****************************************/
153 
154  void CPointMass3DEngine::AddPhysicsModel(const std::string& str_id,
155  CPointMass3DModel& c_model) {
156  m_tPhysicsModels[str_id] = &c_model;
157  }
158 
159  /****************************************/
160  /****************************************/
161 
162  void CPointMass3DEngine::RemovePhysicsModel(const std::string& str_id) {
163  auto it = m_tPhysicsModels.find(str_id);
164  if(it != m_tPhysicsModels.end()) {
165  delete it->second;
166  m_tPhysicsModels.erase(it);
167  }
168  else {
169  THROW_ARGOSEXCEPTION("PointMass3D model id \"" << str_id << "\" not found in point-mass 3D engine \"" << GetId() << "\"");
170  }
171  }
172 
173  /****************************************/
174  /****************************************/
175 
177  "pointmass3d",
178  "Carlo Pinciroli [ilpincy@gmail.com]",
179  "1.0",
180  "A 3D point-mass physics engine.",
181 
182  "This physics engine is a 3D point-mass engine.\n\n"
183 
184  "REQUIRED XML CONFIGURATION\n\n"
185  " <physics_engines>\n"
186  " ...\n"
187  " <pointmass3d id=\"pm3d\" />\n"
188  " ...\n"
189  " </physics_engines>\n\n"
190 
191  "The 'id' attribute is necessary and must be unique among the physics engines.\n"
192  "If two engines share the same id, initialization aborts.\n\n"
193 
194  "OPTIONAL XML CONFIGURATION\n\n"
195 
196  "By default, there will -9.81 m/s gravity in the simulation. This can be\n"
197  "changed by specifying the 'gravity' attribute:\n\n"
198 
199  " <physics_engines>\n"
200  " ...\n"
201  " <pointmass3d id=\"pm3d\" gravity=\"-9.81\"/>\n"
202  " ...\n"
203  " </physics_engines>\n\n"
204 
205  "Multiple physics engines can also be used. If multiple physics engines are used,\n"
206  "the disjoint union of the 3D volumes within the arena assigned to each engine must cover\n"
207  "the entire arena without overlapping. If the entire arena is not covered, robots can\n"
208  "\"escape\" the configured physics engines and cause a fatal exception (this is not an\n"
209  "issue when a single physics engine is used, because the engine covers the entire arena\n"
210  "by default). To use multiple physics engines, use the following syntax (all attributes\n"
211  "are mandatory):\n\n"
212 
213  " <physics_engines>\n"
214  " ...\n"
215  " <pointmass3d id=\"pm0\">\n"
216  " <boundaries>\n"
217  " <top height=\"1.0\"/>\n"
218  " <botton height=\"0.0\"/>\n"
219  " <sides>\n"
220  " <vertex point=\"0.0, 0.0\"/>\n"
221  " <vertex point=\"4.0, 0.0\"/>\n"
222  " <vertex point=\"4.0, 4.0\"/>\n"
223  " <vertex point=\"0.0, 4.0\"/>\n"
224  " </sides>\n"
225  " </boundaries>\n"
226  " </pointmass3d>\n"
227  " <pointmass3d id=\"pm1\">\n"
228  " ...\n"
229  " </pointmass3d>\n"
230  " ...\n"
231  " </physics_engines>\n\n"
232 
233  "A physics engine can be defined having any number of sides >= 3, as long as the\n"
234  "sides from a closed polygon in the 2D plane (vertices must be declared in the\n"
235  "XML file in counter-clockwise order). In the above example, the physics engine\n"
236  "'pm0' is assigned to the area within the arena with lower left coordinates (0,0)\n"
237  "and upper right coordinates (4,4) and vertices are specified in counter\n"
238  "clockwise order: LL, LR, UR, UL.\n\n"
239 
240  "OPTIMIZATION HINTS\n\n"
241 
242  "1. A single physics engine is generally sufficient for small swarms (say <= 50\n"
243  " robots) within a reasonably small arena to obtain faster than real-time\n"
244  " performance with optimized code. For larger swarms and/or large arenas multiple\n"
245  " engines should be used for maximum performance.\n\n"
246 
247  "2. In general, using the same number of ARGoS threads as physics engines gives\n"
248  " maximum performance (1-thread per engine per CPU core).\n\n"
249 
250  "3. Using multiple engines in simulations with any of the following characteristics\n"
251  " generally incurs more overhead due to thread context switching than the\n"
252  " performance benefits from multiple engines:\n\n"
253 
254  " - Small swarms\n"
255  " - Small arenas\n"
256  " - Less available ARGoS threads than assigned physics engines\n"
257  " - Less available CPU cores than assigned ARGoS threads\n\n"
258 
259 
260  "4. A good starting strategy for physics engine boundary assignment is to assign\n"
261  " each physics engine the same amount of area within the arena. This will be\n"
262  " sufficient for most cases. Depending on the nature of the simulation, using\n"
263  " non-homogeneous physics engine sizes may yield increased performance. An example\n"
264  " would be a narrow hallway between larger open areas in the arena--the hallway\n"
265  " will likely experience increased robot density and assigning more physics\n"
266  " engines to that area than the relatively unpopulated open areas may increase\n"
267  " performance.\n\n",
268 
269  "Usable"
270  );
271 
272 }
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node's attribute, or the passed default value.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
REGISTER_PHYSICS_ENGINE(CDynamics2DEngine, "dynamics2d", "Carlo Pinciroli [ilpincy@gmail.com]", "1.0", "A 2D dynamics physics engine.", "This physics engine is a 2D dynamics engine based on the Chipmunk library\n" "(http://code.google.com/p/chipmunk-physics) version 6.0.1.\n\n" "REQUIRED XML CONFIGURATION\n\n" " <physics_engines>\n" " ...\n" " <dynamics2d id=\"dyn2d\" />\n" " ...\n" " </physics_engines>\n\n" "The 'id' attribute is necessary and must be unique among the physics engines.\n" "If two engines share the same id, initialization aborts.\n\n" "OPTIONAL XML CONFIGURATION\n\n" "It is possible to set how many iterations this physics engine performs between\n" "each simulation step. By default, this physics engine performs 10 steps every\n" "two simulation steps. This means that, if the simulation step is 100ms, the\n" "physics engine step is, by default, 10ms. Sometimes, collisions and joints are\n" "not simulated with sufficient precision using these parameters. By increasing\n" "the number of iterations, the temporal granularity of the solver increases and\n" "with it its accuracy, at the cost of higher computational cost. To change the\n" "number of iterations per simulation step use this syntax:\n\n" " <physics_engines>\n" " ...\n" " <dynamics2d id=\"dyn2d\"\n" " iterations=\"20\" />\n" " ...\n" " </physics_engines>\n\n" "The plane of the physics engine can be translated on the Z axis, to simulate\n" "for example hovering objects, such as flying robots. To translate the plane\n" "2m up the Z axis, use the 'elevation' attribute as follows:\n\n" " <physics_engines>\n" " ...\n" " <dynamics2d id=\"dyn2d\"\n" " elevation=\"2.0\" />\n" " ...\n" " </physics_engines>\n\n" "When not specified, the elevation is zero, which means that the plane\n" "corresponds to the XY plane.\n\n" "The friction parameters between the ground and movable boxes and cylinders can\n" "be overridden. You can set both the linear and angular friction parameters.\n" "The default value is 1.49 for each of them. To override the values, use this\n" "syntax (all attributes are optional):\n\n" " <physics_engines>\n" " ...\n" " <dynamics2d id=\"dyn2d\">\n" " <friction box_linear_friction=\"1.0\"\n" " box_angular_friction=\"2.0\"\n" " cylinder_linear_friction=\"3.0\"\n" " cylinder_angular_friction=\"4.0\" />\n" " </dynamics2d>\n" " ...\n" " </physics_engines>\n\n" "For the the robots that use velocity-based control, such as ground robots with\n" "the differential_steering actuator (e.g. the foot-bot and the e-puck), it is\n" "possible to customize robot-specific attributes that set the maximum force and\n" "torque the robot has. The syntax is as follows, taking a foot-bot as example:\n\n" " <arena ...>\n" " ...\n" " <foot-bot id=\"fb0\">\n" " <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n" " <controller config=\"mycntrl\" />\n" " <!-- Specify new value for max_force and max_torque -->\n" " <dynamics2d>\n" " <differential_steering max_force=\"0.1\" max_torque=\"0.1\"/>\n" " </dynamics2d>\n" " </foot-bot>\n" " ...\n" " </arena>\n\n" "The attributes 'max_force' and 'max_torque' are both optional, and they take the\n" "robot-specific default if not set. Check the code of the dynamics2d model of the\n" "robot you're using to know the default values.\n\n" "Multiple physics engines can also be used. If multiple physics engines are used,\n" "the disjoint union of the area within the arena assigned to each engine must cover\n" "the entire arena without overlapping. If the entire arena is not covered, robots can\n" "\"escape\" the configured physics engines and cause a fatal exception (this is not an\n" "issue when a single physics engine is used, because the engine covers the entire arena\n" "by default). To use multiple physics engines, use the following syntax (all attributes\n" "are mandatory):\n\n" " <physics_engines>\n" " ...\n" " <dynamics2d id=\"dyn2d0\">\n" " <boundaries>\n" " <top height=\"1.0\"/>\n" " <botton height=\"0.0\"/>\n" " <sides>\n" " <vertex point=\"0.0, 0.0\"/>\n" " <vertex point=\"4.0, 0.0\"/>\n" " <vertex point=\"4.0, 4.0\"/>\n" " <vertex point=\"0.0, 4.0\"/>\n" " </sides>\n" " </boundaries>\n" " </dynamics2d>\n" " <dynamics2d id=\"dyn2d1\">\n" " ..." " </dynamics2d>\n" " ...\n" " </physics_engines>\n\n" "The 'top' and 'bottom' nodes are relevant for 3D physics engines. For 2D\n" "engines, it safe to set their height to 1.0 and 0.0, respectively. A physics\n" "engine can be defined having any number of sides >= 3, as long as the sides form\n" "a closed polygon in the 2D plane. The vertices must be declared in\n" "counter-clockwise order. In the above example, the physics engine \"dyn2d0\" is\n" "assigned to the area within the arena with lower-left coordinates (0,0) and\n" "upper-right coordinates (4,4) and vertices are specified in counter clockwise\n" "order: south-east, south-west, north-west, north-east.\n\n" "OPTIMIZATION HINTS\n\n" "1. A single physics engine is generally sufficient for small swarms (say <= 50\n" " robots) within a reasonably small arena to obtain faster than real-time\n" " performance with optimized code. For larger swarms and/or large arenas,\n" " multiple engines should be used for maximum performance.\n\n" "2. In general, using the same number of ARGoS threads as physics engines gives\n" " maximum performance (1-thread per engine per CPU core).\n\n" "3. Using multiple engines in simulations with any of the following\n" " characteristics generally incurs more overhead (due to thread context\n" " switching) than the performance benefits from multiple engines:\n" " - Small swarms\n" " - Small arenas\n" " - Less available ARGoS threads than assigned physics engines\n" " - Less available CPU cores than assigned ARGoS threads\n\n" "4. A good starting strategy for physics engine boundary assignment is to assign\n" " each physics engine the same amount of area within the arena. This will be\n" " sufficient for most cases. Depending on the nature of the simulation, using\n" " non-homogeneous physics engine sizes may yield increased performance. An\n" " example would be a narrow hallway between larger open areas in the arena--the\n" " hallway will likely experience increased robot density and assigning more\n" " physics engines to that area than the relatively unpopulated open areas may\n" " increase performance.\n\n" "5. By default, this engine uses the bounding-box tree method for collision shape\n" " indexing. This method is the default in Chipmunk and it works well most of\n" " the times. However, if you are running simulations with hundreds or thousands\n" " of identical robots, a different shape collision indexing is available: the\n" " spatial hash. The spatial hash is a grid stored in a hashmap. To get the max\n" " out of this indexing method, you must set two parameters: the cell size and\n" " the suggested minimum number of cells in the space. According to the\n" " documentation of Chipmunk, the cell size should correspond to the size of the\n" " bounding box of the most common object in the simulation; the minimum number\n" " of cells should be at least 10x the number of objects managed by the physics\n" " engine. To use this indexing method, use this syntax (all attributes are\n" " mandatory):\n\n" " <physics_engines>\n" " ...\n" " <dynamics2d id=\"dyn2d\">\n" " <spatial_hash>\n" " <cell_size=\"1.0\"/>\n" " <cell_num=\"2.0\"/>\n" " </spatial_hash>\n" " </dynamics2d>\n" " ...\n" " </physics_engines>\n", "Usable")
std::vector< SEmbodiedEntityIntersectionItem > TEmbodiedEntityIntersectionData
The basic entity type.
Definition: entity.h:90
Type to use as return value for operation outcome.
Definition: entity.h:352
const std::string & GetId() const
Returns the id of this physics engine.
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
UInt32 GetIterations() const
Returns the number of iterations per simulation clock tick.
A 3D vector class.
Definition: vector3.h:31
virtual void CheckIntersectionWithRay(TEmbodiedEntityIntersectionData &t_data, const CRay3 &c_ray) const
Check which objects in this engine intersect the given ray.
virtual bool AddEntity(CEntity &c_entity)
Adds an entity to the physics engine.
virtual bool RemoveEntity(CEntity &c_entity)
Removes an entity from the physics engine.
virtual void Destroy()
Undoes whatever was done by Init().
virtual bool IsPointContained(const CVector3 &c_point)
Returns true if the given point is contained in this physics engine.
virtual bool IsEntityTransferNeeded() const
virtual size_t GetNumPhysicsModels()
void AddPhysicsModel(const std::string &str_id, CPointMass3DModel &c_model)
virtual void TransferEntities()
Executes the transfer of entities to other engines.
virtual void Reset()
Resets the resource.
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
void RemovePhysicsModel(const std::string &str_id)