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

Merge pull request #9 from mcarfagno/docstrings 

Improve Docstrings
master
mcarfagno 2023-10-25 17:05:26 +01:00 committed by GitHub
parent 6239e769b1 55f560ca7e
commit 8ed16839a3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 129 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py
View File

@ -10,15 +10,16 @@ class MPC:
self, vehicle, T, DT, state_cost, final_state_cost, input_cost, input_rate_cost
):
"""
:param vehicle:
:param T:
:param DT:
:param state_cost:
:param final_state_cost:
:param input_cost:
:param input_rate_cost:
"""
Args:
vehicle ():
T ():
DT ():
state_cost ():
final_state_cost ():
input_cost ():
input_rate_cost ():
"""
self.nx = 4 # number of state vars
self.nu = 2 # umber of input/control vars
@ -44,9 +45,13 @@ class MPC:
def get_linear_model_matrices(self, x_bar, u_bar):
"""
Computes the LTI approximated state space model x' = Ax + Bu + C
:param x_bar:
:param u_bar:
:return:
Args:
x_bar ():
u_bar ():
Returns:
"""
x = x_bar[0]
@ -96,13 +101,16 @@ class MPC:
verbose=False,
):
"""
Optimisation problem defined for the linearised model,
:param initial_state:
:param target:
:param verbose:
:return:
"""
Args:
initial_state ():
target ():
prev_cmd ():
verbose ():
Returns:
"""
assert len(initial_state) == self.nx
# Create variables needed for setting up cvxpy problem
@ -117,29 +125,25 @@ class MPC:
# Ak, Bk, Ck = self.get_linear_model_matrices(x_prev[:,k], u_prev[:,k])
A, B, C = self.get_linear_model_matrices(initial_state, prev_cmd)
# Tracking error cost
for k in range(self.control_horizon):
cost += opt.quad_form(x[:, k + 1] - target[:, k], self.Q)
# Final point tracking cost
cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf)
# Actuation magnitude cost
for k in range(self.control_horizon):
cost += opt.quad_form(target[:, k] - x[:, k + 1], self.Q)
cost += opt.quad_form(u[:, k], self.R)
# Actuation rate of change
if k < (self.control_horizon - 1):
cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
# Actuation rate of change cost
for k in range(1, self.control_horizon):
cost += opt.quad_form(u[:, k] - u[:, k - 1], self.P)
# Kinematics Constrains
# Kinematics Constrains
for k in range(self.control_horizon):
constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
# Actuation rate of change bounds
if k < (self.control_horizon - 1):
constr += [
opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.vehicle.max_d_acc
]
constr += [
opt.abs(u[1, k + 1] - u[1, k]) / self.dt <= self.vehicle.max_d_steer
]
# Final Point tracking
cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf)
# initial state
constr += [x[:, 0] == initial_state]
@ -147,6 +151,17 @@ class MPC:
constr += [opt.abs(u[:, 0]) <= self.vehicle.max_acc]
constr += [opt.abs(u[:, 1]) <= self.vehicle.max_steer]
# Actuation rate of change bounds
constr += [opt.abs(u[0, 0] - prev_cmd[0]) / self.dt <= self.vehicle.max_d_acc]
constr += [opt.abs(u[1, 0] - prev_cmd[1]) / self.dt <= self.vehicle.max_d_steer]
for k in range(1, self.control_horizon):
constr += [
opt.abs(u[0, k] - u[0, k - 1]) / self.dt <= self.vehicle.max_d_acc
]
constr += [
opt.abs(u[1, k] - u[1, k - 1]) / self.dt <= self.vehicle.max_d_steer
]
prob = opt.Problem(opt.Minimize(cost), constr)
solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
return x, u

49
mpc_pybullet_demo/cvxpy_mpc/utils.py
View File

@ -4,11 +4,14 @@ from scipy.interpolate import interp1d
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:
Args:
start_xp (array-like): 1D array of x-positions
start_yp (array-like): 1D array of y-positions
step (float): intepolation step
Returns:
ndarray of shape (3,N) representing the path as x,y,heading
"""
final_xp = []
final_yp = []
@ -34,10 +37,14 @@ 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:
Finds the index of the closest element
Args:
state (array-like): 1D array whose first two elements are x-pos and y-pos
path (ndarray): 2D array of shape (2,N) of x,y points
Returns:
int: the index of the closest element
"""
dx = state[0] - path[0, :]
dy = state[1] - path[1, :]
@ -61,11 +68,16 @@ def get_nn_idx(state, path):
def get_ref_trajectory(state, path, target_v, T, DT):
"""
Reinterpolate the trajectory to get a set N desired target states
:param state:
:param path:
:param target_v:
:return:
Args:
state (array-like): state of the vehicle in world frame
path (ndarray): 2D array representing the path as x,y,heading points in world frame
target_v (float): reference speed
T (float): trajectory duration
DT (float): trajectory time-step
Returns:
ndarray: 2D array representing state space reference trajectory expressed w.r.t ego state
"""
K = int(T / DT)
@ -98,7 +110,14 @@ def get_ref_trajectory(state, path, target_v, T, DT):
def fix_angle_reference(angle_ref, angle_init):
"""
This function returns a "smoothened" angle_ref wrt angle_init so there are no jumps.
Removes jumps greater than 2PI
Args:
angle_ref (array-like):
angle_init (float):
Returns:
array-like:
"""
diff_angle = angle_ref - angle_init
diff_angle = np.unwrap(diff_angle)

22
mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py
View File

@ -3,13 +3,21 @@ import numpy as np
class VehicleModel:
"""
Helper class to hold vehicle info
Helper class that contains the parameters of the vehicle to be controlled
Attributes:
wheelbase: [m]
max_speed: [m/s]
max_acc: [m/ss]
max_d_acc: [m/sss]
max_steer: [rad]
max_d_steer: [rad/s]
"""
def __init__(self):
self.wheelbase = 0.3 # vehicle wheelbase [m]
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.wheelbase = 0.3
self.max_speed = 1.5
self.max_acc = 1.0
self.max_d_acc = 1.0
self.max_steer = np.radians(30)
self.max_d_steer = np.radians(30)

42
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -31,12 +31,9 @@ class MPCSim:
self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
# helper variable to keep track of mpc output
# starting condition is 0,0
self.action = np.zeros(2)
# starting guess
self.action[0] = 1.0 # a
self.action[1] = 0.0 # delta
self.K = int(T / DT)
Q = [20, 20, 10, 20] # state error cost
@ -70,9 +67,9 @@ class MPCSim:
def preview(self, mpc_out):
"""
[TODO:summary]
[TODO:description]
Args:
mpc_out ():
"""
predicted = np.zeros((2, self.K))
predicted[:, :] = mpc_out[0:2, 1:]
@ -85,7 +82,7 @@ class MPCSim:
predicted = (predicted.T.dot(Rotm)).T
predicted[0, :] += self.state[0]
predicted[1, :] += self.state[1]
self.predicted = predicted
return predicted
def run(self):
"""
@ -121,16 +118,16 @@ class MPCSim:
verbose=False,
)
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
# only the first one is used to advance the simulation
self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
self.predict([self.action[0], self.action[1]], DT)
self.state = self.predict(self.state, [self.action[0], self.action[1]], DT)
# use the state trajectory to preview the optimizer output
self.preview(x_mpc.value)
self.predicted = self.preview(x_mpc.value)
self.plot_sim()
def predict(self, u, dt):
def predict(self, state, u, dt):
def kinematics_model(x, t, u):
dxdt = x[2] * np.cos(x[3])
dydt = x[2] * np.sin(x[3])
@ -141,14 +138,10 @@ class MPCSim:
# solve ODE
tspan = [0, dt]
self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
new_state = odeint(kinematics_model, state, tspan, args=(u[:],))[1]
return new_state
def plot_sim(self):
"""
[TODO:summary]
[TODO:description]
"""
self.sim_time = self.sim_time + DT
self.x_history.append(self.state[0])
self.y_history.append(self.state[1])
@ -243,18 +236,11 @@ 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]
Args:
x ():
y ():
yaw ():
"""
LENGTH = 0.5 # [m]
WIDTH = 0.25 # [m]

23
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

@ -18,7 +18,14 @@ DT = 0.2 # discretization step [s]
def get_state(robotId):
""" """
"""
Args:
robotId ():
Returns:
"""
robPos, robOrn = p.getBasePositionAndOrientation(robotId)
linVel, angVel = p.getBaseVelocity(robotId)
@ -33,6 +40,14 @@ def get_state(robotId):
def set_ctrl(robotId, currVel, acceleration, steeringAngle):
"""
Args:
robotId ():
currVel ():
acceleration ():
steeringAngle ():
"""
gearRatio = 1.0 / 21
steering = [0, 2]
wheels = [8, 15]
@ -56,7 +71,6 @@ def set_ctrl(robotId, currVel, acceleration, steeringAngle):
def plot_results(path, x_history, y_history):
""" """
plt.style.use("ggplot")
plt.figure()
plt.title("MPC Tracking Results")
@ -78,7 +92,6 @@ def plot_results(path, x_history, y_history):
def run_sim():
""" """
p.connect(p.GUI)
p.resetDebugVisualizerCamera(
cameraDistance=1.0,
@ -213,10 +226,8 @@ def run_sim():
for x_, y_ in zip(path[0, :], path[1, :]):
p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1])
# starting guess
# starting conditions
action = np.zeros(2)
action[0] = 1.0 # a
action[1] = 0.0 # delta
# Cost Matrices
Q = [20, 20, 10, 20] # state error cost [x,y,v,yaw]