generic_grasp_state.py

Go to the documentation of this file.

#!/usr/bin/python

import roslib
roslib.load_manifest('srs_grasping')
import rospy
import grasping_functions
from shared_state_information import *


from simple_script_server import *
sss = simple_script_server()

from srs_grasping.srv import *

class select_grasp(smach.State):

    def __init__(self):
        smach.State.__init__(
            self,
            outcomes=['succeeded', 'failed', 'preempted'],
            input_keys=['object'],
            output_keys=['grasp_configuration'])
        
        #default grasp categorisation
        #self.grasp_configuration = ""
        self.object_id=9

    def execute(self, userdata):
        
        global listener
        try:
            # transform object_pose into base_link
            object_pose_in = userdata.object.pose
            print object_pose_in
            object_pose_in.header.stamp = listener.getLatestCommonTime("/base_link",object_pose_in.header.frame_id)
            object_pose_bl = listener.transformPose("/base_link", object_pose_in)
        except rospy.ROSException, e:
            print "Transformation not possible: %s"%e
            return 'failed'
        
        get_grasps_from_position = rospy.ServiceProxy('get_grasps_from_position', GetGraspsFromPosition)
        req = GetGraspsFromPositionRequest(self.object_id, object_pose_bl.pose)
        grasp_configuration = (get_grasps_from_position(req)).grasp_configuration
       
        userdata.grasp_configuration = grasp_configuration[1]
        
        return 'succeeded'



class grasp(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['succeeded','not_completed','retry' ,'failed', 'preempted'], input_keys=['grasp_configuration'])

    def get_joint_state(self, msg):
global current_joint_configuration
current_joint_configuration = list(msg.desired.positions)
rospy.spin()

    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'
#return 'failed'