From 5ef8f52b41a63229d94da278d55732f9a04727a0 Mon Sep 17 00:00:00 2001 From: Zhengyu Peng Date: Fri, 10 Dec 2021 16:29:43 -0500 Subject: [PATCH] Update path_generator.py --- software/rpi/path_generator.py | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/software/rpi/path_generator.py b/software/rpi/path_generator.py index 16ed313..b72e43d 100644 --- a/software/rpi/path_generator.py +++ b/software/rpi/path_generator.py @@ -5,22 +5,25 @@ import numpy as np from collections import deque -def gen_forward_path(standby_coordinate): - # assert (g_steps % 4) == 0 - g_steps = 20 - g_radius = 25 +def gen_forward_path(standby_coordinate, g_steps = 20, g_radius = 25): halfsteps = int(g_steps/2) + g_steps = halfsteps*2 path = np.zeros((g_steps, 6, 3)) - path[:, 0, :] = semicircle_generator(g_radius, g_steps) + # path[:, 0, :] = semicircle_generator(g_radius, g_steps) + semi_circle = semicircle_generator(g_radius, g_steps) - mir_path = np.roll(path[:, 0, :], halfsteps, axis=0) - path[:, 2, :] = path[:, 0, :] - path[:, 4, :] = path[:, 0, :] - path[:, 1, :] = mir_path - path[:, 3, :] = mir_path - path[:, 5, :] = mir_path + # mir_path = np.roll(path[:, 0, :], halfsteps, axis=0) + # path[:, 2, :] = path[:, 0, :] + # path[:, 4, :] = path[:, 0, :] + # path[:, 1, :] = mir_path + # path[:, 3, :] = mir_path + # path[:, 5, :] = mir_path + + path[:, [0, 2, 4], :] = np.tile(semi_circle[:, np.newaxis, :], (1, 3, 1)) + path[:, [1, 3, 5], :] = np.tile( + np.roll(semi_circle[:, np.newaxis, :], halfsteps, axis=0), (1, 3, 1)) return path+np.tile(standby_coordinate, (g_steps, 1, 1))