def testSingleSourceGoal():
dT = 0.0005
initialState = [0.0, 0, 0.5*np.pi, 1, 0]
finalState = [1.0, 1.0, -0.5*np.pi, 1, 0]
vConstraints = [1.0, 0.0, 2.0, -2.0, 5.0, -5.0]
kConstraints = [0.785, -0.785, 30.0, -30.0, 30.0, -30.0]
headlandSpeed = vConstraints[0]
headlandSpeedReverse = vConstraints[1]
path = planShortest(kConstraints, vConstraints, headlandSpeed, headlandSpeedReverse, initialState, finalState, dT, 0)
plt.figure()
plt.clf()
plt.title("Final Path")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
plt.arrow(path.poses[0][0], path.poses[0][1], 0.1*np.cos(path.poses[0][2]), 0.1*np.sin(path.poses[0][2]), length_includes_head = True, width = 0.01, head_width = 0.03, color = 'r', alpha = 0.5)
plt.arrow(path.poses[-1][0], path.poses[-1][1], 0.1*np.cos(path.poses[-1][2]), 0.1*np.sin(path.poses[-1][2]), length_includes_head = True, width = 0.01, head_width = 0.03, color = 'b', alpha = 0.5)
plt.plot([i[0] for i in path.poses], [i[1] for i in path.poses], 'k')
plt.savefig("./logs/singleSourceGoal.png")
def goalStateIterationCurvature():
#final orientation
dT = 0.001
initialState = [0.0, -1.0, 0.5*np.pi, 1, 0]
finalStates = [[1, 0.0, -0.5*np.pi, 1, 0] for i in np.linspace(-0.785,0.785,5)]
vConstraints = [1.0, -1.0, 2.0, -2.0, 5.0, -5.0]
kConstraints = [0.785, -0.785, 30.0, -30.0, 30.0, -30.0]
headlandSpeed = vConstraints[0]
headlandSpeedReverse = vConstraints[1]
plt.figure()
plt.clf()
plt.title("Goal State Iteration")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
for finalState in finalStates:
path = planShortest(kConstraints, vConstraints, headlandSpeed, headlandSpeedReverse, initialState, finalState, dT)
plt.plot([i[0] for i in path.poses], [i[1] for i in path.poses])
plt.savefig("./logs/goalStateIterationCurvature.png")
def goalStateIterationCircle():
#circle
dT = 0.001
nIteration = 6
initialState = [0.0, 0, 0.5*np.pi, 1, 0]
finalStates = [[2*(np.cos(i)), 2*(np.sin(i)), i, 1, 0] for i in np.linspace(0,(2-1/nIteration)*np.pi,nIteration-1)]
vConstraints = [1.0, -1.0, 2.0, -2.0, 5.0, -5.0]
kConstraints = [0.785, -0.785, 30.0, -30.0, 30.0, -30.0]
headlandSpeed = vConstraints[0]
headlandSpeedReverse = vConstraints[1]
plt.figure()
plt.clf()
plt.title("Goal State Iteration")
plt.xlabel("x (m)")
plt.ylabel("y (m)")
for finalState in finalStates:
path = planShortest(kConstraints, vConstraints, headlandSpeed, headlandSpeedReverse, initialState, finalState, dT)
plt.plot([i[0] for i in path.poses], [i[1] for i in path.poses])
plt.savefig("./logs/goalStateIterationCircle.png")
def singleSourceGoal():
dT = 0.1
initialState = [0.0, 0.0, 0.5 * np.pi, 0, 0.3]
finalState = [0.0, 15.0, 0.5 * np.pi, 0, 0.3]
vConstraints = [1.0, -1.0, 1.0, -1.0, 10.0, -10.0]
kConstraints = [0.6, -0.6, 1.0, -1.0, 10.0, -10.0]
headlandSpeed = vConstraints[0]
headlandSpeedReverse = vConstraints[1]
t = time.time()
path = planShortest(kConstraints, vConstraints, headlandSpeed,
headlandSpeedReverse, initialState, finalState, dT)
elapsed = time.time() - t
print("Shortest path has a final time of: ", path.finalTime)
print("Time to calculate final path: ", elapsed)
######################### Plotting Data ####################################
plotPath(path)
plotControls(path, dT)
plotCurveAndSpeed(path, dT)
######################### Output to Txt ######################################
controls_file = open('./logs/control_out.txt', 'w')
pose_file = open('./logs/pose_out.txt', 'w')
for row in path.controls:
np.savetxt(controls_file, row)
for row in path.poses:
np.savetxt(pose_file, row)
pose_file.close()
controls_file.close()
#controls = np.loadtxt('./logs/control_out.txt').reshape(-1,2)
print(path.poses)
print(path.controls)
def compareHighLowDim():
dT = 0.005
initialState = [0.0, 0, 0.5 * np.pi, 1, 0]
finalState = [1.0, 1.0, -0.5 * np.pi, 1, 0]
vConstraints = [1.0, -1.0, 2.0, -2.0, 5.0, -5.0]
kConstraints = [0.785, -0.785, 5.0, -5.0, 30.0, -30.0]
headlandSpeed = vConstraints[0]
headlandSpeedReverse = vConstraints[1]
t = time.time()
pathHighDim = planShortest(kConstraints, vConstraints, headlandSpeed,
headlandSpeedReverse, initialState, finalState,
dT)
elapsedHighDim = time.time() - t
print("High dim. path has a final time of: ", pathHighDim.finalTime)
print("Time to calculate high dim. path: ", elapsedHighDim)
t = time.time()
pathLowDim = planShortest(kConstraints,
vConstraints,
headlandSpeed,
headlandSpeedReverse,
initialState,
finalState,
dT,
useDDotArg=False)
elapsedLowDim = time.time() - t
print("Low dim. path has a final time of: ", pathLowDim.finalTime)
print("Time to calculate low dim. path: ", elapsedLowDim)
path = pathHighDim
plt.figure()
plt.arrow(path.poses[0][0],
path.poses[0][1],
0.1 * np.cos(path.poses[0][2]),
0.1 * np.sin(path.poses[0][2]),
length_includes_head=True,
width=0.01,
head_width=0.03,
color='r',
alpha=0.5)
plt.arrow(path.poses[-1][0],
path.poses[-1][1],
0.1 * np.cos(path.poses[-1][2]),
0.1 * np.sin(path.poses[-1][2]),
length_includes_head=True,
width=0.01,
head_width=0.03,
color='b',
alpha=0.5)
plt.plot([i[0] for i in path.poses], [i[1] for i in path.poses], 'k')
path = pathLowDim
plt.plot([i[0] for i in path.poses], [i[1] for i in path.poses], '--k')
plt.legend([
'Constraints on $\ddot{\kappa}$, $\ddot{v}$',
'Constraints only on $\dot{\kappa}$, $\dot{v}$'
])
plt.savefig('./logs/ComparePaths.png')
fig, (ax1, ax2) = plt.subplots(2)
fig.suptitle('Curvature Profile')
ax1.plot([i * dT for i in range(len(path.controls.T[1]))],
path.controls.T[1], 'k')
ax1.set_ylabel('$\kappa$ $(m^{-1})$')
ax2.plot([i * dT for i in range(len(path.controls.T[1]) - 1)],
np.diff(path.controls.T[1]) / dT, 'k')
ax2.set_ylabel('$\dot{\kappa}$ $(m \cdot s)^{-1}$')
kappddot = np.diff(np.diff(path.controls.T[1]) / dT) / dT
for i in range(len(kappddot) - 1):
if np.abs(kappddot[i + 1] - kappddot[i]) > 1 and np.abs(
kappddot[i - 1] - kappddot[i]) > 1:
kappddot[i] = kappddot[i - 1]
ax2.set_xlabel('Time (s)')
plt.savefig('./logs/' + 'ComparePaths' + 'finalCurvature.png')
fig, (ax1, ax2) = plt.subplots(2)
fig.suptitle('Speed Profile')
ax1.plot([i * dT for i in range(len(path.controls.T[0]))],
path.controls.T[0], 'k')
ax1.set_ylabel('Speed $(m/s)$')
acc = np.diff(path.controls.T[0]) / dT
for i in range(len(acc)):
if np.abs(acc[i] - acc[i - 1]) > 1 and np.abs(acc[i] - acc[i + 1]) > 1:
acc[i] = acc[i - 1]
ax2.plot([i * dT for i in range(len(path.controls.T[0]) - 1)], acc, 'k')
ax2.set_ylabel('Accel. $(m/s^2)$')
ax2.set_xlabel('Time (s)')
plt.savefig('./logs/' + 'ComparePaths' + 'finalSpeed.png')
mdic_HighDim = {
"path": pathHighDim.poses,
"controls": pathHighDim.controls
}
mdic_LowDim = {"path": pathLowDim.poses, "controls": pathLowDim.controls}
savemat('./logs/HighDim.mat', mdic_HighDim)
savemat('./logs/LowDim.mat', mdic_LowDim)