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" % (