12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #7 from mcarfagno/use-numpy-interpolation 

rework get_reference_trajectory
master
mcarfagno 2023-10-20 19:23:47 +01:00 committed by GitHub
parent 147670b5de 35a0698c73
commit ac2f901047
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 53 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -23,10 +23,10 @@ params = mpcpy.Params()
# Params
VEL = 1.0 # m/s
# Classes
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])
@ -114,7 +114,7 @@ class MPCSim:
# State Matrices
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)
target = mpcpy.get_ref_trajectory(self.state, self.path, VEL)
x_mpc, u_mpc = self.mpc.optimize_linearized_model(
A,

2
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

@ -247,7 +247,7 @@ def run_sim():
# Get Reference_traj
# NOTE: inputs are in world frame
target, _ = mpcpy.get_ref_trajectory(state, path, params.TARGET_SPEED)
target = mpcpy.get_ref_trajectory(state, path, params.TARGET_SPEED)
# for MPC base link frame is used:
# so x, y, yaw are 0.0, but speed is the same

10
mpc_pybullet_demo/mpcpy/cvxpy_mpc.py
View File

@ -16,6 +16,9 @@ P = Params()
def get_linear_model_matrices(x_bar, u_bar):
"""
Computes the LTI approximated state space model x' = Ax + Bu + C
:param x_bar:
:param u_bar:
:return:
"""
x = x_bar[0]
@ -58,7 +61,12 @@ def get_linear_model_matrices(x_bar, u_bar):
class MPC:
def __init__(self, state_cost, final_state_cost, input_cost, input_rate_cost):
""" """
"""
:param state_cost:
:param final_state_cost:
:param input_cost:
:param input_rate_cost:
"""
self.nx = P.N # number of state vars
self.nu = P.M # umber of input/control vars

75
mpc_pybullet_demo/mpcpy/utils.py
View File

@ -8,6 +8,10 @@ P = Params()
def compute_path_from_wp(start_xp, start_yp, step=0.1):
"""
Computes a reference path given a set of waypoints
:param start_xp:
:param start_yp:
:param step:
:return:
"""
final_xp = []
final_yp = []
@ -34,6 +38,9 @@ def compute_path_from_wp(start_xp, start_yp, step=0.1):
def get_nn_idx(state, path):
"""
Computes the index of the waypoint closest to vehicle
:param state:
:param path:
:return:
"""
dx = state[0] - path[0, :]
dy = state[1] - path[1, :]
@ -58,6 +65,8 @@ def get_nn_idx(state, path):
def normalize_angle(angle):
"""
Normalize an angle to [-pi, pi]
:param angle:
:return:
"""
while angle > np.pi:
angle -= 2.0 * np.pi
@ -68,40 +77,38 @@ def normalize_angle(angle):
def get_ref_trajectory(state, path, target_v):
"""
For each step in the time horizon
modified reference in robot frame
Reinterpolate the trajectory to get a set N desired target states
:param state:
:param path:
:param target_v:
:return:
"""
K = int(P.T / P.DT)
xref = np.zeros((P.N, K + 1))
dref = np.zeros((1, K + 1))
ncourse = path.shape[1]
xref = np.zeros((P.N, K))
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
travel = 0.0
dl = np.hypot(path[0, 1] - path[0, 0], path[1, 1] - path[1, 0])
for i in range(1, K + 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]
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])
dref[0, i] = 0.0
else:
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])
dref[0, i] = 0.0
return xref, dref
cdist = np.append(
[0.0], np.cumsum(np.hypot(np.diff(path[0, :].T), np.diff(path[1, :]).T))
)
cdist = np.clip(cdist, cdist[0], cdist[-1])
start_dist = cdist[ind]
interp_points = [d * P.DT * target_v + start_dist for d in range(1, K + 1)]
xref[0, :] = np.interp(interp_points, cdist, path[0, :])
xref[1, :] = np.interp(interp_points, cdist, path[1, :])
xref[2, :] = target_v
xref[3, :] = np.interp(interp_points, cdist, path[2, :])
# points where the vehicle is at the end of trajectory
xref_cdist = np.interp(interp_points, cdist, cdist)
stop_idx = np.where(xref_cdist == cdist[-1])
xref[2, stop_idx] = 0.0
# transform in car ego frame
dx = xref[0, :] - state[0]
dy = xref[1, :] - state[1]
xref[0, :] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X
xref[1, :] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y
xref[3, :] = normalize_angle(path[2, ind] - state[3]) # Theta
return xref