Skip to content

Commit 03a92fc

Browse files
AtsushiSakaiAtsushiSakai
authored
fix bipedal_planner and add its test (AtsushiSakai#332)
1 parent 3607d72 commit 03a92fc

File tree

4 files changed

+104
-45
lines changed

4 files changed

+104
-45
lines changed

‎Bipedal/bipedal_planner/__init__.py‎

Whitespace-only changes.

‎Bipedal/bipedal_planner/bipedal_planner.py‎

Lines changed: 65 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,17 @@
1212

1313
class BipedalPlanner(object):
1414
def __init__(self):
15+
self.act_p = [] # actual footstep positions
16+
self.ref_p = [] # reference footstep positions
17+
self.com_trajectory = []
1518
self.ref_footsteps = None
1619
self.g = 9.8
1720

1821
def set_ref_footsteps(self, ref_footsteps):
1922
self.ref_footsteps = ref_footsteps
2023

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):
2226
time_split = 100
2327

2428
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
3741

3842
return x, x_dot, y, y_dot
3943

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):
4145
if self.ref_footsteps is None:
4246
print("No footsteps")
4347
return
4448

49+
com_trajectory_for_plot, ax = None, None
50+
4551
# set up plotter
4652
if plot:
4753
fig = plt.figure()
4854
ax = Axes3D(fig)
4955
com_trajectory_for_plot = []
5056

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
5759
xi, xi_dot, yi, yi_dot = 0.0, 0.0, 0.01, 0.0
5860
time = 0.0
5961
n = 0
@@ -62,10 +64,10 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
6264
for i in range(len(self.ref_footsteps)):
6365
# simulate x, y and those of dot of inverted pendulum
6466
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)
6668

6769
# update time
68-
time += T_sup
70+
time += t_sup
6971
n += 1
7072

7173
# 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):
7779
f_x_next, f_y_next, f_theta_next = 0., 0., 0.
7880
else:
7981
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+
))
8994
x_, y_ = list(np.dot(rotate_mat_next, np.array(
9095
[f_x_next / 2., math.pow(-1, n) * f_y_next / 2.])))
9196
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_])))
9398
self.ref_p.append([px, py, f_theta])
9499

95100
# calculate reference COM
96101
xd, xd_dot = px + x_, vx_
97102
yd, yd_dot = py + y_, vy_
98103

99104
# 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)
105110
self.act_p.append([px_star, py_star, f_theta])
106111

107112
# plot
@@ -112,17 +117,22 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
112117
# set up plotter
113118
plt.cla()
114119
# 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])
117124
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)
119127

120128
# update com_trajectory_for_plot
121129
com_trajectory_for_plot.append(self.com_trajectory[c])
122130

123131
# 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")
126136

127137
# plot inverted pendulum
128138
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):
137147
foot_height = 0.04
138148
for j in range(len(self.ref_p)):
139149
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
141152
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=":")
145164
ax.add_patch(rec)
146165
art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z")
147166

148167
# foot rectangle for self.act_p
149168
for j in range(len(self.act_p)):
150169
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
152172
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)
156183
ax.add_patch(rec)
157184
art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z")
158185

‎PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py‎

Lines changed: 15 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -369,13 +369,21 @@ def reeds_shepp_path_planning(sx, sy, syaw,
369369
def main():
370370
print("Reeds Shepp path planner sample start!!")
371371

372-
start_x = -1.0 # [m]
373-
start_y = -4.0 # [m]
374-
start_yaw = np.deg2rad(-20.0) # [rad]
375-
376-
end_x = 5.0 # [m]
377-
end_y = 5.0 # [m]
378-
end_yaw = np.deg2rad(25.0) # [rad]
372+
# start_x = -1.0 # [m]
373+
# start_y = -4.0 # [m]
374+
# start_yaw = np.deg2rad(-20.0) # [rad]
375+
#
376+
# end_x = 5.0 # [m]
377+
# end_y = 5.0 # [m]
378+
# end_yaw = np.deg2rad(25.0) # [rad]
379+
380+
start_x = 0.0 # [m]
381+
start_y = 0.0 # [m]
382+
start_yaw = np.deg2rad(0.0) # [rad]
383+
384+
end_x = 0.0 # [m]
385+
end_y = 0.0 # [m]
386+
end_yaw = np.deg2rad(0.0) # [rad]
379387

380388
curvature = 1.0
381389
step_size = 0.1

‎tests/test_bipedal_planner.py‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
from unittest import TestCase
2+
3+
import sys
4+
sys.path.append("./Bipedal/bipedal_planner/")
5+
try:
6+
from Bipedal.bipedal_planner import bipedal_planner as m
7+
except Exception:
8+
raise
9+
10+
print(__file__)
11+
12+
13+
class Test(TestCase):
14+
15+
def test(self):
16+
bipedal_planner = m.BipedalPlanner()
17+
18+
footsteps = [[0.0, 0.2, 0.0],
19+
[0.3, 0.2, 0.0],
20+
[0.3, 0.2, 0.2],
21+
[0.3, 0.2, 0.2],
22+
[0.0, 0.2, 0.2]]
23+
bipedal_planner.set_ref_footsteps(footsteps)
24+
bipedal_planner.walk(plot=False)

0 commit comments

Comments
 (0)