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()