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: * To run the pybullet demo:
```bash ```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): * To run the simulation-less demo (simpler demo that does not use pybullet, useful for debugging):
```bash ```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. In both cases the script will promt the user for `enter` before starting the demo.

93
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 self, vehicle, T, DT, state_cost, final_state_cost, input_cost, input_rate_cost
): ):
""" """
:param vehicle:
Args: :param T:
vehicle (): :param DT:
T (): :param state_cost:
DT (): :param final_state_cost:
state_cost (): :param input_cost:
final_state_cost (): :param input_rate_cost:
input_cost ():
input_rate_cost ():
""" """
self.nx = 4 # number of state vars self.nx = 4 # number of state vars
self.nu = 2 # umber of input/control 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): def get_linear_model_matrices(self, x_bar, u_bar):
""" """
Computes the approximated LTI state space model x' = Ax + Bu + C Computes the LTI approximated state space model x' = Ax + Bu + C
:param x_bar:
Args: :param u_bar:
x_bar (array-like): :return:
u_bar (array-like):
Returns:
""" """
x = x_bar[0] x = x_bar[0]
@ -101,51 +96,46 @@ class MPC:
verbose=False, verbose=False,
): ):
""" """
Optimisation problem defined for the linearised model,
Args: :param initial_state:
initial_state (array-like): current estimate of [x, y, v, heading] :param target:
target (ndarray): state space reference, in the same frame as the provided current state :param verbose:
prev_cmd (array-like): previous [acceleration, steer]. note this is used in bounds and has to be realistic. :return:
verbose (bool):
Returns:
""" """
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") x = opt.Variable((self.nx, self.control_horizon + 1), name="states")
u = opt.Variable((self.nu, self.control_horizon), name="actions") u = opt.Variable((self.nu, self.control_horizon), name="actions")
cost = 0 cost = 0
constr = [] 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) 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): 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) cost += opt.quad_form(u[:, k], self.R)
# Actuation rate of change cost # Actuation rate of change
for k in range(1, self.control_horizon): if k < (self.control_horizon - 1):
cost += opt.quad_form(u[:, k] - u[:, k - 1], self.P) cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
# Kinematics Constrains # Kinematics Constrains
for k in range(self.control_horizon):
constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C] 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 # initial state
constr += [x[:, 0] == 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[:, 0]) <= self.vehicle.max_acc]
constr += [opt.abs(u[:, 1]) <= self.vehicle.max_steer] 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) prob = opt.Problem(opt.Minimize(cost), constr)
solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False) solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
return x, u 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): def compute_path_from_wp(start_xp, start_yp, step=0.1):
""" """
Computes a reference path given a set of waypoints
Args: :param start_xp:
start_xp (array-like): 1D array of x-positions :param start_yp:
start_yp (array-like): 1D array of y-positions :param step:
step (float): intepolation step :return:
Returns:
ndarray of shape (3,N) representing the path as x,y,heading
""" """
final_xp = [] final_xp = []
final_yp = [] final_yp = []
@ -37,14 +34,10 @@ def compute_path_from_wp(start_xp, start_yp, step=0.1):
def get_nn_idx(state, path): def get_nn_idx(state, path):
""" """
Finds the index of the closest element Computes the index of the waypoint closest to vehicle
:param state:
Args: :param path:
state (array-like): 1D array whose first two elements are x-pos and y-pos :return:
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, :] dx = state[0] - path[0, :]
dy = state[1] - path[1, :] 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): def get_ref_trajectory(state, path, target_v, T, DT):
""" """
Reinterpolate the trajectory to get a set N desired target states
Args: :param state:
state (array-like): state of the vehicle in global frame :param path:
path (ndarray): 2D array representing the path as x,y,heading points in global frame :param target_v:
target_v (float): desired speed :return:
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
""" """
K = int(T / DT) 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]) stop_idx = np.where(xref_cdist == cdist[-1])
xref[2, stop_idx] = 0.0 xref[2, stop_idx] = 0.0
# transform in ego frame # transform in car ego frame
dx = xref[0, :] - state[0] dx = xref[0, :] - state[0]
dy = xref[1, :] - state[1] dy = xref[1, :] - state[1]
xref[0, :] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X 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): def fix_angle_reference(angle_ref, angle_init):
""" """
Removes jumps greater than 2PI to smooth the heading This function returns a "smoothened" angle_ref wrt angle_init so there are no jumps.
Args:
angle_ref (array-like):
angle_init (float):
Returns:
array-like:
""" """
diff_angle = angle_ref - angle_init diff_angle = angle_ref - angle_init
diff_angle = np.unwrap(diff_angle) 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: class VehicleModel:
""" """
Helper class that contains the parameters of the vehicle to be controlled Helper class to hold vehicle info
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): def __init__(self):
self.wheelbase = 0.3 self.wheelbase = 0.3 # vehicle wheelbase [m]
self.max_speed = 1.5 self.max_speed = 1.5 # [m/s]
self.max_acc = 1.0 self.max_acc = 1.0 # [m/ss]
self.max_d_acc = 1.0 self.max_d_acc = 1.0 # [m/sss]
self.max_steer = np.radians(30) self.max_steer = np.radians(30) # [rad]
self.max_d_steer = np.radians(30) 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 # Classes
class MPCSim: class MPCSim:
def __init__(self): 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]) 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 guess
# starting condition is 0,0 self.action = np.zeros(2)
self.control = np.zeros(2) self.action[0] = 1.0 # a
self.action[1] = 0.0 # delta
self.K = int(T / DT) 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 Q = [20, 20, 10, 20] # state error cost
Qf = [30, 30, 30, 30] # state final error cost Qf = [30, 30, 30, 30] # state final error cost
R = [10, 10] # input cost R = [10, 10] # input cost
P = [10, 10] # input rate of change cost P = [10, 10] # input rate of change cost
self.mpc = MPC(VehicleModel(), T, DT, Q, Qf, R, P) 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( self.path = compute_path_from_wp(
[0, 3, 4, 6, 10, 12, 13, 13, 6, 1, 0], [0, 3, 4, 6, 10, 12, 13, 13, 6, 1, 0],
[0, 0, 2, 4, 3, 3, -1, -2, -6, -2, -2], [0, 0, 2, 4, 3, 3, -1, -2, -6, -2, -2],
0.05, 0.05,
) )
# Helper variables to keep track of the sim # Sim help vars
self.sim_time = 0 self.sim_time = 0
self.x_history = [] self.x_history = []
self.y_history = [] self.y_history = []
@ -57,7 +61,7 @@ class MPCSim:
self.h_history = [] self.h_history = []
self.a_history = [] self.a_history = []
self.d_history = [] self.d_history = []
self.optimized_trajectory = None self.predicted = None
# Initialise plot # Initialise plot
plt.style.use("ggplot") plt.style.use("ggplot")
@ -65,26 +69,24 @@ class MPCSim:
plt.ion() plt.ion()
plt.show() plt.show()
def ego_to_global(self, mpc_out): def preview(self, mpc_out):
""" """
transforms optimized trajectory XY points from ego(car) reference [TODO:summary]
into global(map) frame
Args: [TODO:description]
mpc_out ():
""" """
trajectory = np.zeros((2, self.K)) predicted = np.zeros(self.opt_u.shape)
trajectory[:, :] = mpc_out[0:2, 1:] predicted[:, :] = mpc_out[0:2, 1:]
Rotm = np.array( Rotm = np.array(
[ [
[np.cos(self.state[3]), np.sin(self.state[3])], [np.cos(self.state[3]), np.sin(self.state[3])],
[-np.sin(self.state[3]), np.cos(self.state[3])], [-np.sin(self.state[3]), np.cos(self.state[3])],
] ]
) )
trajectory = (trajectory.T.dot(Rotm)).T predicted = (predicted.T.dot(Rotm)).T
trajectory[0, :] += self.state[0] predicted[0, :] += self.state[0]
trajectory[1, :] += self.state[1] predicted[1, :] += self.state[1]
return trajectory self.predicted = predicted
def run(self): def run(self):
""" """
@ -107,31 +109,31 @@ class MPCSim:
return return
# optimization loop # optimization loop
# start=time.time() # 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 # Get Reference_traj -> inputs are in worldframe
target = get_ref_trajectory(self.state, self.path, TARGET_VEL, T, DT) 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( x_mpc, u_mpc = self.mpc.step(
curr_state, curr_state,
target, target,
self.control, self.action,
verbose=False, verbose=False,
) )
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) # NOTE: used only for preview purposes
# only the first one is used to advance the simulation self.opt_u = np.vstack(
(
self.control[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]] np.array(u_mpc.value[0, :]).flatten(),
self.state = self.predict_next_state( np.array(u_mpc.value[1, :]).flatten(),
self.state, [self.control[0], self.control[1]], DT
) )
)
# use the optimizer output to preview the predicted state trajectory self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
self.optimized_trajectory = self.ego_to_global(x_mpc.value) # 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() self.plot_sim()
def predict_next_state(self, state, u, dt): def predict(self, u, dt):
def kinematics_model(x, t, u): def kinematics_model(x, t, u):
dxdt = x[2] * np.cos(x[3]) dxdt = x[2] * np.cos(x[3])
dydt = x[2] * np.sin(x[3]) dydt = x[2] * np.sin(x[3])
@ -142,17 +144,21 @@ class MPCSim:
# solve ODE # solve ODE
tspan = [0, dt] tspan = [0, dt]
new_state = odeint(kinematics_model, state, tspan, args=(u[:],))[1] self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
return new_state
def plot_sim(self): def plot_sim(self):
"""
[TODO:summary]
[TODO:description]
"""
self.sim_time = self.sim_time + DT self.sim_time = self.sim_time + DT
self.x_history.append(self.state[0]) self.x_history.append(self.state[0])
self.y_history.append(self.state[1]) self.y_history.append(self.state[1])
self.v_history.append(self.state[2]) self.v_history.append(self.state[2])
self.h_history.append(self.state[3]) self.h_history.append(self.state[3])
self.a_history.append(self.control[0]) self.a_history.append(self.opt_u[0, 1])
self.d_history.append(self.control[1]) self.d_history.append(self.opt_u[1, 1])
plt.clf() plt.clf()
@ -180,10 +186,10 @@ class MPCSim:
label="vehicle trajectory", label="vehicle trajectory",
) )
if self.optimized_trajectory is not None: if self.predicted is not None:
plt.plot( plt.plot(
self.optimized_trajectory[0, :], self.predicted[0, :],
self.optimized_trajectory[1, :], self.predicted[1, :],
c="tab:green", c="tab:green",
marker="+", marker="+",
alpha=0.5, alpha=0.5,
@ -240,11 +246,18 @@ class MPCSim:
def plot_car(x, y, yaw): def plot_car(x, y, yaw):
""" """
[TODO:summary]
Args: [TODO:description]
x ():
y (): Parameters
yaw (): ----------
x : [TODO:type]
[TODO:description]
y : [TODO:type]
[TODO:description]
yaw : [TODO:type]
[TODO:description]
""" """
LENGTH = 0.5 # [m] LENGTH = 0.5 # [m]
WIDTH = 0.25 # [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): def get_state(robotId):
""" """ """
Args:
robotId ():
Returns:
"""
robPos, robOrn = p.getBasePositionAndOrientation(robotId) robPos, robOrn = p.getBasePositionAndOrientation(robotId)
linVel, angVel = p.getBaseVelocity(robotId) linVel, angVel = p.getBaseVelocity(robotId)
@ -40,14 +33,6 @@ def get_state(robotId):
def set_ctrl(robotId, currVel, acceleration, steeringAngle): def set_ctrl(robotId, currVel, acceleration, steeringAngle):
"""
Args:
robotId ():
currVel ():
acceleration ():
steeringAngle ():
"""
gearRatio = 1.0 / 21 gearRatio = 1.0 / 21
steering = [0, 2] steering = [0, 2]
wheels = [8, 15] wheels = [8, 15]
@ -71,6 +56,7 @@ def set_ctrl(robotId, currVel, acceleration, steeringAngle):
def plot_results(path, x_history, y_history): def plot_results(path, x_history, y_history):
""" """
plt.style.use("ggplot") plt.style.use("ggplot")
plt.figure() plt.figure()
plt.title("MPC Tracking Results") plt.title("MPC Tracking Results")
@ -92,6 +78,7 @@ def plot_results(path, x_history, y_history):
def run_sim(): def run_sim():
""" """
p.connect(p.GUI) p.connect(p.GUI)
p.resetDebugVisualizerCamera( p.resetDebugVisualizerCamera(
cameraDistance=1.0, cameraDistance=1.0,
@ -226,8 +213,10 @@ def run_sim():
for x_, y_ in zip(path[0, :], path[1, :]): for x_, y_ in zip(path[0, :], path[1, :]):
p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1]) p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1])
# starting conditions # starting guess
control = np.zeros(2) action = np.zeros(2)
action[0] = 1.0 # a
action[1] = 0.0 # delta
# Cost Matrices # Cost Matrices
Q = [20, 20, 10, 20] # state error cost [x,y,v,yaw] Q = [20, 20, 10, 20] # state error cost [x,y,v,yaw]
@ -271,21 +260,21 @@ def run_sim():
# simulate one timestep actuation delay # simulate one timestep actuation delay
ego_state[0] = ego_state[0] + ego_state[2] * np.cos(ego_state[3]) * DT 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[1] = ego_state[1] + ego_state[2] * np.sin(ego_state[3]) * DT
ego_state[2] = ego_state[2] + control[0] * DT ego_state[2] = ego_state[2] + action[0] * DT
ego_state[3] = ego_state[3] + control[0] * np.tan(control[1]) / L * DT ego_state[3] = ego_state[3] + action[0] * np.tan(action[1]) / L * DT
# optimization loop # optimization loop
start = time.time() start = time.time()
# MPC step # MPC step
_, u_mpc = mpc.step(ego_state, target, control, verbose=False) _, u_mpc = mpc.step(ego_state, target, action, verbose=False)
control[0] = u_mpc.value[0, 0] action[0] = u_mpc.value[0, 0]
control[1] = u_mpc.value[1, 0] action[1] = u_mpc.value[1, 0]
elapsed = time.time() - start elapsed = time.time() - start
print("CVXPY Optimization Time: {:.4f}s".format(elapsed)) 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: if DT - elapsed > 0:
time.sleep(DT - elapsed) time.sleep(DT - elapsed)

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

@ -11,22 +11,9 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": 1,
"metadata": { "metadata": {},
"ExecuteTime": { "outputs": [],
"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"
]
}
],
"source": [ "source": [
"import numpy as np\n", "import numpy as np\n",
"from scipy.integrate import odeint\n", "from scipy.integrate import odeint\n",
@ -190,13 +177,8 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": 4,
"metadata": { "metadata": {},
"ExecuteTime": {
"end_time": "2024-10-22T08:06:14.547915Z",
"start_time": "2024-10-22T08:06:14.544585Z"
}
},
"outputs": [], "outputs": [],
"source": [ "source": [
"# Define process model\n", "# Define process model\n",
@ -238,23 +220,15 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 4, "execution_count": 5,
"metadata": { "metadata": {},
"ExecuteTime": {
"end_time": "2024-10-22T08:06:17.956990Z",
"start_time": "2024-10-22T08:06:17.847071Z"
}
},
"outputs": [ "outputs": [
{ {
"ename": "NameError", "name": "stdout",
"evalue": "name 'M' is not defined", "output_type": "stream",
"output_type": "error", "text": [
"traceback": [ "CPU times: user 3.39 ms, sys: 0 ns, total: 3.39 ms\n",
"\u001B[0;31m---------------------------------------------------------------------------\u001B[0m", "Wall time: 2.79 ms\n"
"\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"
] ]
} }
], ],
@ -469,9 +443,9 @@
], ],
"metadata": { "metadata": {
"kernelspec": { "kernelspec": {
"name": "python3", "display_name": "Python [conda env:.conda-jupyter] *",
"language": "python", "language": "python",
"display_name": "Python 3 (ipykernel)" "name": "conda-env-.conda-jupyter-py"
}, },
"language_info": { "language_info": {
"codemirror_mode": { "codemirror_mode": {

8
requirements.txt
View File

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