def goto_start():
control_sequence = []
rviz_frames = []
goal_pose = tra.translation_matrix([0, 0, 1.0])
control_sequence.append(
ha.InterpolatedHTransformControlMode(goal_pose,
controller_name='GoStart',
goal_is_relative='0',
name='Initial'))
control_sequence.append(
ha.FramePoseSwitch('Initial',
'Pregrasp',
controller='GoStart',
epsilon='0.05'))
print(goal_pose)
rviz_frames.append(goal_pose)
return cookbook.sequence_of_modes_and_switches_with_saftyOn(
control_sequence), rviz_frames
def create_surface_grasp(object_frame, support_surface_frame, handarm_params,
object_type):
# Get the relevant parameters for hand object combination
print(object_type)
if (object_type in handarm_params['surface_grasp']):
params = handarm_params['surface_grasp'][object_type]
else:
params = handarm_params['surface_grasp']['object']
hand_transform = params['hand_transform']
pregrasp_transform = params['pregrasp_transform']
post_grasp_transform = params['post_grasp_transform'] # TODO: USE THIS!!!
drop_off_config = params['drop_off_config']
downward_force = params['downward_force']
hand_closing_time = params['hand_closing_duration']
hand_synergy = params['hand_closing_synergy']
down_speed = params['down_speed']
up_speed = params['up_speed']
# Set the initial pose above the object
goal_ = np.copy(object_frame) #TODO: this should be support_surface_frame
goal_[:3, 3] = tra.translation_from_matrix(object_frame)
goal_ = goal_.dot(hand_transform)
#the grasp frame is symmetrical - check which side is nicer to reach
#this is a hacky first version for our WAM
zflip_transform = tra.rotation_matrix(math.radians(180.0), [0, 0, 1])
if goal_[0][0] < 0:
goal_ = goal_.dot(zflip_transform)
# hand pose above object
pre_grasp_pose = goal_.dot(pregrasp_transform)
# Set the directions to use TRIK controller with
dirDown = tra.translation_matrix([0, 0, -down_speed])
dirUp = tra.translation_matrix([0, 0, up_speed])
# Set the frames to visualize with RViz
rviz_frames = []
#rviz_frames.append(object_frame)
#rviz_frames.append(goal_)
#rviz_frames.append(pre_grasp_pose)
# assemble controller sequence
control_sequence = []
# # 1. Go above the object - Pregrasp
# control_sequence.append(
# ha.InterpolatedHTransformControlMode(pre_grasp_pose, controller_name='GoAboveIFCO', goal_is_relative='0',
# name='Pre_preGrasp'))
#
# # 1b. Switch when hand reaches the goal pose
# control_sequence.append(ha.FramePoseSwitch('Pre_preGrasp', 'Pregrasp', controller='GoAboveIFCO', epsilon='0.01'))
#Joao test
#control_sequence.append(
# ha.JointControlMode(np.zeros(7), goal_is_relative='0', completion_times=np.array([10, 10, 10, 10, 10, 10, 10]), controller_name='GoToDropJointConfig1', name='GoDropOff1'))
#
# # 7.b Switch when joint is reached
# #control_sequence.append(ha.TimeSwitch('GoDropOff1', 'Pregrasp', duration = hand_closing_time))
#control_sequence.append(ha.JointConfigurationSwitch('GoDropOff1', 'Pregrasp', controller='GoToDropJointConfig1',epsilon=str(math.radians(17.))))
#
# control_sequence.append(
# ha.JointControlMode(np.array([1.1, 1.2, 0.3, 0.4, 0.5, 1.6, 0.7]), goal_is_relative='0', completion_times=np.array([10, 10, 10, 10, 10, 10, 10]),
# controller_name='GoToDropJointConfig2', name='GoDropOff2'))
#
# control_sequence.append(ha.JointConfigurationSwitch('GoDropOff2', 'Pregrasp', controller='GoToDropJointConfig2',epsilon=str(math.radians(7.))))
# 2. Go above the object - Pregrasp
control_sequence.append(
ha.InterpolatedHTransformControlMode(tra.translation_matrix(
[0, 0, 1.0]),
controller_name='GoStart',
goal_is_relative='0',
name='Initial'))
control_sequence.append(
ha.FramePoseSwitch('Initial',
'Pregrasp',
controller='GoStart',
epsilon='0.05'))
print(tra.translation_matrix([0, 0, 1.0]))
#rviz_frames.append(object_frame)
pregrasp_frame = tra.concatenate_matrices(
object_frame, tra.inverse_matrix(pregrasp_transform),
tra.inverse_matrix(hand_transform))
print(pregrasp_frame)
import IPython
#IPython.embed()
rviz_frames.append(tra.concatenate_matrices(pregrasp_frame,
hand_transform))
#rviz_frames.append(pregrasp_frame)
publish_rviz_markers(rviz_frames, 'world', handarm_params)
control_sequence.append(
ha.InterpolatedHTransformControlMode(pregrasp_frame,
controller_name='GoAboveObject',
goal_is_relative='0',
name='Pregrasp'))
#
# # 2b. Switch when hand reaches the goal pose
control_sequence.append(
ha.FramePoseSwitch('Pregrasp',
'finished',
controller='GoAboveObject',
epsilon='0.01'))
#
# # 3. Go down onto the object (relative in world frame) - Godown
# control_sequence.append(
# ha.InterpolatedHTransformControlMode(dirDown, controller_name='GoDown', goal_is_relative='1', name="GoDown",
# reference_frame="world"))
#
# force = np.array([0, 0, 0.5*downward_force, 0, 0, 0])
# # 3b. Switch when goal is reached
# #JOAO CHANGED
# #control_sequence.append(ha.ForceTorqueSwitch('GoDown', 'softhand_close', goal = force,norm_weights = np.array([0, 0, 1, 0, 0, 0]), jump_criterion = "THRESH_UPPER_BOUND", goal_is_relative = '1', frame_id = 'world', port = '2'))
# control_sequence.append(ha.FramePoseSwitch('Pregrasp', 'GoDown', controller = 'GoAboveObject', epsilon = '0.01'))
#
#
# # 4. Maintain the position
# desired_displacement = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0 ], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
# force_gradient = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0 ], [0.0, 0.0, 1.0, 0.005], [0.0, 0.0, 0.0, 1.0]])
# desired_force_dimension = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
#
# if handarm_params['isForceControllerAvailable']:
# control_sequence.append(ha.HandControlMode_ForceHT(name = 'softhand_close', synergy = hand_synergy,
# desired_displacement = desired_displacement,
# force_gradient = force_gradient,
# desired_force_dimension = desired_force_dimension))
# else:
# # if hand is not RBO then create general hand closing mode?
# control_sequence.append(ha.SimpleRBOHandControlMode(goal = np.array([1])))
#
#
# # 4b. Switch when hand closing time ends
# # joao changed
# #control_sequence.append(ha.TimeSwitch('softhand_close', 'PostGraspRotate', duration = hand_closing_time))
# control_sequence.append(ha.FramePoseSwitch('Pregrasp', 'GoDown', controller = 'GoAboveObject', epsilon = '0.01'))
#
#
# # 5. Rotate hand after closing and before lifting it up
# # relative to current hand pose
# control_sequence.append(
# ha.HTransformControlMode(post_grasp_transform, controller_name='PostGraspRotate', name='PostGraspRotate', goal_is_relative='1', ))
#
# # 5b. Switch when hand rotated
# control_sequence.append(ha.FramePoseSwitch('PostGraspRotate', 'GoUp', controller='PostGraspRotate', epsilon='0.01', goal_is_relative='1', reference_frame = 'EE'))
#
# # 6. Lift upwards
# control_sequence.append(ha.InterpolatedHTransformControlMode(dirUp, controller_name = 'GoUpHTransform', name = 'GoUp', goal_is_relative='1', reference_frame="world"))
#
# # 6b. Switch when joint is reached
# control_sequence.append(ha.FramePoseSwitch('GoUp', 'GoDropOff', controller = 'GoUpHTransform', epsilon = '0.01', goal_is_relative='1', reference_frame="world"))
#
# # 7. Go to dropOFF
# control_sequence.append(ha.JointControlMode(drop_off_config, controller_name = 'GoToDropJointConfig', name = 'GoDropOff'))
#
# # 7.b Switch when joint is reached
# control_sequence.append(ha.JointConfigurationSwitch('GoDropOff', 'finished', controller = 'GoToDropJointConfig', epsilon = str(math.radians(7.))))
# 8. Block joints to finish motion and hold object in air
finishedMode = ha.ControlMode(name='finished')
finishedSet = ha.ControlSet()
finishedSet.add(
ha.Controller(name='JointSpaceController',
type='InterpolatedJointController',
goal=np.ones(7) * 0.2,
goal_is_relative=1,
v_max='[0,0]',
a_max='[0,0]'))
finishedMode.set(finishedSet)
control_sequence.append(finishedMode)
return cookbook.sequence_of_modes_and_switches_with_saftyOn(
control_sequence), rviz_frames
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 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,
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(object_frame,
bounding_box,
wall_frame,
handarm_params,
object_type,
pre_grasp_pose=None,
alternative_behavior=None):
# Get the parameters from the handarm_parameters.py file
obj_type_params = {}
obj_params = {}
if object_type in handarm_params['wall_grasp']:
obj_type_params = handarm_params['wall_grasp'][object_type]
if 'object' in handarm_params['wall_grasp']:
obj_params = handarm_params['wall_grasp']['object']
hand_transform = getParam(obj_type_params, obj_params, 'hand_transform')
downward_force = getParam(obj_type_params, obj_params, 'downward_force')
wall_force = getParam(obj_type_params, obj_params, 'wall_force')
slide_IFCO_speed = getParam(obj_type_params, obj_params, 'slide_speed')
pre_approach_transform = getParam(obj_type_params, obj_params,
'pre_approach_transform')
scooping_angle_deg = getParam(obj_type_params, obj_params,
'scooping_angle_deg')
init_joint_config = handarm_params['init_joint_config']
thumb_pos_preshape = getParam(obj_type_params, obj_params,
'thumb_pos_preshape')
post_grasp_transform = getParam(obj_type_params, obj_params,
'post_grasp_transform')
rotate_time = handarm_params['rotate_duration']
down_IFCO_speed = handarm_params['down_IFCO_speed']
# Set the twists to use TRIK controller with
# Down speed is negative because it is defined on the world frame
down_IFCO_twist = np.array([0, 0, -down_IFCO_speed, 0, 0, 0])
# Slide speed is positive because it is defined on the EE frame + rotation of the scooping angle
slide_IFCO_twist_matrix = tra.rotation_matrix(
math.radians(scooping_angle_deg),
[1, 0, 0]).dot(tra.translation_matrix([0, 0, slide_IFCO_speed]))
slide_IFCO_twist = np.array([
slide_IFCO_twist_matrix[0, 3], slide_IFCO_twist_matrix[1, 3],
slide_IFCO_twist_matrix[2, 3], 0, 0, 0
])
rviz_frames = []
if pre_grasp_pose is None:
# this is the EC frame. It is positioned like object and oriented to the wall
ec_frame = np.copy(wall_frame)
ec_frame[:3, 3] = tra.translation_from_matrix(object_frame)
ec_frame = ec_frame.dot(hand_transform)
pre_approach_pose = ec_frame.dot(pre_approach_transform)
else:
pre_approach_pose = pre_grasp_pose
# Rviz debug frames
rviz_frames.append(object_frame)
rviz_frames.append(pre_approach_pose)
# rviz_frames.append(pm.toMatrix(pm.fromMsg(res.reachable_hand_pose)))
control_sequence = []
# # 0. Go to initial nice mid-joint configuration
# control_sequence.append(ha.JointControlMode(goal = init_joint_config, goal_is_relative = '0', name = 'init', controller_name = 'GoToInitController'))
# # 0b. Switch when config is reached
# control_sequence.append(ha.JointConfigurationSwitch('init', 'Pregrasp', controller = 'GoToInitController', epsilon = str(math.radians(1.0))))
# 1. Go above the object
control_sequence.append(
ha.InterpolatedHTransformControlMode(pre_approach_pose,
controller_name='GoAboveObject',
goal_is_relative='0',
name="Pregrasp"))
# 1b. Switch when hand reaches the goal pose
control_sequence.append(
ha.FramePoseSwitch('Pregrasp',
'StayStill',
controller='GoAboveObject',
epsilon='0.01'))
# 2. Go to gravity compensation
control_sequence.append(
ha.CartesianVelocityControlMode(np.array([0, 0, 0, 0, 0, 0]),
controller_name='StayStillCtrl',
name="StayStill",
reference_frame="EE"))
# 2b. Wait for a bit to allow vibrations to attenuate
control_sequence.append(
ha.TimeSwitch('StayStill',
'softhand_pretension',
duration=handarm_params['stay_still_duration']))
# 3. Pretension
speed = np.array([20])
thumb_pos = np.array([0, 0, 0])
diff_pos = np.array([0, 0, 15])
thumb_contact_force = np.array([0])
thumb_grasp_force = np.array([0])
diff_contact_force = np.array([0])
diff_grasp_force = np.array([0])
thumb_pretension = np.array([0])
diff_pretension = np.array([15])
force_feedback_ratio = np.array([0])
prox_level = np.array([0])
touch_level = np.array([0])
mode = np.array([0])
command_count = np.array([0])
control_sequence.append(
ha.ros_CLASHhandControlMode(goal=np.concatenate(
(speed, thumb_pos, diff_pos, thumb_contact_force,
thumb_grasp_force, diff_contact_force, diff_grasp_force,
thumb_pretension, diff_pretension, force_feedback_ratio,
prox_level, touch_level, mode, command_count)),
name='softhand_pretension'))
# 3b. Switch when hand closing time ends
control_sequence.append(
ha.TimeSwitch('softhand_pretension',
'GoDown',
duration=handarm_params['hand_closing_duration']))
# 4. Go down onto the object/table, in world frame
control_sequence.append(
ha.CartesianVelocityControlMode(down_IFCO_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',
'CloseBeforeSlide',
goal=force,
norm_weights=np.array([0, 0, 1, 0, 0, 0]),
jump_criterion="THRESH_UPPER_BOUND",
goal_is_relative='1',
frame_id='world',
port='2'))
# 5. Close a bit before sliding
speed = np.array([30])
thumb_pos = thumb_pos_preshape
diff_pos = np.array([10, 15, 0])
thumb_contact_force = np.array([0])
thumb_grasp_force = np.array([0])
diff_contact_force = np.array([0])
diff_grasp_force = np.array([0])
thumb_pretension = np.array([15])
diff_pretension = np.array([15])
force_feedback_ratio = np.array([0])
prox_level = np.array([0])
touch_level = np.array([0])
mode = np.array([0])
command_count = np.array([1])
control_sequence.append(
ha.ros_CLASHhandControlMode(goal=np.concatenate(
(speed, thumb_pos, diff_pos, thumb_contact_force,
thumb_grasp_force, diff_contact_force, diff_grasp_force,
thumb_pretension, diff_pretension, force_feedback_ratio,
prox_level, touch_level, mode, command_count)),
name='CloseBeforeSlide'))
# 5b. Time switch
control_sequence.append(
ha.TimeSwitch('CloseBeforeSlide',
'SlideToWall',
duration=handarm_params['hand_closing_duration']))
# 6. Go towards the wall to slide object to wall
control_sequence.append(
ha.CartesianVelocityControlMode(slide_IFCO_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',
'softhand_close',
'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,
port='2'))
# 7. Close the hand
speed = np.array([30])
thumb_pos = np.array([0, 50, 30])
diff_pos = np.array([55, 50, 20])
thumb_contact_force = np.array([0])
thumb_grasp_force = np.array([0])
diff_contact_force = np.array([0])
diff_grasp_force = np.array([0])
thumb_pretension = np.array([15])
diff_pretension = np.array([15])
force_feedback_ratio = np.array([0])
prox_level = np.array([0])
touch_level = np.array([0])
mode = np.array([0])
command_count = np.array([1])
control_sequence.append(
ha.ros_CLASHhandControlMode(goal=np.concatenate(
(speed, thumb_pos, diff_pos, thumb_contact_force,
thumb_grasp_force, diff_contact_force, diff_grasp_force,
thumb_pretension, diff_pretension, force_feedback_ratio,
prox_level, touch_level, mode, command_count)),
name='softhand_close'))
# 7b. Switch when hand closing time ends
control_sequence.append(
ha.TimeSwitch('softhand_close',
'PostGraspRotate',
duration=handarm_params['hand_closing_duration']))
# 8. Rotate hand after closing and before lifting it up relative to current hand pose
control_sequence.append(
ha.CartesianVelocityControlMode(post_grasp_transform,
controller_name='PostGraspRotate',
name='PostGraspRotate',
reference_frame='EE'))
# 8b. Switch when hand rotated
control_sequence.append(
ha.TimeSwitch('PostGraspRotate', 'GoUp', duration=rotate_time))
return control_sequence, rviz_frames
def create_surface_grasp(object_frame,
bounding_box,
handarm_params,
object_type,
pre_grasp_pose=None,
alternative_behavior=None):
# Get the parameters from the handarm_parameters.py file
obj_type_params = {}
obj_params = {}
if (object_type in handarm_params['surface_grasp']):
obj_type_params = handarm_params['surface_grasp'][object_type]
if 'object' in handarm_params['surface_grasp']:
obj_params = handarm_params['surface_grasp']['object']
hand_transform = getParam(obj_type_params, obj_params, 'hand_transform')
downward_force = getParam(obj_type_params, obj_params, 'downward_force')
ee_in_goal_frame = getParam(obj_type_params, obj_params,
'ee_in_goal_frame')
lift_time = getParam(obj_type_params, obj_params, 'short_lift_duration')
init_joint_config = handarm_params['init_joint_config']
down_IFCO_speed = handarm_params['down_IFCO_speed']
up_IFCO_speed = handarm_params['up_IFCO_speed']
thumb_pos_closing = getParam(obj_type_params, obj_params, 'thumb_pos')
diff_pos_closing = getParam(obj_type_params, obj_params, 'diff_pos')
thumb_pos_preshape = getParam(obj_type_params, obj_params,
'thumb_pos_preshape')
diff_pos_preshape = getParam(obj_type_params, obj_params,
'diff_pos_preshape')
zflip_transform = tra.rotation_matrix(math.radians(180.0), [0, 0, 1])
if object_frame[0][1] < 0:
object_frame = object_frame.dot(zflip_transform)
if pre_grasp_pose is None:
# Set the initial pose above the object
goal_ = np.copy(object_frame)
goal_ = goal_.dot(
hand_transform
) #this is the pre-grasp transform of the signature frame expressed in the world
goal_ = goal_.dot(ee_in_goal_frame)
else:
goal_ = pre_grasp_pose
# Set the twists to use TRIK controller with
# Down speed is positive because it is defined on the EE frame
down_IFCO_twist = np.array([0, 0, down_IFCO_speed, 0, 0, 0])
# Slow Up speed is also positive because it is defined on the world frame
up_IFCO_twist = np.array([0, 0, up_IFCO_speed, 0, 0, 0])
# Set the frames to visualize with RViz
rviz_frames = []
rviz_frames.append(object_frame)
rviz_frames.append(goal_)
# rviz_frames.append(pm.toMatrix(pm.fromMsg(res.reachable_hand_pose)))
# assemble controller sequence
control_sequence = []
# # 0. Go to initial nice mid-joint configuration
# control_sequence.append(ha.JointControlMode(goal = init_joint_config, goal_is_relative = '0', name = 'init', controller_name = 'GoToInitController'))
# # 0b. Switch when config is reached
# control_sequence.append(ha.JointConfigurationSwitch('init', 'Pregrasp', controller = 'GoToInitController', epsilon = str(math.radians(1.0))))
# 1. Go above the object - Pregrasp
control_sequence.append(
ha.InterpolatedHTransformControlMode(goal_,
controller_name='GoAboveObject',
goal_is_relative='0',
name='Pregrasp'))
# 1b. Switch when hand reaches the goal pose
control_sequence.append(
ha.FramePoseSwitch('Pregrasp',
'StayStill',
controller='GoAboveObject',
epsilon='0.01'))
# 2. Go to gravity compensation
control_sequence.append(
ha.CartesianVelocityControlMode(np.array([0, 0, 0, 0, 0, 0]),
controller_name='StayStillCtrl',
name="StayStill",
reference_frame="EE"))
# 2b. Wait for a bit to allow vibrations to attenuate
control_sequence.append(
ha.TimeSwitch('StayStill',
'softhand_preshape',
duration=handarm_params['stay_still_duration']))
speed = np.array([20])
thumb_pos = thumb_pos_preshape
diff_pos = diff_pos_preshape
thumb_contact_force = np.array([0])
thumb_grasp_force = np.array([0])
diff_contact_force = np.array([0])
diff_grasp_force = np.array([0])
thumb_pretension = np.array([0])
diff_pretension = np.array([0])
force_feedback_ratio = np.array([0])
prox_level = np.array([0])
touch_level = np.array([0])
mode = np.array([0])
command_count = np.array([0])
# 3. Preshape the hand
control_sequence.append(
ha.ros_CLASHhandControlMode(goal=np.concatenate(
(speed, thumb_pos, diff_pos, thumb_contact_force,
thumb_grasp_force, diff_contact_force, diff_grasp_force,
thumb_pretension, diff_pretension, force_feedback_ratio,
prox_level, touch_level, mode, command_count)),
name='softhand_preshape'))
# 3b. Switch when hand is preshaped
control_sequence.append(
ha.TimeSwitch('softhand_preshape',
'GoDown',
duration=handarm_params['hand_closing_duration']))
# 4. Go down onto the object (relative in EE frame) - Godown
control_sequence.append(
ha.CartesianVelocityControlMode(down_IFCO_twist,
controller_name='GoDown',
name="GoDown",
reference_frame="EE"))
# 4b. Switch when force-torque sensor is triggered
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',
port='2'))
# 5. Lift upwards so the hand can close
control_sequence.append(
ha.CartesianVelocityControlMode(up_IFCO_twist,
controller_name='Lift1',
name="LiftHand",
reference_frame="world"))
# 5b. We switch after a short time
control_sequence.append(
ha.TimeSwitch('LiftHand', 'softhand_close', duration=lift_time))
# 6. Close the hand
speed = np.array([30])
thumb_pos = thumb_pos_closing
diff_pos = diff_pos_closing
thumb_contact_force = np.array([0])
thumb_grasp_force = np.array([0])
diff_contact_force = np.array([0])
diff_grasp_force = np.array([0])
thumb_pretension = np.array([15])
diff_pretension = np.array([15])
force_feedback_ratio = np.array([0])
prox_level = np.array([0])
touch_level = np.array([0])
mode = np.array([0])
command_count = np.array([2])
control_sequence.append(
ha.ros_CLASHhandControlMode(goal=np.concatenate(
(speed, thumb_pos, diff_pos, thumb_contact_force,
thumb_grasp_force, diff_contact_force, diff_grasp_force,
thumb_pretension, diff_pretension, force_feedback_ratio,
prox_level, touch_level, mode, command_count)),
name='softhand_close'))
# 6b. Switch when hand closing time ends
control_sequence.append(
ha.TimeSwitch('softhand_close',
'GoUp',
duration=handarm_params['hand_closing_duration']))
return control_sequence, rviz_frames
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
