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")
com_target = PointMass([0, 0, 0], 20.) preview_buffer = PreviewBuffer( u_dim=3, callback=lambda u, dT: com_target.integrate_constant_accel(u, dT)) fsm = WalkingFSM(staircase, robot, swing_height=0.15, cycle=True) mpc = COMTubePredictiveControl( com_target, fsm, preview_buffer, nb_mpc_steps=20, tube_radius=0.01) robot.ik.DEFAULT_WEIGHTS['POSTURE'] = 1e-5 robot.set_pos([0, 0, 2]) # robot initially above contacts fsm.cur_stance.bind(robot) robot.ik.solve(max_it=24) com_target.set_pos(robot.com) robot.ik.tasks['COM'].update_target(com_target) robot.ik.solve(max_it=42) robot.ik.add(DOFTask(robot, robot.WAIST_P, 0.2, weight=1e-3)) robot.ik.add(DOFTask(robot, robot.WAIST_Y, 0., weight=1e-3)) robot.ik.add(DOFTask(robot, robot.WAIST_R, 0., weight=1e-3)) robot.ik.add(DOFTask(robot, robot.ROT_P, 0., weight=1e-3)) robot.ik.add(DOFTask(robot, robot.R_SHOULDER_R, -0.5, weight=1e-3)) robot.ik.add(DOFTask(robot, robot.L_SHOULDER_R, 0.5, weight=1e-3)) robot.ik.add(MinCAMTask(robot, weight=1e-4)) sim.schedule(fsm) sim.schedule(mpc) sim.schedule(preview_buffer) sim.schedule(robot.ik, log_comp_times=True)
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"
com_target = PointMass([0, 0, 0], 20.) preview_buffer = PreviewBuffer( callback=lambda u, dT: com_target.integrate_acceleration(u, dT)) swing_foot = SwingFoot(swing_height=0.15) fsm = WalkingFSM(staircase, robot, swing_foot, cycle=True) mpc = COMTubePreviewControl( com_target, fsm, preview_buffer, nb_mpc_steps=10, tube_radius=0.01) robot.init_ik(robot.whole_body) robot.set_ff_pos([0, 0, 2]) # start IK with the robot above contacts robot.generate_posture(fsm.cur_stance, max_it=50) com_target.set_pos(robot.com) robot.ik.tasks['com'].update_target(com_target) robot.ik.add_task(DOFTask(robot, robot.WAIST_P, 0.2, weight=1e-3)) robot.ik.add_task(DOFTask(robot, robot.WAIST_Y, 0., weight=1e-3)) robot.ik.add_task(DOFTask(robot, robot.WAIST_R, 0., weight=1e-3)) robot.ik.add_task(DOFTask(robot, robot.ROT_P, 0., weight=1e-3)) robot.ik.add_task(DOFTask(robot, robot.R_SHOULDER_R, -0.5, weight=1e-3)) robot.ik.add_task(DOFTask(robot, robot.L_SHOULDER_R, 0.5, weight=1e-3)) robot.ik.add_task(MinCAMTask(robot, weight=1e-4)) robot.ik.tasks['posture'].weight = 1e-5 sim.schedule(fsm) sim.schedule(mpc) sim.schedule(preview_buffer) sim.schedule(robot.ik_process)
class TubePreviewControl(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) """ self.com = com self.fsm = fsm self.last_phase_id = -1 self.nb_mpc_steps = nb_mpc_steps self.preview_buffer = preview_buffer self.target_box = PointMass(fsm.cur_stance.com, 30., color='g') self.thread = None self.thread_lock = None self.tube = None self.tube_radius = tube_radius self.verbose = False def on_tick(self, sim): cur_com = self.com.p cur_comd = self.com.pd cur_stance = self.fsm.cur_stance next_stance = self.fsm.next_stance preview_targets = self.fsm.get_preview_targets() switch_time, horizon, target_com, target_comd = preview_targets if self.verbose: print "\nVelocities:" print "- |cur_comd| =", norm(cur_comd) print "- |target_comd| =", norm(target_comd) print "\nTime:" print "- horizon =", horizon print "- switch_time =", switch_time print "- timestep = ", horizon / self.nb_mpc_steps print "" self.target_box.set_pos(target_com) try: self.tube = COMTube(cur_com, target_com, cur_stance, next_stance, self.tube_radius) except TubeError as e: warning("Tube error: %s" % str(e)) return preview_control = COMPreviewControl(cur_com, cur_comd, target_com, target_comd, self.tube, horizon, switch_time, self.nb_mpc_steps) preview_control.compute_dynamics() try: preview_control.compute_control() self.preview_buffer.update_preview(preview_control) except ValueError as e: warning("MPC couldn't solve QP, constraints may be inconsistent") return sim.report_comp_times({ 'tube_primal_vrep': self.tube.comp_times[0], 'tube_primal_hrep': self.tube.comp_times[1], 'tube_dual_vrep': self.tube.comp_times[2], 'tube_dual_hrep': self.tube.comp_times[3], 'qp_build': preview_control.comp_times[0], 'qp_solve': preview_control.comp_times[1] })