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