Update: Robot Control System


Eigentlich ist es noch zu früh, für ein neues Update, weil der jetzige Prototyp alles andere als stabil funktioniert. Angedacht war es, einen RRT Solver zu implementieren, der wurde zwischendurch aber wieder auskommentiert so dass jetzt ein System entstanden ist was zu gar nichts zu gebrauchen ist als vielmehr ein gutes Beispiel für ein gescheitertes Projekt bezeichnet werden kann. Das Problem im Detail:

Es ist nicht möglich für cluttered Scene Motion Primitive zu erstellen, man kann zwar welche aufstellen muss diese aber in einem RRT-like Solver durchprobieren ob sie passend sind. Da RRT Jedoch viel CPU Zeit benötigt gibt es Probleme. RRT schneller zu machen geht nicht, und bessere Motion Primitive zu erfinden geht auch nicht. Wirklich stabil funktioniert die Anwendung nur, wenn man die Motion Primitive manuell aufruft. Ein Human Operator kann auch in cluttered Scene entscheiden, welche ausgeführt werden müssen. Will man jedoch vollautonome Systeme haben führt an RRT kein Weg vorbei. Meine Prognose derzeit lautet, dass man zwingend schnelle Hardware benötigt, um in solchen Fällen eine Lösung zu finden. Ähnlich wie bei einem Schachcomputer in den 1990’er Jahren. Das heißt, mit Systemen wie der Xeon Phi wäre es vielleicht möglich das System in Echtzeit laufen zu lassen.

'''
titel: Box2D performance
date: May 16, 2017
'''
import time, sys, pygame, random, math, Box2D, os, numpy, threading
from Box2D.b2 import (world, polygonShape, staticBody, dynamicBody)
from Box2D import (b2CircleShape, b2FixtureDef, b2LoopShape, b2PolygonShape,
                   b2RevoluteJointDef, b2_pi)
from pygame.locals import *

class Settings(object):
  def __init__(self):
    self.screen = (600, 338)
    self.ppm = 100.0  # pixels per meter 100
    self.fric = 1.0  # 1.0 friction
    self.dens = 1.0 # 1.0 density
    self.torque = 100 #100
    self.fps = 30  # 30 fps
    self.white = (220, 220, 220)
    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,y = int(box2dpos[0] * self.ppm),int(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
  def angleobject(self,objectid):
    """ smallest angle of object """    
    topangle = []
    for i in range(4):
      temp = (self.getangle(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]     

class Physics(Settings):
  def __init__(self):
    super(Physics, self).__init__()
    self.myworld = world(gravity=(0, 0), doSleep=True)  # 0,-10
    self.body = []
    self.joint = []
    self.rrtfast = False # faster update
  def createbox(self,moveable,p,size):
    if moveable=="dynamic":
      self.body.append(self.myworld.CreateDynamicBody(position=p, angle=0))
      self.body[-1].CreatePolygonFixture(box=size, density=self.dens, friction=self.fric)
    else:
      self.body.append(self.myworld.CreateStaticBody(
        position=p,angle=0,
        fixtures=b2FixtureDef(
          shape=polygonShape(box=size),
          friction=0.1,density=self.dens
      )))
  def createcircle(self,p,r):
    self.body.append(self.myworld.CreateDynamicBody(
      position=p,
      fixtures=b2FixtureDef(
        shape=b2CircleShape(radius=r), 
        restitution=0.5, density=self.dens, friction=self.fric), 
      ))
  def createprismaticjoint(self,id1,id2,axisx,axisy):
    """ create linear joint """
    self.joint.append(self.myworld.CreatePrismaticJoint(
        bodyA=self.body[id1],
        bodyB=self.body[id2],
        localAnchorA=(0, 0),
        localAnchorB=(0, 0),
        axis=(axisx, axisy),
        lowerTranslation=1,
        upperTranslation=3.2,
        enableMotor=True,
        enableLimit=False,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
    ))
  def createrevolutejoint(self,id1,id2):
    """ create rotate joint """
    self.joint.append(self.myworld.CreateRevoluteJoint(
        bodyA=self.body[id1],
        bodyB=self.body[id2],
        localAnchorA=(0, 0),
        localAnchorB=(0, 0),
        enableMotor=True,
        enableLimit=False,
        lowerAngle=math.radians(60),
        upperAngle=math.radians(120),
        maxMotorTorque=self.torque,
        motorSpeed=0,
    ))
  def setspeed(self, id, speed):
    self.joint[id].motorSpeed = speed 
  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 updatePhysics(self):
    self.setposition()
    timestep = 1.0 / self.fps
    velocityIterations = 1
    positionIterations = 1
    self.myworld.Step(timestep, velocityIterations, positionIterations)

class Gripper(Physics):
  def __init__(self):
    super(Gripper, self).__init__()
    self.robotid = 4
    self.objectid = 5
    size=0.015
    self.goal=(300,200)
    self.createbox("static",(3, 3.3),(3, size)) # bottom
    self.createbox("static",(3, 0.1),(3, size)) # top
    self.createbox("static",(0.05, 2),(size, 2)) # left
    self.createbox("static",(5.8, 2),(size, 2)) # right
    self.createcircle((3,2),0.2) # robot
    self.createbox("dynamic",(0.7, 0.8),(.4, .4)) # box
    self.createbox("static",(1, 2.5),(1, size)) # obstacle
    self.createbox("static",(5, 2.4),(1, size)) # obstacle
    self.createbox("dynamic",(0.7, 1.8),(.3, .3)) # box

  def move(self,goal):
    self.goal=goal
  def setposition(self):
    factor=50
    x=self.getpos(self.robotid)[0]-self.goal[0]
    y=self.getpos(self.robotid)[1]-self.goal[1]
    x=1.0*x/factor
    y=1.0*y/factor
    self.body[self.robotid].linearVelocity = (-x, -y)
    self.body[self.robotid].angularVelocity = 0 # prevent rotating
  def relativeobject(self,x,y):
    base = self.getpos(self.objectid)
    angle= self.angleobject(self.objectid)
    p = self.polarpoint(base, angle+90, x)
    p = self.polarpoint(p, angle, y)
    return p

class Dictionary(Gripper):
  def __init__(self):
    super(Dictionary, self).__init__()
    self.rrtspeed="normal" # normal, fast
    pass
  def taskmenu(self):
    self.status="Running"
    temp = self.menuselect1
    taskname="self."+ self.primitive[temp][2]
    eval(taskname)
    self.status="Ready"
  def donothing(self):
    pass
  def circleobject(self,direction,distance):
    # direction = 0 -> central
    # distance=100 -> on hold
    probot= self.getpos(self.robotid)
    pobject= self.getpos(self.objectid)
    angle= self.angle_between_two_points(pobject, probot)
    angle2=angle+direction
    pout = self.polarpoint(pobject, angle2, distance)
    self.move(pout)
  def kick(self):
    probot= self.getpos(self.robotid)
    pobject= self.getpos(self.objectid)
    angle= self.angle_between_two_points(probot,pobject)
    pout = self.polarpoint(pobject, angle, 10)
    self.move(pout)
  def toobject(self,angle,distance):
    probot= self.getpos(self.robotid)
    pobject= self.getpos(self.objectid)
    pout = self.polarpoint(pobject, angle, distance)
    self.move(pout)
  def pushwand(self):
    self.toobject(90,100)
    self.pause()
    self.toobject(90,30)
    self.pause()
    self.toobject(90,100)
    self.pause()
  def sensor(self):
    probot= self.getpos(self.robotid)
    pobject= self.getpos(self.objectid)
    angle=self.angle_between_two_points(pobject,probot)
    rotation = self.getangle(self.objectid)
    print "robot", probot
    print "object", pobject
    print "angle object to robot", angle
    print "angle object", rotation
  def pause(self):
    #pygame.time.wait(1000)
    if self.rrtspeed=="normal":
      for i in range(2*self.fps): # 2 second 
        #pygame.time.wait(1000/self.fps)
        pygame.time.wait(500/self.fps)
        self.updatePhysics()
        self.updateGUI()
        pygame.display.update() 
    if self.rrtspeed=="middle":
      for i in range(2*self.fps): # 2 second 
        self.updatePhysics()
        if i%10==0:
          self.updateGUI()
          pygame.display.update()
          pygame.time.wait(50)
    if self.rrtspeed=="fast":
      for i in range(2*self.fps): # 2 second 
        self.updatePhysics()


       
class Motionprimitives(Dictionary):
  def __init__(self):
    super(Motionprimitives, self).__init__()
    self.primitive =[] # group, name, taskname
    self.primitive.append(("Primitive","circleleft","circleobject(-45,100)")) 
    self.primitive.append(("Primitive","circleright","circleobject(45,100)")) 
    self.primitive.append(("Primitive","kick","circleobject(0,40)")) 
    self.primitive.append(("Primitive","hold","circleobject(0,100)")) 
    self.primitive.append(("Primitive","pushwand","pushwand()")) 
    self.primitive.append(("Sensor","print","sensor()")) 
    self.primitive.append(("Solver","run","solverrun()")) 
    self.primitive.append(("move","center","move((300,150))")) 
    self.primitive.append(("move","left","move((100,150))")) 
    self.primitive.append(("move","right","move((500,150))")) 
    self.primitive.append(("move","object","circleobject(0,100)")) 
    
    '''
    self.primitive.append(("TASK","plan","generateplan()")) 
    self.primitive.append(("TASK","show plan","showplan()")) 
    self.primitive.append(("TASK","run","runplan()")) 
    self.primitive.append(("TASK","task mouse","rrtsolver(\"mouse\")"))
    self.primitive.append(("RRT","maketree","maketree()"))
    self.primitive.append(("STEP","objectmiddle","rrtsolver((270,90))"))
    self.primitive.append(("STEP","objectdown","rrtsolver((300,250))"))
    self.primitive.append(("STEP","objectright","rrtsolver((530,280))"))
    '''

    self.linestart=0
    self.linemax=7
  def paintmotionprimitive(self):
    myfont = pygame.font.SysFont("freesans", 16)
    count=0
    xstart, ystart=400,15
    height=18
    for i in range(self.linestart,self.linestart+self.linemax):
      # select
      color=self.black
      if self.menuselect1==i:
        color=self.blue
        if self.status=="Running": color=self.red # running color
        label = myfont.render(">", True, color)
        self.window.blit(label, (xstart-10,ystart+count*height)) 
      # scrolling
      self.linestart=self.menuselect1-(self.linemax/2)
      if self.linestart<0: self.linestart=0
      elif self.linestart>len(self.primitive)-self.linemax:
        self.linestart=len(self.primitive)-self.linemax
      # textout
      if self.primitive[i][0]==self.primitive[i-1][0]: text = str(i)
      else: text = str(i)+" "+self.primitive[i][0]
      label = myfont.render(text, True, color)
      self.window.blit(label, (xstart,ystart+count*height))
      label = myfont.render(self.primitive[i][1], True, color)
      self.window.blit(label, (xstart+90,ystart+count*height))
      count+=1

class Solver(Motionprimitives):
  def __init__(self):
    super(Solver, self).__init__()
    self.enginetemp = Dictionary()
    self.plan = []
    self.maxprimitive=3
    self.plansize = 20
    self.maxtrial=500
  def enginecopy(self, source, destination):
    for i in range(len(source.body)): 
      destination.body[i].position = source.body[i].position
      destination.body[i].angle = source.body[i].angle
      destination.body[i].linearVelocity = source.body[i].linearVelocity
      destination.body[i].angularVelocity = source.body[i].angularVelocity
    for i in range(len(source.joint)):
      destination.joint[i].motorSpeed = source.joint[i].motorSpeed
      destination.joint[i].bodyA.angularVelocity = source.joint[i].bodyA.angularVelocity
      destination.joint[i].bodyB.angularVelocity = source.joint[i].bodyB.angularVelocity
      # destination.joint1.bodyA.angle = source.joint1.bodyA.angle # prismaticjoint
    # print dir(self.engine.joint1)
    # print self.engine.joint1.Dump     
  def runplan(self):
    self.rrtspeed="middle"
    for i in range(len(self.plan)):
      temp=self.plan[i]
      taskname="self."+self.primitive[temp][2]
      print i,self.primitive[temp][0]+" "+self.primitive[temp][1],taskname
      eval(taskname)
      self.pause()
  def solverrun(self):
    mincost = 9999999
    bestplan=[]
    for trial in range(self.maxtrial):
      # init engine
      self.enginecopy(self,self.enginetemp)
      self.enginetemp.rrtspeed="fast"
      # generate plan
      self.plan = []
      for i in range(self.plansize):
        temp=random.randint(0,self.maxprimitive)
        self.plan.append(temp)
      # execute plan
      for i in range(self.plansize):
        temp=self.plan[i]
        taskname="self.enginetemp."+self.primitive[temp][2]
        eval(taskname)
        self.enginetemp.pause()
      # cost
      temp=self.taskcost()
      if temp<mincost: 
        mincost=temp
        bestplan=self.plan
      print trial, temp, mincost
    self.plan = bestplan
    self.runplan()
  def taskcost(self):
    # pos= self.getpos(self.objectid) # original pos
    goal = self.mouse
    pobject= self.enginetemp.getpos(self.enginetemp.objectid)
    cost = self.calcdistance(pobject,goal)
    pygame.draw.circle(self.window, self.red, pobject, 5, 0)
    pygame.display.update()
    pygame.time.wait(10)
    return cost
    

class GUI(Solver):
  def __init__(self):
    super(GUI, self).__init__()
    random.seed()
    pygame.init()
    self.window = pygame.display.set_mode(self.screen,HWSURFACE|DOUBLEBUF|RESIZABLE)
    self.mouse = (0, 0)
    self.control = 1 # mode
    self.status="Ready"
    self.menuselect1 = 0
  def updateGUI(self):
    pygame.time.wait(1000/self.fps)
    self.window.fill(self.white)
    self.paintobjects()
    self.painttext()
    self.inputhandling()
    self.paintmotionprimitive()
    if self.control==2: self.move(self.mouse)
  def painttext(self):
    myfont = pygame.font.SysFont("freesans", 16)
    text = str(self.control)+" " + str(self.mouse)
    pygame.display.set_caption(text)
    # status "Ready"
    label = myfont.render(self.status, True, self.black)
    self.window.blit(label, (220,15))
  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.control == 1: pass
        if self.control == 2: pass
      if event.type == pygame.KEYDOWN:
        if event.key == pygame.K_1: self.control=1
        if event.key == pygame.K_2: self.control=2
        if event.key == pygame.K_UP: 
          temp=self.menuselect1-1
          if temp==-1: temp=len(self.primitive)-1
          self.menuselect1=temp
        if event.key == pygame.K_DOWN:
          temp=self.menuselect1+1
          if temp==len(self.primitive): temp=0
          self.menuselect1=temp
        if event.key == pygame.K_RIGHT: self.taskmenu()        
  def paintobjects(self):
    # 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(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

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

'''
class RRTinit(Motionprimitives):
  def __init__(self):
    super(RRTinit, self).__init__()
    self.engine = Dictionary()
  def enginecopy(self, source, destination):
    for i in range(len(source.body)): 
      destination.body[i].position = source.body[i].position
      destination.body[i].angle = source.body[i].angle
      destination.body[i].linearVelocity = source.body[i].linearVelocity
      destination.body[i].angularVelocity = source.body[i].angularVelocity
    for i in range(len(source.joint)):
      destination.joint[i].motorSpeed = source.joint[i].motorSpeed
      destination.joint[i].bodyA.angularVelocity = source.joint[i].bodyA.angularVelocity
      destination.joint[i].bodyB.angularVelocity = source.joint[i].bodyB.angularVelocity
      # destination.joint1.bodyA.angle = source.joint1.bodyA.angle # prismaticjoint
    # print dir(self.engine.joint1)
    # print self.engine.joint1.Dump  
  def rrtsolver(self,goal):
    if goal=="mouse": self.rrtgoal = self.mouse
    else: self.rrtgoal = goal
    mincost = 9999999
    bestplan=[]
    for trial in range(self.maxtrial):
      self.enginecopy(self,self.engine)
      #self.engine=Dictionary()
      self.engine.rrtspeed="fast"
      self.generateplan()
      for i in range(len(self.plan)):
        temp=self.plan[i]
        taskname="self.engine."+self.primitive[temp][2]
        #print i,taskname
        eval(taskname)
        self.engine.pause()
      temp=self.taskcost()
      if temp<mincost: 
        mincost=temp
        bestplan=self.plan
      if trial%10==0:
        print "trial ",trial, "cost=",temp,mincost
        pygame.time.wait(50)
    print "bestplan",bestplan
    self.plan = bestplan
    self.runplan()
  def taskcost(self):
    pobject= self.engine.getpos(self.engine.objectid)
    cost = self.calcdistance(pobject,self.rrtgoal)
    pygame.draw.circle(self.window, self.red, pobject, 5, 0)
    pygame.display.update()
    #pygame.time.wait(100)
    return cost


class RRT(RRTinit):
  def __init__(self):
    super(RRT, self).__init__()
    self.plan = []
    self.maxprimitive=7
    self.plansize = 20
    self.maxtrial=500
    self.rrtgoal = (0,0)
  def generateplan(self):
    self.plan = []
    for i in range(self.plansize):
      temp=random.randint(0,self.maxprimitive)
      self.plan.append(temp)
  def showplan(self):
    for i in range(len(self.plan)):
      temp = self.plan[i]
      print i,self.primitive[temp][0]+" "+self.primitive[temp][1]
  def runplan(self):
    print "Goal", self.rrtgoal
    self.rrtspeed="middle"
    for i in range(len(self.plan)):
      temp=self.plan[i]
      taskname="self."+self.primitive[temp][2]
      print i,self.primitive[temp][0]+" "+self.primitive[temp][1],taskname
      eval(taskname)
      self.pause()

class RRT3(RRT):
  def __init__(self):
    super(RRT3, self).__init__()
    self.rrttree = []
    self.fromaction = [] # primitiveid
    self.fromnode = [] 
    # possible improvement: instance array only once then overwrite 
  def maketree(self):
    self.addinitnode()
    for i in range(1000):
      pygame.time.wait(30)
      goal=(random.randint(0,self.screen[0]),random.randint(0,self.screen[1]))
      if i%100==0: 
        print "trial=",i, "goal=",goal
        self.paintrrttree()
      nearestnode=self.nearestrrtnode(goal)
      action=random.randint(0,self.maxprimitive)
      self.addnode(nearestnode,action)
    self.showrrttree()
    print "rrt done"
  def nearestrrtnode(self,goal):
    """ get random RRT node with mincost """
    mincost,minid=9999999,0
    cost = []
    for i in range(len(self.rrttree)):
      pobject= self.rrttree[i].getpos(self.engine.objectid)
      cost.append(self.rrtcost(i,goal))
    # all indicies with mincost, http://stackoverflow.com/questions/6294179/how-to-find-all-occurrences-of-an-element-in-a-list
    mincost=min(cost)
    indices = [i for i, x in enumerate(cost) if x == mincost]
    randomindex=random.randint(0,len(indices)-1)
    return indices[randomindex]
  def paintrrttree(self):
    for i in range(len(self.rrttree)):
      pobject= self.rrttree[i].getpos(self.engine.objectid)
      pygame.draw.circle(self.window, self.red, pobject, 5, 0)
    pygame.display.update()
  def showrrttree(self):
    for i in range(len(self.rrttree)):
      pobject= self.rrttree[i].getpos(self.engine.objectid)
      cost = self.rrtcost(i,(200,200))
      print i, self.fromnode[i], self.fromaction[i], pobject,cost
  def addinitnode(self):
    # delete all
    self.rrttree = []
    self.fromaction = []
    self.fromnode = [] 
    # add init node
    self.fromaction.append(False)
    self.fromnode.append(0)
    self.rrttree.append(Dictionary())
    self.enginecopy(self,self.rrttree[-1])
    self.rrttree[-1].rrtspeed="fast"
  def addnode(self,sourcenode,primitiveid):
    # init
    self.fromaction.append(primitiveid)
    self.fromnode.append(sourcenode)
    self.rrttree.append(Dictionary())
    self.enginecopy(self.rrttree[sourcenode],self.rrttree[-1])
    self.rrttree[-1].rrtspeed="fast"
    # task
    taskname="self.rrttree[-1]."+self.primitive[primitiveid][2]
    eval(taskname)
    self.rrttree[-1].pause()
  def rrtcost(self,nodeid,goal):
    pobject= self.rrttree[nodeid].getpos(self.engine.objectid)
    cost = self.calcdistance(pobject,goal)
    return cost

'''

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