Warning: include(php/utility.php): Failed to open stream: No such file or directory in /home/argos/argos3/doc/api/embedded/a00808_source.php on line 2

Warning: include(): Failed opening 'php/utility.php' for inclusion (include_path='.:/usr/lib64/php') in /home/argos/argos3/doc/api/embedded/a00808_source.php on line 2
The ARGoS Website

dynamics2d_gripping.cpp
Go to the documentation of this file.
1 
7 #include "dynamics2d_gripping.h"
8 #include "dynamics2d_engine.h"
9 
10 #include <argos3/core/simulator/entity/embodied_entity.h>
11 
12 #include <algorithm>
13 
14 namespace argos {
15 
16  /****************************************/
17  /****************************************/
18 
20  CGripperEquippedEntity& c_gripper_entity,
21  cpShape* pt_gripper_shape) :
22  m_cEngine(c_engine),
23  m_cGripperEntity(c_gripper_entity),
24  m_ptGripperShape(pt_gripper_shape),
25  m_pcGrippee(NULL),
26  m_tConstraint(NULL) {
27  m_ptGripperShape->sensor = 1;
28  m_ptGripperShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPER;
29  m_ptGripperShape->data = this;
30  m_tAnchor = cpvzero;
31  }
32 
33  /****************************************/
34  /****************************************/
35 
37  Release();
38  }
39 
40  /****************************************/
41  /****************************************/
42 
43  void CDynamics2DGripper::CalculateAnchor(cpArbiter* pt_arb) {
44  /* Calculate the anchor point on the grippable body
45  as the centroid of the contact points */
46  m_tAnchor = cpvzero;
47  for(SInt32 i = 0; i < cpArbiterGetCount(pt_arb); ++i) {
48  m_tAnchor = cpvadd(m_tAnchor, cpArbiterGetPoint(pt_arb, i));
49  }
50  m_tAnchor = cpvmult(m_tAnchor, 1.0f / cpArbiterGetCount(pt_arb));
51  }
52 
53  /****************************************/
54  /****************************************/
55 
57  m_tConstraint =
58  cpSpaceAddConstraint(m_cEngine.GetPhysicsSpace(),
59  cpPivotJointNew(
60  m_ptGripperShape->body,
61  pc_grippee->GetShape()->body,
62  m_tAnchor));
63  m_tConstraint->maxBias = 0.95f; // Correct overlap
64  m_tConstraint->maxForce = 10000.0f; // Max correction speed
65  m_cGripperEntity.SetGrippedEntity(pc_grippee->GetEmbodiedEntity());
66  m_pcGrippee = pc_grippee;
67  m_pcGrippee->Attach(*this);
68  }
69 
70  /****************************************/
71  /****************************************/
72 
74  if(IsGripping()) {
75  cpSpaceRemoveConstraint(m_cEngine.GetPhysicsSpace(), m_tConstraint);
76  cpConstraintFree(m_tConstraint);
77  m_tConstraint = NULL;
78  m_cGripperEntity.ClearGrippedEntity();
79  m_pcGrippee->Remove(*this);
80  m_pcGrippee = NULL;
81  }
82  }
83 
84  /****************************************/
85  /****************************************/
86 
88  cpShape* pt_shape) :
89  m_cEmbodiedEntity(c_entity),
90  m_ptShape(pt_shape) {
91  m_ptShape->collision_type = CDynamics2DEngine::SHAPE_GRIPPABLE;
92  m_ptShape->data = this;
93  }
94 
95  /****************************************/
96  /****************************************/
97 
99  ReleaseAll();
100  }
101 
102  /****************************************/
103  /****************************************/
104 
106  m_listGrippers.push_back(&c_gripper);
107  }
108 
109  /****************************************/
110  /****************************************/
111 
113  CDynamics2DGripper::TList::iterator it =
114  std::find(m_listGrippers.begin(), m_listGrippers.end(), &c_gripper);
115  if(it != m_listGrippers.end()) {
116  m_listGrippers.erase(it);
117  }
118  }
119 
120  /****************************************/
121  /****************************************/
122 
124  CDynamics2DGripper::TList::iterator it =
125  std::find(m_listGrippers.begin(), m_listGrippers.end(), &c_gripper);
126  if(it != m_listGrippers.end()) {
127  (*it)->Release();
128  }
129  }
130 
131  /****************************************/
132  /****************************************/
133 
135  while(!m_listGrippers.empty()) {
136  m_listGrippers.back()->Release();
137  }
138  }
139 
140  /****************************************/
141  /****************************************/
142 
144  cpSpace* pt_space,
145  void* p_data) {
146  /* Get the shapes involved */
147  CP_ARBITER_GET_SHAPES(pt_arb, ptGripperShape, ptGrippableShape);
148  /* Get a reference to the gripper data */
149  CDynamics2DGripper* pcGripper = reinterpret_cast<CDynamics2DGripper*>(ptGripperShape->data);
150  /* Get a reference to the grippable entity */
151  CDynamics2DGrippable* pcGrippable = reinterpret_cast<CDynamics2DGrippable*>(ptGrippableShape->data);
152  /* If the entities match, ignore the collision forever */
153  return (&(pcGripper->GetGripperEntity().GetParent()) != &(pcGrippable->GetEmbodiedEntity().GetParent()));
154  }
155 
156  /****************************************/
157  /****************************************/
158 
160  cpSpace* pt_space,
161  void* p_data) {
162  /* Get the shapes involved */
163  CP_ARBITER_GET_SHAPES(pt_arb, ptGripperShape, ptGrippableShape);
164  /* Get a reference to the gripper data */
165  CDynamics2DGripper* pcGripper = reinterpret_cast<CDynamics2DGripper*>(ptGripperShape->data);
166  /*
167  * When to process gripping:
168  * 1. when the robot was gripping and it just unlocked the gripper
169  * 2. when the robot was not gripping and it just locked the gripper
170  * in this case, also precalculate the anchor point
171  * Otherwise ignore it
172  */
173  if(pcGripper->IsGripping() && !pcGripper->IsLocked()) {
174  /* Instruct the engine to remove the constraint in a post-step callback */
175  cpSpaceAddPostStepCallback(
176  pt_space,
178  pcGripper,
179  ptGrippableShape->data);
180  return false;
181  }
182  if(!pcGripper->IsGripping() && pcGripper->IsLocked()) {
183  pcGripper->CalculateAnchor(pt_arb);
184  /* Instruct the engine to add the constraint in a post-step callback */
185  cpSpaceAddPostStepCallback(
186  pt_space,
188  pcGripper,
189  reinterpret_cast<CDynamics2DGrippable*>(ptGrippableShape->data));
190  return false;
191  }
192  /* Always return false, anyway the gripper is a sensor shape */
193  return false;
194  }
195 
196  /****************************************/
197  /****************************************/
198 
200  void* p_obj,
201  void* p_data) {
202  /* Get the objects involved */
203  CDynamics2DGripper* pcGripper = reinterpret_cast<CDynamics2DGripper*> (p_obj);
204  CDynamics2DGrippable* pcGrippable = reinterpret_cast<CDynamics2DGrippable*>(p_data);
205  /* Connect the objects */
206  pcGripper->Grip(pcGrippable);
207  }
208 
209  /****************************************/
210  /****************************************/
211 
213  void* p_obj,
214  void* p_data) {
215  /* Get the gripper objects */
216  CDynamics2DGripper* pcGripper = reinterpret_cast<CDynamics2DGripper*> (p_obj);
217  /* Disconnect the objects */
218  pcGripper->Release();
219  }
220 
221  /****************************************/
222  /****************************************/
223 
224 }
signed int SInt32
32-bit signed integer.
Definition: datatypes.h:93
CEmbodiedEntity & GetEmbodiedEntity()
void ClearGrippedEntity()
Clears the reference to the embodied entity currently gripped by this gripper.
void Remove(CDynamics2DGripper &c_gripper)
CDynamics2DGripper(CDynamics2DEngine &c_engine, CGripperEquippedEntity &c_gripper_entity, cpShape *pt_gripper_shape)
void Release(CDynamics2DGripper &c_gripper)
This entity is a link to a body in the physics engine.
void Attach(CDynamics2DGripper &c_gripper)
void AddConstraintBetweenGripperAndGrippable(cpSpace *pt_space, void *p_obj, void *p_data)
void SetGrippedEntity(CEmbodiedEntity &c_entity)
Sets the embodied entity currently gripped by this gripper.
void CalculateAnchor(cpArbiter *pt_arb)
int BeginCollisionBetweenGripperAndGrippable(cpArbiter *pt_arb, cpSpace *pt_space, void *p_data)
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:91
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
int ManageCollisionBetweenGripperAndGrippable(cpArbiter *pt_arb, cpSpace *pt_space, void *p_data)
CDynamics2DGrippable(CEmbodiedEntity &c_entity, cpShape *pt_shape)
void RemoveConstraintBetweenGripperAndGrippable(cpSpace *pt_space, void *p_obj, void *p_data)
void Grip(CDynamics2DGrippable *pc_grippee)
CGripperEquippedEntity & GetGripperEntity()
An entity that stores the state of a robot gripper.