def integrate(self, duration):
if __debug__ and abs(dot(self.contact.n, self.cop)) > 1e-10:
self.cop = self.cop - dot(self.cop,
self.contact.n) * self.contact.n
warn("CoP was offset from contact surface")
omega = sqrt(self.lambda_)
p0 = self.com.p
pd0 = self.com.pd
ch, sh = cosh(omega * duration), sinh(omega * duration)
vrp = self.contact.p + self.cop - gravity / self.lambda_
p = p0 * ch + pd0 * sh / omega - vrp * (ch - 1.)
pd = pd0 * ch + omega * (p0 - vrp) * sh
self.com.set_pos(p)
self.com.set_vel(pd)
def set_cop(self, cop):
"""
Update the CoP location on the contact surface.
Parameters
----------
cop : (3,) array
CoP location in the *local* inertial frame, with origin at the
contact point and axes parallel to the world frame.
"""
if __debug__:
cop_check = dot(self.contact.R.T, cop)
if abs(cop_check[0]) > 1.05 * self.contact.shape[0] \
or abs(cop_check[1]) > 1.05 * self.contact.shape[1]:
warn("CoP outside of contact area")
self.cop = cop
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.schedule(stabilizer_2d)
sim.schedule(stabilizer_3d)
sim.schedule(pendulum)
sim.schedule(robot.ik)
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
pos=[0.15, -0.15, 0.],
# rpy=[-0.19798375, 0.13503151, 0],
rpy=[-0.35, -0.35, 0.05],
friction=0.7)
mass = 38. # [kg]
z_target = 0.8 # [m]
init_pos = array([0., 0., z_target])
vel = (0.7, 0.1, 0.2)
draw_parabola = True
if "--comanoid" in sys.argv:
try:
import comanoid
vel = comanoid.setup(sim, robot, contact)
draw_parabola = False
except ImportError:
warn("comanoid module not available, switching to default")
delta = init_pos - contact.p
e_z = array([0., 0., 1.])
e_x = -normalize(delta - dot(delta, e_z) * e_z)
e_y = cross(e_z, e_x)
init_vel = vel[0] * e_x + vel[1] * e_y + vel[2] * e_z
pendulum = InvertedPendulum(mass, init_pos, init_vel, contact, z_target)
pendulum.draw_parabola = draw_parabola
stance = Stance(com=pendulum.com, right_foot=contact)
stance.bind(robot)
robot.ik.solve()
g = -sim.gravity[2]
lambda_min = 0.1 * g
lambda_max = 2.0 * g
stabilizer = Stabilizer3D(