def start_swing_foot(self):
"""
Initialize swing foot interpolator for single-support state.
"""
self.swing_start = self.swing_foot.pose
self.swing_interp = SwingFoot(self.swing_foot,
self.swing_target,
self.ssp_duration,
takeoff_clearance=0.05,
landing_clearance=0.05)
class WalkingFSM(pymanoid.Process):
"""
Finite State Machine for biped walking.
Parameters
----------
ssp_duration : scalar
Duration of single-support phases, in [s].
dsp_duration : scalar
Duration of double-support phases, in [s].
"""
def __init__(self, ssp_duration, dsp_duration):
super(WalkingFSM, self).__init__()
self.dsp_duration = dsp_duration
self.mpc_timestep = round(0.1 / dt) * dt # update MPC every ~0.1 [s]
self.next_footstep = 2
self.ssp_duration = ssp_duration
self.state = None
#
self.start_standing()
def on_tick(self, sim):
"""
Update function run at every simulation tick.
Parameters
----------
sim : Simulation
Instance of the current simulation.
"""
if self.state == "Standing":
return self.run_standing()
elif self.state == "DoubleSupport":
return self.run_double_support()
elif self.state == "SingleSupport":
return self.run_single_support()
raise Exception("Unknown state: " + self.state)
def start_standing(self):
"""
Switch to standing state.
"""
self.start_walking = False
self.state = "Standing"
return self.run_standing()
def run_standing(self):
"""
Run standing state.
"""
if self.start_walking:
self.start_walking = False
if self.next_footstep < len(footsteps):
return self.start_double_support()
def start_double_support(self):
"""
Switch to double-support state.
"""
if self.next_footstep % 2 == 1: # left foot swings
self.stance_foot = stance.right_foot
else: # right foot swings
self.stance_foot = stance.left_foot
dsp_duration = self.dsp_duration
if self.next_footstep == 2 or self.next_footstep == len(footsteps) - 1:
# double support is a bit longer for the first and last steps
dsp_duration = 4 * self.dsp_duration
self.swing_target = footsteps[self.next_footstep]
self.rem_time = dsp_duration
self.state = "DoubleSupport"
self.start_com_mpc_dsp()
return self.run_double_support()
def start_com_mpc_dsp(self):
self.update_mpc(self.rem_time, self.ssp_duration)
def run_double_support(self):
"""
Run double-support state.
"""
if self.rem_time <= 0.:
return self.start_single_support()
self.run_com_mpc()
self.rem_time -= dt
def start_single_support(self):
"""
Switch to single-support state.
"""
if self.next_footstep % 2 == 1: # left foot swings
self.swing_foot = stance.free_contact('left_foot')
else: # right foot swings
self.swing_foot = stance.free_contact('right_foot')
self.next_footstep += 1
self.rem_time = self.ssp_duration
self.state = "SingleSupport"
self.start_swing_foot()
self.start_com_mpc_ssp()
return self.run_single_support()
def start_swing_foot(self):
"""
Initialize swing foot interpolator for single-support state.
"""
self.swing_start = self.swing_foot.pose
self.swing_interp = SwingFoot(self.swing_foot,
self.swing_target,
self.ssp_duration,
takeoff_clearance=0.05,
landing_clearance=0.05)
def start_com_mpc_ssp(self):
self.update_mpc(0., self.rem_time)
def run_single_support(self):
"""
Run single-support state.
"""
if self.rem_time <= 0.:
stance.set_contact(self.swing_foot)
if self.next_footstep < len(footsteps):
return self.start_double_support()
else: # footstep sequence is over
return self.start_standing()
self.run_swing_foot()
self.run_com_mpc()
self.rem_time -= dt
def run_swing_foot(self):
"""
Run swing foot interpolator for single-support state.
"""
self.swing_foot.set_pose(self.swing_interp.integrate(dt))
def update_mpc(self, dsp_duration, ssp_duration):
nb_preview_steps = 16
T = self.mpc_timestep
nb_init_dsp_steps = int(round(dsp_duration / T))
nb_init_ssp_steps = int(round(ssp_duration / T))
nb_dsp_steps = int(round(self.dsp_duration / T))
A = array([[1., T, T**2 / 2.], [0., 1., T], [0., 0., 1.]])
B = array([T**3 / 6., T**2 / 2., T]).reshape((3, 1))
h = stance.com.z
g = -sim.gravity[2]
zmp_from_state = array([1., 0., -h / g])
C = array([+zmp_from_state, -zmp_from_state])
D = None
e = [[], []]
cur_vertices = self.stance_foot.get_scaled_contact_area(0.9)
next_vertices = self.swing_target.get_scaled_contact_area(0.9)
for coord in [0, 1]:
cur_max = max(v[coord] for v in cur_vertices)
cur_min = min(v[coord] for v in cur_vertices)
next_max = max(v[coord] for v in next_vertices)
next_min = min(v[coord] for v in next_vertices)
e[coord] = [
array([+1000., +1000.]) if i < nb_init_dsp_steps else
array([+cur_max, -cur_min]) if i - nb_init_dsp_steps <=
nb_init_ssp_steps else array([+1000., +1000.]) if i -
nb_init_dsp_steps - nb_init_ssp_steps < nb_dsp_steps else
array([+next_max, -next_min]) for i in range(nb_preview_steps)
]
self.x_mpc = LinearPredictiveControl(
A,
B,
C,
D,
e[0],
x_init=array([stance.com.x, stance.com.xd, stance.com.xdd]),
x_goal=array([self.swing_target.x, 0., 0.]),
nb_steps=nb_preview_steps,
wxt=1.,
wu=0.01)
self.y_mpc = LinearPredictiveControl(
A,
B,
C,
D,
e[1],
x_init=array([stance.com.y, stance.com.yd, stance.com.ydd]),
x_goal=array([self.swing_target.y, 0., 0.]),
nb_steps=nb_preview_steps,
wxt=1.,
wu=0.01)
self.x_mpc.solve()
self.y_mpc.solve()
self.preview_time = 0.
def plot_mpc_preview(self):
import pylab
T = self.mpc_timestep
h = stance.com.z
g = -sim.gravity[2]
trange = [sim.time + k * T for k in range(len(self.x_mpc.X))]
pylab.ion()
pylab.clf()
pylab.subplot(211)
pylab.plot(trange, [v[0] for v in self.x_mpc.X])
pylab.plot(trange, [v[0] - v[2] * h / g for v in self.x_mpc.X])
pylab.subplot(212)
pylab.plot(trange, [v[0] for v in self.y_mpc.X])
pylab.plot(trange, [v[0] - v[2] * h / g for v in self.y_mpc.X])
def run_com_mpc(self):
"""
Run CoM predictive control for single-support state.
"""
if self.preview_time >= self.mpc_timestep:
if self.state == "DoubleSupport":
self.update_mpc(self.rem_time, self.ssp_duration)
else: # self.state == "SingleSupport":
self.update_mpc(0., self.rem_time)
com_jerk = array([self.x_mpc.U[0][0], self.y_mpc.U[0][0], 0.])
stance.com.integrate_constant_jerk(com_jerk, dt)
self.preview_time += dt