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)
