def __init__(self, robot, state_estimator, pendulum, contact_feed,
forward_velocity, nb_mpc_steps, nb_lqr_steps,
max_swing_foot_accel):
super(WalkingPatternGenerator, self).__init__()
first_contact = contact_feed.pop()
second_contact = contact_feed.pop()
third_contact = contact_feed.pop()
support_contact = second_contact
swing_controller = SwingFootController(
robot.left_foot, max_swing_foot_accel)
swing_start = first_contact
swing_target = third_contact
com_target = PointMass(
[0, 0, 0], 0.5 * robot.mass, color='g', visible=True)
ds_com_target = PointMass(
[0, 0, 0], 0.5 * robot.mass, color='b', visible=False)
com_target.set_pos(swing_target.p + [0., 0., robot.leg_length])
com_target.set_vel(forward_velocity * swing_target.t)
time_to_heel_strike = 1. # not computed yet
com_mpc = PredictiveController(
nb_mpc_steps, state_estimator, com_target,
contact_sequence=[support_contact, swing_target],
omega2=pendulum.omega2, swing_duration=time_to_heel_strike)
self.__disable_lqr = False
self.__draw_support_tube = False
self.__ds_tube_handle = None
self.__max_swing_weight = 2e-2
self.__min_swing_weight = 1e-3
self.__simulate_instant_mpc = False
self.com_lqr = None
self.com_mpc = None
self.com_target = com_target
self.contact_feed = contact_feed
self.ds_com_target = ds_com_target
self.forward_velocity = forward_velocity
self.is_in_double_support = False
self.last_mpc_success = None
self.mpc_wait_time = 0.
self.nb_ds_steps = nb_lqr_steps
self.nb_lqr_steps = nb_lqr_steps
self.nb_mpc_steps = nb_mpc_steps
self.next_com_mpc = com_mpc
self.pendulum = pendulum
self.robot = robot
self.startup_flag = True # krooOon (ˆ(oo)ˆ)
self.state_estimator = state_estimator
self.strat_counts = {'cp': 0, 'ds': 0, 'lqr': 0, 'mpc': 0}
self.support_contact = support_contact
self.swing_controller = swing_controller
self.swing_start = swing_start
self.swing_target = swing_target
#
self.switch_controllers(None)
self.switch_ik_tasks('SS')
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")
