grasping_learning_sm.py

Go to the documentation of this file.

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import roslib; roslib.load_manifest('estirabot_apps')
import rospy
import smach
import smach_ros

from pprint import pprint

from smach_ros import SimpleActionState
from smach_ros import ServiceState
from smach import CBState

import tf

from iri_common_smach.st_get_pcl import GetPCL
from iri_common_smach.utils_msg  import build_pose
from iri_common_smach.utils_msg  import build_transform_msg
from iri_common_smach.utils_msg  import build_pose_stamped_msg
from iri_common_smach.get_keystroke import GetKeyStroke
#from iri_common_smach.st_transform_pose   import TransformPoseStamped
from iri_common_smach.homogeneous_product import homogeneous_product_pose_transform
from iri_wam_smach.sm_wam_move_from_pose  import SM_WAM_MoveFromPose
from estirabot_apps_base.sm_move          import SM_ESTIRABOT_GoHome

from actionlib import *
from actionlib.msg import *

# used msgs
from geometry_msgs.msg import PoseStamped,Quaternion
from estirabot_msgs.srv import TransformPose
from iri_wam_common_msgs.srv import wamdriver, wamdriverRequest
from iri_dynamixel_gripper.msg import closeAction, openAction
from iri_bow_object_detector.msg import *
from iri_wam_common_msgs.msg import *
from iri_perception_msgs.srv import PclToDescriptorSet
from iri_perception_msgs.srv import PclToImg
from iri_grasping_learner.srv import FindGoodPoint,LearnNewGrasp
from iri_learning_msgs.msg import GraspInfo

import ipdb

@smach.cb_interface(input_keys=['grasp_point'],
                                        output_keys=['grasp_pose_st', 'target_frame'],
                                        outcomes=['done'])
def build_grasp_pose_from_point_cb(ud):
        p                 = PoseStamped()
        p.pose            = build_pose(ud.grasp_point.x, ud.grasp_point.y, ud.grasp_point.z, 0, 1, 0, 0)
        p.header.frame_id = '/camera_rgb_optical_frame'
        p.header.stamp = rospy.Time.now()
        pub = rospy.Publisher('/debug/grasp_point', PoseStamped, None, False, True)
        rospy.sleep(1) # wait for subscribers
        pub.publish(p)
        ud.grasp_pose_st = p
        
        # transform pose
        #tf_manager = TransformManager()
        #p_transformed = tf_manager.transform_pose('wam_link0', p)
        #ud.grasp_pose_st = p_transformed
        
        ud.target_frame = '/wam_link0'
        
        return 'done'

def grasping_point_goal_cb(userdata, goal):
        goal = GetGraspingPointGoal()
        goal.pointcloud = userdata.pcl_RGB
        return goal


def build_pick_up_goal_from_grasping_pose(pose_st):
        goal = SimpleBhandPickUpGoal()
        
        #goal.fingers_position_for_pre_grasp=["GM 5000"]
        #goal.fingers_position_for_grasp=["GTC"]
        goal.fingers_position_for_pre_grasp=["open"]
        goal.fingers_position_for_grasp=["close"]
        
        # Grasp pose is pose_st
        goal.grasp_pose = pose_st
        
        # Pre-grasp pose -0.15m above grasp
        goal.pre_grasp_modifier.position.x = 0
        goal.pre_grasp_modifier.position.y = 0
        goal.pre_grasp_modifier.position.z = -0.15
        goal.pre_grasp_modifier.orientation.x = 0
        goal.pre_grasp_modifier.orientation.y = 0
        goal.pre_grasp_modifier.orientation.z = 0
        goal.pre_grasp_modifier.orientation.w = 1
        
        # Lift modifier -0.20 on Z
        goal.lift.direction.header.frame_id = "wam_link0"
        goal.lift.direction.header.stamp = pose_st.header.stamp
        goal.lift.direction.vector.x = 0
        goal.lift.direction.vector.y = 0
        goal.lift.direction.vector.z = 1
        goal.lift.desired_distance = 0.40
        
        return goal


def move_pose_away(pose, z_offset):
        transform = build_transform_msg(0, 0, z_offset, 0, 0, 0, 1)
        return homogeneous_product_pose_transform(pose, transform)


def grasp_cloth_goal_cb(userdata, goal):
        #ipdb.set_trace()
        pose_st = userdata.sm_transformed_pose_st
        
        # Set orientation pointing to the table
        pose_st.pose.orientation.x = 0
        pose_st.pose.orientation.y = 1
        pose_st.pose.orientation.z = 0
        pose_st.pose.orientation.w = 0
        pose_st.pose = move_pose_away(pose_st.pose, 0.2)
        
        pub = rospy.Publisher('/debug/grasp_point2', PoseStamped, None, False, True)
        rospy.sleep(1) # wait for subscribers
        pub.publish(pose_st)
        
        goal = build_pick_up_goal_from_grasping_pose(pose_st)
        return goal

        
        
        
        

class MovePoseAway(smach.State):
        def __init__(self, dist_):
                smach.State.__init__(self, outcomes=['succeeded','aborted'],
                                     input_keys=['pose_st'],
                                     output_keys=['pose_st'])
                self.dist = dist_
        
        def execute(self, userdata):
                userdata.pose_st.pose.orientation.x = 0.0
                userdata.pose_st.pose.orientation.y = 0.0
                userdata.pose_st.pose.orientation.z = 0.0
                userdata.pose_st.pose.orientation.w = 1.0
                userdata.pose_st.pose = move_pose_away(userdata.pose_st.pose, self.dist)
                print userdata.pose_st
                return 'succeeded'



class TransformPoseStamped(smach.State):
        def __init__(self, sm_target_frame_):
                smach.State.__init__(self, outcomes=['succeeded','preempted','aborted'],
                                     input_keys=['sm_grasp_pose'],
                                     output_keys=['target_pose'])
                self.service_topic = '/iri_transform_pose/transform_pose'
                self.sm_target_frame = sm_target_frame_
        
        def execute(self, userdata):
                rospy.logdebug('Executing state TransformPoseStamped')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_target_pose = rospy.ServiceProxy(self.service_topic, TransformPose)
                        resp = get_target_pose(userdata.sm_grasp_pose, self.sm_target_frame)
                        
                        userdata.target_pose = resp.target_pose_st
                        return 'succeeded'
                
                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


class GetFindddState(smach.State):
        def __init__(self, topic_):
                smach.State.__init__(self, outcomes=['succeeded','aborted'],
                                     input_keys=['pcl_RGB'],
                                     output_keys=['descriptor_set'])
                self.service_topic = topic_

        def execute(self, userdata):
                rospy.logdebug('Executing state GetFinddd')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_finddd = rospy.ServiceProxy(self.service_topic, PclToDescriptorSet)
                        resp = get_finddd(userdata.pcl_RGB)
                        userdata.descriptor_set = resp.descriptor_set
                        return 'succeeded'

                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


class PclToImageState(smach.State):
        def __init__(self, topic_):
                smach.State.__init__(self, outcomes=['succeeded','aborted'],
                                     input_keys=['pcl_RGB'],
                                     output_keys=['image'])
                self.service_topic = topic_

        def execute(self, userdata):
                rospy.logdebug('Executing state PclToImageState')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_image = rospy.ServiceProxy(self.service_topic, PclToImg)
                        resp = get_image(userdata.pcl_RGB)
                        userdata.image = resp.image
                        return 'succeeded'

                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'



class GetGraspPointState(smach.State):
        def __init__(self, topic_):
                smach.State.__init__(self, outcomes=['succeeded','aborted','unknown'],
                                     input_keys=['image','descriptor_set'],
                                     output_keys=['grasp_info','descriptor_idx','grasp_pose_stamped'])
                self.service_topic = topic_

        def execute(self, userdata):
                rospy.logdebug('Executing state GetGraspPointState')
                rospy.wait_for_service(self.service_topic)
                try:
                        get_grasp_point = rospy.ServiceProxy(self.service_topic, FindGoodPoint)
                        resp = get_grasp_point(userdata.descriptor_set, userdata.image)
                        
                        userdata.grasp_info = resp.grasp_info
                        userdata.descriptor_idx = resp.desc_index
                        pose_st = PoseStamped()
                        pose_st.pose.position.x = userdata.descriptor_set.descriptors[resp.desc_index].point3d.x
                        pose_st.pose.position.y = userdata.descriptor_set.descriptors[resp.desc_index].point3d.y
                        pose_st.pose.position.z = userdata.descriptor_set.descriptors[resp.desc_index].point3d.z
                        pose_st.pose.orientation.w = 1.0 # assuming x,y,z are initialized to 0.0
                        pose_st.header.frame_id = '/camera_rgb_optical_frame'
                        userdata.grasp_pose_stamped = pose_st
                        if resp.knows:
                                return 'succeeded'
                        else:
                                return 'unknown'

                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'


class WaitUserInputState(smach.State):
        def __init__(self):
                smach.State.__init__(self, outcomes=['succeeded','aborted'])

        def execute(self, userdata):
                rospy.loginfo('Press key when arm is positioned (q for aborting)')
                print 'Press key when arm is positioned (q for aborting)'
                getch = GetKeyStroke()
                key = getch()
                if key == 'q':
                        return 'aborted'
                else:
                        return 'succeeded'


class GetGraspInfoFromDemoState(smach.State):
        def __init__(self):
                smach.State.__init__(self, outcomes=['succeeded','aborted'],
                                                output_keys=['grasp_info','grasp_pose_link0'])
                self.listener = tf.TransformListener()

        def execute(self, userdata):
                time_common = self.listener.getLatestCommonTime('/wam_link0','/wam_tcp')
                res = self.listener.lookupTransform('/wam_link0','/wam_tcp',time_common)
                pos = res[0]
                ori = res[1]
                grasp_info = GraspInfo()
                grasp_info.orientation = Quaternion(*ori)
                userdata.grasp_info = grasp_info
                
                # TODO get profundidad
                
                return 'succeeded'


class GraspingLearnerAddPointState(smach.State):
        def __init__(self, topic_):
                smach.State.__init__(self, outcomes=['succeeded','aborted'],
                                     input_keys=['descriptor_set','descriptor_idx','grasp_info'])
                self.service_topic = topic_

        def execute(self, userdata):
                rospy.logdebug('Executing state GraspingLearnerAddPointState')
                rospy.wait_for_service(self.service_topic)
                try:
                        add_grasp_point = rospy.ServiceProxy(self.service_topic, LearnNewGrasp)
                        resp = add_grasp_point(userdata.descriptor_set, userdata.descriptor_idx, userdata.grasp_info)
                        if resp.update_ok:
                                return 'succeeded'
                        else:
                                return 'aborted'

                except rospy.ServiceException, e:
                        print "Service call failed: %s"%e
                        return 'aborted'




def main():
        rospy.init_node('sm_pick_up_cloths')
        
        sm_move_factory = SM_WAM_MoveFromPose('/estirabot/wam_wrapper/joints_move',
                                              '/estirabot/wam_ik/pose_move')
        sm_move         = sm_move_factory.build_sm()
        sm_goHome_factory = SM_ESTIRABOT_GoHome()
        sm_goHome         = sm_goHome_factory.build_sm()
        
        # Create a SMACH state machine
        sm = smach.StateMachine(outcomes=['finishOk','failedVision','failedLearn','failedMoveArm'])

        # Open the container
        with sm:
                # Add states to the container
                
                # Get pointcloud from Kinect
                smach.StateMachine.add('GetPCL', GetPCL('/estirabot/ceiling_kinect/camera/rgb/points'),
                                transitions={'success':'GetFindddState', 
                                                'fail':'failedVision'},
                                remapping={'pcl_RGB':'pcl_RGB'})
                
                # Get Finddd
                smach.StateMachine.add('GetFindddState', GetFindddState('/iri_finddd/get_finddd_from_pointcloud'),
                                transitions={'succeeded':'PclToImageState', 
                                                'aborted':'failedVision'},
                                remapping={'descriptor_set':'descriptor_set'})
                
                # Get image from pointcloud
                smach.StateMachine.add('PclToImageState', PclToImageState('/convert_pcl_to_image'),
                                transitions={'succeeded':'GetGraspPointState', 
                                                'aborted':'failedVision'},
                                remapping={'image':'image'})
                
                # Get grasp point and, if unknown trigger learning action
                smach.StateMachine.add('GetGraspPointState', GetGraspPointState('/get_grasping_point'),
                                transitions={'succeeded':'TransformPoseStamped2', 
                                                'unknown':'TransformPoseStamped1', 
                                                'aborted':'failedVision'},
                                remapping={'grasp_info':'grasp_info',
                                    'descriptor_idx':'descriptor_idx',
                                        'grasp_pose_stamped':'sm_grasp_pose'})
                
                
                # IF UNKNOWN
                # Learn grasping from user demo
                smach.StateMachine.add('TransformPoseStamped1', 
                                        TransformPoseStamped('/wam_link0'),
                                transitions={'succeeded':'MovePoseAway', 
                                                'preempted':'failedMoveArm',
                                                'aborted':'failedMoveArm'},
                                remapping={'target_pose':'grasp_pose_link0'})
                
                # TODO alejar move
                smach.StateMachine.add('MovePoseAway', 
                                        MovePoseAway(0.2),
                                transitions={'succeeded':'MoveToSelectedGraspPoint', 
                                                'aborted':'failedMoveArm'},
                                remapping={'pose_st':'grasp_pose_link0'})
                
                smach.StateMachine.add('MoveToSelectedGraspPoint', sm_move,
                                transitions={'success':'SetGravityCompensation', 
                                                'aborted':'failedMoveArm',
                                                'no_kinematic_solution':'failedMoveArm'},
                                remapping={'pose_st':'grasp_pose_link0'})
                
                
                smach.StateMachine.add('SetGravityCompensation',
                                                ServiceState('/estirabot/wam_wrapper/wam_services',
                                                                                wamdriver,
                                                                                request = wamdriverRequest(1)),
                                                transitions={'succeeded':'WaitUserInputState',
                                                                        'aborted':'failedMoveArm',
                                                                        'preempted':'failedMoveArm'})
                
                smach.StateMachine.add('WaitUserInputState', WaitUserInputState(),
                                transitions={'succeeded':'RemoveGravityCompensation', 
                                                'aborted':'failedLearn'})
                
                smach.StateMachine.add('RemoveGravityCompensation',
                                                ServiceState('/estirabot/wam_wrapper/wam_services',
                                                                                wamdriver,
                                                                                request = wamdriverRequest(0)),
                                                transitions={'succeeded':'GetGraspInfoFromDemoState',
                                                                        'aborted':'failedMoveArm',
                                                                        'preempted':'failedMoveArm'})
                
                smach.StateMachine.add('GetGraspInfoFromDemoState', GetGraspInfoFromDemoState(),
                                transitions={'succeeded':'GraspingLearnerAddPointState', 
                                                'aborted':'failedLearn'},
                                remapping={'grasp_info':'grasp_info'})
                
                smach.StateMachine.add('GraspingLearnerAddPointState', GraspingLearnerAddPointState('/learn_new_grasping'),
                                transitions={'succeeded':'GetPCL', 
                                                'aborted':'failedLearn'})
                
                
                # IF KNOWN
                # Execute known grasping
                smach.StateMachine.add('TransformPoseStamped2', 
                                        TransformPoseStamped('/wam_link0'),
                                transitions={'succeeded':'SimpleBhandPickUpAction', 
                                                'preempted':'failedVision',
                                                'aborted':'failedVision'},
                                remapping={'target_pose':'sm_transformed_pose_st'})
                
                # TODO REPLACE ORIENTATION AND SET OFFSET
                
                smach.StateMachine.add('SimpleBhandPickUpAction', 
                                SimpleActionState('/estirabot/skills/grasp/pickup',
                                                  SimpleBhandPickUpAction,
                                                  goal_cb=grasp_cloth_goal_cb,
                                                  input_keys=['sm_transformed_pose_st']),
                                transitions={'succeeded':'GoHome',
                                                'preempted':'failedMoveArm',
                                                'aborted':'failedMoveArm'})
                
                
                
                
                #smach.StateMachine.add('GetGraspingPointAction', 
                                #SimpleActionState('/estirabot/skills/bow_detector/bow_object_detector/get_grasping_point',
                                                  #GetGraspingPointAction,
                                                  ##goal_slots=['pcl_RGB'],
                                                  #goal_cb=grasping_point_goal_cb,
                                                  #input_keys=['pcl_RGB'],
                                                  #result_slots=['grasping_point']),
                                                  ##server_wait_timeout=rospy.Duration(10.0)),
                                #transitions={'succeeded':'ConvertGraspPoint',
                                                #'preempted':'failedVision',
                                                #'aborted':'failedVision'},
                                #remapping={'grasping_point':'sm_grasp_point'})
                
                
                
                # move to home
                smach.StateMachine.add('GoHome', 
                                        sm_goHome,
                                transitions={'success':'OpenGripper',
                                        'aborted':'failedMoveArm'})
                
                # lwpr
                
                # release
                smach.StateMachine.add('OpenGripper',
                       smach_ros.SimpleActionState('/estirabot/gripper/open_action_',
                                                  openAction),
                       {'succeeded': 'CloseGripper',
                        'preempted': 'failedMoveArm',
                        'aborted'  : 'failedMoveArm'})
                
                smach.StateMachine.add('CloseGripper',
                       smach_ros.SimpleActionState('/estirabot/gripper/close_action_',
                                                  closeAction),
                       {'succeeded': 'finishOk',
                        'preempted': 'failedMoveArm',
                        'aborted'  : 'failedMoveArm'})
                
                # initial
                
    
        #sis = smach_ros.IntrospectionServer('iri_grasping_learner', sm, '/SM_LEARNING2')
        #sis.start()
    
        # Execute SMACH plan
        outcome = sm.execute()
        
        #sis.stop()



if __name__ == '__main__':
        main()