Physics of gripping with the foot-bot

[jonaskuResearch]

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

• After the foot-bot gripped an object, the foot-bot can rotate the gripper using the turret (robot.turret.set_position_control_mode() and robot.turret.set_speed_control_mode()) but the object stays in the same relative position to the foot-bot. In version beta57 (and possibly earlier versions), the object rotates with the gripper around the foot-bot when the turret rotates. The passive control mode (robot.turret.set_passive_mode()) seems to be working fine, as neither the gripper nor the object move.
• As a consequence of the previous issue, there seem to be some issues with unlocking the gripper if the gripper is not aligned with the object. This also affects the passive control mode, as sometimes the gripper and object get misaligned.
• From time to time, the simulation crashes with different messages of the form:

[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 c8c5096 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.

argos3/src/plugins/simulator/physics_engines/dynamics2d/dynamics2d_gripping.cpp

Lines 76 to 84 in da33b87

	cpDampedSpringNew(
	cpShapeGetBody(m_ptGripperShape),
	cpShapeGetBody(pc_grippee->GetShape()),
	cpvzero,
	cpBodyWorld2Local(cpShapeGetBody(pc_grippee->GetShape()),
	m_tGrippeeAnchor),
	m_fRestLength,
	m_cEngine.GetGrippingRigidity(),
	GRIPPING_SPRING_DAMPING));

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