From 89e5796fbc754a1718bb3b1a5e0059354d108ae3 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Sat, 5 Feb 2022 10:16:38 +0000 Subject: [PATCH] fix nosim demo not updating state correctly --- mpc_pybullet_demo/mpc_demo_nosim.py | 303 +++++++++++++++---------- mpc_pybullet_demo/mpc_demo_pybullet.py | 14 +- mpc_pybullet_demo/mpcpy/cvxpy_mpc.py | 10 +- mpc_pybullet_demo/mpcpy/mpc_config.py | 23 +- mpc_pybullet_demo/mpcpy/utils.py | 126 +++++----- 5 files changed, 262 insertions(+), 214 deletions(-) diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index a870798..d0b9321 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -3,6 +3,7 @@ import numpy as np import matplotlib.pyplot as plt from matplotlib import animation +from scipy.integrate import odeint from mpcpy.utils import compute_path_from_wp import mpcpy @@ -11,154 +12,195 @@ import sys import time # Robot Starting position -SIM_START_X=0. -SIM_START_Y=0.5 -SIM_START_V=0. -SIM_START_H=0. -L=0.3 +SIM_START_X = 0.0 +SIM_START_Y = 0.5 +SIM_START_V = 0.0 +SIM_START_H = 0.0 +L = 0.3 -P=mpcpy.Params() +P = mpcpy.Params() + +# Params +VEL = 1.0 # m/s # Classes -class MPCSim(): - +class MPCSim: def __init__(self): # State for the robot mathematical model [x,y,heading] - self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H]) + self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H]) - #starting guess + # starting guess self.action = np.zeros(P.M) - self.action[0] = P.MAX_ACC/2 #a - self.action[1] = 0.0 #delta - - self.opt_u = np.zeros((P.M,P.T)) + self.action[0] = P.MAX_ACC / 2 # a + self.action[1] = 0.0 # delta + + self.opt_u = np.zeros((P.M, P.T)) # Cost Matrices - Q = np.diag([20,20,10,20]) #state error cost - Qf = np.diag([30,30,30,30]) #state final error cost - R = np.diag([10,10]) #input cost - R_ = np.diag([10,10]) #input rate of change cost - - self.mpc = mpcpy.MPC(P.N,P.M,Q,R) + Q = np.diag([20, 20, 10, 20]) # state error cost + Qf = np.diag([30, 30, 30, 30]) # state final error cost + R = np.diag([10, 10]) # input cost + R_ = np.diag([10, 10]) # input rate of change cost + + self.mpc = mpcpy.MPC(P.N, P.M, Q, R) # Interpolated Path to follow given waypoints - self.path = compute_path_from_wp([0,3,4,6,10,12,13,13,6,1,0], - [0,0,2,4,3,3,-1,-2,-6,-2,-2],P.path_tick) + self.path = compute_path_from_wp( + [0, 3, 4, 6, 10, 12, 13, 13, 6, 1, 0], + [0, 0, 2, 4, 3, 3, -1, -2, -6, -2, -2], + P.path_tick, + ) # Sim help vars - self.sim_time=0 - self.x_history=[] - self.y_history=[] - self.v_history=[] - self.h_history=[] - self.a_history=[] - self.d_history=[] - self.predicted=None + self.sim_time = 0 + self.x_history = [] + self.y_history = [] + self.v_history = [] + self.h_history = [] + self.a_history = [] + self.d_history = [] + self.predicted = None - #Initialise plot + # Initialise plot plt.style.use("ggplot") self.fig = plt.figure() plt.ion() plt.show() - def predict_motion(self): - ''' - ''' - predicted=np.zeros(self.opt_u.shape) - x=self.state[0] - y=self.state[1] - v=self.state[2] - th=self.state[3] - - for idx,a,delta in zip(range(len(self.opt_u[0,:])),self.opt_u[0,:],self.opt_u[1,:]): - x = x+v*np.cos(th)*P.dt - y = y+v*np.sin(th)*P.dt - v = v+a*P.dt - th = th + v*np.tan(delta)/L*P.dt - - predicted[0,idx]=x - predicted[1,idx]=y + def preview(self, mpc_out): + """ + [TODO:summary] + [TODO:description] + """ + predicted = np.zeros(self.opt_u.shape) + predicted[:, :] = mpc_out[0:2, 1:] + Rotm = np.array( + [ + [np.cos(self.state[3]), np.sin(self.state[3])], + [-np.sin(self.state[3]), np.cos(self.state[3])], + ] + ) + predicted = (predicted.T.dot(Rotm)).T + predicted[0, :] += self.state[0] + predicted[1, :] += self.state[1] self.predicted = predicted def run(self): - ''' - ''' + """ + [TODO:summary] + + [TODO:description] + """ self.plot_sim() input("Press Enter to continue...") while 1: - - if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<0.1: + if ( + np.sqrt( + (self.state[0] - self.path[0, -1]) ** 2 + + (self.state[1] - self.path[1, -1]) ** 2 + ) + < 0.5 + ): print("Success! Goal Reached") input("Press Enter to continue...") return - - #optimization loop - #start=time.time() - + # optimization loop + # start=time.time() + # dynamycs w.r.t robot frame + curr_state = np.array( + [0, 0, self.state[2], 0] + ) # State Matrices - A,B,C = mpcpy.get_linear_model_matrices(self.state, self.action) - - #TODO: check why taget does not update? - - #Get Reference_traj -> inputs are in worldframe - target, _ = mpcpy.get_ref_trajectory(self.state, - self.path, 1.0) - - x_mpc, u_mpc = self.mpc.optimize_linearized_model(A, B, C, self.state, target, time_horizon=P.T, verbose=False) - - self.opt_u = np.vstack((np.array(u_mpc.value[0,:]).flatten(), - (np.array(u_mpc.value[1,:]).flatten()))) - - self.action[:] = [u_mpc.value[0,1],u_mpc.value[1,1]] + A, B, C = mpcpy.get_linear_model_matrices(curr_state, self.action) + # Get Reference_traj -> inputs are in worldframe + target, _ = mpcpy.get_ref_trajectory( + self.state, self.path, VEL, dl=P.path_tick + ) + x_mpc, u_mpc = self.mpc.optimize_linearized_model( + A, + B, + C, + curr_state, + target, + time_horizon=P.T, + verbose=False, + ) + self.opt_u = np.vstack( + ( + np.array(u_mpc.value[0, :]).flatten(), + (np.array(u_mpc.value[1, :]).flatten()), + ) + ) + self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]] # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) - - self.update_sim(self.action[0],self.action[1]) - self.predict_motion() + self.predict([self.action[0], self.action[1]]) + self.preview(x_mpc.value) self.plot_sim() + def predict(self, u): + def kinematics_model(x, t, u): + dxdt = x[2] * np.cos(x[3]) + dydt = x[2] * np.sin(x[3]) + dvdt = u[0] + dtheta0dt = x[2] * np.tan(u[1]) / P.L + dqdt = [dxdt, dydt, dvdt, dtheta0dt] + return dqdt - def update_sim(self,acc,steer): - ''' - ''' - self.state[0] = self.state[0] + self.state[2]*np.cos(self.state[3])*P.dt - self.state[1] = self.state[1] + self.state[2]*np.sin(self.state[3])*P.dt - self.state[2] = self.state[2] + acc*P.dt - self.state[3] = self.state[3] + self.state[2]*np.tan(steer)/L*P.dt + # solve ODE + tspan = [0, P.DT] + self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1] def plot_sim(self): - ''' - ''' - self.sim_time = self.sim_time+P.dt + """ + [TODO:summary] + + [TODO:description] + """ + self.sim_time = self.sim_time + P.DT self.x_history.append(self.state[0]) self.y_history.append(self.state[1]) self.v_history.append(self.state[2]) self.h_history.append(self.state[3]) - self.a_history.append(self.opt_u[0,1]) - self.d_history.append(self.opt_u[1,1]) + self.a_history.append(self.opt_u[0, 1]) + self.d_history.append(self.opt_u[1, 1]) plt.clf() grid = plt.GridSpec(2, 3) plt.subplot(grid[0:2, 0:2]) - plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time)) + plt.title( + "MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time) + ) - plt.plot(self.path[0,:],self.path[1,:], c='tab:orange', - marker=".", - label="reference track") + plt.plot( + self.path[0, :], + self.path[1, :], + c="tab:orange", + marker=".", + label="reference track", + ) - plt.plot(self.x_history, self.y_history, c='tab:blue', - marker=".", - alpha=0.5, - label="vehicle trajectory") + plt.plot( + self.x_history, + self.y_history, + c="tab:blue", + marker=".", + alpha=0.5, + label="vehicle trajectory", + ) if self.predicted is not None: - plt.plot(self.predicted[0,:], self.predicted[1,:], c='tab:green', - marker="+", - alpha=0.5, - label="mpc opt trajectory") + plt.plot( + self.predicted[0, :], + self.predicted[1, :], + c="tab:green", + marker="+", + alpha=0.5, + label="mpc opt trajectory", + ) # plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue', # marker=".", @@ -175,28 +217,32 @@ class MPCSim(): plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1]) - plt.ylabel('map y') - plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0)) - plt.xlabel('map x') - plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+1, 1.0)) + plt.ylabel("map y") + plt.yticks( + np.arange(min(self.path[1, :]) - 1.0, max(self.path[1, :] + 1.0) + 1, 1.0) + ) + plt.xlabel("map x") + plt.xticks( + np.arange(min(self.path[0, :]) - 1.0, max(self.path[0, :] + 1.0) + 1, 1.0) + ) plt.axis("equal") - #plt.legend() + # plt.legend() plt.subplot(grid[0, 2]) - #plt.title("Linear Velocity {} m/s".format(self.v_history[-1])) - plt.plot(self.a_history,c='tab:orange') + # plt.title("Linear Velocity {} m/s".format(self.v_history[-1])) + plt.plot(self.a_history, c="tab:orange") locs, _ = plt.xticks() - plt.xticks(locs[1:], locs[1:]*P.dt) - plt.ylabel('a(t) [m/ss]') - plt.xlabel('t [s]') + plt.xticks(locs[1:], locs[1:] * P.DT) + plt.ylabel("a(t) [m/ss]") + plt.xlabel("t [s]") plt.subplot(grid[1, 2]) - #plt.title("Angular Velocity {} m/s".format(self.w_history[-1])) - plt.plot(np.degrees(self.d_history),c='tab:orange') - plt.ylabel('gamma(t) [deg]') + # plt.title("Angular Velocity {} m/s".format(self.w_history[-1])) + plt.plot(np.degrees(self.d_history), c="tab:orange") + plt.ylabel("gamma(t) [deg]") locs, _ = plt.xticks() - plt.xticks(locs[1:], locs[1:]*P.dt) - plt.xlabel('t [s]') + plt.xticks(locs[1:], locs[1:] * P.DT) + plt.xlabel("t [s]") plt.tight_layout() @@ -205,23 +251,41 @@ class MPCSim(): def plot_car(x, y, yaw): + """ + [TODO:summary] + + [TODO:description] + + Parameters + ---------- + x : [TODO:type] + [TODO:description] + y : [TODO:type] + [TODO:description] + yaw : [TODO:type] + [TODO:description] + """ LENGTH = 0.5 # [m] WIDTH = 0.25 # [m] OFFSET = LENGTH # [m] - outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET], - [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]]) + outline = np.array( + [ + [-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET], + [WIDTH / 2, WIDTH / 2, -WIDTH / 2, -WIDTH / 2, WIDTH / 2], + ] + ) - Rotm = np.array([[np.cos(yaw), np.sin(yaw)], - [-np.sin(yaw), np.cos(yaw)]]) + Rotm = np.array([[np.cos(yaw), np.sin(yaw)], [-np.sin(yaw), np.cos(yaw)]]) outline = (outline.T.dot(Rotm)).T outline[0, :] += x outline[1, :] += y - plt.plot(np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), 'tab:blue') - + plt.plot( + np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), "tab:blue" + ) def do_sim(): @@ -231,5 +295,6 @@ def do_sim(): except Exception as e: sys.exit(e) -if __name__ == '__main__': + +if __name__ == "__main__": do_sim() diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index 94623d1..894592e 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -31,7 +31,7 @@ def set_ctrl(robotId,currVel,acceleration,steeringAngle): wheels = [8,15] maxForce = 50 - targetVelocity = currVel + acceleration*P.dt + targetVelocity = currVel + acceleration*P.DT #targetVelocity=lastVel #print(targetVelocity) @@ -158,10 +158,10 @@ def run_sim(): state[3] = 0.0 #add 1 timestep delay to input - state[0]=state[0]+state[2]*np.cos(state[3])*P.dt - state[1]=state[1]+state[2]*np.sin(state[3])*P.dt - state[2]=state[2]+action[0]*P.dt - state[3]=state[3]+action[0]*np.tan(action[1])/P.L*P.dt + state[0]=state[0]+state[2]*np.cos(state[3])*P.DT + state[1]=state[1]+state[2]*np.sin(state[3])*P.DT + state[2]=state[2]+action[0]*P.DT + state[3]=state[3]+action[0]*np.tan(action[1])/P.L*P.DT #optimization loop @@ -186,8 +186,8 @@ def run_sim(): set_ctrl(car,state[2],action[0],action[1]) - if P.dt-elapsed>0: - time.sleep(P.dt-elapsed) + if P.DT-elapsed>0: + time.sleep(P.DT-elapsed) if __name__ == '__main__': run_sim() diff --git a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py index 64e3201..1165560 100755 --- a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py +++ b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py @@ -34,15 +34,15 @@ def get_linear_model_matrices(x_bar,u_bar): A[1,2] = st A[1,3] = v*ct A[3,2] = v*td/P.L - A_lin = np.eye(P.N)+P.dt*A + A_lin = np.eye(P.N)+P.DT*A B = np.zeros((P.N,P.M)) B[2,0]=1 B[3,1]=v/(P.L*cd**2) - B_lin=P.dt*B + B_lin=P.DT*B f_xu=np.array([v*ct, v*st, a, v*td/P.L]).reshape(P.N,1) - C_lin = P.dt*(f_xu - np.dot(A, x_bar.reshape(P.N,1)) - np.dot(B, u_bar.reshape(P.M,1))).flatten() + C_lin = P.DT*(f_xu - np.dot(A, x_bar.reshape(P.N,1)) - np.dot(B, u_bar.reshape(P.M,1))).flatten() #return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6) return A_lin, B_lin, C_lin @@ -98,8 +98,8 @@ class MPC(): # Actuation rate of change if t < (time_horizon - 1): _cost += opt.quad_form(u[:,t + 1] - u[:,t], R * 1) - _constraints += [opt.abs(u[0, t + 1] - u[0, t])/P.dt <= P.MAX_D_ACC] - _constraints += [opt.abs(u[1, t + 1] - u[1, t])/P.dt <= P.MAX_D_STEER] + _constraints += [opt.abs(u[0, t + 1] - u[0, t])/P.DT <= P.MAX_D_ACC] + _constraints += [opt.abs(u[1, t + 1] - u[1, t])/P.DT <= P.MAX_D_STEER] if t == 0: diff --git a/mpc_pybullet_demo/mpcpy/mpc_config.py b/mpc_pybullet_demo/mpcpy/mpc_config.py index 8ee5c3d..2b2c2c0 100644 --- a/mpc_pybullet_demo/mpcpy/mpc_config.py +++ b/mpc_pybullet_demo/mpcpy/mpc_config.py @@ -1,15 +1,16 @@ import numpy as np -class Params(): + +class Params: def __init__(self): - self.N = 4 #number of state variables - self.M = 2 #number of control variables - self.T = 10 #Prediction Horizon - self.dt = 0.2 #discretization step + self.N = 4 # number of state variables + self.M = 2 # number of control variables + self.T = 10 # Prediction Horizon + self.DT = 0.2 # discretization step self.path_tick = 0.05 - self.L = 0.3 #vehicle wheelbase - self.MAX_SPEED = 1.5 #m/s - self.MAX_ACC = 1.0 #m/ss - self.MAX_D_ACC = 1.0 #m/sss - self.MAX_STEER = np.radians(30) #rad - self.MAX_D_STEER = np.radians(30) #rad/s + self.L = 0.3 # vehicle wheelbase + self.MAX_SPEED = 1.5 # m/s + self.MAX_ACC = 1.0 # m/ss + self.MAX_D_ACC = 1.0 # m/sss + self.MAX_STEER = np.radians(30) # rad + self.MAX_D_STEER = np.radians(30) # rad/s diff --git a/mpc_pybullet_demo/mpcpy/utils.py b/mpc_pybullet_demo/mpcpy/utils.py index 01a9153..5d23f8f 100755 --- a/mpc_pybullet_demo/mpcpy/utils.py +++ b/mpc_pybullet_demo/mpcpy/utils.py @@ -1,124 +1,106 @@ import numpy as np from scipy.interpolate import interp1d - from .mpc_config import Params -P=Params() -def compute_path_from_wp(start_xp, start_yp, step = 0.1): +P = Params() + + +def compute_path_from_wp(start_xp, start_yp, step=0.1): """ Computes a reference path given a set of waypoints """ - - final_xp=[] - final_yp=[] - delta = step #[m] - - for idx in range(len(start_xp)-1): - section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2))) - - interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int)) - - fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1) - fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1) - + final_xp = [] + final_yp = [] + delta = step # [m] + for idx in range(len(start_xp) - 1): + section_len = np.sum( + np.sqrt( + np.power(np.diff(start_xp[idx : idx + 2]), 2) + + np.power(np.diff(start_yp[idx : idx + 2]), 2) + ) + ) + interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int)) + fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1) + fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1) # watch out to duplicate points! - final_xp=np.append(final_xp,fx(interp_range)[1:]) - final_yp=np.append(final_yp,fy(interp_range)[1:]) - + final_xp = np.append(final_xp, fx(interp_range)[1:]) + final_yp = np.append(final_yp, fy(interp_range)[1:]) dx = np.append(0, np.diff(final_xp)) dy = np.append(0, np.diff(final_yp)) theta = np.arctan2(dy, dx) - - return np.vstack((final_xp,final_yp,theta)) + return np.vstack((final_xp, final_yp, theta)) -def get_nn_idx(state,path): +def get_nn_idx(state, path): """ Computes the index of the waypoint closest to vehicle """ - - dx = state[0]-path[0,:] - dy = state[1]-path[1,:] - dist = np.hypot(dx,dy) + dx = state[0] - path[0, :] + dy = state[1] - path[1, :] + dist = np.hypot(dx, dy) nn_idx = np.argmin(dist) - try: - v = [path[0,nn_idx+1] - path[0,nn_idx], - path[1,nn_idx+1] - path[1,nn_idx]] + v = [ + path[0, nn_idx + 1] - path[0, nn_idx], + path[1, nn_idx + 1] - path[1, nn_idx], + ] v /= np.linalg.norm(v) - - d = [path[0,nn_idx] - state[0], - path[1,nn_idx] - state[1]] - - if np.dot(d,v) > 0: + d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]] + if np.dot(d, v) > 0: target_idx = nn_idx else: - target_idx = nn_idx+1 - + target_idx = nn_idx + 1 except IndexError as e: target_idx = nn_idx - return target_idx + def normalize_angle(angle): """ Normalize an angle to [-pi, pi] """ while angle > np.pi: angle -= 2.0 * np.pi - while angle < -np.pi: angle += 2.0 * np.pi - return angle -def get_ref_trajectory(state, path, target_v): + +def get_ref_trajectory(state, path, target_v, dl=0.1): """ For each step in the time horizon modified reference in robot frame """ - xref = np.zeros((P.N, P.T+1)) - dref = np.zeros((1, P.T+1)) - - #sp = np.ones((1,T +1))*target_v #speed profile - + xref = np.zeros((P.N, P.T + 1)) + dref = np.zeros((1, P.T + 1)) + # sp = np.ones((1,T +1))*target_v #speed profile ncourse = path.shape[1] - ind = get_nn_idx(state, path) - dx=path[0,ind] - state[0] - dy=path[1,ind] - state[1] - - - xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X - xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y - xref[2, 0] = target_v #V - xref[3, 0] = normalize_angle(path[2,ind]- state[3]) #Theta - dref[0, 0] = 0.0 # steer operational point should be 0 - - dl = 0.05 # Waypoints spacing [m] + dx = path[0, ind] - state[0] + dy = path[1, ind] - state[1] + xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X + xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y + xref[2, 0] = target_v # V + xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta + dref[0, 0] = 0.0 # Steer operational point should be 0 travel = 0.0 - - for i in range(1, P.T+1): - travel += abs(target_v) * P.dt #current V or target V? + for i in range(1, P.T + 1): + travel += abs(target_v) * P.DT dind = int(round(travel / dl)) - if (ind + dind) < ncourse: - dx=path[0,ind + dind] - state[0] - dy=path[1,ind + dind] - state[1] - + dx = path[0, ind + dind] - state[0] + dy = path[1, ind + dind] - state[1] xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) - xref[2, i] = target_v #sp[ind + dind] - xref[3, i] = normalize_angle(path[2,ind + dind] - state[3]) + xref[2, i] = target_v # sp[ind + dind] + xref[3, i] = normalize_angle(path[2, ind + dind] - state[3]) dref[0, i] = 0.0 else: - dx=path[0,ncourse - 1] - state[0] - dy=path[1,ncourse - 1] - state[1] - + dx = path[0, ncourse - 1] - state[0] + dy = path[1, ncourse - 1] - state[1] xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) - xref[2, i] = 0.0 #stop? #sp[ncourse - 1] - xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3]) + xref[2, i] = 0.0 # stop? #sp[ncourse - 1] + xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3]) dref[0, i] = 0.0 - return xref, dref