示例#1
0
def testRegression(setupFile, thetaFile):
    """ 
    
    """
    rs=ReadXmlFile(setupFile)
    stateAll, commandAll = loadTrajs(pathDataFolder + "Brent/", 0.01, rs.det)
    stateAll=np.array(stateAll)
    arm=ArmType[rs.Arm]()
    trajReg = np.empty_like(stateAll)
    trajMaker = TrajMaker(rs,0.02,None,rs.path+thetaFile)

    for i in range(stateAll.shape[0]):
        coordHand = arm.mgdEndEffector(stateAll[i][0][2:])
        trajReg[i]=trajMaker.runTrajectory2(coordHand[0], coordHand[1])[0]
        
    plotRegBrent(trajReg, stateAll)
示例#2
0
def plotEstimator(setupFile, target_size, thetaName,x, y):
    arm = ArmType["Arm26"]()
    
    setupFile.det = True
    tm=TrajMaker(setupFile, target_size, False, thetaName, "Inv")
    state1,_,_,_=tm.runTrajectoryForPlot(x, y)
    
    setupFile.det = False
    state2,_,_,_=tm.runTrajectoryForPlot(x, y)
    
    tm=TrajMaker(setupFile, target_size, False, thetaName, "No")
    state3,_,_,_=tm.runTrajectoryForPlot(x, y)
    
    plt.figure(1, figsize=(16,9))
    
    hand=[]
    for state in state1 :
        hand.append(arm.mgdEndEffector(state[2:]))
    hand=np.array(hand)
    plt.plot(hand[:,0],hand[:,1], color="blue")
    
    hand=[]
    for state in state2 :
        hand.append(arm.mgdEndEffector(state[2:]))
    hand=np.array(hand)
    plt.plot(hand[:,0],hand[:,1], color="red")
    
    hand=[]
    for state in state3 :
        hand.append(arm.mgdEndEffector(state[2:]))
    hand=np.array(hand)
    plt.plot(hand[:,0],hand[:,1], color="green")
    
    plt.xlabel("X (m)")
    plt.ylabel("Y (m)")
    imageFolder = setupFile.OPTIpath + "/ImageBank/"
    name=findDataFilename(imageFolder, "Estimation", ".svg")
    plt.savefig(name, bbox_inches='tight')
    plt.show(block = True)