Update: Robot-Control-System


Das Robot-Control-System hat kleinere Verbesserungen erhalten. Zum einen gibt es jetzt einen zweiten Gripper und zum zweiten wurden mehr Motion Primitive verbaut. Wenn man das Programm startet drückt man auf die Taste 5 und sieht dann eine längere Animation ablaufen, einen sogenannten Physics Animation Controller. Dieser besteht aus mehreren Finite-States-Maschines, die als Behavior Tree ineinander verschachtelt sind und erstaunlich komplexe Aktionen ausführen. Für den Benutzer passiert wenig: es ist nur ein Pick&Place Task zu sehen, der zudem noch mehrere Pausen enthält.

Das zentrale Element ist die Behavior Engine, welche sich wie folgt gliedert:

main
  task1
     selectgrasp
     place
     move(0)
     selectgrasp
     place
     move(1)
     selectgrasp
     place

Die Motion Primitive „selectgrasp“, „place“ und „move“ sind noch weiter untergliedert bis am Ende dann Steueranweisungen für die Box2D Physik-Engine ausgeführt werden. Die Leistungsfähigkeit des Roboters bemisst sich nach den Lines of Code für die Behavior Engine, je umfangreicher sie ist, desto mehr kann der Roboter machen. Das Prinzip ist in der Robotik nichts neues: Marc Raibert hat damit experimentiert, die NaturalMotion Engine funktioniert so, Simbicon basiert darauf und bei der Darpa Robotics Challange 2015 wurden die Controller ebenfalls nach diesem Prinzip realisiert. Neu ist jedoch, dass diesmal Python als Programmiersprache verwendet wurde. Damit ist die Verständlichkeit hoffentlich höher, als bei ROS und vergleichbaren Projekten.

'''
Created on 16.02.2017
gantry robot
@author: Manuel Rodriguez
'''
import time, sys, pygame, random, math, Box2D, os, numpy, threading
import logging
from Box2D.b2 import (world, polygonShape, staticBody, dynamicBody)
from Box2D import (b2CircleShape, b2FixtureDef, b2LoopShape, b2PolygonShape,
                   b2RevoluteJointDef, b2_pi)
from locale import currency
from __builtin__ import True
logging.basicConfig(
  stream=sys.stdout,
  level=logging.DEBUG,
  format='%(message)s'
)
log = logging.getLogger(__name__)

class Settings(object):
  def __init__(self):
    self.screen = (600, 500)
    self.ppm = 32.0  # pixels per meter
    self.fric = 1.0  # 0.3 friction
    self.torque = 1000
    self.fps = 55  # 60 fps
    self.pause = 100000 / self.fps
    self.black = (0, 0, 0)
    self.grey = (150, 150, 150)
    self.red = (230, 0, 0)
    self.blue = (0, 0, 230)
  def box2d_to_pygame_pos(self, box2dpos):
    """ convert coordinates """
    x = int(box2dpos[0] * self.ppm)
    y = int(self.screen[1] - box2dpos[1] * self.ppm)
    return (x, y)
  def calcdistance(self, p1, p2):
    """ Euclidean ordinary distance """
    return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)
  def angle_between_two_points(self, p1, p2):
    angle = math.degrees(math.atan2(p2[1] - p1[1], p2[0] - p1[0]))
    angle += 90
    if angle < 0: angle += 360
    return angle
  def polarpoint(self, p1, angle, radius):
    """ polar coordinates for a point on circle """
    angle = (angle - 90) * math.pi / 180
    x = p1[0] + radius * math.cos(angle)
    y = p1[1] + radius * math.sin(angle) 
    x,y=self.floattoint((x,y))
    return (x, y)
  def floattoint(self, p):
    return (int(p[0]), int(p[1]))
  def incircle(self,p1,radius,p2):
    """ checks if p2 is in circle """
    square_dist = (p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2
    return square_dist <= radius ** 2
  def getpixel(self, p):
    surf3d = pygame.surfarray.pixels3d(self.window)
    temp=surf3d[p[0]][p[1]]
    del surf3d # prevent blocking
    return temp

class Physics(Settings):
  def __init__(self):
    super(Physics, self).__init__()
    self.myworld = world(gravity=(0, -10), doSleep=True)  # 0,-10
    self.groundbody = []
    self.body = []
    self.groundbody.append(self.myworld.CreateStaticBody(position=(15, 1),
        shapes=polygonShape(box=(20, 0.05)), angle=-0))  # ground
    self.groundbody.append(self.myworld.CreateStaticBody(position=(0.5, 10),
        shapes=polygonShape(box=(0.05, 10)),))  # left
    self.groundbody.append(self.myworld.CreateStaticBody(position=(18, 7),
        shapes=polygonShape(box=(0.05, 10)),))  # right
    self.groundbody.append(self.myworld.CreateStaticBody(position=(15, 13),
        shapes=polygonShape(box=(20, 0.05)),))  # top

  def getpos(self, id):
    box2dpos = self.body[id].position
    return self.box2d_to_pygame_pos(box2dpos)
  def getangle(self, id):
    angle = self.body[id].angle
    angle = (math.degrees(-angle) + 90) % 360
    return angle       
  def setspeed(self, id, speed):
    self.joint[id].motorSpeed = speed 
  def movelinear(self, id, p):
    self.body[id].linearVelocity = p
  def moveang(self, id, angle):
    self.body[id].angularVelocity = angle
  def updatePhysics(self):
    timestep = 1.0 * 3 / self.fps
    self.adjustgripper()
    self.myworld.Step(timestep, 1000, 10)

class Gripper(Physics):
  def __init__(self):
    super(Gripper, self).__init__()
    self.gripper1 = (0, 0)
    self.gripper2 = (550, 100)
    self.gripperangle1=180
    self.joint = []
    startid = len(self.body)
    log.info('add id single1 ' + str(startid))
    self.body.append(self.myworld.CreateDynamicBody(position=(10, 15), angle=0))
    self.body[-1].CreatePolygonFixture(box=(1, 0.5), density=1, friction=self.fric)
    self.body.append(self.myworld.CreateDynamicBody(position=(10, 10), angle=0))
    self.body[-1].CreatePolygonFixture(box=(0.5, 0.5), density=1, friction=self.fric)
    self.body.append(self.myworld.CreateDynamicBody(position=(9, 5), angle=0))
    self.body[-1].CreatePolygonFixture(box=(0.3, 1), density=1, friction=self.fric)
    self.body.append(self.myworld.CreateDynamicBody(position=(10, 5), angle=0))
    self.body[-1].CreatePolygonFixture(box=(0.3, 1), density=1, friction=self.fric)
    
    self.body.append(self.myworld.CreateDynamicBody(position=(9, 4), angle=0))
    self.body[-1].CreatePolygonFixture(box=(1, 1), density=1, friction=self.fric)

    self.joint.append(self.myworld.CreatePrismaticJoint(# x
        bodyA=self.body[startid],
        bodyB=self.groundbody[3],
        anchor=(0, 0),
        axis=(1, 0),
        lowerTranslation=-6.0,
        upperTranslation=6,
        enableLimit=False,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
        enableMotor=True,
    ))     
    self.joint.append(self.myworld.CreatePrismaticJoint(# up/down
        bodyA=self.body[startid+1],
        bodyB=self.body[startid],
        anchor=(0, 0),
        axis=(0, 1),
        lowerTranslation=-15.0,
        upperTranslation=15,
        enableLimit=False,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
        enableMotor=True,
    ))   
    self.joint.append(self.myworld.CreateRevoluteJoint(# rotate
        bodyA=self.body[startid+1],
        bodyB=self.body[startid+2],
        localAnchorA=(0, 0),
        localAnchorB=(0, 1),
        enableMotor=True,
        enableLimit=False,
        lowerAngle=math.radians(60),
        upperAngle=math.radians(120),
        maxMotorTorque=self.torque,
        motorSpeed=0,
    ))
    
    self.joint.append(self.myworld.CreatePrismaticJoint(# gripper openclose
        bodyA=self.body[startid+2],
        bodyB=self.body[startid + 3],
        localAnchorA=(0, 0),
        localAnchorB=(0, 0),
        axis=(1, 0),
        lowerTranslation=1,
        upperTranslation=3.4,
        enableLimit=True,
        maxMotorForce=self.torque,
        motorSpeed=1.0,
        enableMotor=True,
    ))    

    startid = len(self.body)
    log.info('add id single2 ' + str(startid))
    self.body.append(self.myworld.CreateDynamicBody(position=(10, 13.5), angle=0))
    self.body[-1].CreatePolygonFixture(box=(1, 0.5), density=1, friction=self.fric)
    self.body.append(self.myworld.CreateDynamicBody(position=(10, 10), angle=0))
    self.body[-1].CreatePolygonFixture(box=(0.4, 0.4), density=1, friction=self.fric)
    self.joint.append(self.myworld.CreatePrismaticJoint(# x
        bodyA=self.body[startid],
        bodyB=self.groundbody[3],
        anchor=(0, 0),
        axis=(1, 0),
        lowerTranslation=-6.0,
        upperTranslation=6,
        enableLimit=False,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
        enableMotor=True,
    ))     
    self.joint.append(self.myworld.CreatePrismaticJoint(# up/down
        bodyA=self.body[startid+1],
        bodyB=self.body[startid],
        anchor=(0, 0),
        axis=(0, 1),
        lowerTranslation=-15.0,
        upperTranslation=15,
        enableLimit=False,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
        enableMotor=True,
    ))   
    
  def adjustgripper(self):
    # position
    pos = self.gripper1
    p1 = self.getpos(1)
    dx = pos[0] - p1[0]
    dy = pos[1] - p1[1]
    dx = 2.0 * -dx /100  
    dy = 2.0 * dy / 100  
    self.setspeed(0, dx)
    self.setspeed(1, dy)
    # angle
    a1 = (self.getangle(2) + 90) % 360
    a2 = self.gripperangle1
    diff = a2 - a1
    da=-1.0*diff/100
    self.setspeed(2, da)

    # position gripper2
    pos = self.gripper2
    p1 = self.getpos(6)
    dx = pos[0] - p1[0]
    dy = pos[1] - p1[1]
    dx = 2.0 * -dx /100  
    dy = 2.0 * dy / 100  
    self.setspeed(4, dx)
    self.setspeed(5, dy)

class BehaviorTree(Gripper):
  def __init__(self):
    super(BehaviorTree, self).__init__()
    self.stopflag = False
  def BTstart(self):
    self.stopflag = False
    self.mythread = threading.Thread(target=self.taskmain)
    self.mythread.daemon = True  # stops, if mainprogram ends
    self.mythread.start()
  def BTstop(self):
    log.info('stop request')
    self.stopflag = True
  def taskmain(self):
    self.input = "auto"
    log.info('start thread')
    self.task1()
    self.input = "manuel 1" 
    log.info('end thread')
  def task1(self):
    self.taskselectgrasp()
    self.taskplace()
    self.taskmove(0)
    self.taskselectgrasp()
    self.taskplace()
    self.taskmove(1)
    self.taskselectgrasp()
    self.taskplace()
  def taskselectgrasp(self):
    type = self.grasptype()
    if type=="grasp": 
      self.taskgrasp()
      self.taskpostgrasp()
    if type=="tiltleft" or type=="tiltright":
      self.tasktilt(type)
      self.taskgrasp()
      self.taskpostgrasp()
  def taskpostgrasp(self):
    log.info('postgrasp')  
    # single rest
    p = self.getpos(6)
    self.taskmovexy(1, (p[0], 0))
    self.taskpause()
    self.taskmovexy(1, (550, 100))
    self.taskpause()
    # move up
    p = self.getpoint("grasp")
    self.taskmovexy(0, p[0])
    self.taskpause()
    self.taskrotate(180)
    self.taskpause()
  def taskplace(self):
    log.info('place ')
    p1=(250,200) # middle screen
    self.taskmovexy(0, p1)
    self.taskpause()
    self.taskopengripper()
    self.taskpause()
    p2=(0,0) # rest position
    self.taskmovexy(0, p2)
    self.taskpause()
  def taskmove(self,id):
    if id==0: 
      p1=(150,300)
      p2=(150,430)
      p3=(600,430)
      p4=(400,430)
    if id==1: 
      p1=(450,300)
      p2=(450,430)
      p3=(0,430)
      p4=(200,430)
    self.taskmovexy(1, p1)
    self.taskpause()
    self.taskmovexy(1, p2)
    self.taskpause()
    self.taskmovexy(1, p3)
    self.taskpause()
    self.taskmovexy(1, p4)
    self.taskpause()
    self.taskmovexy(1, (550, 100)) # rest
    self.taskpause()
  def taskgrasp(self):
    log.info('grasp ')
    self.taskrotate(180+self.angleobject())
    self.taskpause()
    p1,p2 = self.getpoint("grasp")[0],self.getpoint("grasp")[3]
    self.taskmovexy(0, p1)
    self.taskpause()
    self.taskmovexy(0, p2)
    self.taskpause()
    # close
    self.taskclosegripper()
    self.taskpause()
  def tasktilt(self,direction):
    log.info('tilt '+str(direction))
    # above
    p = self.getpoint(direction)
    self.taskmovexy(1, p[0])
    self.taskpause()
    # down
    self.taskmovexy(1, p[1])
    self.taskpause()
    # move
    self.taskmovexy(1, p[2])
    self.taskpause()

class Motionprimitive(BehaviorTree):
  def taskpause(self):
    pygame.time.wait(self.pause)
  def taskmovexy(self, id, p):
    log.info('-- movexy ' + str(id) + " " + str(p)) 
    if id == 0: self.gripper1 = p
    if id == 1: self.gripper2 = p 
  def taskopengripper(self):  
    log.info('-- opengripper')
    self.setspeed(3, 1)
  def taskclosegripper(self):
    log.info('-- closegripper')
    self.setspeed(3, -0.5)    
  def taskrotate(self, angle): 
    log.info('-- rotate '+str(angle))
    self.gripperangle1 = angle
        
class Cube(Motionprimitive):
  def __init__(self):
    super(Cube, self).__init__()
    self.objectid=4
  def angleobject(self):
    topangle = []
    for i in range(4):
      temp = (self.getangle(self.objectid) + i * 90) % 360  # angle for all directions
      # only top angles
      if temp < 90: diff = temp
      elif temp > 270: diff = temp - 360
      else: diff = 360
      topangle.append(diff)
    temp = sorted(topangle, key=abs)
    return temp[0]     
  def getpoint(self, action):
    p1 = self.getpos(self.objectid)
    angle = self.angleobject()
    pygame.draw.circle(self.window, self.red, self.floattoint(p1), 5, 2)
    if action == "grasp": 
      p2 = self.polarpoint(p1, angle - 90, 52)  # point left from center
      p3 = self.polarpoint(p2, angle, 100)
      p4 = self.polarpoint(p1, angle + 90, 52)  # point right from center
      p5 = self.polarpoint(p2, angle, 55) 
      pygame.draw.circle(self.window, self.red, self.floattoint(p2), 10, 2)
      pygame.draw.circle(self.window, self.red, self.floattoint(p3), 10, 2)
      pygame.draw.circle(self.window, self.red, self.floattoint(p4), 5, 2)
      pygame.draw.circle(self.window, self.red, self.floattoint(p5), 10, 2)
      return (p3, p2, p4,p5)
    if action == "tiltleft" or action == "tiltright":
      if action == "tiltleft": direction=-1
      if action == "tiltright": direction=1
      p2 = self.polarpoint(p1, angle + direction*90, 48)  # point left from center
      p7 = self.polarpoint(p1, angle + direction* 90, 10) 
      p8 = self.polarpoint(p7, angle, 45)
      p9 = self.polarpoint(p1, angle, 100)  # above center
      pygame.draw.circle(self.window, self.red, self.floattoint(p2), 10, 2)
      pygame.draw.circle(self.window, self.red, self.floattoint(p7), 5, 2)
      pygame.draw.circle(self.window, self.red, self.floattoint(p8), 10, 2)
      pygame.draw.circle(self.window, self.red, self.floattoint(p9), 5, 2)
      return (p9, p8, p2)
  def grasptype(self):
    log.info('check grasptype')
    arealeftfree, arearightfree, leftedge, rightedge = False, False, False, False
    # check grasp
    p = self.getpoint("grasp")
    colorleft, colorright = self.getpixel(p[1]), self.getpixel(p[2])
    #print p[1], colorleft
    if colorleft[0] == 220: arealeftfree = True
    if colorright[0] == 220: arearightfree = True
    # check border
    p = self.getpos(self.objectid)[0]
    if p < 100: leftedge = True
    if p > 510: rightedge = True
    # decision table
    type = None
    if leftedge == False and rightedge==False and arealeftfree == True and arearightfree == True: type= "grasp"
    elif arealeftfree == True and leftedge == False: type = "tiltleft"
    elif arearightfree == True and rightedge == False: type = "tiltright"
    return type
              
class GUI(Cube):
  def __init__(self):
    super(GUI, self).__init__()
    pygame.init()
    self.window = pygame.display.set_mode(self.screen)
    self.clock = pygame.time.Clock()
    self.mouse = (0, 0)
    self.input = "manuel 1"

  def paintobjects(self):
    # Text1
    text = str(self.input)
    text2 = " " + str(self.mouse)
    pygame.display.set_caption(text+text2)
    # box2d
    colors = { staticBody: self.grey,
        dynamicBody: self.blue,
    }
    for body in (self.myworld.bodies):  
      for fixture in body.fixtures:
        shape = fixture.shape
        if hasattr(shape, 'vertices'):  # rectangle
          vertices = [(body.transform * v) * self.ppm for v in shape.vertices]
          vertices = [((v[0]), (self.screen[1] - v[1])) for v in vertices]
          pygame.draw.polygon(self.window, colors[body.type], vertices, 0)
        else:  # ball
          position = body.transform * shape.pos * self.ppm
          x, y = int(position[0]), int(self.screen[1] - position[1])
          radius = int(shape.radius * self.ppm)
          pygame.draw.circle(self.window, colors[body.type], (x, y), radius, 2)
          x2, y2 = x + radius * math.sin(body.angle), y + radius * math.cos(body.angle)
          pygame.draw.line(self.window, colors[body.type], (x, y), (x2, y2), 2)  # top
  def inputhandling(self):
    for event in pygame.event.get(): 
      if event.type == pygame.QUIT: 
        sys.exit(0) 
      if event.type == pygame.MOUSEMOTION:
        self.mouse = event.pos
        if self.input == "manuel 1":
          self.gripper1 = self.mouse
        if self.input == "manuel 2":
          self.gripper2 = self.mouse
        if self.input == "manuel 3":
          self.gripper3 = self.mouse
      if event.type == pygame.MOUSEBUTTONUP:
        pass
      if event.type == pygame.KEYDOWN:
        if event.key == pygame.K_1: self.taskrotate(self.gripperangle1-22.5)
        if event.key == pygame.K_2: self.taskrotate(self.gripperangle1+22.5)
        if event.key == pygame.K_3: self.taskopengripper()
        if event.key == pygame.K_4: self.taskclosegripper()
        if event.key == pygame.K_5: self.BTstart()
        if event.key == pygame.K_6: self.BTstop()
        if event.key == pygame.K_7:
          if self.input == "manuel 1": self.input="manuel 2"
          elif self.input == "manuel 2": self.input="manuel 1"

  def updateGUI(self):
    self.clock.tick(self.fps)  
    self.window.fill((220, 220, 220))
    self.inputhandling()
    self.paintobjects()

class Game: 
  def __init__(self):
    random.seed()
    self.myGUI = GUI()
    for step in range(10000000):
      self.myGUI.updateGUI()
      self.myGUI.updatePhysics()
      self.myGUI.getpoint("grasp")
      pygame.display.update()  
      
 
if __name__ == "__main__":
  myGame = Game()
  
Advertisements

Kommentar verfassen

Trage deine Daten unten ein oder klicke ein Icon um dich einzuloggen:

WordPress.com-Logo

Du kommentierst mit Deinem WordPress.com-Konto. Abmelden / Ändern )

Twitter-Bild

Du kommentierst mit Deinem Twitter-Konto. Abmelden / Ändern )

Facebook-Foto

Du kommentierst mit Deinem Facebook-Konto. Abmelden / Ändern )

Google+ Foto

Du kommentierst mit Deinem Google+-Konto. Abmelden / Ändern )

Verbinde mit %s