From 3158f88d660c7305128b5c8918ba773803022949 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Wed, 25 Oct 2023 14:17:25 +0100 Subject: [PATCH] use neogen for docstrings --- mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py | 42 ++++++++++------- mpc_pybullet_demo/cvxpy_mpc/utils.py | 47 +++++++++++++------- mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py | 9 +++- mpc_pybullet_demo/mpc_demo_nosim.py | 32 +++++-------- mpc_pybullet_demo/mpc_demo_pybullet.py | 19 ++++++-- 5 files changed, 91 insertions(+), 58 deletions(-) diff --git a/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py b/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py index 39ad5fb..7c554dd 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 diff --git a/mpc_pybullet_demo/cvxpy_mpc/utils.py b/mpc_pybullet_demo/cvxpy_mpc/utils.py index 928bb1a..4450d2a 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 (): + start_yp (): + step (): + + Returns: + """ final_xp = [] final_yp = [] @@ -34,10 +37,13 @@ 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: + + Args: + state (): + path (): + + Returns: + """ dx = state[0] - path[0, :] dy = state[1] - path[1, :] @@ -61,11 +67,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 (): + path (): + target_v (): + T (): + DT (): + + Returns: + """ K = int(T / DT) @@ -98,7 +109,13 @@ 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. + + Args: + angle_ref (): + angle_init (): + + Returns: + """ 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..28e09b0 100644 --- a/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py +++ b/mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py @@ -3,7 +3,14 @@ import numpy as np class VehicleModel: """ - Helper class to hold vehicle info + + Attributes: + wheelbase: + max_speed: + max_acc: + max_d_acc: + max_steer: + max_d_steer: """ def __init__(self): diff --git a/mpc_pybullet_demo/mpc_demo_nosim.py b/mpc_pybullet_demo/mpc_demo_nosim.py index 036cb8d..c54147f 100755 --- a/mpc_pybullet_demo/mpc_demo_nosim.py +++ b/mpc_pybullet_demo/mpc_demo_nosim.py @@ -70,9 +70,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 +85,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): """ @@ -124,10 +124,10 @@ class MPCSim: # 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.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): @@ -141,14 +141,9 @@ class MPCSim: # solve ODE tspan = [0, dt] - self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1] + return odeint(kinematics_model, self.state, tspan, args=(u[:],))[1] 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 +238,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..8696163 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,