ARGoS  3
A parallel, multi-engine simulator for swarm robotics
dynamics2d_engine.cpp
Go to the documentation of this file.
1 
7 #include "dynamics2d_engine.h"
8 #include "dynamics2d_model.h"
9 #include "dynamics2d_gripping.h"
10 
11 #include <argos3/core/simulator/simulator.h>
12 #include <argos3/core/simulator/entity/embodied_entity.h>
13 
14 #include <cmath>
15 
16 namespace argos {
17 
18  /****************************************/
19  /****************************************/
20 
22  m_fStaticHashCellSize(0.1f),
23  m_fActiveHashCellSize(2.0f * 0.085036758f),
24  m_nStaticHashCells(1000),
25  m_nActiveHashCells(1000),
26  m_ptSpace(NULL),
27  m_ptGroundBody(NULL),
28  m_fElevation(0.0f) {
29  }
30 
31  /****************************************/
32  /****************************************/
33 
35  try {
36  /* Init parent */
37  CPhysicsEngine::Init(t_tree);
38  /* Parse XML */
39  GetNodeAttributeOrDefault(t_tree, "static_cell_size", m_fStaticHashCellSize, m_fStaticHashCellSize);
40  GetNodeAttributeOrDefault(t_tree, "active_cell_size", m_fActiveHashCellSize, m_fActiveHashCellSize);
41  GetNodeAttributeOrDefault(t_tree, "static_cells", m_nStaticHashCells, m_nStaticHashCells);
42  GetNodeAttributeOrDefault(t_tree, "active_cells", m_nActiveHashCells, m_nActiveHashCells);
43  GetNodeAttributeOrDefault(t_tree, "elevation", m_fElevation, m_fElevation);
44  /* Override volume top and bottom with the value of m_fElevation */
45  if(!GetVolume().TopFace) GetVolume().TopFace = new SHorizontalFace;
46  if(!GetVolume().BottomFace) GetVolume().BottomFace = new SHorizontalFace;
47  GetVolume().TopFace->Height = m_fElevation;
48  GetVolume().BottomFace->Height = m_fElevation;
49  /* Initialize physics */
50  cpInitChipmunk();
51  cpResetShapeIdCounter();
52  /* Used to attach static geometries so that they won't move and to simulate friction */
53  m_ptGroundBody = cpBodyNew(INFINITY, INFINITY);
54  /* Create the space to contain the movable objects */
55  m_ptSpace = cpSpaceNew();
56  /* Subiterations to solve constraints.
57  The more, the better for precision but the worse for speed
58  */
59  m_ptSpace->iterations = GetIterations();
60  /* Resize the space hash.
61  This has dramatic effects on performance.
62  TODO: - find optimal parameters automatically (average entity size)
63  cpSpaceReindexStaticHash(m_ptSpace, m_fStaticHashCellSize, m_nStaticHashCells);
64  cpSpaceResizeActiveHash(m_ptSpace, m_fActiveHashCellSize, m_nActiveHashCells);
65  */
66  /* Gripper-Gripped callback functions */
67  cpSpaceAddCollisionHandler(
68  m_ptSpace,
73  NULL,
74  NULL,
75  NULL);
76  }
77  catch(CARGoSException& ex) {
78  THROW_ARGOSEXCEPTION_NESTED("Error initializing the dynamics 2D engine \"" << GetId() << "\"", ex);
79  }
80  }
81 
82  /****************************************/
83  /****************************************/
84 
86  for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin();
87  it != m_tPhysicsModels.end(); ++it) {
88  it->second->Reset();
89  }
90  cpSpaceReindexStatic(m_ptSpace);
91  }
92 
93  /****************************************/
94  /****************************************/
95 
97  /* Update the physics state from the entities */
98  for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin();
99  it != m_tPhysicsModels.end(); ++it) {
100  it->second->UpdateFromEntityStatus();
101  }
102  /* Perform the step */
103  for(size_t i = 0; i < GetIterations(); ++i) {
104  cpSpaceStep(m_ptSpace, GetPhysicsClockTick());
105  }
106  /* Update the simulated space */
107  for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin();
108  it != m_tPhysicsModels.end(); ++it) {
109  it->second->UpdateEntityStatus();
110  }
111  }
112 
113  /****************************************/
114  /****************************************/
115 
117  /* Empty the physics model map */
118  for(CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.begin();
119  it != m_tPhysicsModels.end(); ++it) {
120  delete it->second;
121  }
122  m_tPhysicsModels.clear();
123  /* Get rid of the physics space */
124  cpSpaceFree(m_ptSpace);
125  cpBodyFree(m_ptGroundBody);
126  }
127 
128  /****************************************/
129  /****************************************/
130 
132  return m_tPhysicsModels.size();
133  }
134 
135  /****************************************/
136  /****************************************/
137 
139  SOperationOutcome cOutcome =
140  CallEntityOperation<CDynamics2DOperationAddEntity, CDynamics2DEngine, SOperationOutcome>
141  (*this, c_entity);
142  cpResetShapeIdCounter();
143  return cOutcome.Value;
144  }
145 
146  /****************************************/
147  /****************************************/
148 
150  SOperationOutcome cOutcome =
151  CallEntityOperation<CDynamics2DOperationRemoveEntity, CDynamics2DEngine, SOperationOutcome>
152  (*this, c_entity);
153  return cOutcome.Value;
154  }
155 
156  /****************************************/
157  /****************************************/
158 
161  const CRay3& Ray;
162 
164  const CRay3& c_ray) :
165  Intersections(t_data),
166  Ray(c_ray) {}
167  };
168 
169  static void Dynamics2DSegmentQueryFunc(cpShape* pt_shape, cpFloat f_t, cpVect, void* pt_data) {
170  /* Get the data associated to this query */
171  SDynamics2DSegmentHitData& sData = *reinterpret_cast<SDynamics2DSegmentHitData*>(pt_data);
172  /* Hit found, is it within the limits? */
173  CDynamics2DModel& cModel = *reinterpret_cast<CDynamics2DModel*>(pt_shape->body->data);
174  CVector3 cIntersectionPoint;
175  sData.Ray.GetPoint(cIntersectionPoint, f_t);
176  if((cIntersectionPoint.GetZ() >= cModel.GetBoundingBox().MinCorner.GetZ()) &&
177  (cIntersectionPoint.GetZ() <= cModel.GetBoundingBox().MaxCorner.GetZ()) ) {
178  /* Yes, a real hit */
179  sData.Intersections.push_back(
180  SEmbodiedEntityIntersectionItem(
181  &cModel.GetEmbodiedEntity(),
182  f_t));
183  }
184  }
185 
187  const CRay3& c_ray) const {
188  /* Query all hits along the ray */
189  SDynamics2DSegmentHitData sHitData(t_data, c_ray);
190  cpSpaceSegmentQuery(
191  m_ptSpace,
192  cpv(c_ray.GetStart().GetX(), c_ray.GetStart().GetY()),
193  cpv(c_ray.GetEnd().GetX() , c_ray.GetEnd().GetY() ),
194  CP_ALL_LAYERS,
195  CP_NO_GROUP,
196  Dynamics2DSegmentQueryFunc,
197  &sHitData);
198  }
199 
200  /****************************************/
201  /****************************************/
202 
204  const CVector3& c_original_pos,
205  const cpBody* pt_body) {
206  c_new_pos.SetX(pt_body->p.x);
207  c_new_pos.SetY(pt_body->p.y);
208  c_new_pos.SetZ(c_original_pos.GetZ());
209  }
210 
211  /****************************************/
212  /****************************************/
213 
215  cpBody* pt_body) {
216  c_new_orient.FromAngleAxis(CRadians(pt_body->a), CVector3::Z);
217  }
218 
219  /****************************************/
220  /****************************************/
221 
222  void CDynamics2DEngine::AddPhysicsModel(const std::string& str_id,
223  CDynamics2DModel& c_model) {
224  m_tPhysicsModels[str_id] = &c_model;
225  }
226 
227  /****************************************/
228  /****************************************/
229 
230  void CDynamics2DEngine::RemovePhysicsModel(const std::string& str_id) {
231  CDynamics2DModel::TMap::iterator it = m_tPhysicsModels.find(str_id);
232  if(it != m_tPhysicsModels.end()) {
233  delete it->second;
234  m_tPhysicsModels.erase(it);
235  }
236  else {
237  THROW_ARGOSEXCEPTION("Dynamics2D model id \"" << str_id << "\" not found in dynamics 2D engine \"" << GetId() << "\"");
238  }
239  }
240 
241  /****************************************/
242  /****************************************/
243 
245  "dynamics2d",
246  "Carlo Pinciroli [ilpincy@gmail.com]",
247  "1.0",
248  "A 2D dynamics physics engine.",
249  "This physics engine is a 2D dynamics engine based on the Chipmunk library\n"
250  "(http://code.google.com/p/chipmunk-physics).\n\n"
251  "REQUIRED XML CONFIGURATION\n\n"
252  " <physics_engines>\n"
253  " ...\n"
254  " <dynamics2d id=\"dyn2d\" />\n"
255  " ...\n"
256  " </physics_engines>\n\n"
257  "The 'id' attribute is necessary and must be unique among the physics engines.\n"
258  "It is used in the subsequent section <arena_physics> to assign entities to\n"
259  "physics engines. If two engines share the same id, initialization aborts.\n\n"
260  "OPTIONAL XML CONFIGURATION\n\n"
261  "The plane of the physics engine can be translated on the Z axis, to simulate\n"
262  "for example hovering objects, such as flying robots. To translate the plane\n"
263  "2m up the Z axis, use the 'elevation' attribute as follows:\n\n"
264  " <physics_engines>\n"
265  " ...\n"
266  " <dynamics2d id=\"dyn2d\"\n"
267  " elevation=\"2.0\" />\n"
268  " ...\n"
269  " </physics_engines>\n\n"
270  "When not specified, the elevation is zero, which means that the plane\n"
271  "corresponds to the XY plane.\n",
272  "Under development"
273  );
274 
275 }
argos::CVector3::GetZ
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
argos::CPhysicsEngine::GetId
const std::string & GetId() const
Returns the id of this physics engine.
Definition: physics_engine.h:284
argos::CDynamics2DEngine::SHAPE_GRIPPER
@ SHAPE_GRIPPER
Definition: dynamics2d_engine.h:49
argos::CVector3::SetX
void SetX(const Real f_x)
Sets the x coordinate of this vector.
Definition: vector3.h:101
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CVector3
A 3D vector class.
Definition: vector3.h:29
argos::CRadians
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
argos::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::ManageCollisionBetweenGripperAndGrippable
int ManageCollisionBetweenGripperAndGrippable(cpArbiter *pt_arb, cpSpace *pt_space, void *p_data)
Definition: dynamics2d_gripping.cpp:159
argos::CDynamics2DEngine::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
Definition: dynamics2d_engine.cpp:34
argos::CPhysicsEngine::GetIterations
UInt32 GetIterations() const
Returns the number of iterations per simulation clock tick.
Definition: physics_engine.h:266
argos::CRay3
Definition: ray3.h:19
argos::CDynamics2DEngine::GetNumPhysicsModels
virtual size_t GetNumPhysicsModels()
Definition: dynamics2d_engine.cpp:131
argos::CVector3::Z
static const CVector3 Z
The z axis.
Definition: vector3.h:40
argos::CDynamics2DEngine::Update
virtual void Update()
Definition: dynamics2d_engine.cpp:96
argos::CDynamics2DEngine
Definition: dynamics2d_engine.h:42
argos::CPhysicsEngine::SHorizontalFace::Height
Real Height
Definition: physics_engine.h:105
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CQuaternion::FromAngleAxis
CQuaternion & FromAngleAxis(const CRadians &c_angle, const CVector3 &c_vector)
Definition: quaternion.h:125
argos::CVector3::GetX
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
argos::CDynamics2DEngine::AddPhysicsModel
void AddPhysicsModel(const std::string &str_id, CDynamics2DModel &c_model)
Definition: dynamics2d_engine.cpp:222
argos::BeginCollisionBetweenGripperAndGrippable
int BeginCollisionBetweenGripperAndGrippable(cpArbiter *pt_arb, cpSpace *pt_space, void *p_data)
Definition: dynamics2d_gripping.cpp:143
argos::CVector3::SetZ
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
Definition: vector3.h:133
argos::CDynamics2DEngine::OrientationPhysicsToSpace
void OrientationPhysicsToSpace(CQuaternion &c_new_orient, cpBody *pt_body)
Definition: dynamics2d_engine.cpp:214
argos::CQuaternion
Definition: quaternion.h:14
argos::CDynamics2DEngine::CDynamics2DEngine
CDynamics2DEngine()
Definition: dynamics2d_engine.cpp:21
THROW_ARGOSEXCEPTION_NESTED
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
Definition: argos_exception.h:115
argos::CDynamics2DEngine::AddEntity
virtual bool AddEntity(CEntity &c_entity)
Adds an entity to the physics engine.
Definition: dynamics2d_engine.cpp:138
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
dynamics2d_gripping.h
argos::SOperationOutcome::Value
bool Value
Definition: entity.h:300
argos::CDynamics2DEngine::RemovePhysicsModel
void RemovePhysicsModel(const std::string &str_id)
Definition: dynamics2d_engine.cpp:230
argos::SDynamics2DSegmentHitData::Ray
const CRay3 & Ray
Definition: dynamics2d_engine.cpp:161
argos::CRay3::GetStart
CVector3 & GetStart()
Definition: ray3.h:37
dynamics2d_engine.h
argos::CPhysicsEngine::GetPhysicsClockTick
Real GetPhysicsClockTick() const
Returns the length of the physics engine tick.
Definition: physics_engine.h:276
argos::CEntity
The basic entity type.
Definition: entity.h:89
argos::CPhysicsEngine::GetVolume
SVolume & GetVolume()
Returns the boundary faces for the volume associated to this engine.
Definition: physics_engine.h:211
argos::CDynamics2DModel
The base class for models in the dynamics 2D engine.
Definition: dynamics2d_model.h:27
argos::CDynamics2DEngine::CheckIntersectionWithRay
virtual void CheckIntersectionWithRay(TEmbodiedEntityIntersectionData &t_data, const CRay3 &c_ray) const
Check which objects in this engine intersect the given ray.
Definition: dynamics2d_engine.cpp:186
argos::CDynamics2DEngine::PositionPhysicsToSpace
void PositionPhysicsToSpace(CVector3 &c_new_pos, const CVector3 &c_original_pos, const cpBody *pt_body)
Definition: dynamics2d_engine.cpp:203
argos::CPhysicsEngine::SVolume::TopFace
SHorizontalFace * TopFace
Definition: physics_engine.h:119
argos::CPhysicsEngine::SVolume::BottomFace
SHorizontalFace * BottomFace
Definition: physics_engine.h:120
argos::GetNodeAttributeOrDefault
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.
Definition: argos_configuration.h:318
dynamics2d_model.h
argos::CDynamics2DEngine::Destroy
virtual void Destroy()
Undoes whatever was done by Init().
Definition: dynamics2d_engine.cpp:116
argos::CPhysicsEngine::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
Definition: physics_engine.cpp:185
argos::CPhysicsEngine::SHorizontalFace
A boundary face for top/bottom parts of the volume.
Definition: physics_engine.h:104
argos::SOperationOutcome
Type to use as return value for operation outcome.
Definition: entity.h:299
argos::SDynamics2DSegmentHitData::SDynamics2DSegmentHitData
SDynamics2DSegmentHitData(TEmbodiedEntityIntersectionData &t_data, const CRay3 &c_ray)
Definition: dynamics2d_engine.cpp:163
argos::CVector3::GetY
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
argos::REGISTER_PHYSICS_ENGINE
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).\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" "It is used in the subsequent section <arena_physics> to assign entities to\n" "physics engines. If two engines share the same id, initialization aborts.\n\n" "OPTIONAL XML CONFIGURATION\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", "Under development")
argos::TEmbodiedEntityIntersectionData
std::vector< SEmbodiedEntityIntersectionItem > TEmbodiedEntityIntersectionData
Definition: physics_engine.h:54
argos::CDynamics2DEngine::Reset
virtual void Reset()
Resets the resource.
Definition: dynamics2d_engine.cpp:85
argos::CDynamics2DEngine::SHAPE_GRIPPABLE
@ SHAPE_GRIPPABLE
Definition: dynamics2d_engine.h:48
argos::CDynamics2DEngine::RemoveEntity
virtual bool RemoveEntity(CEntity &c_entity)
Removes an entity from the physics engine.
Definition: dynamics2d_engine.cpp:149
argos::SDynamics2DSegmentHitData::Intersections
TEmbodiedEntityIntersectionData & Intersections
Definition: dynamics2d_engine.cpp:160
argos::CVector3::SetY
void SetY(const Real f_y)
Sets the y coordinate of this vector.
Definition: vector3.h:117
argos::SDynamics2DSegmentHitData
Definition: dynamics2d_engine.cpp:159
argos::CRay3::GetEnd
CVector3 & GetEnd()
Definition: ray3.h:45