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.
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.
ft_topic_type: The message type of the FT-Topic. Currently supported are:
object_ros_param_path: The name of the ros parameter than contains the name of the current object (usually: /planner_gui/object)
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:
[Result=okay] switch to GoUp (second part) and continue grasp pipeline
[Result=no_object] switch to replan, rerun, GoUp (depending on config)
[Result=too_many] switch to replan, rerun, GoUp (depending on config)
[Result=inactive] switch to GoUp (second part) and continue grasp pipeline
[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.
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:
/ham_state
) switches toReferenceMassMeasurement
the grasp success estimator will perform a reference mass measurement (empty hand)./graspSuccessEstimator/status
via a status code./graspSuccessEstimator/masses
/ham_state
) switches toEstimationMassMeasurement
the grasp success estimator will measure the mass again and compare it to it's reference measurement./graspSuccessEstimator/status
via a status code./graspSuccessEstimator/confidence
and/graspSuccessEstimator/confidence_all
) and the estimated number of objects (/graspSuccessEstimator/num_objects
)/graspSuccessEstimator/masses
grasp_succes_estimator.launch
insoma_utils/planner_gui/launch/
provides default arguments and is the best place to customize the system to different setups.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.
ft_topic_type
: The message type of the FT-Topic. Currently supported are:object_ros_param_path
: The name of the ros parameter than contains the name of the current object (usually:/planner_gui/object
)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 ingrasp_success_estimator.py
.Planner integration
The planner incorporates the grasp success estimator for SurfaceGrasps and WallGrasps in the following way:
GoDown
it will go intoReferenceMassMeasurement
EstimationMassMeasurement
.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 arereactions
andmass
FailureCases
andReaction
inplanner.py
An example can be seen here:
Testing the PR
Most of the content of this PR was tested on bottom-3 using the full system. Details as follow:
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)