def create_surface_grasp(chosen_object, handarm_params, pregrasp_transform, alternative_behavior=None):
object_type = chosen_object['type']
# Get the relevant parameters for hand object combination
if object_type in handarm_params['SurfaceGrasp']:
params = handarm_params['SurfaceGrasp'][object_type]
else:
params = handarm_params['SurfaceGrasp']['object']
# Get params per phase
# Approach phase
downward_force = params['downward_force']
down_speed = params['down_speed']
# Grasping phase
lift_time = params['corrective_lift_duration']
up_speed = params['up_speed']
hand_closing_time = params['hand_closing_duration']
hand_synergy = params['hand_closing_synergy']
success_estimator_timeout = handarm_params['success_estimator_timeout']
# Set the twists to use TRIK controller with
# Down speed is positive because it is defined on the EE frame
down_twist = np.array([0, 0, down_speed, 0, 0, 0])
# Slow Up speed is also positive because it is defined on the world frame
up_twist = np.array([0, 0, up_speed, 0, 0, 0])
# assemble controller sequence
control_sequence = []
# 0. Trigger pre-shaping the hand (if there is a synergy). The first 1 in the name represents a surface grasp.
control_sequence.append(ha.BlockJointControlMode(name = 'softhand_preshape_1_1'))
# 0b. Time to trigger pre-shape
control_sequence.append(ha.TimeSwitch('softhand_preshape_1_1', 'PreGrasp', duration = hand_closing_time))
# 1. Go above the object - Pregrasp
control_sequence.append(ha.InterpolatedHTransformControlMode(pregrasp_transform, controller_name = 'GoAboveObject', goal_is_relative='0', name = 'PreGrasp'))
# 1b. Switch when hand reaches the goal pose
control_sequence.append(ha.FramePoseSwitch('PreGrasp', 'PrepareForMassMeasurement', controller = 'GoAboveObject', epsilon = '0.01'))
# 2. Go to gravity compensation
control_sequence.append(ha.BlockJointControlMode(name = 'PrepareForMassMeasurement'))
# 2b. Wait for a bit to allow vibrations to attenuate
control_sequence.append(ha.TimeSwitch('PrepareForMassMeasurement', 'ReferenceMassMeasurement', duration = 0.5))
# 3. Reference mass measurement with empty hand (TODO can this be replaced by offline calibration?)
control_sequence.append(ha.BlockJointControlMode(name='ReferenceMassMeasurement')) # TODO use gravity comp instead?
# 3b. Switches when reference measurement was done
# 3b.1 Successful reference measurement
control_sequence.append(ha.RosTopicSwitch('ReferenceMassMeasurement', 'GoDown',
ros_topic_name='/graspSuccessEstimator/status', ros_topic_type='Float64',
goal=np.array([RESPONSES.REFERENCE_MEASUREMENT_SUCCESS.value]),
))
# 3b.2 The grasp success estimator module is inactive
control_sequence.append(ha.RosTopicSwitch('ReferenceMassMeasurement', 'GoDown',
ros_topic_name='/graspSuccessEstimator/status', ros_topic_type='Float64',
goal=np.array([RESPONSES.GRASP_SUCCESS_ESTIMATOR_INACTIVE.value]),
))
# 3b.3 Timeout (grasp success estimator module not started, an error occurred or it takes too long)
control_sequence.append(ha.TimeSwitch('ReferenceMassMeasurement', 'GoDown',
duration=success_estimator_timeout))
# 3b.4 There is no special switch for unknown error response (estimator signals REFERENCE_MEASUREMENT_FAILURE)
# Instead the timeout will trigger giving the user an opportunity to notice the erroneous result in the GUI.
# 4. Go down onto the object (relative in EE frame) - Godown
control_sequence.append(ha.CartesianVelocityControlMode(down_twist,
controller_name='GoDown',
name="GoDown",
reference_frame="EE"))
# force threshold that if reached will trigger the closing of the hand
force = np.array([0, 0, downward_force, 0, 0, 0])
# 4b. Switch when force-torque sensor is triggered
control_sequence.append(ha.ForceTorqueSwitch('GoDown',
'LiftHand',
goal = force,
norm_weights = np.array([0, 0, 1, 0, 0, 0]),
jump_criterion = "THRESH_UPPER_BOUND",
goal_is_relative = '1',
frame_id = 'world'))
# 5. Lift upwards so the hand can inflate
control_sequence.append(
ha.CartesianVelocityControlMode(up_twist, controller_name='CorrectiveLift', name="LiftHand",
reference_frame="world"))
# the 1 in softhand_close_1 represents a surface grasp. This way the strategy is encoded in the HA.
mode_name_hand_closing = 'softhand_close_1_0'
# 5b. We switch after a short time
control_sequence.append(ha.TimeSwitch('LiftHand', mode_name_hand_closing, duration=lift_time))
# 6. Call hand controller
control_sequence.append(ha.GeneralHandControlMode(goal = np.array([1]), name = mode_name_hand_closing, synergy = hand_synergy))
# 6b. Switch when hand closing time ends
control_sequence.append(ha.TimeSwitch(mode_name_hand_closing, 'GoUp_1', duration = hand_closing_time))
return control_sequence
def get_transport_recipe(chosen_object,
handarm_params,
reaction,
FailureCases,
grasp_type,
alternative_behavior=None):
object_type = chosen_object['type']
# Get the relevant parameters for hand object combination
if object_type in handarm_params[grasp_type]:
params = handarm_params[grasp_type][object_type]
else:
params = handarm_params[grasp_type]['object']
lift_time = params['lift_duration']
up_speed = params['up_speed']
drop_off_pose = handarm_params['drop_off_pose']
success_estimator_timeout = handarm_params['success_estimator_timeout']
# Up speed is positive because it is defined on the world frame
up_twist = np.array([0, 0, up_speed, 0, 0, 0])
# assemble controller sequence
control_sequence = []
# 1. Lift upwards
control_sequence.append(
ha.CartesianVelocityControlMode(up_twist,
controller_name='GoUpHTransform',
name='GoUp_1',
reference_frame="world"))
# 1b. Switch after half the lift time
control_sequence.append(
ha.TimeSwitch('GoUp_1',
'EstimationMassMeasurement',
duration=lift_time / 2))
# 2. Measure the mass again and estimate number of grasped objects (grasp success estimation)
control_sequence.append(
ha.BlockJointControlMode(name='EstimationMassMeasurement'))
# 2b. Switches after estimation measurement was done
target_cm_okay = 'GoUp_2'
# 2b.1 No object was grasped => go to failure mode.
target_cm_estimation_no_object = reaction.cm_name_for(
FailureCases.MASS_ESTIMATION_NO_OBJECT, default=target_cm_okay)
control_sequence.append(
ha.RosTopicSwitch(
'EstimationMassMeasurement',
target_cm_estimation_no_object,
ros_topic_name='/graspSuccessEstimator/status',
ros_topic_type='Float64',
goal=np.array([RESPONSES.ESTIMATION_RESULT_NO_OBJECT.value]),
))
# 2b.2 More than one object was grasped => failure
target_cm_estimation_too_many = reaction.cm_name_for(
FailureCases.MASS_ESTIMATION_TOO_MANY, default=target_cm_okay)
control_sequence.append(
ha.RosTopicSwitch(
'EstimationMassMeasurement',
target_cm_estimation_too_many,
ros_topic_name='/graspSuccessEstimator/status',
ros_topic_type='Float64',
goal=np.array([RESPONSES.ESTIMATION_RESULT_TOO_MANY.value]),
))
# 2b.3 Exactly one object was grasped => success (continue lifting the object and go to drop off)
control_sequence.append(
ha.RosTopicSwitch(
'EstimationMassMeasurement',
target_cm_okay,
ros_topic_name='/graspSuccessEstimator/status',
ros_topic_type='Float64',
goal=np.array([RESPONSES.ESTIMATION_RESULT_OKAY.value]),
))
# 2b.4 The grasp success estimator module is inactive => directly continue lifting the object and go to drop off
control_sequence.append(
ha.RosTopicSwitch(
'EstimationMassMeasurement',
target_cm_okay,
ros_topic_name='/graspSuccessEstimator/status',
ros_topic_type='Float64',
goal=np.array([RESPONSES.GRASP_SUCCESS_ESTIMATOR_INACTIVE.value]),
))
# 2b.5 Timeout (grasp success estimator module not started, an error occurred or it takes too long)
control_sequence.append(
ha.TimeSwitch('EstimationMassMeasurement',
target_cm_okay,
duration=success_estimator_timeout))
# 2b.6 There is no special switch for unknown error response (estimator signals ESTIMATION_RESULT_UNKNOWN_FAILURE)
# Instead the timeout will trigger giving the user an opportunity to notice the erroneous result in the GUI.
# 3. After estimation measurement control modes.
extra_failure_cms = set()
if target_cm_estimation_no_object != target_cm_okay:
extra_failure_cms.add(target_cm_estimation_no_object)
if target_cm_estimation_too_many != target_cm_okay:
extra_failure_cms.add(target_cm_estimation_too_many)
for cm in extra_failure_cms:
if cm.startswith('failure_rerun'):
# 3.1 Failure control mode representing grasping failure, which might be corrected by re-running the plan.
control_sequence.append(ha.BlockJointControlMode(name=cm))
if cm.startswith('failure_replan'):
# 3.2 Failure control mode representing grasping failure, which can't be corrected and requires to re-plan.
control_sequence.append(ha.BlockJointControlMode(name=cm))
# 3.3 Success control mode. Lift hand even further
control_sequence.append(
ha.CartesianVelocityControlMode(up_twist,
controller_name='GoUpHTransform',
name=target_cm_okay,
reference_frame="world"))
# 3b. Switch after half the lift time
control_sequence.append(
ha.TimeSwitch(target_cm_okay, 'GoDropOff', duration=lift_time / 2))
# 4. Go above the object - Pregrasp
control_sequence.append(
ha.InterpolatedHTransformControlMode(drop_off_pose,
controller_name='GoToDropPose',
goal_is_relative='0',
name='GoDropOff'))
# 4b. Switch when hand reaches the goal pose
control_sequence.append(
ha.FramePoseSwitch('GoDropOff',
'PlaceInIFCO',
controller='GoToDropPose',
epsilon='0.01'))
return control_sequence
def create_wall_grasp(chosen_object, wall_frame, handarm_params, pregrasp_transform, alternative_behavior=None):
object_type = chosen_object['type']
# Get the relevant parameters for hand object combination
if object_type in handarm_params['WallGrasp']:
params = handarm_params['WallGrasp'][object_type]
else:
params = handarm_params['WallGrasp']['object']
# Get params per phase
# Approach phase
downward_force = params['downward_force']
down_speed = params['down_speed']
lift_time = params['corrective_lift_duration']
up_speed = params['up_speed']
wall_force = params['wall_force']
slide_speed = params['slide_speed']
# Grasping phase
pre_grasp_twist = params['pre_grasp_twist']
pre_grasp_rotate_time = params['pre_grasp_rotation_duration']
hand_closing_time = params['hand_closing_duration']
hand_synergy = params['hand_closing_synergy']
# Post-grasping phase
post_grasp_twist = params['post_grasp_twist']
post_grasp_rotate_time = params['post_grasp_rotation_duration']
success_estimator_timeout = handarm_params['success_estimator_timeout']
# Set the twists to use TRIK controller with
# Down speed is negative because it is defined on the world frame
down_twist = np.array([0, 0, -down_speed, 0, 0, 0])
# Slow Up speed is positive because it is defined on the world frame
up_twist = np.array([0, 0, up_speed, 0, 0, 0])
# Slide twist is positive because it is defined on the EE frame
slide_twist = np.array([0, 0, slide_speed, 0, 0, 0])
control_sequence = []
# 0 trigger pre-shaping the hand (if there is a synergy). The 2 in the name represents a wall grasp.
control_sequence.append(ha.BlockJointControlMode(name='softhand_preshape_2_1'))
# 0b. Time for pre-shape
control_sequence.append(ha.TimeSwitch('softhand_preshape_2_1', 'PreGrasp', duration=hand_closing_time))
# 1. Go above the object - Pregrasp
control_sequence.append(ha.InterpolatedHTransformControlMode(pregrasp_transform, controller_name = 'GoAboveObject', goal_is_relative='0', name = 'PreGrasp'))
# 1b. Switch when hand reaches the goal pose
control_sequence.append(ha.FramePoseSwitch('PreGrasp', 'PrepareForMassMeasurement', controller = 'GoAboveObject', epsilon = '0.01'))
# 2. Go to gravity compensation
control_sequence.append(ha.BlockJointControlMode(name = 'PrepareForMassMeasurement'))
# 2b. Wait for a bit to allow vibrations to attenuate
control_sequence.append(ha.TimeSwitch('PrepareForMassMeasurement', 'ReferenceMassMeasurement', duration = 0.5))
# 3. Reference mass measurement with empty hand (TODO can this be replaced by offline calibration?)
control_sequence.append(ha.BlockJointControlMode(name='ReferenceMassMeasurement')) # TODO use gravity comp instead?
# 3b. Switches when reference measurement was done
# 3b.1 Successful reference measurement
control_sequence.append(ha.RosTopicSwitch('ReferenceMassMeasurement', 'GoDown',
ros_topic_name='/graspSuccessEstimator/status', ros_topic_type='Float64',
goal=np.array([RESPONSES.REFERENCE_MEASUREMENT_SUCCESS.value]),
))
# 3b.2 The grasp success estimator module is inactive
control_sequence.append(ha.RosTopicSwitch('ReferenceMassMeasurement', 'GoDown',
ros_topic_name='/graspSuccessEstimator/status', ros_topic_type='Float64',
goal=np.array([RESPONSES.GRASP_SUCCESS_ESTIMATOR_INACTIVE.value]),
))
# 3b.3 Timeout (grasp success estimator module not started, an error occurred or it takes too long)
control_sequence.append(ha.TimeSwitch('ReferenceMassMeasurement', 'GoDown',
duration=success_estimator_timeout))
# 3b.4 There is no special switch for unknown error response (estimator signals REFERENCE_MEASUREMENT_FAILURE)
# Instead the timeout will trigger giving the user an opportunity to notice the erroneous result in the GUI.
# 4. Go down onto the object/table, in world frame
control_sequence.append( ha.CartesianVelocityControlMode(down_twist,
controller_name='GoDown',
name="GoDown",
reference_frame="world"))
# 4b. Switch when force threshold is exceeded
force = np.array([0, 0, downward_force, 0, 0, 0])
control_sequence.append(ha.ForceTorqueSwitch('GoDown',
'LiftHand',
goal=force,
norm_weights=np.array([0, 0, 1, 0, 0, 0]),
jump_criterion="THRESH_UPPER_BOUND",
goal_is_relative='1',
frame_id='world'))
# 5. Lift upwards so the hand doesn't slide on table surface
control_sequence.append(
ha.CartesianVelocityControlMode(up_twist, controller_name='Lift1', name="LiftHand",
reference_frame="world"))
# 5b. We switch after a short time as this allows us to do a small, precise lift motion
control_sequence.append(ha.TimeSwitch('LiftHand', 'SlideToWall', duration=lift_time))
# 6. Go towards the wall to slide object to wall
control_sequence.append(
ha.CartesianVelocityControlMode(slide_twist, controller_name='SlideToWall',
name="SlideToWall", reference_frame="EE"))
# 6b. Switch when the f/t sensor is triggered with normal force from wall
force = np.array([0, 0, wall_force, 0, 0, 0])
control_sequence.append(ha.ForceTorqueSwitch('SlideToWall', 'SlideBackFromWall', 'ForceSwitch', goal=force,
norm_weights=np.array([0, 0, 1, 0, 0, 0]),
jump_criterion="THRESH_UPPER_BOUND", goal_is_relative='1',
frame_id='world', frame=wall_frame))
# 7. Go back a bit to allow the hand to inflate
control_sequence.append(
ha.CartesianVelocityControlMode(pre_grasp_twist, controller_name='SlideBackFromWall',
name="SlideBackFromWall", reference_frame="EE"))
# The 2 in softhand_close_2 represents a wall grasp. This way the strategy is encoded in the HA.
# The 0 encodes the synergy id
mode_name_hand_closing = 'softhand_close_2_0'
# 7b. We switch after a short time
control_sequence.append(ha.TimeSwitch('SlideBackFromWall', mode_name_hand_closing, duration=pre_grasp_rotate_time))
# 8. Maintain contact while closing the hand
# Call general hand controller
control_sequence.append(ha.GeneralHandControlMode(goal = np.array([1]), name = mode_name_hand_closing, synergy = hand_synergy))
# 8b. Switch when hand closing duration ends
control_sequence.append(ha.TimeSwitch(mode_name_hand_closing, 'PostGraspRotate', duration=hand_closing_time))
# 9. Rotate a bit to roll the object in the hand
control_sequence.append(
ha.CartesianVelocityControlMode(post_grasp_twist, controller_name='PostGraspRotate',
name="PostGraspRotate", reference_frame="EE"))
# 9b. We switch after a short time
control_sequence.append(ha.TimeSwitch('PostGraspRotate', 'GoUp_1', duration=post_grasp_rotate_time))
return control_sequence
def get_transport_recipe(chosen_object, handarm_params, reaction, FailureCases,
grasp_type):
object_type = chosen_object['type']
# Get the relevant parameters for hand object combination
if object_type in handarm_params[grasp_type]:
params = handarm_params[grasp_type][object_type]
else:
params = handarm_params[grasp_type]['object']
# ############################ #
# Get params
# ############################ #
up_dist = params['up_dist']
# split the lifted distance into two consecutive lifts (with success estimation in between)
scale_up = 0.7
dir_up1 = tra.translation_matrix([0, 0, scale_up * up_dist])
dir_up2 = tra.translation_matrix([0, 0, (1.0 - scale_up) * up_dist])
success_estimator_timeout = handarm_params['success_estimator_timeout']
drop_off_config = handarm_params['drop_off_config']
# ############################ #
# Assemble controller sequence
# ############################ #
control_sequence = []
# 1. Lift upwards a little bit (half way up)
control_sequence.append(
ha.InterpolatedHTransformControlMode(dir_up1,
controller_name='GoUpHTransform',
name='GoUp_1',
goal_is_relative='1',
reference_frame="world"))
# 1b. Switch when joint configuration (half way up) is reached
control_sequence.append(
ha.FramePoseSwitch('GoUp_1',
'PrepareForEstimationMassMeasurement',
controller='GoUpHTransform',
epsilon='0.01',
goal_is_relative='1',
reference_frame="world"))
# 2. Hold current joint config
control_sequence.append(
ha.BlockJointControlMode(name='PrepareForEstimationMassMeasurement'))
# 2b. Wait for a bit to allow vibrations to attenuate # TODO check if this is required...
control_sequence.append(
ha.TimeSwitch('PrepareForEstimationMassMeasurement',
'EstimationMassMeasurement',
duration=0.5))
# 3. Measure the mass again and estimate number of grasped objects (grasp success estimation)
control_sequence.append(
ha.BlockJointControlMode(name='EstimationMassMeasurement'))
# 3b. Switches after estimation measurement was done
target_cm_okay = 'GoUp_2'
# 3b.1 No object was grasped => go to failure mode.
target_cm_estimation_no_object = reaction.cm_name_for(
FailureCases.MASS_ESTIMATION_NO_OBJECT, default=target_cm_okay)
control_sequence.append(
ha.RosTopicSwitch(
'EstimationMassMeasurement',
target_cm_estimation_no_object,
ros_topic_name='/graspSuccessEstimator/status',
ros_topic_type='Float64',
goal=np.array([RESPONSES.ESTIMATION_RESULT_NO_OBJECT.value]),
))
# 3b.2 More than one object was grasped => failure
target_cm_estimation_too_many = reaction.cm_name_for(
FailureCases.MASS_ESTIMATION_TOO_MANY, default=target_cm_okay)
control_sequence.append(
ha.RosTopicSwitch(
'EstimationMassMeasurement',
target_cm_estimation_too_many,
ros_topic_name='/graspSuccessEstimator/status',
ros_topic_type='Float64',
goal=np.array([RESPONSES.ESTIMATION_RESULT_TOO_MANY.value]),
))
# 3b.3 Exactly one object was grasped => success (continue lifting the object and go to drop off)
control_sequence.append(
ha.RosTopicSwitch(
'EstimationMassMeasurement',
target_cm_okay,
ros_topic_name='/graspSuccessEstimator/status',
ros_topic_type='Float64',
goal=np.array([RESPONSES.ESTIMATION_RESULT_OKAY.value]),
))
# 3b.4 The grasp success estimator module is inactive => directly continue lifting the object and go to drop off
control_sequence.append(
ha.RosTopicSwitch(
'EstimationMassMeasurement',
target_cm_okay,
ros_topic_name='/graspSuccessEstimator/status',
ros_topic_type='Float64',
goal=np.array([RESPONSES.GRASP_SUCCESS_ESTIMATOR_INACTIVE.value]),
))
# 3b.5 Timeout (grasp success estimator module not started, an error occurred or it takes too long)
control_sequence.append(
ha.TimeSwitch('EstimationMassMeasurement',
target_cm_okay,
duration=success_estimator_timeout))
# 3b.6 There is no special switch for unknown error response (estimator signals ESTIMATION_RESULT_UNKNOWN_FAILURE)
# Instead the timeout will trigger giving the user an opportunity to notice the erroneous result in the GUI.
# 4. After estimation measurement control modes.
extra_failure_cms = set()
if target_cm_estimation_no_object != target_cm_okay:
extra_failure_cms.add(target_cm_estimation_no_object)
if target_cm_estimation_too_many != target_cm_okay:
extra_failure_cms.add(target_cm_estimation_too_many)
for cm in extra_failure_cms:
if cm.startswith('failure_rerun'):
# 4.1 Failure control mode representing grasping failure, which might be corrected by re-running the plan.
control_sequence.append(ha.GravityCompensationMode(name=cm))
if cm.startswith('failure_replan'):
# 4.2 Failure control mode representing grasping failure, which can't be corrected and requires to re-plan.
control_sequence.append(ha.GravityCompensationMode(name=cm))
# 4.3 Success control mode. Lift hand even further
control_sequence.append(
ha.InterpolatedHTransformControlMode(dir_up2,
controller_name='GoUpHTransform',
name=target_cm_okay,
goal_is_relative='1',
reference_frame="world"))
# 4.3b Switch when joint configuration is reached
control_sequence.append(
ha.FramePoseSwitch(target_cm_okay,
'GoDropOff',
controller='GoUpHTransform',
epsilon='0.01',
goal_is_relative='1',
reference_frame="world"))
# 5. Go to dropOFF location
control_sequence.append(
ha.JointControlMode(drop_off_config,
controller_name='GoToDropJointConfig',
name='GoDropOff'))
# 5.b Switch when joint is reached
control_sequence.append(
ha.JointConfigurationSwitch('GoDropOff',
'finished',
controller='GoToDropJointConfig',
epsilon=str(math.radians(7.))))
# 6. Block joints to finish motion and hold object in air
control_sequence.append(ha.BlockJointControlMode(name='finished'))
return control_sequence