def execute(self, userdata):
rospy.loginfo('Executing state GRASP')
#Open SDH at the pre-grasp position -----------------------------------------------
sss.move("sdh", "cylopen")
#Get the current arm joint states.
sub = rospy.Subscriber("/arm_controller/state", JointTrajectoryControllerState, self.get_joint_state)
while sub.get_num_connections() == 0:
time.sleep(0.3)
continue
#Move to grasp position with SDH open ---------------------------------------------
#pregrasp
pre_grasp_stamped = PoseStamped();
pre_grasp_stamped.header.frame_id = "/base_link";
pre_grasp_stamped.pose = userdata.grasp_configuration.pre_grasp;
#grasp
grasp_stamped = PoseStamped();
grasp_stamped.header.frame_id = "/base_link";
grasp_stamped.pose = userdata.grasp_configuration.grasp;
#offset
"""
offset_x = 0#(userdata.grasp_configuration.grasp.position.x - userdata.grasp_configuration.pre_grasp.position.x)/3
offset_y = 0#(userdata.grasp_configuration.grasp.position.y - userdata.grasp_configuration.pre_grasp.position.y)/3
offset_z = 0#(userdata.grasp_configuration.grasp.position.z - userdata.grasp_configuration.pre_grasp.position.z)/3
pre_grasp_stamped.pose.position.x += offset_x
pre_grasp_stamped.pose.position.y += offset_y
pre_grasp_stamped.pose.position.z -= offset_z
grasp_stamped.pose.position.x += offset_x
grasp_stamped.pose.position.y += offset_y
grasp_stamped.pose.position.z -= offset_z
"""
sol = False
for i in range(0,10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(current_joint_configuration, pre_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
for j in range(0,10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, grasp_stamped)
sol = True
break
if sol:
break;
if not sol:
return 'failed';
else:
arm_handle = sss.move("arm", [pre_grasp_conf], False)
arm_handle.wait();
rospy.sleep(2);
arm_handle = sss.move("arm", [grasp_conf], False)
arm_handle.wait();
rospy.sleep(2);
#Close SDH based on the grasp configuration to grasp.
arm_handle = sss.move("sdh", [list(userdata.grasp_configuration.sdh_joint_values)], False)
arm_handle.wait();
rospy.sleep(2);
#TODO: Confirm the grasp based on force feedback
successful_grasp = grasping_functions.sdh_tactil_sensor_result();
if successful_grasp:
return 'succeeded'
else:
#TODO: Regrasp (close MORE the fingers)
regrasp = list(userdata.grasp_configuration.sdh_joint_values)
regrasp[2] -= 0.1
regrasp[4] -= 0.1
regrasp[6] -= 0.1
arm_handle = sss.move("sdh", [regrasp], False)
arm_handle.wait();
successful_regrasp = grasping_functions.sdh_tactil_sensor_result();
if successful_regrasp:
return 'succeeded'
else:
return 'failed'
def execute(self, userdata):
#TODO: THIS PART SHOULD BE IN THE PREVIOUS STATE #####################################################
grasp_configuration_id = len(userdata.grasp_configuration) - 1
#TODO: userdata.grasp_configuration_id
for i in range(0, len(userdata.grasp_configuration)):
print userdata.grasp_configuration[i].category
if userdata.grasp_configuration[i].category == "TOP":
grasp_configuration_id = i
#TODO: userdata.grasp_configuration_id
break
#######################################################################################################
category = userdata.grasp_configuration[
grasp_configuration_id].category #TODO: userdata.grasp_configuration_id
if category == "TOP":
userdata.grasp_categorisation = 'top'
sdh_handle = sss.move("sdh", "spheropen")
else:
userdata.grasp_categorisation = 'side'
sdh_handle = sss.move("sdh", "cylopen")
sdh_handle.wait()
rospy.sleep(2)
#Get the current arm joint states.
while self.arm_state.get_num_connections() == 0:
time.sleep(0.3)
continue
#Move to grasp position with SDH open ---------------------------------------------
#pregrasp
pre_grasp_stamped = PoseStamped()
pre_grasp_stamped.header.frame_id = "/base_link"
pre_grasp_stamped.pose = userdata.grasp_configuration[
grasp_configuration_id].pre_grasp
#TODO: userdata.grasp_configuration_id
#grasp
grasp_stamped = PoseStamped()
grasp_stamped.header.frame_id = "/base_link"
grasp_stamped.pose = userdata.grasp_configuration[
grasp_configuration_id].grasp
#TODO: userdata.grasp_configuration_id
#postgrasp
post_grasp_stamped = copy.deepcopy(grasp_stamped)
post_grasp_stamped.pose.position.z += 0.1
sol = False
for w in range(0, 10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(
self.current_arm_state, pre_grasp_stamped)
if (error_code.val == error_code.SUCCESS):
for k in range(0, 10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(
pre_grasp_conf, grasp_stamped)
if (error_code.val == error_code.SUCCESS):
(post_grasp_conf,
error_code) = grasping_functions.callIKSolver(
pre_grasp_conf, post_grasp_stamped)
if (error_code.val == error_code.SUCCESS):
sol = True
break
if sol:
break
if not sol:
sss.say(["I can not catch the object!"], False)
return 'not_completed'
else:
sss.say(["I am grasping the object now!"], False)
#Close SDH based on the grasp configuration to grasp.
arm_handle = sss.move("arm", [pre_grasp_conf, grasp_conf])
arm_handle.wait()
rospy.sleep(4)
#Close SDH based on the grasp configuration to grasp.
ssh_handle = sss.move("sdh", [
list(userdata.grasp_configuration[grasp_configuration_id].
sdh_joint_values)
]) #TODO: userdata.grasp_configuration_id
ssh_handle.wait()
rospy.sleep(2)
#Confirm the grasp based on force feedback
successful_grasp = grasping_functions.sdh_tactil_sensor_result()
#False
if not successful_grasp:
#Regrasp (close MORE the fingers)
regrasp = list(
userdata.grasp_configuration[grasp_configuration_id].
sdh_joint_values) #TODO: userdata.grasp_configuration_id
print "Current config, trying regrasp", regrasp
regrasp[1] += 0.07
regrasp[3] += 0.07
regrasp[5] += 0.07
print "to", regrasp
sdh_handle = sss.move("sdh", [regrasp])
sdh_handle.wait()
rospy.sleep(2)
successful_grasp = grasping_functions.sdh_tactil_sensor_result(
)
#True
if not successful_grasp:
sss.say(["I can not catch the object.!"], False)
return 'not_completed'
arm_handle = sss.move("arm",
[post_grasp_conf, "look_at_table", "hold"])
arm_handle.wait()
rospy.sleep(2)
sss.say(["I have grasped the object with success!"], False)
return 'succeeded'
return 'failed';
else:
arm_handle = sss.move("arm", [pre_grasp_conf], False)
arm_handle.wait();
rospy.sleep(2);
arm_handle = sss.move("arm", [grasp_conf], False)
arm_handle.wait();
rospy.sleep(2);
#Close SDH based on the grasp configuration to grasp.
arm_handle = sss.move("sdh", [list(userdata.grasp_configuration.sdh_joint_values)], False)
arm_handle.wait();
rospy.sleep(2);
#TODO: Confirm the grasp based on force feedback
successful_grasp = grasping_functions.sdh_tactil_sensor_result();
if successful_grasp:
return 'succeeded'
else:
#TODO: Regrasp (close MORE the fingers)
regrasp = list(userdata.grasp_configuration.sdh_joint_values)
regrasp[2] -= 0.1
regrasp[4] -= 0.1
regrasp[6] -= 0.1
arm_handle = sss.move("sdh", [regrasp], False)
arm_handle.wait();
successful_regrasp = grasping_functions.sdh_tactil_sensor_result();
if successful_regrasp:
return 'succeeded'
def execute(self, userdata):
#TODO: THIS PART SHOULD BE IN THE PREVIOUS STATE #####################################################
grasp_configuration_id = len(userdata.grasp_configuration)-1; #TODO: userdata.grasp_configuration_id
for i in range(0, len(userdata.grasp_configuration)):
print userdata.grasp_configuration[i].category
if userdata.grasp_configuration[i].category == "TOP":
grasp_configuration_id = i; #TODO: userdata.grasp_configuration_id
break;
#######################################################################################################
category = userdata.grasp_configuration[grasp_configuration_id].category #TODO: userdata.grasp_configuration_id
if category == "TOP":
userdata.grasp_categorisation = 'top'
sdh_handle=sss.move("sdh", "spheropen")
else:
userdata.grasp_categorisation = 'side'
sdh_handle=sss.move("sdh", "cylopen")
sdh_handle.wait()
rospy.sleep(2)
#Get the current arm joint states.
while self.arm_state.get_num_connections() == 0:
time.sleep(0.3)
continue
#Move to grasp position with SDH open ---------------------------------------------
#pregrasp
pre_grasp_stamped = PoseStamped();
pre_grasp_stamped.header.frame_id = "/base_link";
pre_grasp_stamped.pose = userdata.grasp_configuration[grasp_configuration_id].pre_grasp; #TODO: userdata.grasp_configuration_id
#grasp
grasp_stamped = PoseStamped();
grasp_stamped.header.frame_id = "/base_link";
grasp_stamped.pose = userdata.grasp_configuration[grasp_configuration_id].grasp; #TODO: userdata.grasp_configuration_id
#postgrasp
post_grasp_stamped = copy.deepcopy(grasp_stamped);
post_grasp_stamped.pose.position.z += 0.1;
sol = False
for w in range(0,10):
(pre_grasp_conf, error_code) = grasping_functions.callIKSolver(self.current_arm_state, pre_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
for k in range(0,10):
(grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, grasp_stamped)
if(error_code.val == error_code.SUCCESS):
(post_grasp_conf, error_code) = grasping_functions.callIKSolver(pre_grasp_conf, post_grasp_stamped)
if(error_code.val == error_code.SUCCESS):
sol = True
break
if sol:
break
if not sol:
sss.say(["I can not catch the object!"], False)
return 'not_completed';
else:
sss.say(["I am grasping the object now!"], False)
#Close SDH based on the grasp configuration to grasp.
arm_handle = sss.move("arm", [pre_grasp_conf, grasp_conf])
arm_handle.wait()
rospy.sleep(4)
#Close SDH based on the grasp configuration to grasp.
ssh_handle = sss.move("sdh", [list(userdata.grasp_configuration[grasp_configuration_id].sdh_joint_values)]) #TODO: userdata.grasp_configuration_id
ssh_handle.wait()
rospy.sleep(2);
#Confirm the grasp based on force feedback
successful_grasp = grasping_functions.sdh_tactil_sensor_result();#False
if not successful_grasp:
#Regrasp (close MORE the fingers)
regrasp = list(userdata.grasp_configuration[grasp_configuration_id].sdh_joint_values) #TODO: userdata.grasp_configuration_id
print "Current config, trying regrasp", regrasp
regrasp[1] += 0.07
regrasp[3] += 0.07
regrasp[5] += 0.07
print "to", regrasp
sdh_handle = sss.move("sdh", [regrasp])
sdh_handle.wait()
rospy.sleep(2)
successful_grasp = grasping_functions.sdh_tactil_sensor_result();#True
if not successful_grasp:
sss.say(["I can not catch the object.!"], False)
return 'not_completed'
arm_handle = sss.move("arm",[post_grasp_conf,"look_at_table","hold"])
arm_handle.wait()
rospy.sleep(2)
sss.say(["I have grasped the object with success!"], False)
return 'succeeded'
