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, 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 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 draw_comdd(self):
comdd = self.acc_scale * preview_buffer.cur_control + self.trans
self.comdd_handle = [
draw_line(self.trans, comdd, color='r', linewidth=3),
draw_points([self.trans, comdd], color='r', pointsize=0.005)]