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

Compare commits

base: 12345qiupeng:6f4e6647abd269b9d76e2b5b33d19dca1b73b912
Branches Tags
12345qiupeng:master
..
compare: 12345qiupeng:1e62c0cbe72de1f9d9478a4c825b76070d85693f
Branches Tags
12345qiupeng:master

No commits in common. "6f4e6647abd269b9d76e2b5b33d19dca1b73b912" and "1e62c0cbe72de1f9d9478a4c825b76070d85693f" have entirely different histories.
6f4e6647ab ... 1e62c0cbe7

15 changed files with 149 additions and 289 deletions
Show Stats Expand all files Collapse all files

8
.idea/.gitignore vendored
View File

@ -1,8 +0,0 @@
# Default ignored files
/shelf/
/workspace.xml
# Editor-based HTTP Client requests
/httpRequests/
# Datasource local storage ignored files
/dataSources/
/dataSources.local.xml

15
.idea/inspectionProfiles/Project_Default.xml
View File

@ -1,15 +0,0 @@
<component name="InspectionProjectProfileManager">
<profile version="1.0">
<option name="myName" value="Project Default" />
<inspection_tool class="PyCompatibilityInspection" enabled="true" level="WARNING" enabled_by_default="true">
<option name="ourVersions">
<value>
<list size="2">
<item index="0" class="java.lang.String" itemvalue="3.9" />
<item index="1" class="java.lang.String" itemvalue="3.11" />
</list>
</value>
</option>
</inspection_tool>
</profile>
</component>

6
.idea/inspectionProfiles/profiles_settings.xml
View File

@ -1,6 +0,0 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

10
.idea/misc.xml
View File

@ -1,10 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="Black">
<option name="sdkName" value="Python 3.12 (mpc_python12)" />
</component>
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.12 (mpc_python12)" project-jdk-type="Python SDK" />
<component name="PythonCompatibilityInspectionAdvertiser">
<option name="version" value="3" />
</component>
</project>

8
.idea/modules.xml
View File

@ -1,8 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/mpc_python.iml" filepath="$PROJECT_DIR$/.idea/mpc_python.iml" />
</modules>
</component>
</project>

14
.idea/mpc_python.iml
View File

@ -1,14 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$">
<excludeFolder url="file://$MODULE_DIR$/venv" />
</content>
<orderEntry type="jdk" jdkName="Python 3.12 (mpc_python12)" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="PyDocumentationSettings">
<option name="format" value="PLAIN" />
<option name="myDocStringFormat" value="Plain" />
</component>
</module>

6
.idea/vcs.xml
View File

@ -1,6 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>

4
README.md
View File

@ -9,12 +9,12 @@ This mainly uses **[CVXPY](https://www.cvxpy.org/)** as a framework. This repo c
* To run the pybullet demo:
```bash
python3 ./mpc_pybullet_demo/mpc_demo_pybullet.py
python3 mpc_demo_pybullet.py
```
* To run the simulation-less demo (simpler demo that does not use pybullet, useful for debugging):
```bash
python3 ./mpc_pybullet_demo/mpc_demo_nosim.py
python3 mpc_demo_pybullet.py
```
In both cases the script will promt the user for `enter` before starting the demo.

95
mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py
View File

@ -10,16 +10,15 @@ class MPC:
self, vehicle, T, DT, state_cost, final_state_cost, input_cost, input_rate_cost
):
"""
Args:
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:
"""
self.nx = 4 # number of state vars
self.nu = 2 # umber of input/control vars
@ -44,14 +43,10 @@ class MPC:
def get_linear_model_matrices(self, x_bar, u_bar):
"""
Computes the approximated LTI state space model x' = Ax + Bu + C
Args:
x_bar (array-like):
u_bar (array-like):
Returns:
Computes the LTI approximated state space model x' = Ax + Bu + C
:param x_bar:
:param u_bar:
:return:
"""
x = x_bar[0]
@ -101,51 +96,46 @@ class MPC:
verbose=False,
):
"""
Args:
initial_state (array-like): current estimate of [x, y, v, heading]
target (ndarray): state space reference, in the same frame as the provided current state
prev_cmd (array-like): previous [acceleration, steer]. note this is used in bounds and has to be realistic.
verbose (bool):
Returns:
Optimisation problem defined for the linearised model,
:param initial_state:
:param target:
:param verbose:
:return:
"""
assert len(initial_state) == self.nx
assert len(prev_cmd) == self.nu
assert target.shape == (self.nx, self.control_horizon)
# Create variables needed for setting up cvxpy problem
assert len(initial_state) == self.nx
# Create variables
x = opt.Variable((self.nx, self.control_horizon + 1), name="states")
u = opt.Variable((self.nu, self.control_horizon), name="actions")
cost = 0
constr = []
# NOTE: here the state linearization is performed around the starting condition to simplify the controller.
# This approximation gets more inaccurate as the controller looks at the future.
# To improve performance we can keep track of previous optimized x, u and compute these matrices for each timestep k
# 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 cost
for k in range(1, self.control_horizon):
cost += opt.quad_form(u[:, k] - u[:, k - 1], self.P)
# Actuation rate of change
if k < (self.control_horizon - 1):
cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
# Kinematics Constrains
for k in range(self.control_horizon):
# Kinematics Constrains
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]
@ -153,17 +143,6 @@ 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

52
mpc_pybullet_demo/cvxpy_mpc/utils.py
View File

@ -4,14 +4,11 @@ from scipy.interpolate import interp1d
def compute_path_from_wp(start_xp, start_yp, step=0.1):
"""
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
Computes a reference path given a set of waypoints
:param start_xp:
:param start_yp:
:param step:
:return:
"""
final_xp = []
final_yp = []
@ -37,14 +34,10 @@ def compute_path_from_wp(start_xp, start_yp, step=0.1):
def get_nn_idx(state, path):
"""
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
Computes the index of the waypoint closest to vehicle
:param state:
:param path:
:return:
"""
dx = state[0] - path[0, :]
dy = state[1] - path[1, :]
@ -68,17 +61,11 @@ def get_nn_idx(state, path):
def get_ref_trajectory(state, path, target_v, T, DT):
"""
Args:
state (array-like): state of the vehicle in global frame
path (ndarray): 2D array representing the path as x,y,heading points in global frame
target_v (float): desired speed
T (float): control horizon duration
DT (float): control horizon time-step
Returns:
ndarray: 2D array representing state space trajectory [x_k, y_k, v_k, theta_k] w.r.t ego frame.
Interpolated according to the time-step and the desired velocity
Reinterpolate the trajectory to get a set N desired target states
:param state:
:param path:
:param target_v:
:return:
"""
K = int(T / DT)
@ -102,7 +89,7 @@ def get_ref_trajectory(state, path, target_v, T, DT):
stop_idx = np.where(xref_cdist == cdist[-1])
xref[2, stop_idx] = 0.0
# transform in ego frame
# 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
@ -111,14 +98,7 @@ def get_ref_trajectory(state, path, target_v, T, DT):
def fix_angle_reference(angle_ref, angle_init):
"""
Removes jumps greater than 2PI to smooth the heading
Args:
angle_ref (array-like):
angle_init (float):
Returns:
array-like:
This function returns a "smoothened" angle_ref wrt angle_init so there are no jumps.
"""
diff_angle = angle_ref - angle_init
diff_angle = np.unwrap(diff_angle)

22
mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py
View File

@ -3,21 +3,13 @@ import numpy as np
class VehicleModel:
"""
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]
Helper class to hold vehicle info
"""
def __init__(self):
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)
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]

99
mpc_pybullet_demo/mpc_demo_nosim.py
View File

@ -27,29 +27,33 @@ L = 0.3 # vehicle wheelbase [m]
# Classes
class MPCSim:
def __init__(self):
# State of the robot [x,y,v, heading]
# 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])
# helper variable to keep track of mpc output
# starting condition is 0,0
self.control = np.zeros(2)
# starting guess
self.action = np.zeros(2)
self.action[0] = 1.0 # a
self.action[1] = 0.0 # delta
self.K = int(T / DT)
self.opt_u = np.zeros((2, self.K))
# Weights for Cost Matrices
Q = [20, 20, 10, 20] # state error cost
Qf = [30, 30, 30, 30] # state final error cost
R = [10, 10] # input cost
P = [10, 10] # input rate of change cost
self.mpc = MPC(VehicleModel(), T, DT, Q, Qf, R, P)
# Path from waypoint interpolation
# 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],
0.05,
)
# Helper variables to keep track of the sim
# Sim help vars
self.sim_time = 0
self.x_history = []
self.y_history = []
@ -57,7 +61,7 @@ class MPCSim:
self.h_history = []
self.a_history = []
self.d_history = []
self.optimized_trajectory = None
self.predicted = None
# Initialise plot
plt.style.use("ggplot")
@ -65,26 +69,24 @@ class MPCSim:
plt.ion()
plt.show()
def ego_to_global(self, mpc_out):
def preview(self, mpc_out):
"""
transforms optimized trajectory XY points from ego(car) reference
into global(map) frame
[TODO:summary]
Args:
mpc_out ():
[TODO:description]
"""
trajectory = np.zeros((2, self.K))
trajectory[:, :] = mpc_out[0:2, 1:]
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])],
]
)
trajectory = (trajectory.T.dot(Rotm)).T
trajectory[0, :] += self.state[0]
trajectory[1, :] += self.state[1]
return trajectory
predicted = (predicted.T.dot(Rotm)).T
predicted[0, :] += self.state[0]
predicted[1, :] += self.state[1]
self.predicted = predicted
def run(self):
"""
@ -107,31 +109,31 @@ class MPCSim:
return
# optimization loop
# start=time.time()
# dynamycs w.r.t robot frame
curr_state = np.array([0, 0, self.state[2], 0])
# Get Reference_traj -> inputs are in worldframe
target = get_ref_trajectory(self.state, self.path, TARGET_VEL, T, DT)
# dynamycs w.r.t robot frame
curr_state = np.array([0, 0, self.state[2], 0])
x_mpc, u_mpc = self.mpc.step(
curr_state,
target,
self.control,
self.action,
verbose=False,
)
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
# only the first one is used to advance the simulation
self.control[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
self.state = self.predict_next_state(
self.state, [self.control[0], self.control[1]], DT
# NOTE: used only for preview purposes
self.opt_u = np.vstack(
(
np.array(u_mpc.value[0, :]).flatten(),
np.array(u_mpc.value[1, :]).flatten(),
)
)
# use the optimizer output to preview the predicted state trajectory
self.optimized_trajectory = self.ego_to_global(x_mpc.value)
self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
self.predict([self.action[0], self.action[1]], DT)
self.preview(x_mpc.value)
self.plot_sim()
def predict_next_state(self, state, u, dt):
def predict(self, u, dt):
def kinematics_model(x, t, u):
dxdt = x[2] * np.cos(x[3])
dydt = x[2] * np.sin(x[3])
@ -142,17 +144,21 @@ class MPCSim:
# solve ODE
tspan = [0, dt]
new_state = odeint(kinematics_model, state, tspan, args=(u[:],))[1]
return new_state
self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
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])
self.v_history.append(self.state[2])
self.h_history.append(self.state[3])
self.a_history.append(self.control[0])
self.d_history.append(self.control[1])
self.a_history.append(self.opt_u[0, 1])
self.d_history.append(self.opt_u[1, 1])
plt.clf()
@ -180,10 +186,10 @@ class MPCSim:
label="vehicle trajectory",
)
if self.optimized_trajectory is not None:
if self.predicted is not None:
plt.plot(
self.optimized_trajectory[0, :],
self.optimized_trajectory[1, :],
self.predicted[0, :],
self.predicted[1, :],
c="tab:green",
marker="+",
alpha=0.5,
@ -240,11 +246,18 @@ class MPCSim:
def plot_car(x, y, yaw):
"""
[TODO:summary]
Args:
x ():
y ():
yaw ():
[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]

37
mpc_pybullet_demo/mpc_demo_pybullet.py
View File

@ -18,14 +18,7 @@ DT = 0.2 # discretization step [s]
def get_state(robotId):
"""
Args:
robotId ():
Returns:
"""
""" """
robPos, robOrn = p.getBasePositionAndOrientation(robotId)
linVel, angVel = p.getBaseVelocity(robotId)
@ -40,14 +33,6 @@ 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]
@ -71,6 +56,7 @@ 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")
@ -92,6 +78,7 @@ def plot_results(path, x_history, y_history):
def run_sim():
""" """
p.connect(p.GUI)
p.resetDebugVisualizerCamera(
cameraDistance=1.0,
@ -226,8 +213,10 @@ 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 conditions
control = np.zeros(2)
# starting guess
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]
@ -271,21 +260,21 @@ def run_sim():
# simulate one timestep actuation delay
ego_state[0] = ego_state[0] + ego_state[2] * np.cos(ego_state[3]) * DT
ego_state[1] = ego_state[1] + ego_state[2] * np.sin(ego_state[3]) * DT
ego_state[2] = ego_state[2] + control[0] * DT
ego_state[3] = ego_state[3] + control[0] * np.tan(control[1]) / L * DT
ego_state[2] = ego_state[2] + action[0] * DT
ego_state[3] = ego_state[3] + action[0] * np.tan(action[1]) / L * DT
# optimization loop
start = time.time()
# MPC step
_, u_mpc = mpc.step(ego_state, target, control, verbose=False)
control[0] = u_mpc.value[0, 0]
control[1] = u_mpc.value[1, 0]
_, u_mpc = mpc.step(ego_state, target, action, verbose=False)
action[0] = u_mpc.value[0, 0]
action[1] = u_mpc.value[1, 0]
elapsed = time.time() - start
print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
set_ctrl(car, state[2], control[0], control[1])
set_ctrl(car, state[2], action[0], action[1])
if DT - elapsed > 0:
time.sleep(DT - elapsed)

54
notebooks/1.0-lti-system-modelling.ipynb
View File

@ -11,22 +11,9 @@
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-22T08:05:56.118290Z",
"start_time": "2024-10-22T08:05:46.550696Z"
}
},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"Matplotlib is building the font cache; this may take a moment.\n"
]
}
],
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"from scipy.integrate import odeint\n",
@ -190,13 +177,8 @@
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-22T08:06:14.547915Z",
"start_time": "2024-10-22T08:06:14.544585Z"
}
},
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
@ -238,23 +220,15 @@
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-22T08:06:17.956990Z",
"start_time": "2024-10-22T08:06:17.847071Z"
}
},
"execution_count": 5,
"metadata": {},
"outputs": [
{
"ename": "NameError",
"evalue": "name 'M' is not defined",
"output_type": "error",
"traceback": [
"\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
"\u001B[0;31mNameError\u001B[0m Traceback (most recent call last)",
"File \u001B[0;32m<timed exec>:1\u001B[0m\n",
"\u001B[0;31mNameError\u001B[0m: name 'M' is not defined"
"name": "stdout",
"output_type": "stream",
"text": [
"CPU times: user 3.39 ms, sys: 0 ns, total: 3.39 ms\n",
"Wall time: 2.79 ms\n"
]
}
],
@ -469,9 +443,9 @@
],
"metadata": {
"kernelspec": {
"name": "python3",
"display_name": "Python [conda env:.conda-jupyter] *",
"language": "python",
"display_name": "Python 3 (ipykernel)"
"name": "conda-env-.conda-jupyter-py"
},
"language_info": {
"codemirror_mode": {

8
requirements.txt
View File

@ -1,6 +1,6 @@
pybullet>=2.7.2
cvxpy>=1.0.28
pybullet==2.7.2
cvxpy==1.0.28
dccp
numpy>=1.22
osqp>=0.6.1
scipy>=1.10.0
osqp==0.6.1
scipy==1.10.0