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)
G_list = []
h_list = []
C1, d1 = self.tube.dual_hrep[0]
E, f = None, None
if state_constraints:
E, f = self.tube.full_hrep
if 0 <= switch_step < self.nb_mpc_steps - 1:
C2, d2 = self.tube.dual_hrep[1]
for k in xrange(self.nb_mpc_steps):
if k <= switch_step:
G_list.append(C1)
h_list.append(d1)
else: # k > switch_step
G_list.append(C2)
h_list.append(d2)
G = block_diag(*G_list)
h = hstack(h_list)
self.preview_control = PreviewControl(
A, B, G, h, x_init, x_goal, self.nb_mpc_steps, E, f)
self.preview_control.switch_step = switch_step
self.preview_control.timestep = dT
self.preview_control.compute_dynamics()
try:
self.preview_control.compute_control()
self.preview_buffer.update_preview(self.preview_control)
except ValueError:
print "MPC couldn't solve QP, constraints may be inconsistent"
class COMTubePreviewControl(Process):
def __init__(self, com, fsm, preview_buffer, nb_mpc_steps, tube_radius):
"""
Create a new feedback controller that continuously runs the preview
controller and sends outputs to a COMAccelBuffer.
INPUT:
- ``com`` -- PointMass containing current COM state
- ``fsm`` -- instance of finite state machine
- ``preview_buffer`` -- PreviewBuffer to send MPC outputs to
- ``nb_mpc_steps`` -- discretization step of the preview window
- ``tube_radius`` -- tube radius (in L1 norm)
"""
super(COMTubePreviewControl, 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.
"""
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_velocity(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 "PreviewControl error: %s" % str(e)
return
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
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)
G_list = []
h_list = []
C1, d1 = self.tube.dual_hrep[0]
E, f = None, None
if state_constraints:
E, f = self.tube.full_hrep
if 0 <= switch_step < self.nb_mpc_steps - 1:
C2, d2 = self.tube.dual_hrep[1]
for k in xrange(self.nb_mpc_steps):
if k <= switch_step:
G_list.append(C1)
h_list.append(d1)
else: # k > switch_step
G_list.append(C2)
h_list.append(d2)
G = block_diag(*G_list)
h = hstack(h_list)
self.preview_control = PreviewControl(
A, B, G, h, x_init, x_goal, self.nb_mpc_steps, E, f)
self.preview_control.switch_step = switch_step
self.preview_control.timestep = dT
self.preview_control.compute_dynamics()
try:
self.preview_control.compute_control()
self.preview_buffer.update_preview(self.preview_control)
except ValueError:
print "MPC couldn't solve QP, constraints may be inconsistent"
