ilpincy / argos3

A parallel, multi-engine simulator for heterogeneous swarm robotics
http://www.argos-sim.info/
268 stars 121 forks source link

Physics of gripping with the foot-bot #211

Open jonaskuResearch opened 2 years ago

jonaskuResearch commented 2 years ago

In version beta59 the gripper of the foot-bot is not working correctly. To be more precise, @dagarzonr and I discovered the following issues:

[FATAL] CGrid::GetEntitiesAt() : Position <-4.70572e+28,3.63994e+29,0.1> out of bounds X -> -5:5 Y -> -4:4 Z -> -2:2

or

[FATAL] While updating the LED grid for LED "cyl33.leds_0.led_0" [FATAL] CGrid::PositionToCell() : Position <1.59e+30,-1.22988e+31,0.16> out of bounds X -> -5:5 Y -> -4:4 Z -> -2:2

We managed to narrow the last issue to the case that two foot-bots are grabbing the same object.

We believe that this is caused by the way that the physics between gripper and grippee are handled. Commit c8c50964bc8c9eb0eec87e1e4b1c3f837a9913f5 changed the underlying physics from a PivotJoint to a DampedSpring. According to our understanding, the damped spring is a point-to-point connection and does not take into account the rotation. As such when the turret rotates, the gripper and object get misaligned, since the spring only tries to resolve the linear distance between the foot-bot and the object. This could be the cause of the first two issues that we encountered. For the third issue, this might be a way that the physics resolution is implemented. While it is not completely clear to us, our working hypothesis is that the physics solver resolves one spring, which creates a larger force on the second spring. When the second spring resolves, a larger force on the first spring is created again. Over the iterations of the physics solver, the forces get arbitrarily high and when the physics resolution is finished, the object, the foot-bots or all are outside of the experimental bounds, creating the errors as detailed above in the third issue.

When manually replacing the DampedSpring with a PivotJoint (in the snippet below) and recompiling argos3, all three issues seem to be resolved, further indicating that this is an issue with the underlying physics implementation. https://github.com/ilpincy/argos3/blob/da33b8786293dad40307e4182bb791add1e89172/src/plugins/simulator/physics_engines/dynamics2d/dynamics2d_gripping.cpp#L76-L84

While this is our best hypothesis, we are not completely sure that this is the correct cause and further investigation might be needed.