Impact-based object placing

alex-mitrevski commented 3 years ago

Summary

Addresses https://github.com/b-it-bots/mas_domestic_robotics/issues/224

The PR introduces impact detection in the place action (based on force measurements), which contributes to a more intelligent execution of the action. As suggested in the issue description, the robot first goes to a designated pose and then moves its arm down until it detects impact with the placing surface, at which point the object is released. Only force measurements along the z-axis of the base are taken into account for impact detection.

Tests to verify the functionality

I did some tests with the HSR as a proof of concept. Due to the built-in safety features of the HSR, the impact that the arm makes with the surface has to be quite small, so the arm was only allowed to move down at the default low speed. Impacts were detected successfully for objects with different weights, which suggests that the procedure is general and transferable across objects.

A video showing the functionality in action can be found on the b-it-bots channel.

TODOs before merging

[ ] ~Test with objects grasped in a top-down fashion. In the initial tests, the z-axis of the sensor coincided with the z-axis of the base, so I didn't have to worry about transforming the measurements, but this might be necessary when the sensor axes are rotated~ After thinking about this for a while, I think it would be better to address this point with a separate PR; placing after a top-down grasp is a new feature that will require its own tests
[x] Introduce a context variable in the action (like in the pickup_action) that will allow a release action - just as before - to be performed as well (for instance, impact-based placing is not suitable for drawers or containers) - This has been resolved by adding a new Boolean field to the goal (release_on_impact), which can be used to control whether to detect impact on placing