Пример #1
0
def draw_world(cart, ball, rod, goal, info=''):
    """Draw a snapshot of the world"""
    f = Frame.Frame()
    x, y, z = cart.getPosition()
    cart_shape = Shapes.rotated_rectangle((x, y), 0, cart_width, cart_height)
    f.add_polygons([cart_shape], color="green", alpha=0.5)

    x, y, z = rod.getPosition()
    angle = get_rotation_around_z(rod)
    rod_shape = Shapes.rotated_rectangle((x, y), angle, 0.03, 1)
    f.add_polygons([rod_shape], color="gray50")

    x, y, z = ball.getPosition()
    f.add_circle((x, y), color='red3', alpha=0.8, radius=0.1)

    f.add_text(pos=(-2, 2),
               face='Serif 36',
               scale=0.003,
               markup=text_info,
               color=Color.Color("darkgreen"))

    # Draw start and goal
    f.add_circle((goal, 0), color='blue', alpha=0.5, radius=0.05)

    viewer.add_frame(f)
    time.sleep(0.01)
def show_frame(v, p, robot):
    """Show the current probability and the current robot"""
    f = Frame.Frame()
    for l in landmarks:
        f.add_circle(pos=(l[1], l[0]), color=Color.Color("blue"), radius=3)
    for r in p:
        poly = Shapes.arrow_head_polygon((r.x, r.y), r.orientation, scale=0.5)
        f.add_polygons([poly], color=Color.Color("red"), alpha=0.2)
    poly = Shapes.arrow_head_polygon((robot.x, robot.y),
                                     robot.orientation,
                                     scale=0.5)
    f.add_polygons([poly], color=Color.Color("black"), alpha=0.9)
    v.add_frame(f)
Пример #3
0
  def draw_robot(self, draw_path=True, text_info=''):
    """Create a new frame and draw a representation of the robot in it"""
    x,y,phi = self.state.get_pose()
    if self.viewer:
      f = Frame.Frame()

      if draw_path:
        f.add_lines(color='black',
                    alpha=0.5,
                    lines=[self.path[:]],
                    linewidth=0.02)
      
      # Draw start and goal
      f.add_circle(self.init,
                   color='green',
                   alpha=0.5,
                   radius=0.05)
      for g in self.goals:
        f.add_circle(g,
                     color='blue',
                     alpha=0.5,
                     radius=0.05)

      # Our robot representation is built of an arrow head
      # with two wheels.
      poly = Shapes.arrow_head_polygon((x, y),
                                       phi,
                                       scale=0.02)
      f.add_polygons([poly],
                     color="red",
                     alpha=0.5)
      wheels = Shapes.rectangle_pair((x,y),
                                     5.,2.,7.,
                                     angle=phi,
                                     scale=0.02)
      f.add_polygons(wheels,
                     color="black",
                     alpha=0.5)

      # Add some anotation
      f.add_text(pos=(-0.5,-0.5), size=0.1, text="Time=%.2fs"%self.time,color="darkgreen")
      dy = -0.6
      for i,s in enumerate(text_info.split('\n')):
        f.add_text(pos=(-0.5,dy-0.1*i), size=0.07,
                   text=s,color="darkgreen")

      self.viewer.add_frame(f)
      time.sleep(0.01)
Пример #4
0
  def draw_robot(self, draw_path=True, text_info=''):
    """Create a new frame and draw a representation of the robot in it"""
    x,y,phi = self.state.get_pose()
    phi_d = self.get_dir_to_goal()
    if self.viewer:
      f = Frame.Frame()
      if draw_path:
        f.add_lines(color=Color.Color("black"),
                    lines=[self.path[:]],
                    linewidth=0.01)
      
      # Draw start and goal
      for p,color in ( (self.init, "green"),
                       (self.goal, "blue") ):
        if p:
          f.add_circle(p,
                       color=color,
                       alpha=0.5,
                       radius=0.05)

      # Our robot representation is built of an arrow head
      # with two wheels.
      poly = Shapes.arrow_head_polygon((x, y),
                                       phi,
                                       scale=0.02)
      f.add_polygons([poly],
                     color="red",
                     alpha=0.5)
      wheels = Shapes.rectangle_pair((x,y),
                                     5.,2.,7.,
                                     angle=phi,
                                     scale=0.02)
      f.add_polygons(wheels,
                     color="black",
                     alpha=0.5)

      # Add some anotation
      f.add_text(pos=(-0.5,-0.5), size=0.1, text="Time=%.2fs"%self.time,color="darkgreen")
      dy = -0.6
      for i,s in enumerate(text_info.split('\n')):
        f.add_text(pos=(-0.5,dy-0.1*i), size=0.07,
                   text=s,color="darkgreen")

      self.viewer.add_frame(f)
      time.sleep(0.01)
Пример #5
0
  def draw_robot(self, draw_path=True):
    """Create a new frame and draw a representation of the robot in it"""
    if self.viewer:
      f = Frame.Frame()
      if draw_path:
        f.add_lines(color=Color.Color("black"),
                    lines=[self.path[:]],
                    linewidth=0.01)
      
      # Draw start and goal
      for p,color in ( (self.init, "green"),
                       (self.goal, "blue") ):
        if p:
          f.add_circle(p,
                       color=color,
                       alpha=0.5,
                       radius=0.05)

      # Our robot representation is built of an arrow head
      # with two wheels.
      poly = Shapes.arrow_head_polygon((self.x, self.y),
                                       self.phi,
                                       scale=0.02)
      f.add_polygons([poly],
                     color=Color.Color("red"),
                     alpha=0.5)
      wheels = Shapes.rectangle_pair((self.x,self.y),
                                     5.,2.,7.,
                                     angle=self.phi,
                                     scale=0.02)
      f.add_polygons(wheels,
                     color=Color.Color("black"),
                     alpha=0.5)

      # Add some anotation
      f.add_text(pos=(-0.5,-0.5),
                 size=0.1,
                 text="Time=%.2fs"%(self.time),
                 color=Color.Color("darkgreen"))

      self.viewer.add_frame(f)
      time.sleep(0.01)
Пример #6
0
def draw_world(cart,ball,rod, goal, info=''):
  """Draw a snapshot of the world"""
  f = Frame.Frame()
  x,y,z = cart.getPosition()
  cart_shape = Shapes.rotated_rectangle( (x,y),
                                         0,
                                         cart_width,
                                         cart_height)
  f.add_polygons([cart_shape],
                 color="green",
                 alpha=0.5)

  x,y,z = rod.getPosition()
  angle = get_rotation_around_z(rod)
  rod_shape = Shapes.rotated_rectangle( (x,y),
                                        angle,
                                        0.03,
                                        1)
  f.add_polygons([rod_shape],
                 color="gray50")
  
  x,y,z = ball.getPosition()
  f.add_circle((x,y),
               color='red3',
               alpha=0.8,
               radius=0.1)

  f.add_text(pos=(-2,2),
             face='Serif 36',
             scale=0.003,
             markup=text_info,
             color=Color.Color("darkgreen"))

  # Draw start and goal
  f.add_circle((goal,0),
               color='blue',
               alpha=0.5,
               radius=0.05)

  viewer.add_frame(f)
  time.sleep(0.01)
def show_frame(v,p,robot):
  """Show the current probability and the current robot"""
  f = Frame.Frame()
  for l in landmarks:
    f.add_circle(pos=(l[1],l[0]),
                 color=Color.Color("blue"),
                 radius=3)
  for r in p:
    poly = Shapes.arrow_head_polygon((r.x, r.y),
                                     r.orientation,
                                     scale=0.5)
    f.add_polygons([poly],
                   color=Color.Color("red"),
                   alpha=0.2)
  poly = Shapes.arrow_head_polygon((robot.x, robot.y),
                                   robot.orientation,
                                   scale=0.5
                                   )
  f.add_polygons([poly],
                 color=Color.Color("black"),
                 alpha=0.9)
  v.add_frame(f)
Пример #8
0
    def draw_robot(self, draw_path=True):
        """Create a new frame and draw a representation of the robot in it"""
        if self.viewer:
            f = Frame.Frame()
            if draw_path:
                f.add_lines(color=Color.Color("black"),
                            lines=[self.path[:]],
                            linewidth=0.01)

            # Draw start and goal
            for p, color in ((self.init, "green"), (self.goal, "blue")):
                if p:
                    f.add_circle(p, color=color, alpha=0.5, radius=0.05)

            # Our robot representation is built of an arrow head
            # with two wheels.
            poly = Shapes.arrow_head_polygon((self.x, self.y),
                                             self.phi,
                                             scale=0.02)
            f.add_polygons([poly], color=Color.Color("red"), alpha=0.5)
            wheels = Shapes.rectangle_pair((self.x, self.y),
                                           5.,
                                           2.,
                                           7.,
                                           angle=self.phi,
                                           scale=0.02)
            f.add_polygons(wheels, color=Color.Color("black"), alpha=0.5)

            # Add some anotation
            f.add_text(pos=(-0.5, -0.5),
                       size=0.1,
                       text="Time=%.2fs" % (self.time),
                       color=Color.Color("darkgreen"))

            self.viewer.add_frame(f)
            time.sleep(0.01)
Пример #9
0
    # Show text, the rod, and the ball
    f = Frame.Frame()
    f.add_text(pos=(-0.9, 0.5),
               face='Serif 18',
               scale=0.003,
               markup=(u"Time=%.2fs\n"
                       u"φ=%.2f\n"
                       u"ω=%.2f\n"
                       u"τ=%.2f\n"
                       u"K<sub>p</sub>=%.2f\n"
                       u"K<sub>d</sub>=%.2f\n") %
               (total_time, angle, angle_rate_of_change, torque, Kp, Kd),
               color=Color.Color("darkgreen"))

    x, y, z = rod.getPosition()
    rod_shape = Shapes.rotated_rectangle((x, y), angle, 0.03, 1)
    f.add_polygons([rod_shape], color="gray50")

    x, y, z = ball.getPosition()
    f.add_circle((x, y), color='red3', alpha=0.8, radius=0.1)

    viewer.add_frame(f)
    time.sleep(0.01)

    # Step the world
    world.step(dt)
    total_time += dt

viewer.wait()
Пример #10
0
import Euv.Color as Color
import Euv.Shapes as Shapes
import time
import math

N = 200
v = Euv.Viewer(
    size=(800, 600), view_port_center=(0, 0), view_port_width=800, max_num_frames=N, recording=True, flip_y=True
)
for i in range(N):
    f = Frame.Frame()
    x = 400 * 1.0 * (i - N / 2) / N
    n = 40
    for j in range(n):
        f.add_circle(pos=(x + j * 5, 20 * math.sin(2 * math.pi * (i + j) / n)), color=Color.Color("red"), radius=3)
    f.add_lines(color=Color.Color("midnightblue"), lines=[[(x + 5, 105), (x + 35, 105)]])
    f.add_polygons(
        color=Color.Color("magenta3"),
        polygons=[[(x, 100), (x + 10, 100), (x + 5, 110)], [(x + 30, 100), (x + 40, 100), (x + 35, 110)]],
    )
    arw = Shapes.arrow_head_polygon((x, -25), scale=10)
    rect = Shapes.rotated_rectangle((x, 25), width=50, height=10, angle=1.0 * i / N * math.pi * 2)
    f.add_polygons(polygons=[arw, rect], color="cyan", alpha=0.5)
    f.add_text(pos=(250, 200), face="Sans", size=20, text="Frame %d" % i, color=Color.Color("darkgreen"))
    if v.user_break():
        break
    v.add_frame(f)
    time.sleep(1.0 / 40)

v.wait()
Пример #11
0
    # Here is the control feedback
    world_joint.addTorque(torque)

    # Show text, the rod, and the ball
    f = Frame.Frame()
    f.add_text(
        pos=(-0.9, 0.5),
        face="Serif 18",
        scale=0.003,
        markup=("<b>%s</b>\n" u"Time=%.2fs\n" u"φ=%.2f\n" u"ω=%.2f\n" u"τ=%.2f\n" "energy=%.2f\n" "%s\n")
        % (state_info, total_time, angle, angle_rate_of_change, torque, energy, message),
        color=Color.Color("darkgreen"),
    )

    x, y, z = rod.getPosition()
    rod_shape = Shapes.rotated_rectangle((x, y), angle, 0.03, 1)
    f.add_polygons([rod_shape], color="gray50")

    x, y, z = ball.getPosition()
    f.add_circle((x, y), color="red3", alpha=0.8, radius=0.1)

    viewer.add_frame(f)
    time.sleep(0.01)

    # Step the world
    world.step(dt)
    total_time += dt

viewer.wait()
Пример #12
0
 n = 40
 for j in range(n):
   f.add_circle(pos=(x+j*5,20*math.sin(2*math.pi*(i+j)/n)),
                color=Color.Color("red"),
                radius=3)
 f.add_lines(color=Color.Color("midnightblue"),
             lines=[[(x+5,105),(x+35,105)]])
 f.add_polygons(color=Color.Color("magenta3"),
                polygons = [[(x,100),
                             (x+10,100),
                             (x+5,110)],
                            [(x+30,100),
                             (x+40,100),
                             (x+35,110)],
                            ])
 arw = Shapes.arrow_head_polygon((x,-25),
                                 scale=10)
 rect = Shapes.rotated_rectangle((x,-150),
                                 width=50,
                                 height=10,
                                 angle=-1.0*i/N * math.pi*2,
                                 )
 wheels = Shapes.rectangle_pair((x,-25),
                                5.,2.,7.,
                                left_angle = 1.0*i/N * math.pi*2,
                                scale=10)
 f.add_polygons(polygons = [arw,
                            rect],
                color="cyan",
                alpha=0.5)
 f.add_polygons(polygons = wheels,
                color="black",