1 ```""" ``` 2 ```Bipedal Walking with modifying designated footsteps ``` 3 ```author: Takayuki Murooka (takayuki5168) ``` 4 ```""" ``` 5 1 ```import numpy as np ``` 6 1 ```import math ``` 7 1 ```from matplotlib import pyplot as plt ``` 8 1 ```import matplotlib.patches as pat ``` 9 1 ```from mpl_toolkits.mplot3d import Axes3D ``` 10 1 ```import mpl_toolkits.mplot3d.art3d as art3d ``` 11 12 13 1 ```class BipedalPlanner(object): ``` 14 1 ``` def __init__(self): ``` 15 1 ``` self.act_p = [] # actual footstep positions ``` 16 1 ``` self.ref_p = [] # reference footstep positions ``` 17 1 ``` self.com_trajectory = [] ``` 18 1 ``` self.ref_footsteps = None ``` 19 1 ``` self.g = 9.8 ``` 20 21 1 ``` def set_ref_footsteps(self, ref_footsteps): ``` 22 1 ``` self.ref_footsteps = ref_footsteps ``` 23 24 1 ``` def inverted_pendulum(self, x, x_dot, px_star, y, y_dot, py_star, z_c, ``` 25 ``` time_width): ``` 26 1 ``` time_split = 100 ``` 27 28 1 ``` for i in range(time_split): ``` 29 1 ``` delta_time = time_width / time_split ``` 30 31 1 ``` x_dot2 = self.g / z_c * (x - px_star) ``` 32 1 ``` x += x_dot * delta_time ``` 33 1 ``` x_dot += x_dot2 * delta_time ``` 34 35 1 ``` y_dot2 = self.g / z_c * (y - py_star) ``` 36 1 ``` y += y_dot * delta_time ``` 37 1 ``` y_dot += y_dot2 * delta_time ``` 38 39 1 ``` if i % 10 == 0: ``` 40 1 ``` self.com_trajectory.append([x, y]) ``` 41 42 1 ``` return x, x_dot, y, y_dot ``` 43 44 1 ``` def walk(self, t_sup=0.8, z_c=0.8, a=10, b=1, plot=False): ``` 45 1 ``` if self.ref_footsteps is None: ``` 46 0 ``` print("No footsteps") ``` 47 0 ``` return ``` 48 49 ``` # set up plotter ``` 50 1 ``` com_trajectory_for_plot, ax = None, None ``` 51 1 ``` if plot: ``` 52 0 ``` fig = plt.figure() ``` 53 0 ``` ax = Axes3D(fig) ``` 54 0 ``` com_trajectory_for_plot = [] ``` 55 56 1 ``` px, py = 0.0, 0.0 # reference footstep position ``` 57 1 ``` px_star, py_star = px, py # modified footstep position ``` 58 1 ``` xi, xi_dot, yi, yi_dot = 0.0, 0.0, 0.01, 0.0 ``` 59 1 ``` time = 0.0 ``` 60 1 ``` n = 0 ``` 61 1 ``` self.ref_p.append([px, py, 0]) ``` 62 1 ``` self.act_p.append([px, py, 0]) ``` 63 1 ``` for i in range(len(self.ref_footsteps)): ``` 64 ``` # simulate x, y and those of dot of inverted pendulum ``` 65 1 ``` xi, xi_dot, yi, yi_dot = self.inverted_pendulum( ``` 66 ``` xi, xi_dot, px_star, yi, yi_dot, py_star, z_c, t_sup) ``` 67 68 ``` # update time ``` 69 1 ``` time += t_sup ``` 70 1 ``` n += 1 ``` 71 72 ``` # calculate px, py, x_, y_, vx_, vy_ ``` 73 1 ``` f_x, f_y, f_theta = self.ref_footsteps[n - 1] ``` 74 1 ``` rotate_mat = np.array([[math.cos(f_theta), -math.sin(f_theta)], ``` 75 ``` [math.sin(f_theta), math.cos(f_theta)]]) ``` 76 77 1 ``` if n == len(self.ref_footsteps): ``` 78 1 ``` f_x_next, f_y_next, f_theta_next = 0., 0., 0. ``` 79 ``` else: ``` 80 1 ``` f_x_next, f_y_next, f_theta_next = self.ref_footsteps[n] ``` 81 1 ``` rotate_mat_next = np.array( ``` 82 ``` [[math.cos(f_theta_next), -math.sin(f_theta_next)], ``` 83 ``` [math.sin(f_theta_next), math.cos(f_theta_next)]]) ``` 84 85 1 ``` Tc = math.sqrt(z_c / self.g) ``` 86 1 ``` C = math.cosh(t_sup / Tc) ``` 87 1 ``` S = math.sinh(t_sup / Tc) ``` 88 89 1 ``` px, py = list(np.array([px, py]) ``` 90 ``` + np.dot(rotate_mat, ``` 91 ``` np.array([f_x, -1 * math.pow(-1, n) * f_y]) ``` 92 ``` )) ``` 93 1 ``` x_, y_ = list(np.dot(rotate_mat_next, np.array( ``` 94 ``` [f_x_next / 2., math.pow(-1, n) * f_y_next / 2.]))) ``` 95 1 ``` vx_, vy_ = list(np.dot(rotate_mat_next, np.array( ``` 96 ``` [(1 + C) / (Tc * S) * x_, (C - 1) / (Tc * S) * y_]))) ``` 97 1 ``` self.ref_p.append([px, py, f_theta]) ``` 98 99 ``` # calculate reference COM ``` 100 1 ``` xd, xd_dot = px + x_, vx_ ``` 101 1 ``` yd, yd_dot = py + y_, vy_ ``` 102 103 ``` # calculate modified footsteps ``` 104 1 ``` D = a * math.pow(C - 1, 2) + b * math.pow(S / Tc, 2) ``` 105 1 ``` px_star = -a * (C - 1) / D * (xd - C * xi - Tc * S * xi_dot) \ ``` 106 ``` - b * S / (Tc * D) * (xd_dot - S / Tc * xi - C * xi_dot) ``` 107 1 ``` py_star = -a * (C - 1) / D * (yd - C * yi - Tc * S * yi_dot) \ ``` 108 ``` - b * S / (Tc * D) * (yd_dot - S / Tc * yi - C * yi_dot) ``` 109 1 ``` self.act_p.append([px_star, py_star, f_theta]) ``` 110 111 ``` # plot ``` 112 1 ``` if plot: ``` 113 0 ``` self.plot_animation(ax, com_trajectory_for_plot, px_star, ``` 114 ``` py_star, z_c) ``` 115 1 ``` if plot: ``` 116 0 ``` plt.show() ``` 117 118 ``` def plot_animation(self, ax, com_trajectory_for_plot, px_star, py_star, ``` 119 ``` z_c): # pragma: no cover ``` 120 ``` # for plot trajectory, plot in for loop ``` 121 ``` for c in range(len(self.com_trajectory)): ``` 122 ``` if c > len(com_trajectory_for_plot): ``` 123 ``` # set up plotter ``` 124 ``` plt.cla() ``` 125 ``` # for stopping simulation with the esc key. ``` 126 ``` plt.gcf().canvas.mpl_connect( ``` 127 ``` 'key_release_event', ``` 128 ``` lambda event: ``` 129 ``` [exit(0) if event.key == 'escape' else None]) ``` 130 ``` ax.set_zlim(0, z_c * 2) ``` 131 ``` ax.set_xlim(0, 1) ``` 132 ``` ax.set_ylim(-0.5, 0.5) ``` 133 134 ``` # update com_trajectory_for_plot ``` 135 ``` com_trajectory_for_plot.append(self.com_trajectory[c]) ``` 136 137 ``` # plot com ``` 138 ``` ax.plot([p[0] for p in com_trajectory_for_plot], ``` 139 ``` [p[1] for p in com_trajectory_for_plot], [ ``` 140 ``` 0 for _ in com_trajectory_for_plot], ``` 141 ``` color="red") ``` 142 143 ``` # plot inverted pendulum ``` 144 ``` ax.plot([px_star, com_trajectory_for_plot[-1][0]], ``` 145 ``` [py_star, com_trajectory_for_plot[-1][1]], ``` 146 ``` [0, z_c], color="green", linewidth=3) ``` 147 ``` ax.scatter([com_trajectory_for_plot[-1][0]], ``` 148 ``` [com_trajectory_for_plot[-1][1]], ``` 149 ``` [z_c], color="green", s=300) ``` 150 151 ``` # foot rectangle for self.ref_p ``` 152 ``` foot_width = 0.06 ``` 153 ``` foot_height = 0.04 ``` 154 ``` for j in range(len(self.ref_p)): ``` 155 ``` angle = self.ref_p[j][2] + \ ``` 156 ``` math.atan2(foot_height, ``` 157 ``` foot_width) - math.pi ``` 158 ``` r = math.sqrt( ``` 159 ``` math.pow(foot_width / 3., 2) + math.pow( ``` 160 ``` foot_height / 2., 2)) ``` 161 ``` rec = pat.Rectangle(xy=( ``` 162 ``` self.ref_p[j][0] + r * math.cos(angle), ``` 163 ``` self.ref_p[j][1] + r * math.sin(angle)), ``` 164 ``` width=foot_width, ``` 165 ``` height=foot_height, ``` 166 ``` angle=self.ref_p[j][ ``` 167 ``` 2] * 180 / math.pi, ``` 168 ``` color="blue", fill=False, ``` 169 ``` ls=":") ``` 170 ``` ax.add_patch(rec) ``` 171 ``` art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z") ``` 172 173 ``` # foot rectangle for self.act_p ``` 174 ``` for j in range(len(self.act_p)): ``` 175 ``` angle = self.act_p[j][2] + \ ``` 176 ``` math.atan2(foot_height, ``` 177 ``` foot_width) - math.pi ``` 178 ``` r = math.sqrt( ``` 179 ``` math.pow(foot_width / 3., 2) + math.pow( ``` 180 ``` foot_height / 2., 2)) ``` 181 ``` rec = pat.Rectangle(xy=( ``` 182 ``` self.act_p[j][0] + r * math.cos(angle), ``` 183 ``` self.act_p[j][1] + r * math.sin(angle)), ``` 184 ``` width=foot_width, ``` 185 ``` height=foot_height, ``` 186 ``` angle=self.act_p[j][ ``` 187 ``` 2] * 180 / math.pi, ``` 188 ``` color="blue", fill=False) ``` 189 ``` ax.add_patch(rec) ``` 190 ``` art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z") ``` 191 192 ``` plt.draw() ``` 193 ``` plt.pause(0.001) ``` 194 195 196 1 ```if __name__ == "__main__": ``` 197 0 ``` bipedal_planner = BipedalPlanner() ``` 198 199 0 ``` footsteps = [[0.0, 0.2, 0.0], ``` 200 ``` [0.3, 0.2, 0.0], ``` 201 ``` [0.3, 0.2, 0.2], ``` 202 ``` [0.3, 0.2, 0.2], ``` 203 ``` [0.0, 0.2, 0.2]] ``` 204 0 ``` bipedal_planner.set_ref_footsteps(footsteps) ``` 205 0 ``` bipedal_planner.walk(plot=True) ```

Read our documentation on viewing source code .