From 6e9ee7452b979fb89cd1f1979ac67c49afbd1df6 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Wed, 23 Feb 2022 11:36:22 -0500 Subject: [PATCH] Update path_generator.py --- software/raspberry pi/path_generator.py | 42 ++++++------------------- 1 file changed, 10 insertions(+), 32 deletions(-) diff --git a/software/raspberry pi/path_generator.py b/software/raspberry pi/path_generator.py index 0d88f80..55b26fe 100644 --- a/software/raspberry pi/path_generator.py +++ b/software/raspberry pi/path_generator.py @@ -50,44 +50,21 @@ def gen_walk_path(standby_coordinate, 'type': 'motion'} -def gen_fastforward_path(standby_coordinate, - g_steps=20, - y_radius=50, - z_radius=30, - x_radius=10): - assert (g_steps % 2) == 0 - - halfsteps = int(g_steps/2) - - path = np.zeros((g_steps, 6, 3)) - semi_circle_r = semicircle2_generator( - g_steps, y_radius, z_radius, x_radius) - semi_circle_l = semicircle2_generator( - g_steps, y_radius, z_radius, -x_radius) - - path[:, [0, 2], :] = np.tile(semi_circle_r[:, np.newaxis, :], (1, 2, 1)) - path[:, 1, :] = np.roll(semi_circle_r, halfsteps, axis=0) - path[:, 4, :] = semi_circle_l - path[:, [3, 5], :] = np.tile( - np.roll(semi_circle_l[:, np.newaxis, :], halfsteps, axis=0), (1, 2, 1)) - - return path+np.tile(standby_coordinate, (g_steps, 1, 1)) - - -def gen_fastbackward_path(standby_coordinate, - g_steps=20, - y_radius=50, - z_radius=30, - x_radius=10): +def gen_fastwalk_path(standby_coordinate, + g_steps=20, + y_radius=50, + z_radius=30, + x_radius=10, + reverse=False): assert (g_steps % 2) == 0 halfsteps = int(g_steps/2) path = np.zeros((g_steps, 6, 3)) semi_circle_r = semicircle2_generator( - g_steps, y_radius, z_radius, x_radius, reverse=True) + g_steps, y_radius, z_radius, x_radius, reverse=reverse) semi_circle_l = semicircle2_generator( - g_steps, y_radius, z_radius, -x_radius, reverse=True) + g_steps, y_radius, z_radius, -x_radius, reverse=reverse) path[:, [0, 2], :] = np.tile(semi_circle_r[:, np.newaxis, :], (1, 2, 1)) path[:, 1, :] = np.roll(semi_circle_r, halfsteps, axis=0) @@ -95,7 +72,8 @@ def gen_fastbackward_path(standby_coordinate, path[:, [3, 5], :] = np.tile( np.roll(semi_circle_l[:, np.newaxis, :], halfsteps, axis=0), (1, 2, 1)) - return path+np.tile(standby_coordinate, (g_steps, 1, 1)) + return {'coord': path+np.tile(standby_coordinate, (g_steps, 1, 1)), + 'type': 'motion'} def gen_leftturn_path(standby_coordinate,