From edd768ccdfebc1e2a02a9a7254c8b3c211107312 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 26 Jan 2019 19:16:01 +0900 Subject: [PATCH] add bipedal planner, but there's bug yet --- Bipedal/bipedal_planner/bipedal_planner.py | 115 ++++++++++++++++----- 1 file changed, 91 insertions(+), 24 deletions(-) diff --git a/Bipedal/bipedal_planner/bipedal_planner.py b/Bipedal/bipedal_planner/bipedal_planner.py index 350c912c..899e03a6 100644 --- a/Bipedal/bipedal_planner/bipedal_planner.py +++ b/Bipedal/bipedal_planner/bipedal_planner.py @@ -1,34 +1,101 @@ -import numpy +import numpy as np +import math +from matplotlib import pyplot as plt -class FootstepPlanner(object): - def __init__(self): - pass - - def plan(self, goal): - pass - class BipedalPlanner(object): def __init__(self): - self.footstep_planner = FootstepPlanner() - - ''' - self.footstep = [[0.0, 0.2], - [0.3, 0.2], - [0.3, 0.2], - [0.3, 0.2], - [0.3, 0.2], - [0.0, 0.2]] - ''' + self.footstep = None + self.g = 9.8 def set_footstep(self, footstep): self.footstep = footstep - def send_goal(self, goal): - footstep = self.footstep_planner.plan(goal) - if footstep != None: - self.footstep = footstep + 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): + delta_time = time_width / time_split + + x_dot2 = self.g / z_c * (x - px_star) + x += x_dot * delta_time + x_dot += x_dot2 * delta_time + + y_dot2 = self.g / z_c * (y - py_star) + y += y_dot * delta_time + y_dot += y_dot2 * delta_time + + self.com_trajectory.append([x, y]) + + return x, x_dot, y, y_dot + + def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False): + if self.footstep == None: + print("No footstep") + return + + self.com_trajectory = [] + self.footsteps = [] + + px, py = 0., 0. + px_star, py_star = px, py + xi, xi_dot, yi, yi_dot = 0., 0., 0., 0. + time = 0. + n = 0 + for i in range(len(self.footstep)): + # simulate x, y o finverted pendulum + xi, xi_dot, yi, yi_dot = self.inverted_pendulum(xi, xi_dot, px_star, yi, yi_dot, py_star, z_c, T_sup) + + # update time + time += T_sup + n += 1 + + + # calculate px, py, x_, y_, vx_, vy_ + f_x, f_y, f_theta = self.footstep[n - 1] + rotate_mat = np.array([[math.cos(f_theta), -math.sin(f_theta)], + [math.sin(f_theta), math.cos(f_theta)]]) + + if n == len(self.footstep): + f_x_next, f_y_next, f_theta_next = 0., 0., 0. + else: + f_x_next, f_y_next, f_theta_next = self.footstep[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)]]) + + T_c = math.sqrt(z_c / self.g) + C = math.cosh(T_sup / T_c) + S = math.sinh(T_sup / T_c) + + 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_]))) + + + # calculate reference COM + xd, xd_dot = px + x_, vx_ + yd, yd_dot = py + y_, vy_ + + # calculate modified footstep + 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) + + self.footsteps.append([px_star, py_star]) + + if plot: + plt.plot([i[0] for i in self.footsteps], [i[1] for i in self.footsteps]) + plt.plot([i[0] for i in self.com_trajectory], [i[1] for i in self.com_trajectory]) + plt.show() + + if __name__ == "__main__": bipedal_planner = BipedalPlanner() - goal = None - bipedal_planner.send_goal(goal) + + footstep = [[0.0, 0.2, 0.0], + [0.3, 0.2, 0.0], + [0.3, 0.2, 0.0], + [0.3, 0.2, 0.0], + [0.0, 0.2, 0.0]] + bipedal_planner.set_footstep(footstep) + bipedal_planner.walk(plot=True)