Compare commits
10 Commits
1e62c0cbe7
...
6f4e6647ab
Author | SHA1 | Date |
---|---|---|
邱棚 | 6f4e6647ab | |
mcarfagno | fbd5f792d7 | |
mcarfagno | f397acdbda | |
mcarfagno | 8ed16839a3 | |
mcarfagno | 55f560ca7e | |
mcarfagno | 98977302e5 | |
mcarfagno | 1f22d8a786 | |
mcarfagno | 3158f88d66 | |
mcarfagno | 6239e769b1 | |
mcarfagno | 7369fedcb3 |
|
@ -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
|
|
@ -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>
|
|
@ -0,0 +1,6 @@
|
||||||
|
<component name="InspectionProjectProfileManager">
|
||||||
|
<settings>
|
||||||
|
<option name="USE_PROJECT_PROFILE" value="false" />
|
||||||
|
<version value="1.0" />
|
||||||
|
</settings>
|
||||||
|
</component>
|
|
@ -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>
|
|
@ -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>
|
|
@ -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>
|
|
@ -0,0 +1,6 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<project version="4">
|
||||||
|
<component name="VcsDirectoryMappings">
|
||||||
|
<mapping directory="" vcs="Git" />
|
||||||
|
</component>
|
||||||
|
</project>
|
|
@ -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_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):
|
* To run the simulation-less demo (simpler demo that does not use pybullet, useful for debugging):
|
||||||
```bash
|
```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.
|
In both cases the script will promt the user for `enter` before starting the demo.
|
||||||
|
|
|
@ -10,15 +10,16 @@ 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:
|
|
||||||
: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.nx = 4 # number of state vars
|
||||||
self.nu = 2 # umber of input/control 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):
|
def get_linear_model_matrices(self, x_bar, u_bar):
|
||||||
"""
|
"""
|
||||||
Computes the LTI approximated state space model x' = Ax + Bu + C
|
Computes the approximated LTI state space model x' = Ax + Bu + C
|
||||||
:param x_bar:
|
|
||||||
:param u_bar:
|
Args:
|
||||||
:return:
|
x_bar (array-like):
|
||||||
|
u_bar (array-like):
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
x = x_bar[0]
|
x = x_bar[0]
|
||||||
|
@ -96,46 +101,51 @@ class MPC:
|
||||||
verbose=False,
|
verbose=False,
|
||||||
):
|
):
|
||||||
"""
|
"""
|
||||||
Optimisation problem defined for the linearised model,
|
|
||||||
:param initial_state:
|
Args:
|
||||||
:param target:
|
initial_state (array-like): current estimate of [x, y, v, heading]
|
||||||
:param verbose:
|
target (ndarray): state space reference, in the same frame as the provided current state
|
||||||
:return:
|
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(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")
|
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
|
# Actuation rate of change cost
|
||||||
if k < (self.control_horizon - 1):
|
for k in range(1, self.control_horizon):
|
||||||
cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
|
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]
|
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]
|
||||||
|
|
||||||
|
@ -143,6 +153,17 @@ 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
|
||||||
|
|
|
@ -4,11 +4,14 @@ 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
|
|
||||||
:param start_xp:
|
Args:
|
||||||
:param start_yp:
|
start_xp (array-like): 1D array of x-positions
|
||||||
:param step:
|
start_yp (array-like): 1D array of y-positions
|
||||||
:return:
|
step (float): intepolation step
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
ndarray of shape (3,N) representing the path as x,y,heading
|
||||||
"""
|
"""
|
||||||
final_xp = []
|
final_xp = []
|
||||||
final_yp = []
|
final_yp = []
|
||||||
|
@ -34,10 +37,14 @@ def compute_path_from_wp(start_xp, start_yp, step=0.1):
|
||||||
|
|
||||||
def get_nn_idx(state, path):
|
def get_nn_idx(state, path):
|
||||||
"""
|
"""
|
||||||
Computes the index of the waypoint closest to vehicle
|
Finds the index of the closest element
|
||||||
:param state:
|
|
||||||
:param path:
|
Args:
|
||||||
:return:
|
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, :]
|
dx = state[0] - path[0, :]
|
||||||
dy = state[1] - path[1, :]
|
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):
|
def get_ref_trajectory(state, path, target_v, T, DT):
|
||||||
"""
|
"""
|
||||||
Reinterpolate the trajectory to get a set N desired target states
|
|
||||||
:param state:
|
Args:
|
||||||
:param path:
|
state (array-like): state of the vehicle in global frame
|
||||||
:param target_v:
|
path (ndarray): 2D array representing the path as x,y,heading points in global frame
|
||||||
:return:
|
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)
|
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])
|
stop_idx = np.where(xref_cdist == cdist[-1])
|
||||||
xref[2, stop_idx] = 0.0
|
xref[2, stop_idx] = 0.0
|
||||||
|
|
||||||
# transform in car ego frame
|
# transform in 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
|
||||||
|
@ -98,7 +111,14 @@ 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):
|
||||||
"""
|
"""
|
||||||
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 = angle_ref - angle_init
|
||||||
diff_angle = np.unwrap(diff_angle)
|
diff_angle = np.unwrap(diff_angle)
|
||||||
|
|
|
@ -3,13 +3,21 @@ import numpy as np
|
||||||
|
|
||||||
class VehicleModel:
|
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):
|
def __init__(self):
|
||||||
self.wheelbase = 0.3 # vehicle wheelbase [m]
|
self.wheelbase = 0.3
|
||||||
self.max_speed = 1.5 # [m/s]
|
self.max_speed = 1.5
|
||||||
self.max_acc = 1.0 # [m/ss]
|
self.max_acc = 1.0
|
||||||
self.max_d_acc = 1.0 # [m/sss]
|
self.max_d_acc = 1.0
|
||||||
self.max_steer = np.radians(30) # [rad]
|
self.max_steer = np.radians(30)
|
||||||
self.max_d_steer = np.radians(30) # [rad/s]
|
self.max_d_steer = np.radians(30)
|
||||||
|
|
|
@ -27,33 +27,29 @@ L = 0.3 # vehicle wheelbase [m]
|
||||||
# Classes
|
# Classes
|
||||||
class MPCSim:
|
class MPCSim:
|
||||||
def __init__(self):
|
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])
|
self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
|
||||||
|
|
||||||
# starting guess
|
# helper variable to keep track of mpc output
|
||||||
self.action = np.zeros(2)
|
# starting condition is 0,0
|
||||||
self.action[0] = 1.0 # a
|
self.control = np.zeros(2)
|
||||||
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)
|
||||||
|
|
||||||
# Interpolated Path to follow given waypoints
|
# Path from waypoint interpolation
|
||||||
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,
|
||||||
)
|
)
|
||||||
|
|
||||||
# Sim help vars
|
# Helper variables to keep track of the sim
|
||||||
self.sim_time = 0
|
self.sim_time = 0
|
||||||
self.x_history = []
|
self.x_history = []
|
||||||
self.y_history = []
|
self.y_history = []
|
||||||
|
@ -61,7 +57,7 @@ class MPCSim:
|
||||||
self.h_history = []
|
self.h_history = []
|
||||||
self.a_history = []
|
self.a_history = []
|
||||||
self.d_history = []
|
self.d_history = []
|
||||||
self.predicted = None
|
self.optimized_trajectory = None
|
||||||
|
|
||||||
# Initialise plot
|
# Initialise plot
|
||||||
plt.style.use("ggplot")
|
plt.style.use("ggplot")
|
||||||
|
@ -69,24 +65,26 @@ class MPCSim:
|
||||||
plt.ion()
|
plt.ion()
|
||||||
plt.show()
|
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)
|
trajectory = np.zeros((2, self.K))
|
||||||
predicted[:, :] = mpc_out[0:2, 1:]
|
trajectory[:, :] = 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])],
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
predicted = (predicted.T.dot(Rotm)).T
|
trajectory = (trajectory.T.dot(Rotm)).T
|
||||||
predicted[0, :] += self.state[0]
|
trajectory[0, :] += self.state[0]
|
||||||
predicted[1, :] += self.state[1]
|
trajectory[1, :] += self.state[1]
|
||||||
self.predicted = predicted
|
return trajectory
|
||||||
|
|
||||||
def run(self):
|
def run(self):
|
||||||
"""
|
"""
|
||||||
|
@ -109,31 +107,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.action,
|
self.control,
|
||||||
verbose=False,
|
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))
|
# print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
|
||||||
self.predict([self.action[0], self.action[1]], DT)
|
# only the first one is used to advance the simulation
|
||||||
self.preview(x_mpc.value)
|
|
||||||
|
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()
|
self.plot_sim()
|
||||||
|
|
||||||
def predict(self, u, dt):
|
def predict_next_state(self, state, 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])
|
||||||
|
@ -144,21 +142,17 @@ class MPCSim:
|
||||||
|
|
||||||
# solve ODE
|
# solve ODE
|
||||||
tspan = [0, dt]
|
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):
|
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.opt_u[0, 1])
|
self.a_history.append(self.control[0])
|
||||||
self.d_history.append(self.opt_u[1, 1])
|
self.d_history.append(self.control[1])
|
||||||
|
|
||||||
plt.clf()
|
plt.clf()
|
||||||
|
|
||||||
|
@ -186,10 +180,10 @@ class MPCSim:
|
||||||
label="vehicle trajectory",
|
label="vehicle trajectory",
|
||||||
)
|
)
|
||||||
|
|
||||||
if self.predicted is not None:
|
if self.optimized_trajectory is not None:
|
||||||
plt.plot(
|
plt.plot(
|
||||||
self.predicted[0, :],
|
self.optimized_trajectory[0, :],
|
||||||
self.predicted[1, :],
|
self.optimized_trajectory[1, :],
|
||||||
c="tab:green",
|
c="tab:green",
|
||||||
marker="+",
|
marker="+",
|
||||||
alpha=0.5,
|
alpha=0.5,
|
||||||
|
@ -246,18 +240,11 @@ class MPCSim:
|
||||||
|
|
||||||
def plot_car(x, y, yaw):
|
def plot_car(x, y, yaw):
|
||||||
"""
|
"""
|
||||||
[TODO:summary]
|
|
||||||
|
|
||||||
[TODO:description]
|
Args:
|
||||||
|
x ():
|
||||||
Parameters
|
y ():
|
||||||
----------
|
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]
|
||||||
|
|
|
@ -18,7 +18,14 @@ 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)
|
||||||
|
|
||||||
|
@ -33,6 +40,14 @@ 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]
|
||||||
|
@ -56,7 +71,6 @@ 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")
|
||||||
|
@ -78,7 +92,6 @@ 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,
|
||||||
|
@ -213,10 +226,8 @@ 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 guess
|
# starting conditions
|
||||||
action = np.zeros(2)
|
control = 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]
|
||||||
|
@ -260,21 +271,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] + action[0] * DT
|
ego_state[2] = ego_state[2] + control[0] * DT
|
||||||
ego_state[3] = ego_state[3] + action[0] * np.tan(action[1]) / L * DT
|
ego_state[3] = ego_state[3] + control[0] * np.tan(control[1]) / L * DT
|
||||||
|
|
||||||
# optimization loop
|
# optimization loop
|
||||||
start = time.time()
|
start = time.time()
|
||||||
|
|
||||||
# MPC step
|
# MPC step
|
||||||
_, u_mpc = mpc.step(ego_state, target, action, verbose=False)
|
_, u_mpc = mpc.step(ego_state, target, control, verbose=False)
|
||||||
action[0] = u_mpc.value[0, 0]
|
control[0] = u_mpc.value[0, 0]
|
||||||
action[1] = u_mpc.value[1, 0]
|
control[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], action[0], action[1])
|
set_ctrl(car, state[2], control[0], control[1])
|
||||||
|
|
||||||
if DT - elapsed > 0:
|
if DT - elapsed > 0:
|
||||||
time.sleep(DT - elapsed)
|
time.sleep(DT - elapsed)
|
||||||
|
|
|
@ -11,9 +11,22 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 1,
|
"execution_count": 2,
|
||||||
"metadata": {},
|
"metadata": {
|
||||||
"outputs": [],
|
"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": [
|
"source": [
|
||||||
"import numpy as np\n",
|
"import numpy as np\n",
|
||||||
"from scipy.integrate import odeint\n",
|
"from scipy.integrate import odeint\n",
|
||||||
|
@ -177,8 +190,13 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 4,
|
"execution_count": 3,
|
||||||
"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",
|
||||||
|
@ -220,15 +238,23 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 5,
|
"execution_count": 4,
|
||||||
"metadata": {},
|
"metadata": {
|
||||||
|
"ExecuteTime": {
|
||||||
|
"end_time": "2024-10-22T08:06:17.956990Z",
|
||||||
|
"start_time": "2024-10-22T08:06:17.847071Z"
|
||||||
|
}
|
||||||
|
},
|
||||||
"outputs": [
|
"outputs": [
|
||||||
{
|
{
|
||||||
"name": "stdout",
|
"ename": "NameError",
|
||||||
"output_type": "stream",
|
"evalue": "name 'M' is not defined",
|
||||||
"text": [
|
"output_type": "error",
|
||||||
"CPU times: user 3.39 ms, sys: 0 ns, total: 3.39 ms\n",
|
"traceback": [
|
||||||
"Wall time: 2.79 ms\n"
|
"\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": {
|
"metadata": {
|
||||||
"kernelspec": {
|
"kernelspec": {
|
||||||
"display_name": "Python [conda env:.conda-jupyter] *",
|
"name": "python3",
|
||||||
"language": "python",
|
"language": "python",
|
||||||
"name": "conda-env-.conda-jupyter-py"
|
"display_name": "Python 3 (ipykernel)"
|
||||||
},
|
},
|
||||||
"language_info": {
|
"language_info": {
|
||||||
"codemirror_mode": {
|
"codemirror_mode": {
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in New Issue