Compare commits

...

10 Commits

Author SHA1 Message Date
邱棚 6f4e6647ab feat: 第一次提交 2024-10-22 17:12:23 +08:00
mcarfagno fbd5f792d7 improve naming 2023-11-10 09:40:55 +00:00
mcarfagno f397acdbda improve docstring clarity 2023-10-28 10:47:35 +01:00
mcarfagno 8ed16839a3
Merge pull request #9 from mcarfagno/docstrings
Improve Docstrings
2023-10-25 17:05:26 +01:00
mcarfagno 55f560ca7e set rate of change limit on u_0 2023-10-25 17:04:35 +01:00
mcarfagno 98977302e5 use neogen for docstrings 2023-10-25 14:46:06 +01:00
mcarfagno 1f22d8a786 use neogen for docstrings 2023-10-25 14:41:04 +01:00
mcarfagno 3158f88d66 use neogen for docstrings 2023-10-25 14:17:25 +01:00
mcarfagno 6239e769b1 add notes on how to improve controller 2023-10-24 11:46:56 +01:00
mcarfagno 7369fedcb3 tidy up comments, remove useless vars 2023-10-24 09:19:22 +01:00
15 changed files with 288 additions and 148 deletions

8
.idea/.gitignore vendored Normal file
View File

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

View File

@ -0,0 +1,15 @@
<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>

View File

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

10
.idea/misc.xml Normal file
View File

@ -0,0 +1,10 @@
<?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 Normal file
View File

@ -0,0 +1,8 @@
<?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 Normal file
View File

@ -0,0 +1,14 @@
<?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 Normal file
View File

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

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

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
@ -43,10 +44,14 @@ 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:
Computes the approximated LTI state space model x' = Ax + Bu + C
Args:
x_bar (array-like):
u_bar (array-like):
Returns:
"""
x = x_bar[0]
@ -96,46 +101,51 @@ class MPC:
verbose=False,
):
"""
Optimisation problem defined for the linearised model,
:param initial_state:
:param target:
:param verbose:
:return:
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:
"""
assert len(initial_state) == self.nx
assert len(prev_cmd) == self.nu
assert target.shape == (self.nx, self.control_horizon)
# Create variables
# Create variables needed for setting up cvxpy problem
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
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]
@ -143,6 +153,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

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,17 @@ 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 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
"""
K = int(T / DT)
@ -89,7 +102,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 car ego frame
# transform in 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
@ -98,7 +111,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 to smooth the heading
Args:
angle_ref (array-like):
angle_init (float):
Returns:
array-like:
"""
diff_angle = angle_ref - angle_init
diff_angle = np.unwrap(diff_angle)

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)

View File

@ -27,33 +27,29 @@ L = 0.3 # vehicle wheelbase [m]
# Classes
class MPCSim:
def __init__(self):
# State for the robot mathematical model [x,y,heading]
# State of the robot [x,y,v, heading]
self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
# starting guess
self.action = np.zeros(2)
self.action[0] = 1.0 # a
self.action[1] = 0.0 # delta
# helper variable to keep track of mpc output
# starting condition is 0,0
self.control = np.zeros(2)
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)
# Interpolated Path to follow given waypoints
# Path from waypoint interpolation
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,
)
# Sim help vars
# Helper variables to keep track of the sim
self.sim_time = 0
self.x_history = []
self.y_history = []
@ -61,7 +57,7 @@ class MPCSim:
self.h_history = []
self.a_history = []
self.d_history = []
self.predicted = None
self.optimized_trajectory = None
# Initialise plot
plt.style.use("ggplot")
@ -69,24 +65,26 @@ class MPCSim:
plt.ion()
plt.show()
def preview(self, mpc_out):
def ego_to_global(self, mpc_out):
"""
[TODO:summary]
transforms optimized trajectory XY points from ego(car) reference
into global(map) frame
[TODO:description]
Args:
mpc_out ():
"""
predicted = np.zeros(self.opt_u.shape)
predicted[:, :] = mpc_out[0:2, 1:]
trajectory = np.zeros((2, self.K))
trajectory[:, :] = 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
trajectory = (trajectory.T.dot(Rotm)).T
trajectory[0, :] += self.state[0]
trajectory[1, :] += self.state[1]
return trajectory
def run(self):
"""
@ -109,31 +107,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.action,
self.control,
verbose=False,
)
# 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(),
)
)
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)
# 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
)
# use the optimizer output to preview the predicted state trajectory
self.optimized_trajectory = self.ego_to_global(x_mpc.value)
self.plot_sim()
def predict(self, u, dt):
def predict_next_state(self, state, u, dt):
def kinematics_model(x, t, u):
dxdt = x[2] * np.cos(x[3])
dydt = x[2] * np.sin(x[3])
@ -144,21 +142,17 @@ 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])
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.control[0])
self.d_history.append(self.control[1])
plt.clf()
@ -186,10 +180,10 @@ class MPCSim:
label="vehicle trajectory",
)
if self.predicted is not None:
if self.optimized_trajectory is not None:
plt.plot(
self.predicted[0, :],
self.predicted[1, :],
self.optimized_trajectory[0, :],
self.optimized_trajectory[1, :],
c="tab:green",
marker="+",
alpha=0.5,
@ -246,18 +240,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]

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
action = np.zeros(2)
action[0] = 1.0 # a
action[1] = 0.0 # delta
# starting conditions
control = np.zeros(2)
# Cost Matrices
Q = [20, 20, 10, 20] # state error cost [x,y,v,yaw]
@ -260,21 +271,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] + action[0] * DT
ego_state[3] = ego_state[3] + action[0] * np.tan(action[1]) / L * DT
ego_state[2] = ego_state[2] + control[0] * DT
ego_state[3] = ego_state[3] + control[0] * np.tan(control[1]) / L * DT
# optimization loop
start = time.time()
# MPC step
_, u_mpc = mpc.step(ego_state, target, action, verbose=False)
action[0] = u_mpc.value[0, 0]
action[1] = u_mpc.value[1, 0]
_, u_mpc = mpc.step(ego_state, target, control, verbose=False)
control[0] = u_mpc.value[0, 0]
control[1] = u_mpc.value[1, 0]
elapsed = time.time() - start
print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
set_ctrl(car, state[2], action[0], action[1])
set_ctrl(car, state[2], control[0], control[1])
if DT - elapsed > 0:
time.sleep(DT - elapsed)

View File

@ -11,9 +11,22 @@
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"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"
]
}
],
"source": [
"import numpy as np\n",
"from scipy.integrate import odeint\n",
@ -177,8 +190,13 @@
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"execution_count": 3,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-22T08:06:14.547915Z",
"start_time": "2024-10-22T08:06:14.544585Z"
}
},
"outputs": [],
"source": [
"# Define process model\n",
@ -220,15 +238,23 @@
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"execution_count": 4,
"metadata": {
"ExecuteTime": {
"end_time": "2024-10-22T08:06:17.956990Z",
"start_time": "2024-10-22T08:06:17.847071Z"
}
},
"outputs": [
{
"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"
"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"
]
}
],
@ -443,9 +469,9 @@
],
"metadata": {
"kernelspec": {
"display_name": "Python [conda env:.conda-jupyter] *",
"name": "python3",
"language": "python",
"name": "conda-env-.conda-jupyter-py"
"display_name": "Python 3 (ipykernel)"
},
"language_info": {
"codemirror_mode": {

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