BehaviorTree in Python


bt

'''
Created on 17.01.2017
Behavior Tree
@author: Manuel Rodriguez

Game - GUI - Task - BehaviorTree - Sceneunderstanding - Physics - Physics_old
'''
import time, sys, pygame, random, math, Box2D, os
from Box2D.b2 import (world, polygonShape, staticBody, dynamicBody)
from Box2D import (b2CircleShape, b2FixtureDef, b2LoopShape, b2PolygonShape,
                   b2RevoluteJointDef, b2_pi)
from random import randint
from locale import currency
from __builtin__ import True
import numpy

SCREEN_WIDTH, SCREEN_HEIGHT = 700, 500
PPM = 32.0  # pixels per meter
FRIC = 1.0  # 0.3 friction
TORQUE = 1000
FPS = 50  # 60 fps

# ----------calculating------------------
def box2d_to_pygame_pos(box2dpos, ppm, screenheight):
  """ 
  PPM = pixel per meter
  screenheight = pygame screenheight
  """
  x = int(box2dpos[0] * ppm)
  y = int(screenheight - box2dpos[1] * ppm)
  return (x, y)
def calcdistance(p1, p2):
  """ Euclidean ordinary distance """
  return math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)

class Physics_old(object):
  def __init__(self):
    self.myworld = world(gravity=(0, -10), doSleep=True)  # 0,-10
    self.goalgripper=(500,200)
    self.way=[]
    self.groundbody = []
    self.body = []
    self.groundbody.append(self.myworld.CreateStaticBody(position=(10, 2),
        shapes=polygonShape(box=(10, 0.01)), angle=-0))  # ground
    self.groundbody.append(self.myworld.CreateStaticBody(position=(1, 10),
        shapes=polygonShape(box=(0.001, 10)),))  # left
    self.groundbody.append(self.myworld.CreateStaticBody(position=(20, 10),
        shapes=polygonShape(box=(0.001, 10)),))  # right
    self.groundbody.append(self.myworld.CreateStaticBody(position=(10, 15),
        shapes=polygonShape(box=(10, 0.001)),))  # top
    self.groundbody.append(self.myworld.CreateStaticBody(position=(12, 5),
        shapes=polygonShape(box=(0.001, 4)),))  # middle

    self.body.append(self.myworld.CreateDynamicBody(position=(17, 15), angle=0))
    self.body[-1].CreatePolygonFixture(box=(0.5, 0.5))
    self.body.append(self.myworld.CreateDynamicBody(position=(17, 4), angle=0))
    self.body[-1].CreatePolygonFixture(box=(0.2, 1), density=1, friction=FRIC)
    self.body.append(self.myworld.CreateDynamicBody(position=(17, 4), angle=0))
    self.body[-1].CreatePolygonFixture(box=(0.2, 1), density=1, friction=FRIC)

    self.body.append(self.myworld.CreateDynamicBody(position=(3, 15), angle=0))
    self.body[-1].CreatePolygonFixture(box=(1, 1), density=1, friction=FRIC)
    self.body.append(self.myworld.CreateDynamicBody(position=(5, 5), angle=0))
    self.body[-1].CreatePolygonFixture(box=(1, 1), density=1, friction=FRIC)
 
    self.joint = []
    '''
    self.joint.append(self.myworld.CreateRevoluteJoint(
        bodyA=self.body[0],
        bodyB=self.body[1],
        localAnchorA=(2, 0),
        localAnchorB=(2, 0),
        enableMotor=True,
        enableLimit=False,
        lowerAngle=math.radians(60),
        upperAngle=math.radians(120),
        maxMotorTorque=TORQUE,
        motorSpeed=0,
    ))
    '''
    self.joint.append(self.myworld.CreatePrismaticJoint( # x
        bodyA=self.body[0],
        bodyB=self.groundbody[3],
        anchor=(0, 0),
        axis=(1, 0),
        lowerTranslation=-5.0,
        upperTranslation=20,
        enableLimit=True,
        maxMotorForce=TORQUE,
        motorSpeed=0.0,
        enableMotor=True,
    ))
    self.joint.append(self.myworld.CreatePrismaticJoint(
        bodyA=self.body[1],
        bodyB=self.body[0],
        anchor=(0, 0),
        axis=(0, 1),
        lowerTranslation=-15.0,
        upperTranslation=15,
        enableLimit=True,
        maxMotorForce=TORQUE,
        motorSpeed=0.0,
        enableMotor=True,
    ))   
    self.joint.append(self.myworld.CreatePrismaticJoint( # gripper
        bodyA=self.body[2],
        bodyB=self.body[1],
        anchor=(0, 0),
        axis=(-1, 0),
        lowerTranslation=1,
        upperTranslation=2.8,
        enableLimit=True,
        maxMotorForce=TORQUE,
        motorSpeed=0.0,
        enableMotor=True,
    )) 
  def getpos(self, id):
    box2dpos = self.body[id].position
    return box2d_to_pygame_pos(box2dpos, PPM, SCREEN_HEIGHT)
  def getangle(self, id):
    angle = self.body[id].angle
    angle = (math.degrees(-angle) + 90) % 360
    return angle
  def setspeed(self, id, speed):
    # id=0: left, right
    # id=1, up, down
    # id=2, open,close
    self.joint[id].motorSpeed = speed
  def updatePhysic(self):
    self.adjustgripper()
    self.waypoints()
    timestep = 1.0 * 3.0 / 60  
    self.myworld.Step(timestep, 1000, 10)


class Physics(Physics_old):
  def __init__(self):
    super(Physics, self).__init__()
    self.state=""
    self.stateid=0
    self.statenext=""
  def adjustgripper(self):
    p1=self.getpos(1)
    dx = self.goalgripper[0] - p1[0]
    dy = self.goalgripper[1] - p1[1]
    if math.fabs(dx) < 20: dx = 1.0 * dx / 50  # small steps
    else: dx = 2.0 * dx / math.fabs(dx)  # normale steps
    if math.fabs(dy) < 20: dy = 1.0 * dy / 50  # small steps
    else: dy = 2.0 * dy / math.fabs(dy)  # normale steps
    self.setspeed(0, -dx)
    self.setspeed(1, dy)     
  def waypoints(self):
    objectid=3
    self.way=[]
    self.way.append((self.getpos(1)[0],100))
    self.way.append((self.getpos(objectid)[0],100))
    self.way.append((self.getpos(objectid)[0],self.getpos(objectid)[1]-100))

class Sceneunderstanding(Physics):
  def __init__(self):
    super(Sceneunderstanding, self).__init__()
  def getpixel(self,x,y):
    surf3d = pygame.surfarray.pixels3d( self.window )
    if surf3d[x][y][2]==140: temp="blue" # blue pixel found
    else: temp=False
    return temp
  def object_graspable(self,objectid):
    left= (self.getpos(objectid)[0]-50,self.getpos(objectid)[1])
    right= (self.getpos(objectid)[0]+50,self.getpos(objectid)[1])
    cond1,cond2,cond3,cond4=False,False,False,False
    if self.getpixel(left[0],left[1])!="blue": cond1=True
    if self.getpixel(right[0],right[1])!="blue": cond2=True
    if left[0]>40: cond3=True
    if right[0]<370: cond4=True
    if cond1==True and cond2==True and cond3==True and cond4==True:
      temp=True
    else: temp=False
    return temp  
  def updateScene(self):
    pass
      

class BehaviorTree(Sceneunderstanding):
  def __init__(self):
    super(BehaviorTree, self).__init__()
    self.frame=0
    self.actionid=0
    self.action = []
    self.action.append(("movetoobject","random"))
    self.action.append(("down",0))
    self.action.append(("slide","random"))
    self.action.append(("up",0))
    self.action.append(("ifgotocheckgrasp",6))
    self.action.append(("ifgoto",0))
    # grasp
    self.action.append(("movetopregrasp",4))
    self.action.append(("opengripper",0))
    self.action.append(("downgrasp",0))
    self.action.append(("closegripper",0))
    self.action.append(("movetotop",0))
    self.action.append(("movetosectionright",0))
    self.action.append(("opengripper",0))
    self.action.append(("closegripper",0))

  def updateBTframe(self):
    if self.frame%20==0: print "Frame", self.frame
    if self.frame%100==0: self.BTactionupdate()
    self.frame+=1
  def BTactionupdate(self):
    temp=self.action[self.actionid]
    print "Actionid", self.actionid, temp
    if temp[0]=="ifgoto" and self.taskifgoto()==True:
      self.actionid=self.action[self.actionid][1]
      print "change actionid", self.actionid
    elif temp[0]=="ifgotocheckgrasp" and self.taskifgotocheckgrasp()==True:
      self.actionid=self.action[self.actionid][1]
      print "change actionid", self.actionid
    else:
      self.actionid+=1
      if self.actionid>len(self.action)-1: self.actionid=0
    self.parsing(temp)
  def parsing(self,instruction):
    if instruction[0]=="down": self.taskdown()
    if instruction[0]=="up": self.taskup()
    if instruction[0]=="movetoobject": self.taskmovetoobject(instruction[1])
    if instruction[0]=="movetopregrasp": self.taskmovetopregrasp(instruction[1])
    if instruction[0]=="movetotop": self.taskmovetotop()
    if instruction[0]=="movetosectionright": self.taskmovetosectionright()
    if instruction[0]=="movetosectionleft": self.taskmovetosectionleft()      
    if instruction[0]=="downgrasp": self.taskdowngrasp()      
    if instruction[0]=="slide": self.taskslide(instruction[1])
    if instruction[0]=="closegripper": self.taskclosegripper()
    if instruction[0]=="opengripper": self.taskopengripper()
    if instruction[0]=="checkgrasp": self.taskcheckgrasp()


class Task(BehaviorTree):
  def __init__(self):
    super(Task, self).__init__()
  def taskifgoto(self):
    return True
  def taskifgotocheckgrasp(self):
    temp=False
    if self.object_graspable(4)==True: temp=True
    return temp

  def taskdown(self):
    p=self.getpos(1) # gripper
    p=(p[0],p[1]+60) # down
    self.goalgripper=p
  def taskup(self):
    p=self.getpos(1) # gripper
    p=(p[0],p[1]-50) 
    self.goalgripper=p
  def taskmovetoobject(self, objectid): # path to object
    if objectid=="random": objectid=random.randint(3,4)
    p=self.getpos(objectid) # object
    p=(p[0]-20,p[1]-100) # over object
    self.goalgripper=p
  def taskmovetopregrasp(self, objectid): # path to object pregrasp
    p=self.getpos(objectid) # object
    p=(p[0]-45,p[1]-100) # over object
    self.goalgripper=p
  def taskdowngrasp(self):
    p=self.getpos(1) # gripper
    p=(p[0],p[1]+90) # down
    self.goalgripper=p
  def taskslide(self,id): # -1=left
    if id=="random": 
      temp=random.randint(0,1)
      if temp==0: temp=-1
      id=temp
    direction=id*50
    p=self.getpos(1) # gripper
    p=(p[0]+direction,p[1]) 
    self.goalgripper=p
  def taskopengripper(self):
    self.setspeed(2,2)
  def taskclosegripper(self):
    self.setspeed(2,-2)
  def taskmovetotop(self): # to top
    p=self.getpos(1) # gripper
    p=(p[0],100) 
    self.goalgripper=p
  def taskmovetosectionright(self): 
    self.goalgripper=(450,100)
  def taskmovetosectionleft(self): 
    self.goalgripper=(250,100)
    
         
class GUI(Task):
  def __init__(self):
    super(GUI, self).__init__()
    pygame.init()
    self.window = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT))
    self.clock = pygame.time.Clock()
    self.mouse = (0,0)
    self.input = "mouse"
  def drawobjects(self):
    # Text1
    myfont = pygame.font.SysFont("freesans", 18)
    text = "control: " + str(self.input)
    label = myfont.render(text, 1, (0, 0, 0))
    self.window.blit(label, (40, 20))
    text = "mouse: " + str(self.mouse)
    label = myfont.render(text, 1, (0, 0, 0))
    self.window.blit(label, (40, 40))
    text = "behavior: " 
    label = myfont.render(text, 1, (0, 0, 0))
    self.window.blit(label, (40, 60))
    text = "possibleactions: " 
    label = myfont.render(text, 1, (0, 0, 0))
    self.window.blit(label, (40, 80))    
    # waypoints
    for i in range(len(self.way)):
      p = self.way[i]
      radius=10
      pygame.draw.circle(self.window, (200,0,0), p, radius, 2)
    # box2d
    colors = { staticBody: (100, 100, 100),
        dynamicBody: (0, 0, 140),
    }
    for body in (self.myworld.bodies):  
      for fixture in body.fixtures:
        shape = fixture.shape
        if hasattr(shape, 'vertices'):  # rectangle
          vertices = [(body.transform * v) * PPM for v in shape.vertices]
          vertices = [((v[0]), (SCREEN_HEIGHT - v[1])) for v in vertices]
          pygame.draw.polygon(self.window, colors[body.type], vertices, 0)
        else:  # ball
          position = body.transform * shape.pos * PPM
          x, y = int(position[0]), int(SCREEN_HEIGHT - position[1])
          radius = int(shape.radius * 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=="mouse":
          self.goalgripper=self.mouse
      if event.type == pygame.MOUSEBUTTONUP:
        self.mouseclick()
      if event.type == pygame.KEYDOWN:
        if event.key == pygame.K_1:
          self.taskopengripper()    
        if event.key == pygame.K_2:
          self.taskclosegripper()
  def mouseclick(self):
    gripper=0
    if self.input=="mouse": self.input="auto"
    elif self.input=="auto": self.input="mouse"
    if gripper==1:
      dist=calcdistance(self.physicsengine.getpos(1),self.physicsengine.getpos(2))
      if dist>85: self.physicsengine.setspeed(2,-2)
      else: self.physicsengine.setspeed(2,2)
  def updateGUI(self):
    self.clock.tick(FPS)  
    self.window.fill((220, 220, 220))
    self.inputhandling()
    self.drawobjects()
    pygame.display.flip()  # display update
    

class Game: 
  def __init__(self):
    self.myGUI = GUI()
    for step in range(10000000):
      self.myGUI.updateGUI()
      self.myGUI.updatePhysic()
      self.myGUI.updateScene()
      if self.myGUI.input=="auto": self.myGUI.updateBTframe()


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