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