def update_target_pose(self, dt):
assert self.retimed_traj is not None
from_start = self.foot_link.p - self.start_pose[4:]
to_goal = self.end_pose[4:] - self.foot_link.p
dist_to_goal = norm(to_goal)
dist_from_start = norm(from_start)
x = dist_to_goal / (dist_from_start + dist_to_goal)
quat = quat_slerp(self.foot.quat, self.end_pose[0:4], 1. - x)
self.foot_vel = self.retimed_traj.Evald(dt)
self.foot.set_pos(self.retimed_traj.Eval(dt))
self.foot.set_quat(quat)
def on_tick(self, sim):
if self.handle is None:
self.update_cone()
for contact in self.stance.contacts:
if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10:
self.update_contact_poses()
self.update_cone()
break
if norm(self.stance.com.p - self.last_com) > 1e-10:
self.update_contact_poses()
self.update_cone()
self.last_com = self.stance.com.p
def xprod_ratio(self):
"""Used to test the validity of our assumption on neglecting Dz x Dp."""
t = 0.
for k in xrange(self.nb_steps):
contact = self.contacts[k]
Delta_p = self.Delta_X[k, 0:3]
Delta_z = self.Delta_Z[k]
for (i, v) in enumerate(contact.vertices):
e1 = cross(v - self.P_ref[k], Delta_z)
e2 = cross(self.Z_ref[k] - v, Delta_p)
min_norm = norm(e1 + e2)
t += norm(cross(Delta_p, Delta_z)) / min_norm
return t / self.nb_steps
def interpolate_foot_path(self):
p0 = self.foot.p
p1 = self.end_pose[4:]
R = rotation_matrix_from_quat(self.end_pose[:4])
t, n = R[0:3, 0], R[0:3, 2]
v1 = 0.5 * t - 0.5 * n
if norm(self.foot_vel) > 1e-4:
v0 = self.foot_vel
self.path = interpolate_uab_hermite_topp(p0, v0, p1, v1)
self.sd_beg = norm(v0) / norm(self.path.Evald(0.))
else: # choose initial direction
v0 = 0.3 * self.foot.t + 0.7 * self.foot.n
self.path = interpolate_uab_hermite_topp(p0, v0, p1, v1)
self.sd_beg = 0.
def solve_nlp(self):
t_solve_start = time()
X = self.nlp.solve()
self.solve_time = time() - t_solve_start
# print "Symbols:", self.nlp.var_symbols
N = self.nb_steps
Y = X[:-6].reshape((N, 3 + 3 + 3 + 1 + 16))
P = Y[:, 0:3]
V = Y[:, 3:6]
# U = Y[:, 6:9] # not used
dT = Y[:, 9]
L = Y[:, 10:26]
p_last = X[-6:-3]
v_last = X[-3:]
Z, Ld = self.compute_Z_and_Ld(P, L)
cp_nog_last = p_last + v_last / self.omega
cp_nog_last_des = self.end_com + self.end_comd / self.omega
cp_error = norm(cp_nog_last - cp_nog_last_des)
T_swing = sum(dT[:self.nb_steps / 2])
if self.swing_duration is not None and \
T_swing > 1.25 * self.swing_duration:
self.update_swing_dT_min(self.swing_dT_min / 2)
self.cp_error = cp_error
if self.cp_error > 0.1: # and not self.preview.is_empty:
print("Warning: preview not updated as CP error was", cp_error)
self.nlp.warm_start(X)
self.preview.update(P, V, Z, dT, self.omega2)
self.p_last = p_last
self.v_last = v_last
self.print_results()
def compute_geom_reachable_polygon(robot, stance, xlim, ylim, draw=True):
init_com = stance.com.p
feasible_points = []
handles = []
points = generate_2d_grid(xlim, ylim, xres=4, yres=6)
for point in points:
stance.com.set_x(init_com[0] + point[0])
stance.com.set_y(init_com[1] + point[1])
robot.ik.solve(warm_start=True)
# self.draw_polytope_slice()
# draw_polygon()
if norm(robot.com - stance.com.p) < 0.02:
feasible_points.append(robot.com)
if draw:
handles.append(draw_point(robot.com, pointsize=5e-3))
# handle.append(draw_point(stance.com.p))
feasible = [array([p[0], p[1]]) for p in feasible_points]
hull = ConvexHull(feasible)
vertices = [feasible[i] for i in hull.vertices]
if draw:
z_avg = pylab.mean([p[2] for p in feasible_points])
vertices_3d = [array([v[0], v[1], z_avg]) for v in vertices]
handles.extend([draw_point(v) for v in vertices_3d])
stance.com.set_pos(init_com)
return vertices
def solve_nlp(self):
t_solve_start = time()
X = self.nlp.solve()
self.solve_time = time() - t_solve_start
# print "Symbols:", self.nlp.var_symbols
N = self.nb_steps
Y = X[:-6].reshape((N, 3 + 3 + 3 + 3 + 1))
P = Y[:, 0:3]
V = Y[:, 3:6]
# U = Y[:, 6:9] # not used
Z = Y[:, 9:12]
dT = Y[:, 12]
p_last = X[-6:-3]
v_last = X[-3:]
cp_nog_last = p_last + v_last / self.omega
cp_nog_last_des = self.end_com + self.end_comd / self.omega
cp_error = norm(cp_nog_last - cp_nog_last_des)
T_swing = sum(dT[:self.nb_steps / 2])
if self.swing_duration is not None and \
T_swing > 1.25 * self.swing_duration:
self.update_swing_dT_min(self.swing_dT_min / 2)
self.cp_error = cp_error
if self.cp_error > 0.1: # and not self.preview.is_empty:
print "Warning: preview not updated as CP error was", cp_error
return
self.nlp.warm_start(X) # save as initial guess for next iteration
self.preview.update(P, V, Z, dT, self.omega2)
self.p_last = p_last
self.v_last = v_last
self.print_results()
def interpolate_path(self):
target_p = self.fsm.next_stance.com
target_pd = self.fsm.next_stance.comd
p0 = self.body.p
p1 = target_p
if norm(p1 - p0) < 1e-3:
return None, None
v1 = target_pd
if norm(self.body.pd) > 1e-4:
v0 = self.body.pd
path = interpolate_houba_topp(p0, p1, v0, v1, hack=True)
sd_beg = norm(v0) / norm(path.Evald(0.))
else: # choose initial direction
delta = normalize(p1 - p0)
ref = normalize(self.fsm.cur_stance.comd)
v0 = 0.7 * delta + 0.3 * ref
path = interpolate_houba_topp(p0, p1, v0, v1, hack=True)
sd_beg = 0.
return path, sd_beg
def on_tick(self, sim):
if self.finished:
return
elif norm(self.end_pose[4:] - self.foot.p) < 5e-3:
self.finished = True
self.foot.set_pose(self.end_pose)
self.foot_vel *= 0.
return
self.compute_trajectory()
self.update_target_pose(sim.dt)
def interpolate_path(self):
target_pose = self.body.end_pose
target_p = target_pose[4:]
R = rotation_matrix_from_quat(target_pose[:4])
t, n = R[0:3, 0], R[0:3, 2]
target_pd = 0.5 * t - 0.5 * n
p0 = self.body.p
p1 = target_p
if norm(p1 - p0) < 1e-3:
return None, None
v1 = target_pd
if norm(self.body.pd) > 1e-4:
v0 = self.body.pd
path = interpolate_houba_topp(p0, p1, v0, v1)
sd_beg = norm(v0) / norm(path.Evald(0.))
else: # choose initial direction
v0 = 0.3 * self.body.t + 0.7 * self.body.n
path = interpolate_houba_topp(p0, p1, v0, v1)
sd_beg = 0.
return path, sd_beg
def on_tick(self, sim):
if self.handle is None:
self.create_polyhedron(self.stance.contacts)
return
for contact in self.stance.contacts:
if norm(contact.pose - self.contact_poses[contact.name]) > 1e-10:
self.update_contacts()
self.create_polyhedron(self.stance.contacts)
return
if self.nr_iter < self.max_iter:
self.refine_polyhedron()
self.nr_iter += 1
def on_tick(self, sim):
"""
Update the FSM state after a tick of the control loop.
INPUT:
- ``sim`` -- instance of current simulation
"""
if self.is_over:
return
if self.cur_stance.is_single_support:
foot_target = self.next_stance.target_foot.effector_pose[4:]
if norm(self.free_foot.p - foot_target) < 0.01:
self.step()
else: # self.cur_stance.is_double_support
# dist_to_edge = self.next_stance.dist_to_sep_edge(self.com.p)
close_to_target = norm(self.cur_stance.com - self.com.p) < 0.05
# small_velocity = norm(self.com.pd) < 0.1
if close_to_target:
self.com.set_velocity(0. * self.com.pd) # kron
self.step()
def on_tick(self, sim):
"""
Apply regular impulses to the inverted pendulum.
Parameters
----------
sim : pymanoid.Simulation
Simulation instance.
"""
self.nb_ticks += 1
if self.handle is not None and self.nb_ticks % 15 == 0:
self.handle = None
if self.nb_ticks % 50 == 0:
com = self.pendulum.com.p
comd = self.pendulum.com.pd
dv = 2. * numpy.random.random(3) - 1.
dv[2] *= 0.5 # push is weaker in vertical direction
dv *= self.gain / norm(dv)
self.pendulum.com.set_vel(comd + dv)
self.handle = draw_arrow(com - dv, com, color='b', linewidth=0.01)
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
print "Results"
print "-------"
def on_tick(self, sim):
if norm(self.robot.com - self.last_com) > 1e-2:
self.last_com = self.robot.com
self.update_polygon()
if self.handle is None:
self.update_handle()
def on_tick(self, sim):
if norm(self.pendulum.com.p - self.pendulum.target) < 2e-3:
info("Stopping simulation as pendulum converged")
sim.stop()
