def process_grasp_msg(self, grasp_msg):
        """@brief - Attempt to grasp the object and lift it

        First moves the arm to pregrasp the object, then attempts to grasp and lift it.
        The grasp and lifting phase is currently completely open loop
        
        """
        try:
            #Lock the openrave environment - it will restore the robot and collision state
            #to it's previous state when it finishes automagically.

            with self.global_data.or_env:

                #Reject multiple grasp messages sent too fast. They are likely a mistake
                #FIXME - We should probably just set the queue size to 1.
                if (time() - self.last_grasp_time) < 30:
                    return [], []
                self.last_grasp_time = time()

                print grasp_msg
                grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS
                grasp_status_msg = "grasp_succeeded"
                success = 1

                # Send the robot to its home position if it is actually running
                #and not currently there
                if self.global_data.robot_running and not tp.is_home():
                    print 'go home'
                    tp.go_home(self.global_data)
                    #This sometimes fails near the end of the trajectory because the last few
                    #steps of the trajectory are slow because our blending parameters
                    #are probably wrong. It is basically in the home position, so we
                    #don't check for success in reaching the home position because meaningful
                    #failures are rare.

                #Open the hand - Leaves spread angle unchanged
                if success and self.global_data.robot_running:
                    success, grasp_status_msg, positions = tp.open_barrett()
                    if not success:
                        grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR

                if success:
                    #Pregrasp the object

                    #Convert the grasp message to a openrave transform
                    grasp_tran = pm.toMatrix(
                        pm.fromMsg(grasp_msg.final_grasp_pose))
                    grasp_tran[0:3, 3] /= 1000  #mm to meters

                    #Move the hand to the pregrasp spread angle - Update openrave's robot pose
                    if self.global_data.robot_running:
                        tp.MoveHandSrv(1,
                                       [0, 0, 0, grasp_msg.pre_grasp_dof[0]])
                        tp.update_robot(self.global_data.or_env.GetRobots()[0])

                        print 'pre-grasp'

                    #Publish the object TFs
                    self.model_manager()
                    print 'after model_manager()'

                    #Calculate a trajectory to the pregrasp pose and move to it if necessary
                    success, final_tran, dof_list, j = tp.pregrasp_object(
                        self.global_data,
                        grasp_msg.object_name,
                        False,
                        grasp_tran,
                        run_it=self.global_data.robot_running)
                    print 'after pre-grasp'

                    #Failures shouldn't happen if grasp analysis is being used, so that's wierd.
                    if not success:
                        pdb.set_trace()

                    #Make sure that the openrave environment is up to date in pregrasp pose
                    if self.global_data.robot_running:
                        tp.update_robot(self.global_data.or_env.GetRobots()[0])
                    else:
                        #If the robot isn't actually running, just update the openrave visualization
                        #so that the user can see what was planned.
                        if success:
                            print "setting robot joints"
                            print j
                            self.global_data.or_env.GetRobots(
                            )[0].SetActiveDOFs(range(6))
                            self.global_data.or_env.GetRobots(
                            )[0].SetActiveDOFValues(dof_list[-1])
                            self.global_data.or_env.GetRobots(
                            )[0].SetActiveDOFs(range(6, 10))
                            self.global_data.or_env.GetRobots(
                            )[0].SetActiveDOFValues(
                                list(grasp_msg.pre_grasp_dof[1:]) +
                                [grasp_msg.pre_grasp_dof[0]])
                            self.global_data.or_env.GetRobots(
                            )[0].SetActiveDOFs(range(6))
                            self.global_data.or_env.UpdatePublishedBodies()
                            pdb.set_trace()
                            success = False

                    #If the pregrasp was unreachable, record the type of issue
                    if not success:
                        #If there is no joint list, the pregrasp transform had no inverse
                        #kinematics solution, probably.
                        if not j:
                            grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                            grasp_status_msg = "Pregrasp tran Out of range!"
                        #Otherwise, something even wierder happened.
                        else:
                            grasp_status = graspit_msgs.msg.GraspStatus.FAILED
                            grasp_status_msg = "Unknown planner failure!"

            #Move from the pregrasp to the final grasp pose in a straight line along the approach
            #direction
            if success:
                if not Hao:
                    #Move forward open loop
                    success = tp.move_forward(0.05, True)
                else:
                    #Move forward using force transducer and palm contact sensors to detect any
                    #collisions
                    hu.GuardedMoveUntilHit(self.global_data, array([0, 0, 1]),
                                           'PALM', 0.05, 20)
                    success = True

                if not success:
                    #If we failed to move forward - which might happen because
                    #we do not test if the final grasp pose is reachable along
                    #a straight line trajectory because the joint range limits
                    #set on the robot are not actually exactly the same as those
                    #in openrave and it usually doesn't cause any problems
                    #in general trajectory testing.
                    grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                    grasp_status_msg = "Unable to move forward to grasp!"

            if Hao:
                """new routine"""
                #Close the hand using the force torque and contact sensors to detect contacts and
                #stop the fingers
                hu.GuardedCloseHand(self.global_data)
            else:
                """old routine"""
                if success:
                    #Just close the hand open loop, leaving the spread angle where it is now.
                    success, grasp_status_msg, joint_angles = tp.move_hand([
                        grasp_msg.pre_grasp_dof[1], grasp_msg.pre_grasp_dof[2],
                        grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]
                    ])

                if success:
                    #Close the hand completely until the motors stall or they hit
                    #the final grasp DOFS
                    success, grasp_status_msg, joint_angles = tp.move_hand([
                        grasp_msg.final_grasp_dof[1],
                        grasp_msg.final_grasp_dof[2],
                        grasp_msg.final_grasp_dof[3],
                        grasp_msg.final_grasp_dof[0]
                    ])

                if success:
                    #Now close the hand completely until the motors stall.
                    success, grasp_status_msg, joint_angles = tp.close_barrett(
                    )
                if not success:
                    grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR

            #Now wait for user input on whether or not to lift the object
            if success:
                selection = int(raw_input('Lift up (1) or not (0): '))
                if selection == 1:
                    print 'lift up the object'
                    #Lift the object using the staubli's straight line path planner
                    success = tp.lift_arm(.05, True)
                    if not success:
                        grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                        grasp_status_msg = "Couldn't lift object"
                else:
                    print 'not lift up the object'

            #Maybe decide if it has been successfully lifted here...
            if success:
                rospy.logwarn(grasp_status_msg)
            else:
                rospy.logfatal(grasp_status_msg)
            #Tell graspit whether the grasp succeeded
            self.graspit_status_publisher.publish(grasp_status,
                                                  grasp_status_msg, -1)

            print grasp_status_msg

            return grasp_status, grasp_status_msg

        except Exception as e:
            #print any exceptions and drop in to the debugger.
            import traceback
            print traceback.format_exc()
            pdb.set_trace()
    def process_grasp_msg(self, grasp_msg):
        """@brief - Attempt to grasp the object and lift it

        First moves the arm to pregrasp the object, then attempts to grasp and lift it.
        The grasp and lifting phase is currently completely open loop
        
        """
        try:
            #Lock the openrave environment - it will restore the robot and collision state
            #to it's previous state when it finishes automagically.
            
            with self.global_data.or_env:

                #Reject multiple grasp messages sent too fast. They are likely a mistake
                #FIXME - We should probably just set the queue size to 1.
                if (time() - self.last_grasp_time) < 30:
                    return [], []
                self.last_grasp_time = time()

                print grasp_msg
                grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS
                grasp_status_msg = "grasp_succeeded"
                success = 1
                
                # Send the robot to its home position if it is actually running
                #and not currently there
                if self.global_data.robot_running and not tp.is_home():
                    print 'go home'
                    tp.go_home(self.global_data)
                    #This sometimes fails near the end of the trajectory because the last few
                    #steps of the trajectory are slow because our blending parameters
                    #are probably wrong. It is basically in the home position, so we
                    #don't check for success in reaching the home position because meaningful
                    #failures are rare.


                #Open the hand - Leaves spread angle unchanged
                if success and self.global_data.robot_running:
                    success, grasp_status_msg, positions = tp.open_barrett()
                    if not success:
                        grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR

                
                if success:
                    #Pregrasp the object
                    
                    #Convert the grasp message to a openrave transform
                    grasp_tran = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
                    grasp_tran[0:3,3] /=1000 #mm to meters

                    #Move the hand to the pregrasp spread angle - Update openrave's robot pose
                    if self.global_data.robot_running:
                        tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]])
                        tp.update_robot(self.global_data.or_env.GetRobots()[0])

                        print 'pre-grasp'

                    #Publish the object TFs
                    self.model_manager()
                    print 'after model_manager()'

                    #Calculate a trajectory to the pregrasp pose and move to it if necessary
                    success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, grasp_msg.object_name,  False, grasp_tran, run_it = self.global_data.robot_running)
                    print 'after pre-grasp'

                    #Failures shouldn't happen if grasp analysis is being used, so that's wierd.
                    if not success:
                        pdb.set_trace()
                        
                    #Make sure that the openrave environment is up to date in pregrasp pose
                    if self.global_data.robot_running:
                        tp.update_robot(self.global_data.or_env.GetRobots()[0])
                    else:
                        #If the robot isn't actually running, just update the openrave visualization
                        #so that the user can see what was planned.
                        if success:
                            print "setting robot joints"
                            print j
                            self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6))
                            self.global_data.or_env.GetRobots()[0].SetActiveDOFValues(dof_list[-1])
                            self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6,10))
                            self.global_data.or_env.GetRobots()[0].SetActiveDOFValues(list(grasp_msg.pre_grasp_dof[1:]) + [grasp_msg.pre_grasp_dof[0]])                    
                            self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6))
                            self.global_data.or_env.UpdatePublishedBodies()
                            pdb.set_trace()
                            success = False

                    #If the pregrasp was unreachable, record the type of issue
                    if not success:
                        #If there is no joint list, the pregrasp transform had no inverse
                        #kinematics solution, probably. 
                        if not j:
                            grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                            grasp_status_msg = "Pregrasp tran Out of range!"
                        #Otherwise, something even wierder happened.
                        else:
                            grasp_status = graspit_msgs.msg.GraspStatus.FAILED
                            grasp_status_msg = "Unknown planner failure!"


            #Move from the pregrasp to the final grasp pose in a straight line along the approach
            #direction
            if success:
                if not Hao:
                    #Move forward open loop
                    success = tp.move_forward(0.05, True)
                else:
                    #Move forward using force transducer and palm contact sensors to detect any
                    #collisions
                    hu.GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20)
                    success = True

                if not success:
                    #If we failed to move forward - which might happen because
                    #we do not test if the final grasp pose is reachable along
                    #a straight line trajectory because the joint range limits
                    #set on the robot are not actually exactly the same as those
                    #in openrave and it usually doesn't cause any problems
                    #in general trajectory testing. 
                    grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                    grasp_status_msg = "Unable to move forward to grasp!"

            if Hao:
                """new routine"""
                #Close the hand using the force torque and contact sensors to detect contacts and
                #stop the fingers
                hu.GuardedCloseHand(self.global_data)
            else:        
                """old routine"""
                if success:
                    #Just close the hand open loop, leaving the spread angle where it is now. 
                    success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.pre_grasp_dof[1],grasp_msg.pre_grasp_dof[2], grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]])


                if success:
                    #Close the hand completely until the motors stall or they hit
                    #the final grasp DOFS
                    success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.final_grasp_dof[1],grasp_msg.final_grasp_dof[2], grasp_msg.final_grasp_dof[3], grasp_msg.final_grasp_dof[0]])            


                if success:
                    #Now close the hand completely until the motors stall.
                    success, grasp_status_msg, joint_angles = tp.close_barrett()
                if not success:
                    grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR

            #Now wait for user input on whether or not to lift the object
            if success:
                selection = int(raw_input('Lift up (1) or not (0): '))
                if selection == 1:
                    print 'lift up the object'
                    #Lift the object using the staubli's straight line path planner
                    success = tp.lift_arm(.05, True)
                    if not success:
                        grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                        grasp_status_msg = "Couldn't lift object"
                else:
                    print 'not lift up the object'


            #Maybe decide if it has been successfully lifted here...
            if success:
                rospy.logwarn(grasp_status_msg)
            else:
                rospy.logfatal(grasp_status_msg)
            #Tell graspit whether the grasp succeeded
            self.graspit_status_publisher.publish(grasp_status, grasp_status_msg, -1)
            
            print grasp_status_msg

            return grasp_status, grasp_status_msg

        except Exception as e:
            #print any exceptions and drop in to the debugger.
            import traceback
            print traceback.format_exc()
            pdb.set_trace()
Esempio n. 3
0
    def process_grasp_msg(self, grasp_msg):
        """@brief - Attempt to grasp the object and lift it

        First moves the arm to pregrasp the object, then attempts to grasp and lift it.
        The grasp and lifting phase is currently completely open loop
        
        """
        if (time() - self.last_grasp_time) < 30:
            return [], []
        self.last_grasp_time = time()
        print grasp_msg
        grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS
        grasp_status_msg = "grasp_succeeded"
        success = 1
        if not tp.is_home():
           print 'go home'
           tp.go_home(self.global_data)
        #    if not success:
        #        grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
        #        grasp_status_msg = "Unable to go home"
               
        if success:
            success, grasp_status_msg, positions = tp.open_barrett()
            if not success:
                grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
                
        if success:
            grasp_tran = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose))
            grasp_tran[0:3,3] /=1000 #mm to meters
            tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]])
            tp.update_robot(self.global_data.or_env.GetRobots()[0])

            print 'pre-grasp'
            self.model_manager()
#            success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, file_name_dict[self.target_object_name],  grasp_tran)
            print 'after model_manager()'
            success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, self.target_object_name,  False, grasp_tran)
            print 'after pre-grasp'
            #raw_input("Press enter...")
            tp.update_robot(self.global_data.or_env.GetRobots()[0])
            if not success:
                if not j:
                    grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                    grasp_status_msg = "Pregrasp tran Out of range!"
                else:
                    grasp_status = graspit_msgs.msg.GraspStatus.FAILED
                    grasp_status_msg = "Unknown planner failure!"

        if success:
            if not Hao:
                success = tp.move_forward(0.05, True)
            else:
                hu.GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20)
                success = True
            
            if not success:
                grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                grasp_status_msg = "Unable to move forward to grasp!"

        if Hao:
            """new routine"""
            hu.GuardedCloseHand(self.global_data)
        else:        
            """old routine"""
            if success:
                success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.pre_grasp_dof[1],grasp_msg.pre_grasp_dof[2], grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]])


            if success:
                success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.final_grasp_dof[1],grasp_msg.final_grasp_dof[2], grasp_msg.final_grasp_dof[3], grasp_msg.final_grasp_dof[0]])            


            if success:
                success, grasp_status_msg, joint_angles = tp.close_barrett()
            if not success:
                grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR
                        

        if success:
            selection = int(raw_input('Lift up (1) or not (0): '))
            if selection == 1:
                print 'lift up the object'
                success = tp.lift_arm(.05, True)
                if not success:
                    grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE
                    grasp_status_msg = "Couldn't lift object"
            else:
                print 'not lift up the object'
            

        #Maybe decide if it has been successfully lifted here...
        if success:
            rospy.logwarn(grasp_status_msg)
        else:
            rospy.logfatal(grasp_status_msg)
        self.graspit_status_publisher.publish(grasp_status, grasp_status_msg)
        print grasp_status_msg
        return grasp_status, grasp_status_msg