diff --git a/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py b/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py index 39ad5fb..b75b51c 100755 --- a/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py +++ b/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py @@ -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 @@ -44,9 +45,13 @@ 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: + + Args: + x_bar (): + u_bar (): + + Returns: + """ x = x_bar[0] @@ -96,13 +101,16 @@ class MPC: verbose=False, ): """ - Optimisation problem defined for the linearised model, - :param initial_state: - :param target: - :param verbose: - :return: - """ + Args: + initial_state (): + target (): + prev_cmd (): + verbose (): + + Returns: + + """ assert len(initial_state) == self.nx # Create variables needed for setting up cvxpy problem @@ -117,29 +125,25 @@ class MPC: # 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] @@ -147,6 +151,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 diff --git a/mpc_pybullet_demo/cvxpy_mpc/utils.py b/mpc_pybullet_demo/cvxpy_mpc/utils.py index 928bb1a..f1d9726 100755 --- a/mpc_pybullet_demo/cvxpy_mpc/utils.py +++ b/mpc_pybullet_demo/cvxpy_mpc/utils.py @@ -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,16 @@ 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 world frame + path (ndarray): 2D array representing the path as x,y,heading points in world frame + target_v (float): reference speed + T (float): trajectory duration + DT (float): trajectory time-step + + Returns: + ndarray: 2D array representing state space reference trajectory expressed w.r.t ego state """ K = int(T / DT) @@ -98,7 +110,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 + + Args: + angle_ref (array-like): + angle_init (float): + + Returns: + array-like: """ diff_angle = angle_ref - angle_init diff_angle = np.unwrap(diff_angle) diff --git a/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py b/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py index 44c042e..2127c6d 100644 --- a/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py +++ b/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py @@ -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) diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index 036cb8d..eee22b3 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -31,12 +31,9 @@ class MPCSim: 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.action = np.zeros(2) - # starting guess - self.action[0] = 1.0 # a - self.action[1] = 0.0 # delta - self.K = int(T / DT) Q = [20, 20, 10, 20] # state error cost @@ -70,9 +67,9 @@ class MPCSim: def preview(self, mpc_out): """ - [TODO:summary] - [TODO:description] + Args: + mpc_out (): """ predicted = np.zeros((2, self.K)) predicted[:, :] = mpc_out[0:2, 1:] @@ -85,7 +82,7 @@ class MPCSim: predicted = (predicted.T.dot(Rotm)).T predicted[0, :] += self.state[0] predicted[1, :] += self.state[1] - self.predicted = predicted + return predicted def run(self): """ @@ -121,16 +118,16 @@ class MPCSim: verbose=False, ) # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) - # only the first one is used to advance the simulation + self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]] - self.predict([self.action[0], self.action[1]], DT) + self.state = self.predict(self.state, [self.action[0], self.action[1]], DT) # use the state trajectory to preview the optimizer output - self.preview(x_mpc.value) + self.predicted = self.preview(x_mpc.value) self.plot_sim() - def predict(self, u, dt): + def predict(self, state, u, dt): def kinematics_model(x, t, u): dxdt = x[2] * np.cos(x[3]) dydt = x[2] * np.sin(x[3]) @@ -141,14 +138,10 @@ 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]) @@ -243,18 +236,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] diff --git a/mpc_pybullet_demo/mpc_demo_pybullet.py b/mpc_pybullet_demo/mpc_demo_pybullet.py index aa73cd2..4ac94c5 100644 --- a/mpc_pybullet_demo/mpc_demo_pybullet.py +++ b/mpc_pybullet_demo/mpc_demo_pybullet.py @@ -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 + # starting conditions 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]