def SetEnd(self, robotid, currentik, initik, T0_TSY):
        
        robotid.SetActiveDOFValues(currentik)
        
        T_rh = None
        T_lf = None

        # Defines a box in which to have the end-effector manipulate
        if self.use_manipbox :
            hand_box = matrix( self.manipbox_dim + [-1000,1000,-1000,1000,-1000,1000] )
            T_rh = T0_TSY
            T_lh = T0_TSY
        else :
            hand_box = matrix([-1000,1000,-1000,1000,-1000,1000,-1000,1000,-1000,1000,-1000,1000])
            T_rh = self.robotManips[1].GetEndEffectorTransform()
            T_lh = self.robotManips[0].GetEndEffectorTransform()

        # Define Task Space Region strings
        # Left Hand
        TSRStringLH1 = SerializeTSR(0,'NULL',T_lh,eye(4),hand_box)
        TSRStringLH0 = SerializeTSR(0,'NULL',self.robotManips[0].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
        # Right Hand
        TSRStringRH1 = SerializeTSR(1,'NULL',T_rh,eye(4),hand_box)
        TSRStringRH0 = SerializeTSR(1,'NULL',self.robotManips[1].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
        # Left Foot
        TSRStringLF1 = SerializeTSR(2,'NULL',self.robotManips[2].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
        # Right Foot
        TSRStringRF1 = SerializeTSR(3,'NULL',self.robotManips[3].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))
        # Head
        TSRStringH = SerializeTSR(4,'NULL',self.robotManips[4].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,0,0,0,0,0,0,0,0]))

        if(type(initik) == type("")):
            robotid.SetActiveDOFValues(str2num(initik))
        else:
            robotid.SetActiveDOFValues(initik)

        robotid.GetController().Reset(0)
        
        # Left Foot
        TSRStringLF2 = SerializeTSR(2,'NULL',self.robotManips[2].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,-100,100,0,0,0,0,0,0]))
        # Right Foot
        TSRStringRF2 = SerializeTSR(3,'NULL',self.robotManips[3].GetEndEffectorTransform(),eye(4),matrix([0,0,0,0,-100,100,0,0,0,0,0,0]))

        # We have the strings. Let's chain them together.
        
        self.TSRChainStringFeetandHead_current2init_bh = SerializeTSRChain(0,0,1,1,TSRStringLH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRH1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
        self.TSRChainStringFeetandHead_current2init_lh = SerializeTSRChain(0,0,1,1,TSRStringRH0,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
        self.TSRChainStringFeetandHead_current2init_rh = SerializeTSRChain(0,0,1,1,TSRStringLH0,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringLF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF1,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
        self.TSRChainStringFeetandHead_init2home = SerializeTSRChain(0,0,1,1,TSRStringLF2,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringRF2,'NULL',[])+' '+SerializeTSRChain(0,0,1,1,TSRStringH,'NULL',[])
def traj2ach(env,robot,traj,fname,robotJointValsOffset,robotJointVelsOffset,deltatimeOffset):

    myAchTraj=openravepy.RaveCreateTrajectory(robot.GetEnv(),'')
    config=deepcopy(traj.GetConfigurationSpecification())
    robotJointValsDOF = traj.GetConfigurationSpecification().GetGroupFromName("joint_values "+robot.GetName()).dof
    robotJointVelsDOF = traj.GetConfigurationSpecification().GetGroupFromName("joint_velocities "+robot.GetName()).dof
    
    #config.AddDeltaTimeGroup()
    myAchTraj.Init(config)

    trajLength = traj.GetNumWaypoints()
    numJoints = len(robot.GetJoints())
    
    freq = 1.0/25.0
    for i in range(trajLength):
        wp = traj.GetWaypoint(i)
        awp = deepcopy(wp)
        # print "joint values"
        q = wp[robotJointValsOffset:(robotJointValsOffset+robotJointValsDOF)]
        #print "Rave2RealHubo says"
        #print robotJointValsOffset
        #print (robotJointValsOffset+numJoints)
        
        # print q
        # print "joint velocities"
        qdot = wp[robotJointVelsOffset:(robotJointVelsOffset+robotJointVelsDOF)]
        # print qdot
        # print "deltatime"
        dt = wp[deltatimeOffset]
        # print dt
        if(dt != 0.0):
            awp[deltatimeOffset] = 0.5

        myAchTraj.Insert(i,awp)
        
        # pose['RHY'] = q[1]
        # pose['RHR'] = q[2]
        # pose['RHP'] = q[3]
        # pose['RKN'] = q[4]
        # pose['RAP'] = q[5]
        # pose['RAR'] = q[6]
        # pose['LHY'] = q[26]
        # pose['LHR'] = q[27]
        # pose['LHP'] = q[28]
        # pose['LKN'] = q[29]
        # pose['LAP'] = q[30]
        # pose['LAR'] = q[31]
        # pose['RSP'] = q[7]
        # pose['RSR'] = q[8]
        # pose['RSY'] = q[9]
        # pose['REB'] = q[10]
        # pose['RWY'] = q[11]
        # pose['RWR'] = q[13]
        # pose['RWP'] = q[12]
        # pose['LSP'] = q[19]
        # pose['LSR'] = q[20]
        # pose['LSY'] = q[21]
        # pose['LEB'] = q[22]
        # pose['LWY'] = q[23]
        # pose['LWR'] = q[25]
        # pose['LWP'] = q[24]
        # pose['NKY'] = q[14]
        # pose['NK1'] = q[15]
        # pose['NK2'] = q[16]
        # pose['WST'] = q[0]
        # pose['RF1'] = q[18]
        # pose['RF2'] = q[17]
        # pose['RF3'] = 0
        # pose['RF4'] = 0
        # pose['RF5'] = 0
        # pose['LF1'] = q[32]
        # pose['LF2'] = 0
        # pose['LF3'] = 0
        # pose['LF4'] = 0
        # pose['LF5'] = 0
        
    # openravepy.planningutils.RetimeTrajectory(myAchTraj,hastimestamps=True) # inputs: trajectory, hastimestamps.

    openravepy.planningutils.RetimeActiveDOFTrajectory(myAchTraj,robot,hastimestamps=False,maxvelmult=1,maxaccelmult=1,plannername='ParabolicTrajectoryRetimer')

    achTrajLength = myAchTraj.GetNumWaypoints()

    retimedRobotJointValsOffset = myAchTraj.GetConfigurationSpecification().GetGroupFromName("joint_values "+robot.GetName()).offset
    retimedRobotJointValsDOF = myAchTraj.GetConfigurationSpecification().GetGroupFromName("joint_values "+robot.GetName()).dof
    
    activedofs = str2num(myAchTraj.GetConfigurationSpecification().GetGroupFromName("joint_values "+robot.GetName()).name[len("joint_values "+robot.GetName()):]).astype(int)
    fRaveRetimed = open(fname+'_retimed.txt','w')
    fRaveRetimed.write(myAchTraj.serialize(0))
    fRaveRetimed.close()

    f = open(fname+'.traj','w')

    for i in range(achTrajLength):
        atwp = myAchTraj.GetWaypoint(i)
        allq = zeros(numJoints)
        aq = atwp[retimedRobotJointValsOffset:(retimedRobotJointValsOffset+retimedRobotJointValsDOF)]
        for aIdx, a in enumerate(activedofs):
            allq[a] = aq[aIdx]
            
        # No Fingers
        myAchQ = [allq[1], allq[2], allq[3], allq[4], allq[5], allq[6], allq[26], allq[27], allq[28], allq[29], allq[30], allq[31], allq[7], allq[8], allq[9], allq[10], allq[11], allq[13], allq[12], allq[19], allq[20], allq[21], allq[22], allq[23], allq[25], allq[24], allq[14], allq[15], allq[16], allq[0], 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]

        # print myAchQ
        f.write(' '.join([str(x) for x in myAchQ])+'\n')

    f.close()
Пример #3
0
def traj2ach(env, robot, traj, fname, robotJointValsOffset,
             robotJointVelsOffset, deltatimeOffset):

    myAchTraj = openravepy.RaveCreateTrajectory(robot.GetEnv(), '')
    config = deepcopy(traj.GetConfigurationSpecification())
    robotJointValsDOF = traj.GetConfigurationSpecification().GetGroupFromName(
        "joint_values " + robot.GetName()).dof
    robotJointVelsDOF = traj.GetConfigurationSpecification().GetGroupFromName(
        "joint_velocities " + robot.GetName()).dof

    #config.AddDeltaTimeGroup()
    myAchTraj.Init(config)

    trajLength = traj.GetNumWaypoints()
    numJoints = len(robot.GetJoints())

    freq = 1.0 / 25.0
    for i in range(trajLength):
        wp = traj.GetWaypoint(i)
        awp = deepcopy(wp)
        # print "joint values"
        q = wp[robotJointValsOffset:(robotJointValsOffset + robotJointValsDOF)]
        #print "Rave2RealHubo says"
        #print robotJointValsOffset
        #print (robotJointValsOffset+numJoints)

        # print q
        # print "joint velocities"
        qdot = wp[robotJointVelsOffset:(robotJointVelsOffset +
                                        robotJointVelsDOF)]
        # print qdot
        # print "deltatime"
        dt = wp[deltatimeOffset]
        # print dt
        if (dt != 0.0):
            awp[deltatimeOffset] = 0.5

        myAchTraj.Insert(i, awp)

        # pose['RHY'] = q[36]
        # pose['RHR'] = q[37]
        # pose['RHP'] = q[38]
        # pose['RKN'] = q[39]
        # pose['RAP'] = q[40]
        # pose['RAR'] = q[41]
        # pose['LHY'] = q[11]
        # pose['LHR'] = q[12]
        # pose['LHP'] = q[13]
        # pose['LKN'] = q[14]
        # pose['LAP'] = q[15]
        # pose['LAR'] = q[16]
        # pose['RSP'] = q[26]
        # pose['RSR'] = q[27]
        # pose['RSY'] = q[28]
        # pose['REB'] = q[29]
        # pose['RWY'] = q[30]
        # pose['RWR'] = q[32]
        # pose['RWP'] = q[31]
        # pose['LSP'] = q[0]
        # pose['LSR'] = q[1]
        # pose['LSY'] = q[2]
        # pose['LEB'] = q[3]
        # pose['LWY'] = q[4]
        # pose['LWR'] = q[6]
        # pose['LWP'] = q[5]
        # pose['NKY'] = q[17]
        # pose['NK1'] = q[18]
        # pose['NK2'] = q[19]
        # pose['WST'] = q[10]
        # pose['RF1'] = q[33]
        # pose['RF2'] = q[42]
        # pose['RF3'] = q[45]
        # pose['RF4'] = q[48]
        # pose['RF5'] = 0
        # pose['LF1'] = q[7]
        # pose['LF2'] = q[20]
        # pose['LF3'] = q[23]
        # pose['LF4'] = 0
        # pose['LF5'] = 0

    # openravepy.planningutils.RetimeTrajectory(myAchTraj,hastimestamps=True) # inputs: trajectory, hastimestamps.

    openravepy.planningutils.RetimeActiveDOFTrajectory(
        myAchTraj,
        robot,
        hastimestamps=False,
        maxvelmult=1,
        maxaccelmult=1,
        plannername='ParabolicTrajectoryRetimer')

    achTrajLength = myAchTraj.GetNumWaypoints()

    retimedRobotJointValsOffset = myAchTraj.GetConfigurationSpecification(
    ).GetGroupFromName("joint_values " + robot.GetName()).offset
    retimedRobotJointValsDOF = myAchTraj.GetConfigurationSpecification(
    ).GetGroupFromName("joint_values " + robot.GetName()).dof

    activedofs = str2num(
        myAchTraj.GetConfigurationSpecification().GetGroupFromName(
            "joint_values " +
            robot.GetName()).name[len("joint_values " +
                                      robot.GetName()):]).astype(int)
    fRaveRetimed = open(fname + '_retimed.txt', 'w')
    fRaveRetimed.write(myAchTraj.serialize(0))
    fRaveRetimed.close()

    f = open(fname + '.traj', 'w')

    for i in range(achTrajLength):
        atwp = myAchTraj.GetWaypoint(i)
        allq = zeros(numJoints)
        aq = atwp[retimedRobotJointValsOffset:(retimedRobotJointValsOffset +
                                               retimedRobotJointValsDOF)]
        for aIdx, a in enumerate(activedofs):
            # With Fingers
            # myAchQ = [q[36], q[37], q[38], q[39], q[40], q[41], q[11], q[12], q[13], q[14], q[15], q[16], q[26], q[27], q[28], q[29], q[30], q[32], q[31], q[0], q[1], q[2], q[3], q[4], q[6], q[5], q[17], q[18], q[19], q[10], q[33], q[42], q[45], q[48], 0, q[7], q[20], q[23],  0, 0]
            allq[a] = aq[aIdx]

        # No Fingers
        myAchQ = [
            allq[36], allq[37], allq[38], allq[39], allq[40], allq[41],
            allq[11], allq[12], allq[13], allq[14], allq[15], allq[16],
            allq[26], allq[27], allq[28], allq[29], allq[30], allq[32],
            allq[31], allq[0], allq[1], allq[2], allq[3], allq[4], allq[6],
            allq[5], allq[17], allq[18], allq[19], allq[10], 0, 0, 0, 0, 0, 0,
            0, 0, 0, 0
        ]

        # print myAchQ
        f.write(' '.join([str(x) for x in myAchQ]) + '\n')

    f.close()