12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

use neogen for docstrings

master
mcarfagno 2023-10-25 14:41:04 +01:00
parent 3158f88d66
commit 1f22d8a786
1 changed files with 18 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
mpc_pybullet_demo/cvxpy_mpc/utils.py
View File

@ -6,12 +6,12 @@ def compute_path_from_wp(start_xp, start_yp, step=0.1):
""" """
Args: Args:
start_xp (): start_xp (array-like): 1D array of x-positions
start_yp (): start_yp (array-like): 1D array of y-positions
step (): step (float): intepolation step
Returns: Returns:
ndarray of shape (3,N) representing the path as x,y,heading
""" """
final_xp = [] final_xp = []
final_yp = [] final_yp = []
@ -37,13 +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):
""" """
Finds the index of the closest element
Args: Args:
state (): state (array-like): 1D array whose first two elements are x-pos and y-pos
path (): path (ndarray): 2D array of shape (2,N) of x,y points
Returns: 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, :]
@ -69,14 +70,14 @@ def get_ref_trajectory(state, path, target_v, T, DT):
""" """
Args: Args:
state (): state (array-like): state of the vehicle in world frame
path (): path (ndarray): 2D array representing the path as x,y,heading points in world frame
target_v (): target_v (float): reference speed
T (): T (float): trajectory duration
DT (): DT (float): trajectory time-step
Returns: Returns:
ndarray: 2D array representing state space reference trajectory expressed w.r.t ego state
""" """
K = int(T / DT) K = int(T / DT)
@ -109,13 +110,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):
""" """
Removes jumps greater than 2PI
Args: Args:
angle_ref (): angle_ref (array-like):
angle_init (): angle_init (float):
Returns: 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)