예제 #1
0
def testSwitchPlaces(show=True):
    #trajoptpy.SetInteractive(True)
    
    rospy.init_node('testSwitchPlaces',anonymous=True)
    rp = RavenPlanner(raven_constants.Arm.Both, thread=True)
    
    #rightCurrPose = tfx.pose([-0.068,-0.061,-0.129],tfx.tb_angles(-139.6,88.5,111.8),frame=raven_constants.Frames.Link0)
    #leftCurrPose = tfx.pose([-.072,-.015,-.153],tfx.tb_angles(43.9,78.6,100.9),frame=raven_constants.Frames.Link0)
    
    
    leftCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Left]),raven_constants.Frames.Link0)
    rightCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Right]),raven_constants.Frames.Link0)


    rp.getTrajectoryPoseToPose(raven_constants.Arm.Left, leftCurrPose, rightCurrPose, n_steps=50)
    rp.getTrajectoryPoseToPose(raven_constants.Arm.Right, rightCurrPose, leftCurrPose, n_steps=50)
    
    #rp.getPoseTrajectory(raven_constants.Arm.Left, rightCurrPose, n_steps=50)
    #rp.getPoseTrajectory(raven_constants.Arm.Right, leftCurrPose, n_steps=50)
    #rp.getPoseTrajectory(raven_constants.Arm.Left, leftCurrPose+[0.01,0,0], n_steps=50)
    #rp.getPoseTrajectory(raven_constants.Arm.Right, rightCurrPose-[0.01,0,0], n_steps=50)

    
    print 'waiting'
    rp.waitForTrajReady()
    print 'woooooooo'
    
    if rospy.is_shutdown():
        return
    
    #IPython.embed()
    
    if not show:
        return
    rp.env.SetViewer('qtcoin')
    
    leftPoseTraj = rp.poseTraj[raven_constants.Arm.Left]
    rightPoseTraj = rp.poseTraj[raven_constants.Arm.Right]
    
    for left, right in zip(leftPoseTraj,rightPoseTraj):
        if rospy.is_shutdown():
            break
        rp.updateOpenraveJoints('L', rp.getJointsFromPose('L', left, rp.getCurrentGrasp('L')), grasp=rp.getCurrentGrasp('L'))
        rp.updateOpenraveJoints('R', rp.getJointsFromPose('R', right, rp.getCurrentGrasp('R')), grasp=rp.getCurrentGrasp('R'))
        rospy.loginfo('Press enter to go to next step')
        raw_input()
        
    return
    
    leftTraj = rp.jointTraj[raven_constants.Arm.Left]
    rightTraj = rp.jointTraj[raven_constants.Arm.Right]
    
    for left, right in zip(leftTraj,rightTraj):
        if rospy.is_shutdown():
            break
        rp.updateOpenraveJoints('L', left)
        rp.updateOpenraveJoints('R', right)
        rospy.loginfo('Press enter to go to next step')
        raw_input()
예제 #2
0
 def setStartAndEndPose(self, armName, startPose, endPose, **kwargs):
     startGrasp = kwargs.get('startGrasp',kwargs.get('grasp',0))
     endGrasp = kwargs.get('endGrasp',kwargs.get('grasp',0))
     startJoints = self.getJointsFromPose(armName, startPose, grasp=startGrasp)
     endJoints = self.getJointsFromPose(armName, endPose, grasp=endGrasp)
     
     self.trajStartGrasp[armName] = startGrasp
     self.trajEndGrasp[armName] = endGrasp
     startPose = raven_util.convertToFrame(tfx.pose(startPose), raven_constants.Frames.Link0)
     self.trajStartPose[armName] = startPose
     endPose = raven_util.convertToFrame(tfx.pose(endPose), raven_constants.Frames.Link0)
     self.trajEndPose[armName] = endPose
     self.trajStartJoints[armName] = startJoints
     self.trajEndJoints[armName] = endJoints
예제 #3
0
def testFromAbove(show=True):
    trajoptpy.SetInteractive(True)
    
    rospy.init_node('testFromAbove',anonymous=True)
    rp = RavenPlanner([raven_constants.Arm.Left, raven_constants.Arm.Right], thread=True)
    
    leftCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Left]),raven_constants.Frames.Link0)
    rightCurrPose = raven_util.convertToFrame(tfx.pose([0,0,0],frame=rp.toolFrame[raven_constants.Arm.Right]),raven_constants.Frames.Link0)

    rp.getTrajectoryFromPose(raven_constants.Arm.Left, leftCurrPose-[0,0.05,0], n_steps=50, approachDir=np.array([0,0,1]), block=False)
    rp.getTrajectoryFromPose(raven_constants.Arm.Right, rightCurrPose-[0,0.05,0], n_steps=50, approachDir=np.array([0,0,1]))
    
    #IPython.embed()
    
    print 'waiting'
    rp.waitForTrajReady()
    print 'woooooooo'
    
    if rospy.is_shutdown():
        return
    
    #IPython.embed()
    
    if not show:
        return
    rp.env.SetViewer('qtcoin')
    
    leftPoseTraj = rp.poseTraj[raven_constants.Arm.Left]
    rightPoseTraj = rp.poseTraj[raven_constants.Arm.Right]
    

    
    leftTraj = rp.jointTraj[raven_constants.Arm.Left]
    rightTraj = rp.jointTraj[raven_constants.Arm.Right]
    
    for right in rightTraj:
        if rospy.is_shutdown():
            break
        rp.updateOpenraveJoints('L', left)
        rp.updateOpenraveJoints('R', right)
        rospy.loginfo('Press enter to go to next step')
        raw_input()
    
    IPython.embed()
예제 #4
0
    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
예제 #5
0
    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
예제 #6
0
    def getJointsFromPose(self, armName, pose, grasp, quiet=False):
        """
        Calls IK server and returns a dictionary of {jointType : jointPos}
        
        jointType is from raven_2_msgs.msg.Constants
        jointPos is position in radians

        Needs to return finger1 and finger2
        """

        pose = raven_util.convertToFrame(tfx.pose(pose), self.refFrame)
        
        joints = kin.invArmKin(armName, pose, grasp)
        
        if joints is None:
            rospy.loginfo('IK failed!')
            if quiet:
                return None
            else:
                raise RuntimeError("IK failed!")
        
        return joints
예제 #7
0
    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