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