12
12
13
13
class BipedalPlanner (object ):
14
14
def __init__ (self ):
15
+ self .act_p = [] # actual footstep positions
16
+ self .ref_p = [] # reference footstep positions
17
+ self .com_trajectory = []
15
18
self .ref_footsteps = None
16
19
self .g = 9.8
17
20
18
21
def set_ref_footsteps (self , ref_footsteps ):
19
22
self .ref_footsteps = ref_footsteps
20
23
21
- def inverted_pendulum (self , x , x_dot , px_star , y , y_dot , py_star , z_c , time_width ):
24
+ def inverted_pendulum (self , x , x_dot , px_star , y , y_dot , py_star , z_c ,
25
+ time_width ):
22
26
time_split = 100
23
27
24
28
for i in range (time_split ):
@@ -37,23 +41,21 @@ def inverted_pendulum(self, x, x_dot, px_star, y, y_dot, py_star, z_c, time_widt
37
41
38
42
return x , x_dot , y , y_dot
39
43
40
- def walk (self , T_sup = 0.8 , z_c = 0.8 , a = 10 , b = 1 , plot = False ):
44
+ def walk (self , t_sup = 0.8 , z_c = 0.8 , a = 10 , b = 1 , plot = False ):
41
45
if self .ref_footsteps is None :
42
46
print ("No footsteps" )
43
47
return
44
48
49
+ com_trajectory_for_plot , ax = None , None
50
+
45
51
# set up plotter
46
52
if plot :
47
53
fig = plt .figure ()
48
54
ax = Axes3D (fig )
49
55
com_trajectory_for_plot = []
50
56
51
- self .com_trajectory = []
52
- self .ref_p = [] # reference footstep positions
53
- self .act_p = [] # actual footstep positions
54
-
55
- px , py = 0.0 , 0.0 # reference footstep position
56
- px_star , py_star = px , py # modified footstep position
57
+ px , py = 0.0 , 0.0 # reference footstep position
58
+ px_star , py_star = px , py # modified footstep position
57
59
xi , xi_dot , yi , yi_dot = 0.0 , 0.0 , 0.01 , 0.0
58
60
time = 0.0
59
61
n = 0
@@ -62,10 +64,10 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
62
64
for i in range (len (self .ref_footsteps )):
63
65
# simulate x, y and those of dot of inverted pendulum
64
66
xi , xi_dot , yi , yi_dot = self .inverted_pendulum (
65
- xi , xi_dot , px_star , yi , yi_dot , py_star , z_c , T_sup )
67
+ xi , xi_dot , px_star , yi , yi_dot , py_star , z_c , t_sup )
66
68
67
69
# update time
68
- time += T_sup
70
+ time += t_sup
69
71
n += 1
70
72
71
73
# calculate px, py, x_, y_, vx_, vy_
@@ -77,31 +79,34 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
77
79
f_x_next , f_y_next , f_theta_next = 0. , 0. , 0.
78
80
else :
79
81
f_x_next , f_y_next , f_theta_next = self .ref_footsteps [n ]
80
- rotate_mat_next = np .array ([[math .cos (f_theta_next ), - math .sin (f_theta_next )],
81
- [math .sin (f_theta_next ), math .cos (f_theta_next )]])
82
-
83
- T_c = math .sqrt (z_c / self .g )
84
- C = math .cosh (T_sup / T_c )
85
- S = math .sinh (T_sup / T_c )
86
-
87
- px , py = list (np .array (
88
- [px , py ]) + np .dot (rotate_mat , np .array ([f_x , - 1 * math .pow (- 1 , n ) * f_y ])))
82
+ rotate_mat_next = np .array (
83
+ [[math .cos (f_theta_next ), - math .sin (f_theta_next )],
84
+ [math .sin (f_theta_next ), math .cos (f_theta_next )]])
85
+
86
+ Tc = math .sqrt (z_c / self .g )
87
+ C = math .cosh (t_sup / Tc )
88
+ S = math .sinh (t_sup / Tc )
89
+
90
+ px , py = list (np .array ([px , py ])
91
+ + np .dot (rotate_mat ,
92
+ np .array ([f_x , - 1 * math .pow (- 1 , n ) * f_y ])
93
+ ))
89
94
x_ , y_ = list (np .dot (rotate_mat_next , np .array (
90
95
[f_x_next / 2. , math .pow (- 1 , n ) * f_y_next / 2. ])))
91
96
vx_ , vy_ = list (np .dot (rotate_mat_next , np .array (
92
- [(1 + C ) / (T_c * S ) * x_ , (C - 1 ) / (T_c * S ) * y_ ])))
97
+ [(1 + C ) / (Tc * S ) * x_ , (C - 1 ) / (Tc * S ) * y_ ])))
93
98
self .ref_p .append ([px , py , f_theta ])
94
99
95
100
# calculate reference COM
96
101
xd , xd_dot = px + x_ , vx_
97
102
yd , yd_dot = py + y_ , vy_
98
103
99
104
# calculate modified footsteps
100
- D = a * math .pow (C - 1 , 2 ) + b * math .pow (S / T_c , 2 )
101
- px_star = - a * (C - 1 ) / D * (xd - C * xi - T_c * S * xi_dot ) - \
102
- b * S / (T_c * D ) * (xd_dot - S / T_c * xi - C * xi_dot )
103
- py_star = - a * (C - 1 ) / D * (yd - C * yi - T_c * S * yi_dot ) - \
104
- b * S / (T_c * D ) * (yd_dot - S / T_c * yi - C * yi_dot )
105
+ D = a * math .pow (C - 1 , 2 ) + b * math .pow (S / Tc , 2 )
106
+ px_star = - a * (C - 1 ) / D * (xd - C * xi - Tc * S * xi_dot ) \
107
+ - b * S / (Tc * D ) * (xd_dot - S / Tc * xi - C * xi_dot )
108
+ py_star = - a * (C - 1 ) / D * (yd - C * yi - Tc * S * yi_dot ) \
109
+ - b * S / (Tc * D ) * (yd_dot - S / Tc * yi - C * yi_dot )
105
110
self .act_p .append ([px_star , py_star , f_theta ])
106
111
107
112
# plot
@@ -112,17 +117,22 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
112
117
# set up plotter
113
118
plt .cla ()
114
119
# for stopping simulation with the esc key.
115
- plt .gcf ().canvas .mpl_connect ('key_release_event' ,
116
- lambda event : [exit (0 ) if event .key == 'escape' else None ])
120
+ plt .gcf ().canvas .mpl_connect (
121
+ 'key_release_event' ,
122
+ lambda event :
123
+ [exit (0 ) if event .key == 'escape' else None ])
117
124
ax .set_zlim (0 , z_c * 2 )
118
- ax .set_aspect ('equal' , 'datalim' )
125
+ ax .set_xlim (0 , 1 )
126
+ ax .set_ylim (- 0.5 , 0.5 )
119
127
120
128
# update com_trajectory_for_plot
121
129
com_trajectory_for_plot .append (self .com_trajectory [c ])
122
130
123
131
# plot com
124
- ax .plot ([p [0 ] for p in com_trajectory_for_plot ], [p [1 ] for p in com_trajectory_for_plot ], [
125
- 0 for p in com_trajectory_for_plot ], color = "red" )
132
+ ax .plot ([p [0 ] for p in com_trajectory_for_plot ],
133
+ [p [1 ] for p in com_trajectory_for_plot ], [
134
+ 0 for _ in com_trajectory_for_plot ],
135
+ color = "red" )
126
136
127
137
# plot inverted pendulum
128
138
ax .plot ([px_star , com_trajectory_for_plot [- 1 ][0 ]],
@@ -137,22 +147,39 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
137
147
foot_height = 0.04
138
148
for j in range (len (self .ref_p )):
139
149
angle = self .ref_p [j ][2 ] + \
140
- math .atan2 (foot_height , foot_width ) - math .pi
150
+ math .atan2 (foot_height ,
151
+ foot_width ) - math .pi
141
152
r = math .sqrt (
142
- math .pow (foot_width / 3. , 2 ) + math .pow (foot_height / 2. , 2 ))
143
- rec = pat .Rectangle (xy = (self .ref_p [j ][0 ] + r * math .cos (angle ), self .ref_p [j ][1 ] + r * math .sin (angle )),
144
- width = foot_width , height = foot_height , angle = self .ref_p [j ][2 ] * 180 / math .pi , color = "blue" , fill = False , ls = ":" )
153
+ math .pow (foot_width / 3. , 2 ) + math .pow (
154
+ foot_height / 2. , 2 ))
155
+ rec = pat .Rectangle (xy = (
156
+ self .ref_p [j ][0 ] + r * math .cos (angle ),
157
+ self .ref_p [j ][1 ] + r * math .sin (angle )),
158
+ width = foot_width ,
159
+ height = foot_height ,
160
+ angle = self .ref_p [j ][
161
+ 2 ] * 180 / math .pi ,
162
+ color = "blue" , fill = False ,
163
+ ls = ":" )
145
164
ax .add_patch (rec )
146
165
art3d .pathpatch_2d_to_3d (rec , z = 0 , zdir = "z" )
147
166
148
167
# foot rectangle for self.act_p
149
168
for j in range (len (self .act_p )):
150
169
angle = self .act_p [j ][2 ] + \
151
- math .atan2 (foot_height , foot_width ) - math .pi
170
+ math .atan2 (foot_height ,
171
+ foot_width ) - math .pi
152
172
r = math .sqrt (
153
- math .pow (foot_width / 3. , 2 ) + math .pow (foot_height / 2. , 2 ))
154
- rec = pat .Rectangle (xy = (self .act_p [j ][0 ] + r * math .cos (angle ), self .act_p [j ][1 ] + r * math .sin (angle )),
155
- width = foot_width , height = foot_height , angle = self .act_p [j ][2 ] * 180 / math .pi , color = "blue" , fill = False )
173
+ math .pow (foot_width / 3. , 2 ) + math .pow (
174
+ foot_height / 2. , 2 ))
175
+ rec = pat .Rectangle (xy = (
176
+ self .act_p [j ][0 ] + r * math .cos (angle ),
177
+ self .act_p [j ][1 ] + r * math .sin (angle )),
178
+ width = foot_width ,
179
+ height = foot_height ,
180
+ angle = self .act_p [j ][
181
+ 2 ] * 180 / math .pi ,
182
+ color = "blue" , fill = False )
156
183
ax .add_patch (rec )
157
184
art3d .pathpatch_2d_to_3d (rec , z = 0 , zdir = "z" )
158
185
0 commit comments