Grasp success estimator

This PR together with https://github.com/SoMa-Project/soma_utils/pull/133 fixes issues https://github.com/SoMa-Project/soma_utils/issues/115. And thus make use of the reactive behavior introduced in PR https://github.com/SoMa-Project/soma_utils/pull/127. To get details on re-plan and re-run see description here https://github.com/SoMa-Project/soma_utils/issues/119.

This PR introduces the grasp success estimator (using the FT-Sensor to estimate mass) into the planner.

Estimator interface

The estimator is a separate ros node and provides the following interface:

If the current control mode (topic: /ham_state) switches to ReferenceMassMeasurement the grasp success estimator will perform a reference mass measurement (empty hand).
- It will signal the completion if that task on the topic /graspSuccessEstimator/status via a status code.
- It will also publish an array containing one element (the reference mass) to the topic /graspSuccessEstimator/masses
If the current control mode (topic: /ham_state) switches to EstimationMassMeasurement the grasp success estimator will measure the mass again and compare it to it's reference measurement.
- It will signal the completion of that task on the same topic as before /graspSuccessEstimator/status via a status code.
- In case the status code corresponds to a result (no object, one object, more than one object) it also publishes it's confidence (/graspSuccessEstimator/confidence and /graspSuccessEstimator/confidence_all) and the estimated number of objects (/graspSuccessEstimator/num_objects)
- It will also publish an array containing two elements (the reference mass, the estimation mass) to the topic /graspSuccessEstimator/masses
To launch the node you'll need to provide four parameters. The launch file grasp_succes_estimator.launch in soma_utils/planner_gui/launch/ provides default arguments and is the best place to customize the system to different setups.
1. ft_topic_name: Name of the FT-Topic to use. The force/torque values are supposed to be in EE frame. It is important to set the mass values in the object parameter file accordingly.
  _E.g. At RBO the topic /ft_notool already accounts for the weight of the EE (recommended), whereas /ft_sensorwrench does not.
2. ft_topic_type: The message type of the FT-Topic. Currently supported are:
  - Float64MultiArray [force_x, force_y, force_z, torque_x, torque_y, torque_z]
  - WrenchStamped (make sure it is in EE frame)
3. object_ros_param_path: The name of the ros parameter than contains the name of the current object (usually: /planner_gui/object)
4. path_to_object_parameters: The location of the yaml file that contains the required mass information (usually: data/object_param.yaml)

For a description of the status codes look at the enum class RESPONSES implemented in grasp_success_estimator.py.

Planner integration

The planner incorporates the grasp success estimator for SurfaceGrasps and WallGrasps in the following way:

Before the CM GoDown it will go into ReferenceMassMeasurement
The old CM GoUp was splitted into two halfs. In between the planner will go into EstimationMassMeasurement.
- The reason behind this is, that we want to get the estimation measurement as early as possible but after the object was at least lifted a little.
- The planner will add up to five reactions (depending on the config file) to the result of the estimator's result:
  1. [Result=okay] switch to GoUp (second part) and continue grasp pipeline
  2. [Result=no_object] switch to replan, rerun, GoUp (depending on config)
  3. [Result=too_many] switch to replan, rerun, GoUp (depending on config)
  4. [Result=inactive] switch to GoUp (second part) and continue grasp pipeline
  5. [Result=timeout] switch to GoUp (second part) and continue grasp pipeline after timeout

Configuration

The reactive behavior can be configured on an per-object basis using the file data/object_param.yaml The attributes that have to be set are reactions and mass

reactions (list of pair: result <-> reaction)
- Implementation in classes FailureCases and Reaction in planner.py
- currently supported reactions are:
  - CONTINUE (simply continue the grasp pipeline)
  - REPLAN (stops current grasp and allows to re-plan from view position)
  - RERUN (stops current grasp and allows to re-run the last plan again)
mass (parameters for gaussian distribution model describing the mass of the object to be grasped units are kg)
If no reaction is given at all the default is CONTINUE otherwise an error is thrown if a parameter is missing.

An example can be seen here:

  mango:
    SurfaceGrasp: 
      success: 1.0
      min: [-0.14, -0.1]
      max: [0.14, 0.05]
      reactions: 
        mass_estimation_no_object: REPLAN
        mass_estimation_too_many: REPLAN
    WallGrasp: 
      success: 1.0
      min: [-1000.0, -0.075]
      max: [-0.05, 0.045]
      reactions: 
        mass_estimation_no_object: REPLAN
        mass_estimation_too_many: REPLAN
    EdgeGrasp: {'success': 0}
    mass: {'mean': 0.28052, 'stddev': 0.05958}

Testing the PR

Most of the content of this PR was tested on bottom-3 using the full system. Details as follow:

SurfaceGrasp + Wallgrasp tested
Estimation result tested: no object + exactly one object
reactions tested: success, re-plan, re-run (using a slightly older version that shouldn't differ too much)

Further tests wouldn't hurt

Note: This PR also includes an update to the submodule hybrid-automaton-tools-py. It might make sense to merge these changes into the actual reopsitory (See: https://gitlab.tubit.tu-berlin.de/rbo-lab/hybrid-automaton-tools-py/merge_requests/3)

SoMa-Project / ec_grasp_planner