github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 14:48:02 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix bipedal_planner and add its test (#332)

Browse Source
This commit is contained in:
Atsushi Sakai
2020-05-28 22:03:17 +09:00
committed by GitHub
parent 3607d72b60
commit 03a92fc23e
4 changed files with 101 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
Bipedal/bipedal_planner/__init__.py Normal file
View File

99
Bipedal/bipedal_planner/bipedal_planner.py
View File

@@ -12,13 +12,17 @@ import mpl_toolkits.mplot3d.art3d as art3d
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):
@@ -37,23 +41,21 @@ class BipedalPlanner(object):
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
@@ -62,10 +64,10 @@ class BipedalPlanner(object):
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_
@@ -77,19 +79,22 @@ class BipedalPlanner(object):
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)]])
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)
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])))
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
@@ -97,11 +102,11 @@ class BipedalPlanner(object):
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
@@ -112,17 +117,22 @@ class BipedalPlanner(object):
# 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]],
@@ -137,22 +147,39 @@ class BipedalPlanner(object):
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")

20
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -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]
# 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]
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

24
tests/test_bipedal_planner.py Normal file
View File

@@ -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)