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 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_hybrid_automaton(roblib_trajectory):
hybrid_automaton = ha.HybridAutomaton(current_control_mode='goto_0')
# go through trajectory points and add control mode + switch
points = roblib_trajectory.shape[0]
in_contact = False
for t in range(points):
# add motion depending on contact change
next_in_contact = (roblib_trajectory[t, 7] == 1)
normal = roblib_trajectory[t, 8:11]
normal_undefined = np.linalg.norm(normal) == 0
ctrl_mode_name = 'goto_{}'.format(t)
next_ctrl_mode_name = 'goto_{}'.format(t + 1)
ctrller_name = 'cntrl_{}'.format(t)
mykp = np.array([300, 200, 150, 200, 10, 10, 5])
mykv = np.array([2, 4, 2, 0.8, 0.2, 0.2, 0.15])
#mykp = np.array([900, 2500, 600, 500, 50, 50, 8])
#mykv = np.array([10, 20, 5, 2, 0.5, 0.5, 0.05])
myv_max = np.array([0.1, 0.04]) #np.array([0.25, 0.08])
if absolute_motion:
if t == 0:
print "(absolute goal)"
rlab_traj = roblib_trajectory[:, :7].transpose()
print rlab_traj
ctrl_mode = ha.JointControlMode(rlab_traj,
name=ctrl_mode_name,
controller_name=ctrller_name)
# that's important to avoid jerky behavior on the spot! --> it's actually the minimum_completion_time
#ctrl_mode.controlset.controllers[0].properties['completion_times'] = '[1,1]1.0'
ctrl_mode.controlset.controllers[0].properties['kp'] = mykp
ctrl_mode.controlset.controllers[0].properties['kv'] = mykv
ctrl_switch = ha.JointConfigurationSwitch(
ctrl_mode_name,
next_ctrl_mode_name,
controller=ctrller_name,
epsilon=str(math.radians(5.0)))
elif (not in_contact or normal_undefined) and not next_in_contact:
#if True:
# free space motion
print "(free space -> free space)"
if t == 0:
ctrl_mode = ha.JointControlMode(roblib_trajectory[t, :7],
name=ctrl_mode_name,
controller_name=ctrller_name)
# that's important to avoid jerky behavior on the spot! --> it's actually the minimum_completion_time
ctrl_mode.controlset.controllers[0].properties[
'completion_times'] = '[1,1]1.0'
#ctrl_mode.controlset.controllers[0].properties['kp'] = mykp
#ctrl_mode.controlset.controllers[0].properties['kv'] = mykv
ctrl_switch = ha.JointConfigurationSwitch(
ctrl_mode_name,
next_ctrl_mode_name,
controller=ctrller_name,
epsilon=str(math.radians(5.0)))
elif t == (points - 1): # last one
goal = roblib_trajectory[t, :7] - roblib_trajectory[t - 1, :7]
ctrl_mode = ha.JointControlMode(goal,
goal_is_relative='1',
name=ctrl_mode_name,
controller_name=ctrller_name)
ctrl_switch = ha.JointConfigurationSwitch(
ctrl_mode_name,
next_ctrl_mode_name,
controller=ctrller_name,
epsilon=str(math.radians(5.0)),
goal_is_relative='1')
#tmp = ha.ForceTorqueSwitch(ctrl_mode_name, next_ctrl_mode_name, goal = np.array([0., 0., 0., 0, 0, 0]), norm_weights = np.array([1, 1, 1, 0, 0, 0]), negate = '1', epsilon='1', jump_criterion = "NORM_L2", frame_id = '', goal_is_relative = '1')
#ctrl_switch.add(tmp.con)
else:
goal = (roblib_trajectory[t, :7] -
roblib_trajectory[t - 1, :7]) * 2.
ctrl_mode = ha.JointControlMode(goal,
goal_is_relative='1',
name=ctrl_mode_name,
controller_name=ctrller_name)
ctrl_switch = ha.JointConfigurationSwitch(
ctrl_mode_name,
next_ctrl_mode_name,
name='TheSpecialOne',
controller=ctrller_name,
epsilon=str(math.radians(5.0)),
goal_is_relative='1')
elif (not in_contact or normal_undefined) and next_in_contact:
# move until contact
print "(free space -> in contact)"
if False:
goal = extrapolate_transform(roblib_trajectory[t - 1, -7:],
roblib_trajectory[t, -7:], 1.2)
ctrl_mode = ha.HTransformControlMode(
goal,
name=ctrl_mode_name,
controller_name=ctrller_name,
goal_is_relative='1')
ctrl_mode.controlset.controllers[0].properties[
'v_max'] = myv_max
goal = (roblib_trajectory[t, :7] -
roblib_trajectory[t - 1, :7]) * 2.
#goal = extrapolate_configuration(roblib_trajectory[t-1, :7], roblib_trajectory[t, :7], 1.3)
ctrl_mode = ha.JointControlMode(goal,
goal_is_relative='1',
name=ctrl_mode_name,
controller_name=ctrller_name)
#ctrl_mode.controlset.controllers[0].properties['kp'] = mykp
#ctrl_mode.controlset.controllers[0].properties['kv'] = mykv
ctrl_mode.controlset.controllers[0].properties['v_max'] = np.array(
[0.1] * 7)
ctrl_switch = ha.ForceTorqueSwitch(
ctrl_mode_name,
next_ctrl_mode_name,
epsilon='4',
goal=np.array([0., 0., 0., 0, 0, 0]),
norm_weights=np.array([1, 1, 1, 0, 0, 0]),
negate='1',
jump_criterion="NORM_L2",
frame_id='',
goal_is_relative='1')
time_switch = ha.TimeSwitch(ctrl_mode_name,
next_ctrl_mode_name,
duration=1.0)
ctrl_switch.add(time_switch.conditions[0])
elif in_contact and next_in_contact:
# move until contact changes
print "(in contact -> in contact)"
goal = extrapolate_configuration(roblib_trajectory[t - 1, :7],
roblib_trajectory[t, :7], 1.5)
ctrl_mode = ha.JointControlMode(goal,
name=ctrl_mode_name,
controller_name=ctrller_name)
ctrl_switch = ha.ForceTorqueSwitch(
ctrl_mode_name,
next_ctrl_mode_name,
goal=np.array([0., 0., 0., 0, 0, 0]),
norm_weights=np.array([1, 1, 1, 0, 0, 0]),
negate='1',
epsilon='1',
jump_criterion="NORM_L2",
frame_id='',
goal_is_relative='1')
elif in_contact and not next_in_contact:
# move until no more contact
print "(in contact -> free space) normal_defined:", ~normal_undefined
ft_in_ee = tra.translation_matrix([0, 0, 0])
goal = extrapolate_transform([0, 0, 0, 0, 0, 0, 1],
roblib_trajectory[t, -7:], 1.0)
delta = extrapolate_transform(roblib_trajectory[t - 1, -7:],
roblib_trajectory[t, -7:], 1.0)
normal_in_ee = np.dot(
tra.inverse_matrix(
transform_from_vector(roblib_trajectory[t - 1,
-7:]))[:3, :3],
normal)
normal_in_ft = np.dot(ft_in_ee[:3, :3], normal_in_ee)
print "Normal in world: ", normal # in world
print "Normal in ee: ", normal_in_ee
# project onto normal plane
print delta[:3, 3]
#print desired_delta
dist = np.linalg.norm(delta[:3, 3])
delta[:3, 3] = delta[:3, 3] * (0.03 / dist)
print dist
print normal_in_ft
print delta
#ctrl_mode = ha.HTransformControlMode(delta, name = ctrl_mode_name, controller_name = ctrller_name, goal_is_relative='1')
#ctrl_mode = ha.InterpolatedHTransformImpedanceControlMode(goal, name = ctrl_mode_name, controller_name = ctrller_name, goal_is_relative='1')
# three things needed:
# 1) desired_displacement: delta vector in ee frame
# 2) force_gradient: normal vector in ee frame
ctrl_mode = ha.ControlMode(name=ctrl_mode_name).set(
ha.NakamuraControlSet().add(
ha.ForceHTransformController(
name=ctrller_name,
desired_distance=dist * 1.2,
desired_displacement=tra.translation_matrix(delta[:3,
3]),
#force_gradient=tra.translation_matrix([0, 0, 0.005]),
force_gradient=tra.translation_matrix(-normal_in_ee *
0.0025),
desired_force_dimension=np.array(-normal_in_ft +
[0, 0, 0]))))
ctrl_mode.controlset.controllers[0].properties[
'desired_min_force'] = '-5'
ctrl_mode.controlset.controllers[0].properties[
'desired_max_force'] = '-3'
#ctrl_mode = ha.ControlMode(name = ctrl_mode_name).set(ha.NakamuraControlSet().add(ha.ForceHTransformController(name = ctrller_name, desired_distance = dist*1.2, desired_displacement=tra.translation_matrix(delta[:3, 3]), force_gradient=tra.translation_matrix(normal * 0.005), desired_force_dimension=np.array([0, 0, 1, 0, 0, 0]))))
ctrl_mode.controlset.controllers[0].properties['v_max'] = np.array(
[0.01, 0.01]) #myv_max
ctrl_switch = ha.ForceTorqueSwitch(
ctrl_mode_name,
next_ctrl_mode_name,
epsilon='1.2',
goal_is_relative='0',
negate='0',
goal=np.array([0., 0., 0., 0, 0, 0]),
norm_weights=np.array([1, 1, 1, 0, 0, 0]),
jump_criterion="NORM_L1",
frame_id='')
pose_condition = ha.JumpCondition('FrameDisplacementSensor',
goal=np.zeros(3),
controller='',
jump_criterion='NORM_L2',
epsilon='{}'.format(dist * 0.5),
negate='1',
goal_is_relative='1',
frame_id='EE',
norm_weights=np.ones(3))
ctrl_switch.add(pose_condition)
#del ctrl_switch.conditions[0]
if t == (points - 1):
hybrid_automaton.add([ctrl_mode])
elif not absolute_motion:
hybrid_automaton.add([ctrl_mode, ctrl_switch])
in_contact = next_in_contact
# add final gravity mode
#hybrid_automaton.add(ha.GravityCompensationMode(name = 'goto_{}'.format(points)))
#hybrid_automaton.add(ha.JointControlMode(np.zeros(7), name = 'goto_{}'.format(points), goal_is_relative='1'))
return hybrid_automaton
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