Skip to content

Commit

Permalink
fix bipedal_planner and add its test (AtsushiSakai#332)
Browse files Browse the repository at this point in the history
  • Loading branch information
@AtsushiSakai
AtsushiSakai authored May 28, 2020
1 parent 3607d72 commit 03a92fc
Show file tree
Hide file tree
Showing 4 changed files with 104 additions and 45 deletions.
Empty file added Bipedal/bipedal_planner/__init__.py
View file
Empty file.
103 changes: 65 additions & 38 deletions Bipedal/bipedal_planner/bipedal_planner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,17 @@

class BipedalPlanner(object):
def __init__(self):
self.act_p = [] # actual footstep positions
self.ref_p = [] # reference footstep positions
self.com_trajectory = []
self.ref_footsteps = None
self.g = 9.8

def set_ref_footsteps(self, ref_footsteps):
self.ref_footsteps = ref_footsteps

def inverted_pendulum(self, x, x_dot, px_star, y, y_dot, py_star, z_c, time_width):
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):
Expand All @@ -37,23 +41,21 @@ def inverted_pendulum(self, x, x_dot, px_star, y, y_dot, py_star, z_c, time_widt

return x, x_dot, y, y_dot

def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
def walk(self, t_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
if self.ref_footsteps is None:
print("No footsteps")
return

com_trajectory_for_plot, ax = None, None

# set up plotter
if plot:
fig = plt.figure()
ax = Axes3D(fig)
com_trajectory_for_plot = []

self.com_trajectory = []
self.ref_p = [] # reference footstep positions
self.act_p = [] # actual footstep positions

px, py = 0.0, 0.0 # reference footstep position
px_star, py_star = px, py # modified footstep position
px, py = 0.0, 0.0 # reference footstep position
px_star, py_star = px, py # modified footstep position
xi, xi_dot, yi, yi_dot = 0.0, 0.0, 0.01, 0.0
time = 0.0
n = 0
Expand All @@ -62,10 +64,10 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
for i in range(len(self.ref_footsteps)):
# simulate x, y and those of dot of inverted pendulum
xi, xi_dot, yi, yi_dot = self.inverted_pendulum(
xi, xi_dot, px_star, yi, yi_dot, py_star, z_c, T_sup)
xi, xi_dot, px_star, yi, yi_dot, py_star, z_c, t_sup)

# update time
time += T_sup
time += t_sup
n += 1

# calculate px, py, x_, y_, vx_, vy_
Expand All @@ -77,31 +79,34 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
f_x_next, f_y_next, f_theta_next = 0., 0., 0.
else:
f_x_next, f_y_next, f_theta_next = self.ref_footsteps[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])))
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)]])

Tc = math.sqrt(z_c / self.g)
C = math.cosh(t_sup / Tc)
S = math.sinh(t_sup / Tc)

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_])))
[(1 + C) / (Tc * S) * x_, (C - 1) / (Tc * S) * y_])))
self.ref_p.append([px, py, f_theta])

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

# calculate modified footsteps
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)
D = a * math.pow(C - 1, 2) + b * math.pow(S / Tc, 2)
px_star = -a * (C - 1) / D * (xd - C * xi - Tc * S * xi_dot) \
- b * S / (Tc * D) * (xd_dot - S / Tc * xi - C * xi_dot)
py_star = -a * (C - 1) / D * (yd - C * yi - Tc * S * yi_dot) \
- b * S / (Tc * D) * (yd_dot - S / Tc * yi - C * yi_dot)
self.act_p.append([px_star, py_star, f_theta])

# plot
Expand All @@ -112,17 +117,22 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
# set up plotter
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event:
[exit(0) if event.key == 'escape' else None])
ax.set_zlim(0, z_c * 2)
ax.set_aspect('equal', 'datalim')
ax.set_xlim(0, 1)
ax.set_ylim(-0.5, 0.5)

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

# plot com
ax.plot([p[0] for p in com_trajectory_for_plot], [p[1] for p in com_trajectory_for_plot], [
0 for p in com_trajectory_for_plot], color="red")
ax.plot([p[0] for p in com_trajectory_for_plot],
[p[1] for p in com_trajectory_for_plot], [
0 for _ in com_trajectory_for_plot],
color="red")

# plot inverted pendulum
ax.plot([px_star, com_trajectory_for_plot[-1][0]],
Expand All @@ -137,22 +147,39 @@ def walk(self, T_sup=0.8, z_c=0.8, a=10, b=1, plot=False):
foot_height = 0.04
for j in range(len(self.ref_p)):
angle = self.ref_p[j][2] + \
math.atan2(foot_height, foot_width) - math.pi
math.atan2(foot_height,
foot_width) - math.pi
r = math.sqrt(
math.pow(foot_width / 3., 2) + math.pow(foot_height / 2., 2))
rec = pat.Rectangle(xy=(self.ref_p[j][0] + r * math.cos(angle), self.ref_p[j][1] + r * math.sin(angle)),
width=foot_width, height=foot_height, angle=self.ref_p[j][2] * 180 / math.pi, color="blue", fill=False, ls=":")
math.pow(foot_width / 3., 2) + math.pow(
foot_height / 2., 2))
rec = pat.Rectangle(xy=(
self.ref_p[j][0] + r * math.cos(angle),
self.ref_p[j][1] + r * math.sin(angle)),
width=foot_width,
height=foot_height,
angle=self.ref_p[j][
2] * 180 / math.pi,
color="blue", fill=False,
ls=":")
ax.add_patch(rec)
art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z")

# foot rectangle for self.act_p
for j in range(len(self.act_p)):
angle = self.act_p[j][2] + \
math.atan2(foot_height, foot_width) - math.pi
math.atan2(foot_height,
foot_width) - math.pi
r = math.sqrt(
math.pow(foot_width / 3., 2) + math.pow(foot_height / 2., 2))
rec = pat.Rectangle(xy=(self.act_p[j][0] + r * math.cos(angle), self.act_p[j][1] + r * math.sin(angle)),
width=foot_width, height=foot_height, angle=self.act_p[j][2] * 180 / math.pi, color="blue", fill=False)
math.pow(foot_width / 3., 2) + math.pow(
foot_height / 2., 2))
rec = pat.Rectangle(xy=(
self.act_p[j][0] + r * math.cos(angle),
self.act_p[j][1] + r * math.sin(angle)),
width=foot_width,
height=foot_height,
angle=self.act_p[j][
2] * 180 / math.pi,
color="blue", fill=False)
ax.add_patch(rec)
art3d.pathpatch_2d_to_3d(rec, z=0, zdir="z")

Expand Down
22 changes: 15 additions & 7 deletions PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -369,13 +369,21 @@ def reeds_shepp_path_planning(sx, sy, syaw,
def main():
print("Reeds Shepp path planner sample start!!")

start_x = -1.0 # [m]
start_y = -4.0 # [m]
start_yaw = np.deg2rad(-20.0) # [rad]

end_x = 5.0 # [m]
end_y = 5.0 # [m]
end_yaw = np.deg2rad(25.0) # [rad]
# start_x = -1.0 # [m]
# start_y = -4.0 # [m]
# start_yaw = np.deg2rad(-20.0) # [rad]
#
# end_x = 5.0 # [m]
# end_y = 5.0 # [m]
# end_yaw = np.deg2rad(25.0) # [rad]

start_x = 0.0 # [m]
start_y = 0.0 # [m]
start_yaw = np.deg2rad(0.0) # [rad]

end_x = 0.0 # [m]
end_y = 0.0 # [m]
end_yaw = np.deg2rad(0.0) # [rad]

curvature = 1.0
step_size = 0.1
Expand Down
24 changes: 24 additions & 0 deletions tests/test_bipedal_planner.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
from unittest import TestCase

import sys
sys.path.append("./Bipedal/bipedal_planner/")
try:
from Bipedal.bipedal_planner import bipedal_planner as m
except Exception:
raise

print(__file__)


class Test(TestCase):

def test(self):
bipedal_planner = m.BipedalPlanner()

footsteps = [[0.0, 0.2, 0.0],
[0.3, 0.2, 0.0],
[0.3, 0.2, 0.2],
[0.3, 0.2, 0.2],
[0.0, 0.2, 0.2]]
bipedal_planner.set_ref_footsteps(footsteps)
bipedal_planner.walk(plot=False)

0 comments on commit 03a92fc

Please sign in to comment.