Exemple #1
0
class RavenArm:
    """
    Class for controlling the end effectors of the Raven
    """

    def __init__ (self, armName, closedGraspValue=0.,defaultPoseSpeed=.01):
        self.armName = armName

        if armName == raven_constants.Arm.Left:
            self.toolFrame = raven_constants.Frames.LeftTool
        else:
            self.toolFrame = raven_constants.Frames.RightTool
            
        self.commandFrame = raven_constants.Frames.Link0
        
        self.ravenController = RavenController(self.armName, closedGraspValue=closedGraspValue, defaultPoseSpeed=defaultPoseSpeed)

 
    #############################
    # start, pause, and stop ####
    #############################
        
    def start(self):
        """
        Starts the raven arm (releases the e-brake)
        (does nothing if raven arm is already running)
        """
        print 'starting raven arm ', self.armName
        return self.ravenController.start()

    def pause(self):
        """
        Pauses by clearing the stages
        """
        self.ravenController.clearStages()

    def isPaused(self):
        """
        True if no stages
        """
        return len(self.ravenController.stages) == 0
        
    def stop(self):
        return self.ravenController.stop()

    #######################
    # trajectory commands #
    #######################

    def executePoseTrajectory(self, poseTraj, block=True, duration=None, speed=None, ignoreOrientation=False):
        """
        poseTraj is a tuple/list of poses

        Can set either duration or speed (not both)

        duration is either the time of the whole trajectory
        or a list of the duration of each segment of the trajectory

        speed is the joint speed
        """
        if duration != None:
            if type(duration) != tuple:
                duration = [duration/len(poseTraj) for _ in range(len(poseTraj))]
            
            if len(duration) != len(poseTraj):
                return

        prevPose = None
        for pose in poseTraj:
            self.goToGripperPose(tfx.pose(pose), startPose=prevPose, block=False, duration=duration, speed=speed, ignoreOrientation=ignoreOrientation)
            prevPose = pose

        if block:
            return self.blockUntilPaused()
        return True

    def executeStampedPoseTrajectory(self, stampedPoses, block=True):
        """
        stampedPoses is a list of tfx poses with time stamps
        """

        prevTime = rospy.Time.now()
        prevPose = None

        for stampedPose in stampedPoses:
            duration = (stampedPose.stamp - prevTime).to_sec()

            pose = tfx.pose(stampedPose.position, stampedPose.orientation)
            self.goToGripperPose(pose, startPose=prevPose, block=False, duration=duration)

            prevTime = stampedPose.stamp
            prevPose = pose

        if block:
            return self.blockUntilPaused()
        return True
    
    def executeDeltaPoseTrajectory(self, deltaPoses, startPose=None, block=True, speed=None, ignoreOrientation=False):
        """
        Each deltaPose in deltaPoses is with respect to the startPose
        
        Assuming deltaPoses and startPose in same frame (0_link)
        """
        if startPose is None:
            startPose = self.getGripperPose()
            if startPose is None:
                return
        
        endPoses = [raven_util.endPose(startPose, deltaPose, self.commandFrame) for deltaPose in deltaPoses]
        #IPython.embed()
        print
        print 'START', startPose
        print 'END', endPoses[-1]
        print
        '''IPython.embed()
        '''
        return self.executePoseTrajectory(endPoses, block=block, speed=speed, ignoreOrientation=ignoreOrientation)
            

    def executeStampedDeltaPoseTrajectory(self, stampedDeltaPoses, startPose=None, block=True):
        """
        stampedDeltaPoses is a list of tfx poses with time stamps

        each stampedDeltaPose is endPose - startPose, where
        each endPose is different but the startPose is the same

        This function is intended to be used where each
        endPose is from the vision system and the one
        startPose is the gripper position according to vision
        """
        durations = []
        prevTime = rospy.Time.now()
        for stampedDeltaPose in stampedDeltaPoses:
            durations.append(stampedDeltaPose.stamp - prevTime)
            prevTime = stampedDeltaPose.stamp

        if startPose == None:
            startPose = self.getGripperPose()
            if startPose == None:
                return

        startPose = raven_util.convertToFrame(tfx.pose(startPose), self.commandFrame)

        endPoses = []
        for deltaPose in stampedDeltaPoses:
            endPoses.append(raven_util.endPose(startPose, deltaPose, self.commandFrame))

        prevPose = None
        for duration, endPose in zip(duration, endPoses):
            self.goToGripperPose(endPose, startPose=prevPose, block=False, duration=duration)
            prevPose = endPose

        if block:
            return self.blockUntilPaused()
        return True

    def executeJointTrajectory(self, jointTraj, block=True, duration=None, speed=None):
        """
        jointTraj is a tuple/list of joints dictionaries

        joints is a dictionary of {jointType : jointPos}

        jointType is from raven_2_msgs.msg.Constants
        jointPos is position in radians

        Can set either duration or speed (not both)

        duration is either the time of the whole trajectory
        or a list of the duration of each segment of the trajectory

        speed is a scalar factor (compared to default joint speed)
        """
        if duration != None:
            if type(duration) != tuple:
                duration = [duration/len(jointTraj) for _ in range(len(jointTraj))]
            
            if len(duration) != len(jointTraj):
                return

        prevJoints = None
        for joints in jointTraj:
            joints = dict(joints)
            self.goToJoints(joints, startJoints=prevJoints, block=False, duration=duration, speed=speed)
            prevJoints = joints

        if block:
            self.blockUntilPaused()
        return True

    def executeStampedJointTrajectory(self, stampedJoints, block=True):
        """
        stampedJoints is a tuple of ((stamp, joints), ....)

        joints is dictionary of {jointType : jointPos}
        
        jointType is from raven_2_msgs.msg.Constants
        jointPos is position in radians
        """
        
        prevTime = rospy.Time.now()
        prevJoints = None

        for stamp, joints in stampedJoints:
            duration = (stamp - prevTime).to_sec()
            joints = dict(joints) # so we don't clobber, just in case
            
            self.goToJoints(joints, startJoints=prevJoints, block=False, duration=duration)

            prevTime = stamp
            prevJoints = joints

        if block:
            return self.blockUntilPaused()
        return True

    #####################
    # command methods   #
    #####################

    def goToGripperPose(self, endPose, startPose=None, block=True, duration=None, speed=None, ignoreOrientation=False):
        """        
        Move to endPose

        If startPose is not specified, default to current pose according to tf
        (w.r.t. Link0)
        """
        if startPose == None:
            startPose = self.getGripperPose()
            if startPose == None:
                return

        startPose = raven_util.convertToFrame(tfx.pose(startPose), self.commandFrame)
        endPose = raven_util.convertToFrame(tfx.pose(endPose), self.commandFrame)


        if ignoreOrientation:
            endPose.orientation = startPose.orientation

        self.ravenController.goToPose(endPose, start=startPose, duration=duration, speed=speed)

        if block:
            return self.blockUntilPaused()
        return True

    def goToGripperPoseDelta(self, deltaPose, startPose=None, block=True, duration=None, speed=None, ignoreOrientation=False):
        """
        Move by deltaPose

        If startPose is not specified, default to current pose according to tf
        (w.r.t. Link0)
        """
        if startPose == None:
            startPose = self.getGripperPose()
            if startPose == None:
                return

        endPose = raven_util.endPose(startPose, deltaPose, self.commandFrame)

        self.goToGripperPose(endPose, startPose=startPose, block=block, duration=duration, speed=speed, ignoreOrientation=ignoreOrientation)


    def goToJoints(self, desJoints, startJoints=None, block=True, duration=None, speed=None):
        """
        desJoints is dictionary of {jointType : jointPos}
        
        jointType is from raven_2_msgs.msg.Constants
        jointPos is position in radians
        
        speed is a factor gain compared to default speeds
        """
        
        self.ravenController.goToJoints(desJoints, startJoints=startJoints, duration=duration, speed=speed)

        if block:
            return self.blockUntilPaused()
        return True
            
    def closeGripper(self,duration=2.5, block=True):
        self.setGripper(0.,duration=duration,block=block)

                
    def openGripper(self,duration=2.5, block=True):
        self.setGripper(1., duration=duration, block=block)

    def setGripper(self, grasp,duration=2.5, block=True):
        self.ravenController.clearStages()
        self.ravenController.setGripper(grasp,duration=duration)
        
        if block:
            rospy.sleep(duration)


    #######################
    # state info methods  #
    #######################

    def getGripperPose(self,frame=raven_constants.Frames.Link0):
        """
        Returns gripper pose w.r.t. frame

        geometry_msgs.msg.Pose
        """
        pose = tfx.pose([0,0,0],frame=self.toolFrame)
        return tfx.convertToFrame(pose, self.commandFrame, pose.frame, wait=10)
        
        gripperPose = self.ravenController.currentPose

        if gripperPose != None:
            gripperPose = raven_util.convertToFrame(tfx.pose(gripperPose), frame)

        return gripperPose

    def getCurrentJoints(self):
        """
        Returns is dictionary of {jointType : jointPos}
        
        jointType is from raven_2_msgs.msg.Constants
        jointPos is position in radians
        """
        return self.ravenController.getCurrentJoints()



    #################
    # other methods #
    #################
    
    @property
    def name(self):
        return self.armName
    
    def blockUntilPaused(self, timeoutTime=999999):
        timeout = raven_util.Timeout(timeoutTime)
        timeout.start()

        while not self.isPaused():
            if rospy.is_shutdown() or timeout.hasTimedOut():
                return False
            rospy.sleep(.05)

        return True