Example #1
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
Example #2
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
Example #3
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
    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))
Example #5
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
 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)]