def on_tick(self, sim):
     if preview_buffer.preview is None:
         return
     com_pre, comd_pre = com_target.p, com_target.pd
     com_free, comd_free = com_target.p, com_target.pd
     dT = preview_buffer.preview.timestep
     self.handles = []
     self.handles.append(
         draw_point(com_target.p, color='m', pointsize=0.007))
     for preview_index in xrange(len(preview_buffer.preview.U) / 3):
         com_pre0 = com_pre
         j = 3 * preview_index
         comdd = preview_buffer.preview.U[j:j + 3]
         com_pre = com_pre + comd_pre * dT + comdd * .5 * dT ** 2
         comd_pre += comdd * dT
         color = \
             'b' if preview_index <= preview_buffer.preview.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))
Beispiel #2
0
 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
Beispiel #3
0
 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
Beispiel #4
0
 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
Beispiel #5
0
 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_polyhedron(vertices, '*.-#', color=color))
 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_polyhedron(vertices, '*.-#', color=color))
Beispiel #7
0
    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
Beispiel #8
0
    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 xrange(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))
Beispiel #9
0
 def draw(self, step=0.02):
     self.handles['leg'] = draw_line(self.com.p,
                                     self.contact.p + self.cop,
                                     linewidth=4,
                                     color='g')
     self.handles['target'] = draw_line(self.contact.p, self.target, 'b')
     self.handles['target_p'] = draw_point(self.target, 'b', pointsize=0.01)
     if not __debug__ or not self.draw_parabola:
         return
     p, t = self.com.p, 0.
     parabola = []
     while p[2] > self.com.z - 1:
         p = self.com.p + self.com.pd * t + gravity * t**2 / 2
         parabola.append(p)
         t += step
     if not parabola:
         return
     self.handles['parabola'] = draw_trajectory(parabola, pointsize=0)