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 draw(self):
com, comd = self.com_state.p, self.com_state.pd
cp = com + comd / self.omega + gravity / self.omega2
zmp = self.zmp_state.p
handles = [
draw_point(com, color='r', pointsize=0.025),
draw_point(cp, color='b', pointsize=0.025),
draw_point(zmp, color='r', pointsize=0.025),
draw_line(com, cp, color='b', linewidth=1),
draw_line(zmp, com, color='r', linewidth=4)
]
return handles
def play(self,
sim,
slowdown=1.,
wrench_drawer=None,
post_preview_duration=None,
callback=None):
handles = None
com, comd = self.P[0], self.V[0]
if wrench_drawer is not None:
wrench_drawer.point_mass.set_pos(com)
wrench_drawer.point_mass.set_vel(comd)
wrench_drawer.point_mass.set_transparency(0.5)
t, k, rem_time = 0., -1, -1.,
duration = 2 * self.duration if post_preview_duration is None else \
self.duration + post_preview_duration
while t <= duration:
if rem_time < 1e-10:
if k < self.nb_steps - 1:
k += 1
rem_time = self.get_dT(k)
omega2 = self.get_omega2(k)
cp = com + comd / sqrt(omega2) + sim.gravity / omega2
zmp = self.get_zmp(k) if k < self.nb_steps else cp
comdd = omega2 * (com - zmp) + sim.gravity
handles = [
draw_point(zmp, color='r'),
draw_line(zmp, com, color='r', linewidth=4),
draw_point(cp, color='b'),
draw_line(cp, com, color='b', linewidth=2),
draw_point(com, color='r', pointsize=0.05)
]
if wrench_drawer is not None:
try:
wrench_drawer.recompute(self.get_contact(k),
comdd,
am=None)
except ValueError:
print "Warning: wrench validation failed at t = %.2f s" % t
dt = min(rem_time, sim.dt)
com, comd = integrate_fip(com, comd, zmp, dt, omega2)
if wrench_drawer is not None:
wrench_drawer.point_mass.set_pos(com)
wrench_drawer.point_mass.set_vel(comd)
sleep(slowdown * dt)
if callback is not None:
callback()
rem_time -= dt
t += dt
if wrench_drawer is not None:
wrench_drawer.handles = []
return handles
def draw(self, dcm, cop):
"""
Draw extra points to illustrate stabilizer behavior.
Parameters
----------
dcm : (3,) array
Divergent component of motion.
cop : (3,) array
Center of pressure.
"""
n = self.pendulum.contact.n
dcm_ground = dcm - dot(n, dcm) * n
self.handles = [
draw_point(dcm_ground, color='b'),
draw_point(cop, color='g')]
def compute(self, draw_height=None):
assert len(self.working_set) > 0 and len(self.working_set[0]) == 2
self.all_vertices = []
for i_cur, p_cur in enumerate(self.working_set):
p_cur = array(p_cur)
A_voronoi, b_voronoi = [], []
for i_other, p_other in enumerate(self.working_set):
if i_other == i_cur:
continue
p_other = array(p_other)
p_mid = 0.5 * (p_other + p_cur)
a = p_other - p_cur
A_voronoi.append(a)
b_voronoi.append(dot(a, p_mid))
A_voronoi = vstack(A_voronoi)
b_voronoi = hstack(b_voronoi)
self.stance.com.set_x(p_cur[0])
self.stance.com.set_y(p_cur[1])
self.robot.ik.solve(warm_start=True)
proj_vertices = compute_local_actuation_dependent_polygon(
self.robot, self.stance, method="bretl")
A_proj, b_proj = compute_polytope_halfspaces(proj_vertices)
A = vstack([A_proj, A_voronoi])
b = hstack([b_proj, b_voronoi])
if draw_height is not None and (dot(A, p_cur) > b).any():
self.sample_handles.append(
draw_point([p_cur[0], p_cur[1], draw_height],
color='r',
pointsize=5e-3))
continue
elif draw_height is not None:
self.sample_handles.append(
draw_point([p_cur[0], p_cur[1], draw_height],
color='g',
pointsize=5e-3))
vertices = pypoman.compute_polytope_vertices(A, b)
if draw_height is not None:
self.polygons.append(
draw_horizontal_polygon(vertices,
draw_height,
combined='b-#'))
self.all_vertices.extend(vertices)
return self.all_vertices
def draw(self, color='b', pointsize=0.005):
handles = []
if self.is_empty:
return handles
com, comd = self.P[0], self.V[0]
handles.append(draw_point(com, color, pointsize))
for k in xrange(self.nb_steps):
com_prev = com
dT = self.get_dT(k)
omega2 = self.get_omega2(k)
zmp = self.get_zmp(k)
com, comd = integrate_fip(com, comd, zmp, dT, omega2)
handles.append(draw_point(com, color, pointsize))
handles.append(draw_line(com_prev, com, color, linewidth=3))
com_prev = com
com = com + comd / sqrt(omega2) # stationary value at CP
handles.append(draw_point(com, color, pointsize))
handles.append(draw_line(com_prev, com, color, linewidth=3))
return handles
def draw_primal(self, tube):
self.poly_handles = []
colors = [(0.5, 0.5, 0., 0.3), (0., 0.5, 0.5, 0.3)]
if tube.start_stance.label.startswith('SS'):
colors.reverse()
for (i, vertices) in enumerate(tube.primal_vrep):
color = colors[i]
if len(vertices) == 1:
self.poly_handles.append(
draw_point(vertices[0], color=color, pointsize=0.01))
else:
self.poly_handles.extend(
draw_polytope(vertices, '*.-#', color=color))
def on_tick(self, sim):
"""
Entry point called at each simulation tick.
Parameters
----------
sim : Simulation
Instance of the current simulation.
"""
if preview_buffer.is_empty:
return
com_pre, comd_pre = com_target.p, com_target.pd
com_free, comd_free = com_target.p, com_target.pd
self.handles = []
self.handles.append(
draw_point(com_target.p, color='m', pointsize=0.007))
for preview_index in range(preview_buffer.nb_steps):
com_pre0 = com_pre
j = 3 * preview_index
comdd = preview_buffer._U[j:j + 3]
dT = preview_buffer._dT[preview_index]
com_pre = com_pre + comd_pre * dT + comdd * .5 * dT ** 2
comd_pre += comdd * dT
color = \
'b' if preview_index <= preview_buffer.switch_step \
else 'y'
self.handles.append(
draw_point(com_pre, color=color, pointsize=0.005))
self.handles.append(
draw_line(com_pre0, com_pre, color=color, linewidth=3))
if self.draw_free_traj:
com_free0 = com_free
com_free = com_free + comd_free * dT
self.handles.append(
draw_point(com_free, color='g', pointsize=0.005))
self.handles.append(
draw_line(com_free0, com_free, color='g', linewidth=3))
def draw(self):
"""
Draw the interpolated foot path.
Returns
-------
handle : openravepy.GraphHandle
OpenRAVE graphical handle. Must be stored in some variable,
otherwise the drawn object will vanish instantly.
"""
foot_path = self.retimed_traj.path
if foot_path is None:
return []
ds = self.discrtimestep
handles = [draw_point(foot_path.Eval(0), color='m', pointsize=0.007)]
for s in arange(ds, foot_path.duration + ds, ds):
handles.append(
draw_point(foot_path.Eval(s), color='b', pointsize=0.01))
handles.append(
draw_line(foot_path.Eval(s - ds),
foot_path.Eval(s),
color='b',
linewidth=3))
return handles
def on_tick(self, sim):
from pymanoid.gui import draw_point
self.handles = [
draw_point(stab.dcm, color=stab.pendulum.color, pointsize=0.01)
for stab in self.stabilizers
if stab.pendulum.is_visible]
