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 .

Loading