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 compute_preview_control(self,
switch_time,
horizon,
state_constraints=False):
"""Compute controller and store it in ``self.preview_control``."""
cur_com = self.com.p
cur_comd = self.com.pd
target_com = self.target_com.p
target_comd = self.target_com.pd
dT = horizon / self.nb_mpc_steps
I = eye(3)
A = array(bmat([[I, dT * I], [zeros((3, 3)), I]]))
B = array(bmat([[.5 * dT**2 * I], [dT * I]]))
x_init = hstack([cur_com, cur_comd])
x_goal = hstack([target_com, target_comd])
switch_step = int(switch_time / dT)
C = [None] * self.nb_mpc_steps
D = [None] * self.nb_mpc_steps
e = [None] * self.nb_mpc_steps
D1, e1 = self.tube.dual_hrep[0]
if 0 <= switch_step < self.nb_mpc_steps - 1:
D2, e2 = self.tube.dual_hrep[1]
for k in xrange(self.nb_mpc_steps):
D[k] = D1 if k <= switch_step else D2
e[k] = e1 if k <= switch_step else e2
if state_constraints:
E, f = self.tube.full_hrep # E * com[k] <= f
raise NotImplementedError("add state constraints to [CDe]_list")
self.preview_control = LinearPredictiveControl(A,
B,
C,
D,
e,
x_init,
x_goal,
self.nb_mpc_steps,
wxt=1000.,
wu=1.)
self.preview_control.switch_step = switch_step
self.preview_control.timestep = dT
self.preview_control.build()
try:
self.preview_control.solve()
U = self.preview_control.U.flatten()
dT = [self.preview_control.timestep] * self.nb_mpc_steps
self.preview_buffer.update_preview(U, dT)
# <dirty why="used in PreviewDrawer">
self.preview_buffer.nb_steps = self.nb_mpc_steps
self.preview_buffer.switch_step = self.preview_control.switch_step
# </dirty>
except ValueError:
print "MPC couldn't solve QP, constraints may be inconsistent"
def __init__(self,
nb_steps,
duration,
omega2,
state_estimator,
com_target,
stance,
tube_radius=0.02,
tube_margin=0.01):
start_com = state_estimator.com
start_comd = state_estimator.comd
tube = COMTube(start_com, com_target.p, stance, tube_radius,
tube_margin)
dt = duration / nb_steps
I = eye(3)
A = asarray(bmat([[I, dt * I], [zeros((3, 3)), I]]))
B = asarray(bmat([[.5 * dt**2 * I], [dt * I]]))
x_init = hstack([start_com, start_comd])
x_goal = hstack([com_target.p, com_target.pd])
C1, e1 = tube.primal_hrep
D2, e2 = tube.dual_hrep
C1 = hstack([C1, zeros(C1.shape)])
C2 = zeros((D2.shape[0], A.shape[1]))
D1 = zeros((C1.shape[0], B.shape[1]))
C = vstack([C1, C2])
D = vstack([D1, D2])
e = hstack([e1, e2])
lmpc = LinearPredictiveControl(A,
B,
C,
D,
e,
x_init,
x_goal,
nb_steps,
wxt=1.,
wxc=1e-1,
wu=1e-1)
lmpc.build()
try:
lmpc.solve()
U = lmpc.U
P = lmpc.X[:-1, 0:3]
V = lmpc.X[:-1, 3:6]
Z = P + (gravity - U) / omega2
preview = ZMPPreviewBuffer([stance])
preview.update(P, V, Z, [dt] * nb_steps, omega2)
except ValueError as e:
print "%s error:" % type(self).__name__, e
preview = None
self.lmpc = lmpc
self.preview = preview
self.tube = tube
class COMTubePredictiveControl(pymanoid.Process):
"""
Feedback controller that continuously runs the preview controller and sends
outputs to a COMAccelBuffer.
Parameters
----------
com : PointMass
Current state (position and velocity) of the COM.
fsm : WalkingFSM
Instance of finite state machine.
preview_buffer : PreviewBuffer
MPC outputs are sent to this buffer.
nb_mpc_steps : int
Discretization step of the preview window.
tube_radius : scalar
Tube radius in [m] for the L1 norm.
"""
def __init__(self, com, fsm, preview_buffer, nb_mpc_steps, tube_radius):
super(COMTubePredictiveControl, self).__init__()
self.com = com
self.fsm = fsm
self.last_phase_id = -1
self.nb_mpc_steps = nb_mpc_steps
self.preview_buffer = preview_buffer
self.preview_control = None
self.target_com = PointMass(fsm.cur_stance.com.p, 30., color='g')
self.tube = None
self.tube_radius = tube_radius
def on_tick(self, sim):
"""
Entry point called at each simulation tick.
Parameters
----------
sim : Simulation
Instance of the current simulation.
"""
preview_targets = self.fsm.get_preview_targets()
switch_time, horizon, target_com, target_comd = preview_targets
self.target_com.set_pos(target_com)
self.target_com.set_vel(target_comd)
try:
self.compute_preview_tube()
except Exception as e:
print("Tube error: %s" % str(e))
return
try:
self.compute_preview_control(switch_time, horizon)
except Exception as e:
print("COMTubePredictiveControl error: %s" % str(e))
return
sim.log_comp_time(
'qp_build', self.preview_control.build_time)
sim.log_comp_time(
'qp_solve', self.preview_control.solve_time)
sim.log_comp_time(
'qp_solve_and_build', self.preview_control.solve_and_build_time)
def compute_preview_tube(self):
"""Compute preview tube and store it in ``self.tube``."""
cur_com, target_com = self.com.p, self.target_com.p
cur_stance = self.fsm.cur_stance
next_stance = self.fsm.next_stance
self.tube = COMTube(
cur_com, target_com, cur_stance, next_stance, self.tube_radius)
t0 = time.time()
self.tube.compute_primal_vrep()
t1 = time.time()
self.tube.compute_primal_hrep()
t2 = time.time()
self.tube.compute_dual_vrep()
t3 = time.time()
self.tube.compute_dual_hrep()
t4 = time.time()
sim.log_comp_time('tube_primal_vrep', t1 - t0)
sim.log_comp_time('tube_primal_hrep', t2 - t1)
sim.log_comp_time('tube_dual_vrep', t3 - t2)
sim.log_comp_time('tube_dual_hrep', t4 - t3)
def compute_preview_control(self, switch_time, horizon,
state_constraints=False):
"""Compute controller and store it in ``self.preview_control``."""
cur_com = self.com.p
cur_comd = self.com.pd
target_com = self.target_com.p
target_comd = self.target_com.pd
dT = horizon / self.nb_mpc_steps
eye3 = eye(3)
A = array(bmat([[eye3, dT * eye3], [zeros((3, 3)), eye3]]))
B = array(bmat([[.5 * dT ** 2 * eye3], [dT * eye3]]))
x_init = hstack([cur_com, cur_comd])
x_goal = hstack([target_com, target_comd])
switch_step = int(switch_time / dT)
C = [None] * self.nb_mpc_steps
D = [None] * self.nb_mpc_steps
e = [None] * self.nb_mpc_steps
D1, e1 = self.tube.dual_hrep[0]
if 0 <= switch_step < self.nb_mpc_steps - 1:
D2, e2 = self.tube.dual_hrep[1]
for k in range(self.nb_mpc_steps):
D[k] = D1 if k <= switch_step else D2
e[k] = e1 if k <= switch_step else e2
if state_constraints:
E, f = self.tube.full_hrep # E * com[k] <= f
raise NotImplementedError("add state constraints to [CDe]_list")
self.preview_control = LinearPredictiveControl(
A, B, C, D, e, x_init, x_goal, self.nb_mpc_steps, wxt=1000., wu=1.)
self.preview_control.switch_step = switch_step
self.preview_control.timestep = dT
try:
self.preview_control.solve()
U = self.preview_control.U.flatten()
dT = [self.preview_control.timestep] * self.nb_mpc_steps
self.preview_buffer.update_preview(U, dT)
# <dirty why="used in PreviewDrawer">
self.preview_buffer.nb_steps = self.nb_mpc_steps
self.preview_buffer.switch_step = self.preview_control.switch_step
# </dirty>
except ValueError:
print("MPC couldn't solve QP, constraints may be inconsistent")
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
def __init__(self, nb_steps, state_estimator, preview_ref):
PVZ_ref, contacts = preview_ref.discretize(nb_steps)
P_ref = PVZ_ref[:, 0:3]
X_ref = PVZ_ref[:, 0:6]
Z_ref = PVZ_ref[:, 6:9]
GZ_ref = Z_ref - P_ref
dt = preview_ref.duration / nb_steps
I, fzeros = eye(3), zeros((4, 3))
omega2 = preview_ref.omega2
omega = sqrt(omega2)
a = omega * dt
A = asarray(
bmat([[cosh(a) * I, sinh(a) / omega * I],
[omega * sinh(a) * I, cosh(a) * I]]))
B = vstack([(1. - cosh(a)) * I, -omega * sinh(a) * I])
x_init = hstack([state_estimator.com, state_estimator.comd])
Delta_x_init = x_init - X_ref[0]
Delta_x_goal = zeros((6, ))
C = [None] * nb_steps
D = [None] * nb_steps
e = [None] * nb_steps
for k in xrange(nb_steps):
contact = contacts[k]
force_inequalities = contact.force_inequalities
C_fric = hstack([force_inequalities, fzeros])
D_fric = -force_inequalities
e_fric = dot(force_inequalities, GZ_ref[k])
if not all(e_fric > -1e-10):
print "Warning: reference violates friction cone constraints"
C_cop = zeros((4, 6))
D_cop = zeros((4, 3))
e_cop = zeros(4)
for (i, vertex) in enumerate(contact.vertices):
successor = (i + 1) % len(contact.vertices)
edge = contact.vertices[successor] - vertex
normal = cross(P_ref[k] - vertex, edge)
C_cop[i, 0:3] = cross(edge, Z_ref[k] - vertex) # * Delta_p
D_cop[i, :] = normal # * Delta_z
e_cop[i] = -dot(normal, GZ_ref[k])
if not all(e_cop > -1e-10):
print "Warning: reference violates friction cone constraints"
C[k] = vstack([C_fric, C_cop])
D[k] = vstack([D_fric, D_cop])
e[k] = hstack([e_fric, e_cop])
lmpc = LinearPredictiveControl(A,
B,
C,
D,
e,
Delta_x_init,
Delta_x_goal,
nb_steps,
wxt=1.,
wxc=1e-3,
wu=1e-3)
lmpc.build()
try:
lmpc.solve()
X = X_ref + lmpc.X
Z = Z_ref[:-1] + lmpc.U
P, V = X[:, 0:3], X[:, 3:6]
assert len(X) == nb_steps + 1
assert len(Z) == nb_steps
preview = ZMPPreviewBuffer(preview_ref.contact_sequence)
preview.update(P, V, Z, [dt] * nb_steps, omega2)
if __debug__:
self.Delta_X = lmpc.X
self.Delta_Z = lmpc.U
self.X_ref = X_ref
self.P_ref = P_ref
self.Z_ref = Z_ref
self.contacts = contacts
except ValueError as e:
print "%s error:" % type(self).__name__, e
preview = None
self.lmpc = lmpc
self.nb_steps = nb_steps
self.preview = preview