def open_spline_chain_main():
#x = [0, 3, -20, -10]
#y = [0, 30, -5, -3]
#th = [np.pi/2., np.pi, -np.pi/2., 0.]
x = np.cos(np.deg2rad(135.))*np.array([-10., 1., 50.])
y = np.sin(np.deg2rad(135.))*np.array([-10., 1., 50.])
th = [np.deg2rad(135.), np.deg2rad(135.), np.deg2rad(135.)]
X = np.column_stack((x, y))
# Random desired discretization along the splines for demonstration purposes
Ns = np.random.random_integers(100., 200., (X.shape[0]-1,)) # one less spline than waypoints.
sx, sy, sth, length, u, coeffs = PiazziSpline.splineOpenChain(X, th, Ns)
plt.plot(sx, sy, 'k-', linewidth=2.0)
#test_x = 3.0
#test_y = 30.01
test_x = 0.013
test_y = 0.008
u_star, closest, distance = closestPointOnSpline2D(length, sx, sy, test_x, test_y, u, coeffs, guess=None)
print "closest X = {:.3f}, {:.3f}, u* = {}".format(closest[0], closest[1], u_star)
# Figure out the LOS controller using this example
tangent_th = np.interp(u_star, u, sth)
print "tangent th = {:.3f} deg".format(tangent_th*180.0/np.pi)
lookAhead = 0.1 # how far forward in u
lookaheadState = splineToEuclidean2D(coeffs, min(u_star + lookAhead, u[-1]))
print "lookahead X = {:.3f}, {:.3f}".format(lookaheadState[0], lookaheadState[1])
dx_global = lookaheadState[0] - closest[0]
dy_global = lookaheadState[1] - closest[1]
print "dx_global = {:.3f}, dy_global = {:.3f}".format(dx_global, dy_global)
dx_frenet = dx_global*math.cos(tangent_th) + dy_global*math.sin(tangent_th)
dy_frenet = dx_global*math.sin(tangent_th) - dy_global*math.cos(tangent_th)
print "y error = {:.3f}, dx_frenet = {:.3f}, dy_frenet = {:.3f}".format(distance, dx_frenet, dy_frenet)
# need sign of distance to spline to change
# look at sign of cross product to determine "handed-ness"
angle_from_closest_to_test = math.atan2(closest[1] - test_y, closest[0] - test_x)
print "angle from closest to test = {:.3f} deg".format(angle_from_closest_to_test*180./np.pi)
sign_test = np.cross([math.cos(tangent_th), math.sin(tangent_th)], [math.cos(angle_from_closest_to_test), math.sin(angle_from_closest_to_test)])
distance *= np.sign(sign_test)
print "distance to spline after sign update = {:.3f}".format(distance)
# resulting triangle
relative_angle = math.atan2((distance - dy_frenet), dx_frenet)
global_angle = tangent_th + relative_angle
print "relative angle = {:.3f} deg, global angle = {:.3f} deg".format(relative_angle*180./np.pi, global_angle*180./np.pi)
# plot
plt.plot(test_x, test_y, 'r+', markersize=12., markeredgewidth=2.0)
plt.plot(closest[0], closest[1], 'gx', markersize=12., markeredgewidth=2.0)
plt.plot(lookaheadState[0], lookaheadState[1], 'go', markersize=12., markeredgewidth=2.0)
result_line = np.array([[test_x, test_y], closest])
lookAhead_line = np.array([closest, lookaheadState])
plt.plot(result_line[:, 0], result_line[:, 1], 'b-')
plt.plot(lookAhead_line[:, 0], lookAhead_line[:, 1], 'g-')
plt.arrow(closest[0], closest[1], 10.*math.cos(tangent_th), 10.*math.sin(tangent_th), linestyle='dashed')
d = math.sqrt(math.pow(lookaheadState[0] - test_x, 2) + math.pow(lookaheadState[1] - test_y, 2))
plt.arrow(test_x, test_y, d*math.cos(global_angle), d*math.sin(global_angle), linestyle='dotted')
plt.axis('equal')
plt.title("length = {:.2f}, u = {:.4f}, distance = {:.2f}".format(length[-1], u_star, distance))
plt.show()
def single_spline_main():
#x = [0, -5.]
#y = [0., 10.]
#th = [0., np.pi]
x = [10., -10.]
y = [-10., 10.]
th = [-np.pi/4., -np.pi/4.]
N = 500
sx, sy, sth, length, u, coeffs = PiazziSpline.piazziSpline(x[0], y[0], th[0], x[1], y[1], th[1], N=N)
plt.plot(sx, sy, 'k-', linewidth=2.0)
test_x = 0.5
test_y = 0.0
u_star, closest, distance = closestPointOnSpline2D(length, sx, sy, test_x, test_y, u, coeffs, guess=None)
#print "closest X = {:.3f}, {:.3f}, u* = {:.2f}".format(closest[0], closest[1], u_star)
# Figure out the LOS controller using this example
tangent_th = np.interp(u_star, u, sth)
#print "tangent th = {:.3f} deg".format(tangent_th*180.0/np.pi)
lookAhead = 0.01 # how far forward in u
lookaheadState = splineToEuclidean2D(coeffs, max(0.0, min(u_star + lookAhead, 1.0)))
#print "lookahead X = {:.3f}, {:.3f}".format(lookaheadState[0], lookaheadState[1])
dx_global = lookaheadState[0] - closest[0]
dy_global = lookaheadState[1] - closest[1]
#print "dx_global = {:.3f}, dy_global = {:.3f}".format(dx_global, dy_global)
dx_frenet = dx_global*math.cos(tangent_th) + dy_global*math.sin(tangent_th)
dy_frenet = dx_global*math.sin(tangent_th) - dy_global*math.cos(tangent_th)
#print "y error = {:.3f}, dx_frenet = {:.3f}, dy_frenet = {:.3f}".format(distance, dx_frenet, dy_frenet)
# need sign of distance to spline to change
# look at sign of cross product to determine "handed-ness"
angle_from_closest_to_test = math.atan2(closest[1] - test_y, closest[0] - test_x)
#print "angle from closest to test = {:.3f} deg".format(angle_from_closest_to_test*180./np.pi)
sign_test = np.cross([math.cos(tangent_th), math.sin(tangent_th)], [math.cos(angle_from_closest_to_test), math.sin(angle_from_closest_to_test)])
distance *= np.sign(sign_test)
#print "distance to spline after sign update = {:.3f}".format(distance)
# resulting triangle
relative_angle = math.atan2((distance - dy_frenet), dx_frenet)
global_angle = relative_angle + tangent_th
print "relative angle = {:.3f} deg, global angle = {:.3f} deg".format(relative_angle*180./np.pi, global_angle*180./np.pi)
u_star, global_angle = LOS_angle(length, sx, sy, sth, u, coeffs, test_x, test_y, lookAhead=lookAhead)
#print "relative angle = {:.3f} deg, global angle = {:.3f} deg".format(relative_angle*180./np.pi, global_angle*180./np.pi)
# plot
plt.plot(test_x, test_y, 'r+', markersize=12., markeredgewidth=2.0)
plt.plot(closest[0], closest[1], 'gx', markersize=12., markeredgewidth=2.0)
plt.plot(lookaheadState[0], lookaheadState[1], 'go', markersize=12., markeredgewidth=2.0)
result_line = np.array([[test_x, test_y], closest])
lookAhead_line = np.array([closest, lookaheadState])
plt.plot(result_line[:, 0], result_line[:, 1], 'b-')
plt.plot(lookAhead_line[:, 0], lookAhead_line[:, 1], 'g-')
plt.arrow(closest[0], closest[1], 10.*math.cos(tangent_th), 10.*math.sin(tangent_th), linestyle='dashed')
d = math.sqrt(math.pow(lookaheadState[0] - test_x, 2) + math.pow(lookaheadState[1] - test_y, 2))
plt.arrow(test_x, test_y, d*math.cos(global_angle), d*math.sin(global_angle), linestyle='dotted')
plt.axis('equal')
plt.title("length = {:.2f}, u = {:.4f}, distance = {:.2f}".format(length[-1], u_star, distance))
plt.show()
def closed_spline_chain_main():
# a chain test
x = [10, 0, -10, 0]
y = [0, 10, 0, -10]
X = np.column_stack((x, y))
th = [np.pi/2.0, np.pi, -np.pi/2.0, 0]
Ns = np.random.random_integers(100., 200., (X.shape[0],))
sx, sy, sth, length, u, coeffs = PiazziSpline.splineClosedChain(X, th, Ns)
plt.plot(sx, sy, 'k-', linewidth=2.0)
test_x = 11.0
test_y = -0.5
u_star, closest, distance = closestPointOnSpline2D(length, sx, sy, test_x, test_y, u, coeffs, guess=None)
print "closest X = {:.3f}, {:.3f}, u* = {}".format(closest[0], closest[1], u_star)
# Figure out the LOS controller using this example
tangent_th = np.interp(u_star, u, sth)
print "tangent th = {:.3f} deg".format(tangent_th*180.0/np.pi)
lookAhead = 0.2 # how far forward in u (remember u goes up to spline_count in a spline chain, not just up to 1)
wrapped_lookahead = np.mod(u_star + lookAhead, u[-1]) # this will make u wrap around the closed spline
lookaheadState = splineToEuclidean2D(coeffs, wrapped_lookahead)
print "lookahead X = {:.3f}, {:.3f}".format(lookaheadState[0], lookaheadState[1])
dx_global = lookaheadState[0] - closest[0]
dy_global = lookaheadState[1] - closest[1]
print "dx_global = {:.3f}, dy_global = {:.3f}".format(dx_global, dy_global)
dx_frenet = dx_global*math.cos(tangent_th) + dy_global*math.sin(tangent_th)
dy_frenet = dx_global*math.sin(tangent_th) - dy_global*math.cos(tangent_th)
print "y error = {:.3f}, dx_frenet = {:.3f}, dy_frenet = {:.3f}".format(distance, dx_frenet, dy_frenet)
# need sign of distance to spline to change
# look at sign of cross product to determine "handed-ness"
angle_from_closest_to_test = math.atan2(closest[1] - test_y, closest[0] - test_x)
print "angle from closest to test = {:.3f} deg".format(angle_from_closest_to_test*180./np.pi)
sign_test = np.cross([math.cos(tangent_th), math.sin(tangent_th)], [math.cos(angle_from_closest_to_test), math.sin(angle_from_closest_to_test)])
distance *= np.sign(sign_test)
print "distance to spline after sign update = {:.3f}".format(distance)
# resulting triangle
relative_angle = math.atan2((distance - dy_frenet), dx_frenet)
global_angle = tangent_th + relative_angle
print "relative angle = {:.3f} deg, global angle = {:.3f} deg".format(relative_angle*180./np.pi, global_angle*180./np.pi)
# plot
plt.plot(test_x, test_y, 'r+', markersize=12., markeredgewidth=2.0)
plt.plot(closest[0], closest[1], 'gx', markersize=12., markeredgewidth=2.0)
plt.plot(lookaheadState[0], lookaheadState[1], 'go', markersize=12., markeredgewidth=2.0)
result_line = np.array([[test_x, test_y], closest])
lookAhead_line = np.array([closest, lookaheadState])
plt.plot(result_line[:, 0], result_line[:, 1], 'b-')
plt.plot(lookAhead_line[:, 0], lookAhead_line[:, 1], 'g-')
plt.arrow(closest[0], closest[1], 10.*math.cos(tangent_th), 10.*math.sin(tangent_th), linestyle='dashed')
d = math.sqrt(math.pow(lookaheadState[0] - test_x, 2) + math.pow(lookaheadState[1] - test_y, 2))
plt.arrow(test_x, test_y, d*math.cos(global_angle), d*math.sin(global_angle), linestyle='dotted')
plt.axis('equal')
plt.title("length = {:.2f}, u = {:.4f}, distance = {:.2f}".format(length[-1], u_star, distance))
plt.show()
