def compute_controls(self): """ Compute pendulum controls for the current simulation step. Returns ------- cop : (3,) array COP coordinates in the world frame. lambda_ : scalar Leg stiffness coefficient :math:`\\lambda \\geq 0`. """ if __debug__: z_crit = self.z_crit(self.state) if z_crit < 0.: error("Cannot stabilize: z_crit=%.1f < 0" % z_crit) else: # z_crit >= 0. info("Maybe capturable: z_crit=%.1f > 0" % z_crit) g = -gravity[2] omega_i = -self.state.xd / self.state.x bc_integral = (self.state.zd + omega_i * self.state.z) / g # Note that bc_integral = (zd_bar + omega_i * z_bar) / g, # however there is no need to compute z_bar here since # zd_bar + omega_i * z_bar = zd + omega_i * z. if abs(self.state.yd + omega_i * self.state.y) > 1e-10: raise Exception("2D balance only applies to planar motions") self.solver.set_bc_integral(bc_integral) self.solver.set_omega_i(omega_i) self.solver.set_omega_f(sqrt(g / self.pendulum.z_target)) self.solver.solve() cop = array([0., 0., 0.]) return cop, self.solver.get_lambda()
def on_tick(self, sim): """ Update pendulum controls based on stabilizing solution. Parameters ---------- sim : pymanoid.Simulation Simulation instance. """ self.update_state() try: cop, lambda_ = self.compute_controls() self.pendulum.set_cop(cop) self.pendulum.set_lambda(lambda_) except RuntimeError as e: if "CoP" in str(e) or "Ill-posed problem detected" in str(e): self.T = None # don't try to draw trajectory error("Convex problem has no solution") warn("Details: %s" % str(e)) else: # surprise! raise
sim.step() nb_launches, nb_samples = 0, 0 while nb_launches < TOTAL_LAUNCHES or nb_samples < TOTAL_SAMPLES: scenario = sample_scenario() for stabilizer in [stabilizer_2d, stabilizer_3d]: stabilizer_2d.pause() stabilizer_3d.pause() stabilizer.resume() reset_state(scenario) if not stabilizer.solver.optimal_found: warn("Unfeasible sample") continue while norm(pendulum.com.p - pendulum.target) > 2e-3: if not stabilizer.solver.optimal_found or stabilizer.T is None: error("Unfeasible state encountered during execution") break sim.step() print "\n------------------------------------------\n" print "Launch %d for %s" % (nb_launches + 1, type(stabilizer).__name__) stabilizer.solver.print_debug_info() nb_2d_samples = len(stabilizer_2d.solver.solve_times) nb_3d_samples = len(stabilizer_3d.solver.solve_times) nb_samples = min(nb_2d_samples, nb_3d_samples) nb_launches += 1 print "Results" print "-------" for stabilizer in [stabilizer_2d, stabilizer_3d]: msg = "Solve time: %.1f +/- %.1f ms over %d samples, %d launches" % (