improve docstring clarity

master
mcarfagno 2023-10-28 10:47:35 +01:00
parent 8ed16839a3
commit f397acdbda
3 changed files with 25 additions and 20 deletions

View File

@ -44,11 +44,11 @@ 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
Args: Args:
x_bar (): x_bar (array-like):
u_bar (): u_bar (array-like):
Returns: Returns:
@ -103,15 +103,17 @@ class MPC:
""" """
Args: Args:
initial_state (): initial_state (array-like): current estimate of [x, y, v, heading]
target (): target (ndarray): state space reference, in the same frame as the provided current state
prev_cmd (): prev_cmd (array-like): previous [acceleration, steer]. note this is used in bounds and has to be realistic.
verbose (): verbose (bool):
Returns: 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 needed for setting up cvxpy problem # 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")

View File

@ -70,14 +70,15 @@ def get_ref_trajectory(state, path, target_v, T, DT):
""" """
Args: Args:
state (array-like): state of the vehicle in world frame state (array-like): state of the vehicle in global frame
path (ndarray): 2D array representing the path as x,y,heading points in world frame path (ndarray): 2D array representing the path as x,y,heading points in global frame
target_v (float): reference speed target_v (float): desired speed
T (float): trajectory duration T (float): control horizon duration
DT (float): trajectory time-step DT (float): control horizon time-step
Returns: Returns:
ndarray: 2D array representing state space reference trajectory expressed w.r.t ego state 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)
@ -101,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
@ -110,7 +111,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 Removes jumps greater than 2PI to smooth the heading
Args: Args:
angle_ref (array-like): angle_ref (array-like):

View File

@ -65,7 +65,7 @@ class MPCSim:
plt.ion() plt.ion()
plt.show() plt.show()
def preview(self, mpc_out): def ego_to_world(self, mpc_out):
""" """
Args: Args:
@ -121,13 +121,15 @@ class MPCSim:
# only the first one is used to advance the simulation # only the first one is used to advance the simulation
self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]] self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
self.state = self.predict(self.state, [self.action[0], self.action[1]], DT) self.state = self.predict_next_state(
self.state, [self.action[0], self.action[1]], DT
)
# use the state trajectory to preview the optimizer output # use the optimizer output to preview the predicted state trajectory
self.predicted = self.preview(x_mpc.value) self.predicted = self.ego_to_world(x_mpc.value)
self.plot_sim() self.plot_sim()
def predict(self, state, 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])