From 03a92fc23eb97b0d58bea37c4f8cf22c3ef02fb3 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Thu, 28 May 2020 22:03:17 +0900 Subject: [PATCH] fix bipedal_planner and add its test (#332) --- Bipedal/bipedal_planner/__init__.py | 0 Bipedal/bipedal_planner/bipedal_planner.py | 99 ++++++++++++------- .../reeds_shepp_path_planning.py | 20 ++-- tests/test_bipedal_planner.py | 24 +++++ 4 files changed, 101 insertions(+), 42 deletions(-) create mode 100644 Bipedal/bipedal_planner/__init__.py create mode 100644 tests/test_bipedal_planner.py diff --git a/Bipedal/bipedal_planner/__init__.py b/Bipedal/bipedal_planner/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/Bipedal/bipedal_planner/bipedal_planner.py b/Bipedal/bipedal_planner/bipedal_planner.py index 6502ce5b..60c03024 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -12,13 +12,17 @@ import mpl_toolkits.mplot3d.art3d as art3d class BipedalPlanner(object): def __init__(self): + self.act_p = [] # actual footstep positions + self.ref_p = [] # reference footstep positions + self.com_trajectory = [] self.ref_footsteps = None self.g = 9.8 def set_ref_footsteps(self, ref_footsteps): self.ref_footsteps = ref_footsteps - def inverted_pendulum(self, x, x_dot, px_star, y, y_dot, py_star, z_c, time_width): + def inverted_pendulum(self, x, x_dot, px_star, y, y_dot, py_star, z_c, + time_width): time_split = 100 for i in range(time_split): @@ -37,23 +41,21 @@ class BipedalPlanner(object): return x, x_dot, y, y_dot - def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False): + def walk(self, t_sup=0.8, z_c=0.8, a=10, b=1, plot=False): if self.ref_footsteps is None: print("No footsteps") return + com_trajectory_for_plot, ax = None, None + # set up plotter if plot: fig = plt.figure() ax = Axes3D(fig) com_trajectory_for_plot = [] - self.com_trajectory = [] - self.ref_p = [] # reference footstep positions - self.act_p = [] # actual footstep positions - - px, py = 0.0, 0.0 # reference footstep position - px_star, py_star = px, py # modified footstep position + px, py = 0.0, 0.0 # reference footstep position + px_star, py_star = px, py # modified footstep position xi, xi_dot, yi, yi_dot = 0.0, 0.0, 0.01, 0.0 time = 0.0 n = 0 @@ -62,10 +64,10 @@ class BipedalPlanner(object): for i in range(len(self.ref_footsteps)): # simulate x, y and those of dot of inverted pendulum xi, xi_dot, yi, yi_dot = self.inverted_pendulum( - xi, xi_dot, px_star, yi, yi_dot, py_star, z_c, T_sup) + xi, xi_dot, px_star, yi, yi_dot, py_star, z_c, t_sup) # update time - time += T_sup + time += t_sup n += 1 # calculate px, py, x_, y_, vx_, vy_ @@ -77,19 +79,22 @@ class BipedalPlanner(object): f_x_next, f_y_next, f_theta_next = 0., 0., 0. else: f_x_next, f_y_next, f_theta_next = self.ref_footsteps[n] - rotate_mat_next = np.array([[math.cos(f_theta_next), -math.sin(f_theta_next)], - [math.sin(f_theta_next), math.cos(f_theta_next)]]) + rotate_mat_next = np.array( + [[math.cos(f_theta_next), -math.sin(f_theta_next)], + [math.sin(f_theta_next), math.cos(f_theta_next)]]) - T_c = math.sqrt(z_c / self.g) - C = math.cosh(T_sup / T_c) - S = math.sinh(T_sup / T_c) + Tc = math.sqrt(z_c / self.g) + C = math.cosh(t_sup / Tc) + S = math.sinh(t_sup / Tc) - px, py = list(np.array( - [px, py]) + np.dot(rotate_mat, np.array([f_x, -1 * math.pow(-1, n) * f_y]))) + px, py = list(np.array([px, py]) + + np.dot(rotate_mat, + np.array([f_x, -1 * math.pow(-1, n) * f_y]) + )) x_, y_ = list(np.dot(rotate_mat_next, np.array( [f_x_next / 2., math.pow(-1, n) * f_y_next / 2.]))) vx_, vy_ = list(np.dot(rotate_mat_next, np.array( - [(1 + C) / (T_c * S) * x_, (C - 1) / (T_c * S) * y_]))) + [(1 + C) / (Tc * S) * x_, (C - 1) / (Tc * S) * y_]))) self.ref_p.append([px, py, f_theta]) # calculate reference COM @@ -97,11 +102,11 @@ class BipedalPlanner(object): yd, yd_dot = py + y_, vy_ # calculate modified footsteps - D = a * math.pow(C - 1, 2) + b * math.pow(S / T_c, 2) - px_star = -a * (C - 1) / D * (xd - C * xi - T_c * S * xi_dot) - \ - b * S / (T_c * D) * (xd_dot - S / T_c * xi - C * xi_dot) - py_star = -a * (C - 1) / D * (yd - C * yi - T_c * S * yi_dot) - \ - b * S / (T_c * D) * (yd_dot - S / T_c * yi - C * yi_dot) + D = a * math.pow(C - 1, 2) + b * math.pow(S / Tc, 2) + px_star = -a * (C - 1) / D * (xd - C * xi - Tc * S * xi_dot) \ + - b * S / (Tc * D) * (xd_dot - S / Tc * xi - C * xi_dot) + py_star = -a * (C - 1) / D * (yd - C * yi - Tc * S * yi_dot) \ + - b * S / (Tc * D) * (yd_dot - S / Tc * yi - C * yi_dot) self.act_p.append([px_star, py_star, f_theta]) # plot @@ -112,17 +117,22 @@ class BipedalPlanner(object): # set up plotter plt.cla() # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + plt.gcf().canvas.mpl_connect( + 'key_release_event', + lambda event: + [exit(0) if event.key == 'escape' else None]) ax.set_zlim(0, z_c * 2) - ax.set_aspect('equal', 'datalim') + ax.set_xlim(0, 1) + ax.set_ylim(-0.5, 0.5) # update com_trajectory_for_plot com_trajectory_for_plot.append(self.com_trajectory[c]) # plot com - ax.plot([p[0] for p in com_trajectory_for_plot], [p[1] for p in com_trajectory_for_plot], [ - 0 for p in com_trajectory_for_plot], color="red") + ax.plot([p[0] for p in com_trajectory_for_plot], + [p[1] for p in com_trajectory_for_plot], [ + 0 for _ in com_trajectory_for_plot], + color="red") # plot inverted pendulum ax.plot([px_star, com_trajectory_for_plot[-1][0]], @@ -137,22 +147,39 @@ class BipedalPlanner(object): foot_height = 0.04 for j in range(len(self.ref_p)): angle = self.ref_p[j][2] + \ - math.atan2(foot_height, foot_width) - math.pi + math.atan2(foot_height, + foot_width) - math.pi r = math.sqrt( - math.pow(foot_width / 3., 2) + math.pow(foot_height / 2., 2)) - rec = pat.Rectangle(xy=(self.ref_p[j][0] + r * math.cos(angle), self.ref_p[j][1] + r * math.sin(angle)), - width=foot_width, height=foot_height, angle=self.ref_p[j][2] * 180 / math.pi, color="blue", fill=False, ls=":") + math.pow(foot_width / 3., 2) + math.pow( + foot_height / 2., 2)) + rec = pat.Rectangle(xy=( + self.ref_p[j][0] + r * math.cos(angle), + self.ref_p[j][1] + r * math.sin(angle)), + width=foot_width, + height=foot_height, + angle=self.ref_p[j][ + 2] * 180 / math.pi, + color="blue", fill=False, + ls=":") ax.add_patch(rec) art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z") # foot rectangle for self.act_p for j in range(len(self.act_p)): angle = self.act_p[j][2] + \ - math.atan2(foot_height, foot_width) - math.pi + math.atan2(foot_height, + foot_width) - math.pi r = math.sqrt( - math.pow(foot_width / 3., 2) + math.pow(foot_height / 2., 2)) - rec = pat.Rectangle(xy=(self.act_p[j][0] + r * math.cos(angle), self.act_p[j][1] + r * math.sin(angle)), - width=foot_width, height=foot_height, angle=self.act_p[j][2] * 180 / math.pi, color="blue", fill=False) + math.pow(foot_width / 3., 2) + math.pow( + foot_height / 2., 2)) + rec = pat.Rectangle(xy=( + self.act_p[j][0] + r * math.cos(angle), + self.act_p[j][1] + r * math.sin(angle)), + width=foot_width, + height=foot_height, + angle=self.act_p[j][ + 2] * 180 / math.pi, + color="blue", fill=False) ax.add_patch(rec) art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z") diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 9c72a0c9..3bed9e29 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -369,13 +369,21 @@ def reeds_shepp_path_planning(sx, sy, syaw, def main(): print("Reeds Shepp path planner sample start!!") - start_x = -1.0 # [m] - start_y = -4.0 # [m] - start_yaw = np.deg2rad(-20.0) # [rad] + # start_x = -1.0 # [m] + # start_y = -4.0 # [m] + # start_yaw = np.deg2rad(-20.0) # [rad] + # + # end_x = 5.0 # [m] + # end_y = 5.0 # [m] + # end_yaw = np.deg2rad(25.0) # [rad] - end_x = 5.0 # [m] - end_y = 5.0 # [m] - end_yaw = np.deg2rad(25.0) # [rad] + start_x = 0.0 # [m] + start_y = 0.0 # [m] + start_yaw = np.deg2rad(0.0) # [rad] + + end_x = 0.0 # [m] + end_y = 0.0 # [m] + end_yaw = np.deg2rad(0.0) # [rad] curvature = 1.0 step_size = 0.1 diff --git a/tests/test_bipedal_planner.py b/tests/test_bipedal_planner.py new file mode 100644 index 00000000..66049c68 --- /dev/null +++ b/tests/test_bipedal_planner.py @@ -0,0 +1,24 @@ +from unittest import TestCase + +import sys +sys.path.append("./Bipedal/bipedal_planner/") +try: + from Bipedal.bipedal_planner import bipedal_planner as m +except Exception: + raise + +print(__file__) + + +class Test(TestCase): + + def test(self): + bipedal_planner = m.BipedalPlanner() + + footsteps = [[0.0, 0.2, 0.0], + [0.3, 0.2, 0.0], + [0.3, 0.2, 0.2], + [0.3, 0.2, 0.2], + [0.0, 0.2, 0.2]] + bipedal_planner.set_ref_footsteps(footsteps) + bipedal_planner.walk(plot=False)