Update: Robot-Control-System


Das hier häufiger thematisierte Robot-Control-System hat ein kleineres Update erhalten. Zum einen wurde die Entwicklungsumgebung von Eclipse auf Geany gewechselt. Aber auch in der Software selbst wurde etwas verändert. Der Box2D PPM Parameter wurde nach oben erhöht, dadurch reagiert die App wesentlich flüssiger. Fairerweise muss jedoch erwähnt werden, dass sie die Ablaufumgebung selber (Game-Engine + Physics-Engine) inzwischen eingependelt hat. Das heißt, es gibt einige Routinen um Objekte zu Joints zu definieren und das wars dann. Viel entscheidener dürfte die sogenante BehaviorEngine sein, also der Teil worüber letztlich der Roboter in der Simulation gesteuert wird. Hier wurde bei dieser Version ein Verfahren eingesetzt, was eine Finite-State-Machine mit Hilfe von linguistischen Methoden beschreibt. Damit ist gemeint, dass es sehr viele Subfunktionen gibt (mehr als 20 für ein simples Peg-in-hole-Problem) die sich lesen wie der Ablaufcode bei der Lego Mindstorms Challange. Es ist kein richtiges Programm, sondern eher so eine Art von Script wo feste Parameter genutzt werden. Diese Subfunktionen haben sprechende Namen erhalten, was aus programmtechnischer Sicht überflüssig ist, sondern eher dem Software-Engineering zuzuordnen ist.

Anders formuliert, es ist nicht gelungen den Abstraktionsgrad der Behavior Engine zu erhöhen. Es gibt nur Lowlevel Primitive die von der Simulation bereitgestellt werden wie moveto, rotate, opengripper und daraus setzt die BehaviorEngine in eine Art von Lexikon dann den Task zusammen. Nicht als deklaratives Modell sondern bottom up.

Die Programmbedienung selber ist simpel: nach dem Starten im Python Interpreter drückt man auf die Taste a, was die BehaviorEngine aktiviert. Diese führt dann den Peg-in-hole-Task aus. Der ist relativ umfangreich und endet irgendwann.

BEHAVIOR ENGINE
Gehen wir noch ein wenig auf die BehaviorEngine ein. Rein formal besteht sie aus einer Ansammlung von Makros welche jedesmal folgende Befehle enthalten:

self.setgoal((300,45))
self.taskpause()
self.setangle(180)
self.gripperopen(-1)

Diese werden mit wechselnden Parametern und in unterschiedlicher Reihenfolge aufgerufen. Dadurch werden Motion Primitive wie „posinit“ und „gripperdown“ bereitgestellt. Die Frage die sich stellt lautet, ob man programmiertechnisch das nicht hätte anders implementieren können. Beispielsweise als Tabelle wie beim Q-Learning wo man die Parameter automatisch generiert. Macht es überhaupt Sinn, manuellen Code zu schreiben und diesem auch noch ausführliche Namen zu geben, wenn absehbar ist, dass es kein richtiger Code ist? Keines der definierten Methoden ist für andere Aufgaben wie z.B. inverted pendulum swingup wiederverwendbar. Dort müsste man erneut die BehaviorEngine from scratch erstellen.

Auf der anderen Seite tut das Programm was es soll und man kann es leicht verändern. Das heißt, wenn in dem Script irgendwo ein Bug auftaucht, kann man schnell identifzieren welches Codesegment dafür verantwortlich ist, und dort einen Parameter manuell anpassen. Man kann das nicht so sehr als programmieren bezeichnen sondern eher mit Lochkarten-schreiben vergleichen für eine Musik-Orgel. Das heißt, nur der Teil vom Robot-Control-System der die Game-Engine selber beinhaltet ist echter Python Code, die BehaviorEngine selber ist so eine Art von natürlichsprachlichem Kommentar.

'''
titel: Physics Controller with many manuel methods
date: March 7, 2017
author: Manuel Rodriguez
'''
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 = (400, 260)
    self.ppm = 100.0  # pixels per meter 100
    self.fric = 1.0  # 0.3 friction
    self.torque = 1000
    self.fps = 30  # 60 fps
    self.pause = 50000 / 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,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

class Physics(Settings):
  def __init__(self):
    super(Physics, self).__init__()
    self.myworld = world(gravity=(0, 10), doSleep=True)  # 0,-10
    self.body = []
    self.joint = []
  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=1, friction=self.fric)
    else:
      self.body.append(self.myworld.CreateStaticBody(position=p,
      shapes=polygonShape(box=size), angle=0))
  def createprismaticjoint(self,id1,id2,axisx,axisy,offset):
    """ create linear joint """
    self.joint.append(self.myworld.CreatePrismaticJoint(
        bodyA=self.body[id1],
        bodyB=self.body[id2],
        localAnchorA=(0, 0),
        localAnchorB=(0, offset),
        axis=(axisx, axisy),
        lowerTranslation=1,
        upperTranslation=3.2,
        enableMotor=True,
        enableLimit=False,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
    ))
  def createrevolutejoint(self,id1,id2,offset1,offset2):
    """ create rotate joint """
    self.joint.append(self.myworld.CreateRevoluteJoint(
        bodyA=self.body[id1],
        bodyB=self.body[id2],
        localAnchorA=(offset1, 0),
        localAnchorB=(offset2, 0),
        enableMotor=True,
        enableLimit=False,
        lowerAngle=math.radians(60),
        upperAngle=math.radians(120),
        maxMotorTorque=100*self.torque,
        motorSpeed=0,
    ))
  def setspeed(self, id, speed):
    self.joint[id].motorSpeed = speed 
  def clearforce(self):
    for i in range(len(self.joint)): self.setspeed(i,0)
  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.adjustgripper()
    timestep = 1.0 / self.fps
    velocityIterations = 500
    positionIterations = 10
    self.myworld.Step(timestep, velocityIterations, positionIterations)

class Gripper(Physics):
  def __init__(self):
    super(Gripper, self).__init__()
    self.createbox("dynamic",(2, 1),(.1, .1)) # gripper center
    self.createbox("dynamic",(2.2, 0),(.4, .1)) # paddle bottom
    self.createbox("static",(2, 2.5),(2, .02)) # bottom
    self.createbox("static",(0.05, 1),(.02, 2)) # left
    self.createbox("static",(3.8, 1),(.02, 2)) # right
    self.createbox("dynamic",(1.2, 0),(.2, .2)) # box
    self.createbox("dynamic",(2, 1),(.3, .08)) # finger1
    self.createbox("dynamic",(2.2, 1),(.3, .08)) # finger2
    self.createbox("static",(2.3, 2.10),(.02, .3)) # wand
    self.createbox("static",(2.3, 1.05),(.02, .3)) # wand down
    self.createprismaticjoint(0,1,0,1,-.1)
    self.createprismaticjoint(1,2,1,0,.1)
    self.createrevolutejoint(6,0,.3,0)
    self.joint.append(self.myworld.CreatePrismaticJoint(
        bodyA=self.body[6],
        bodyB=self.body[7],
        localAnchorA=(0, 0),
        localAnchorB=(0, 0),
        axis=(0, 1),
        lowerTranslation=0,
        upperTranslation=.7,
        enableMotor=True,
        enableLimit=True,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
    ))
    self.goal = (100,100)
    self.angle = 180
  def adjustgripper(self):
    # pos
    p1,p2= self.getpos(0), self.goal
    factor = 20
    diff = (p1[0] - p2[0], p1[1] - p2[1])
    dxy = (1.0 * diff[0] / factor, 1.0 * diff[1] / factor )
    self.setspeed(1, dxy[0])
    self.setspeed(0, dxy[1])
    # angle
    diff = self.getangle(6)- self.angle
    if diff<-180: diff=360-math.fabs(diff);
    factor=20.0
    da=diff/factor
    self.setspeed(2, da)
  def setangle(self,angle):
    self.angle=angle
  def setgoal(self,goal):
    self.goal=goal
  def gripperopen(self,speed):
    self.setspeed(3,speed)
      
class BehaviorTree(Gripper):
  def __init__(self):
    super(BehaviorTree, self).__init__()
    self.taskname="task1"
  def BTstart(self,taskname):
    self.taskname=taskname
    self.mythread = threading.Thread(target=self.taskmain)      
    self.mythread.daemon = True  # stops, if mainprogram ends
    self.mythread.start()
  def taskmain(self):
    print "task start"
    if self.taskname=="start": self.peginhole()
    print "task end"
  def peginhole(self):
    # self.getpos(0) # gripper
    # self.getpos(5) # box
    self.graspobject()
    self.objectinit()
    self.insertmain()
    self.releasegripper()
    self.correctivepush()
    self.movetorightside()
  def taskpause(self):
    pygame.time.wait(self.pause)
  def graspobject(self):
    self.movetograsp()
    self.gripperdown()
    self.gripperup()
  def movetograsp(self):
    p= (self.getpos(5)[0]-35,self.getpos(5)[1]-90)
    self.setgoal(p)
    self.gripperopen(1)
    self.taskpause()
  def gripperdown(self):
    p= (self.getpos(0)[0],self.getpos(0)[1]+25)
    self.setgoal(p)
    self.taskpause()
    self.gripperopen(-1)
    self.taskpause()
  def gripperup(self):
    p= (self.getpos(0)[0],self.getpos(0)[1]-50)
    self.setgoal(p)
    self.taskpause()
  def objectinit(self):
    self.rotateinit()
    self.posinit()
    self.insertintohole()
  def rotateinit(self):
    self.setangle(180-45)
    self.taskpause()
    self.setangle(90)
    self.taskpause()
  def posinit(self):
    p=(130,170)
    self.setgoal(p)
    self.taskpause()
    self.setangle(90+22.5) # rotate correction
    self.taskpause()
  def insertintohole(self):
    holepos=(228,180)
    diff=(holepos[0]-self.getpos(5)[0],holepos[1]-self.getpos(5)[1])
    goal=(20,35)
    change=(diff[0]-goal[0],diff[1]-goal[1])
    p=(self.getpos(0)[0]+change[0],self.getpos(0)[1]+change[1])
    self.setgoal(p)
    self.taskpause()
  def insertmain(self):
    self.rotateandpush()
    self.pushtoright()
  def rotateandpush(self):
    self.setangle(90)
    goal=(self.getpos(0)[0]+15,self.getpos(0)[1]+39)
    self.setgoal(goal)
    self.taskpause()
  def pushtoright(self):
    goal=(self.getpos(0)[0]+10,self.getpos(0)[1])
    self.setgoal(goal)
    self.taskpause()
  def releasegripper(self):
    self.gripperdown2()
    self.grippermoveaway()
  def gripperdown2(self):
    self.gripperopen(.2) # slow open
    self.taskpause()
    goal=(self.getpos(0)[0],self.getpos(0)[1]+5)
    self.setgoal(goal)
    self.taskpause()
  def grippermoveaway(self):
    goal=(self.getpos(0)[0]-50,self.getpos(0)[1])
    self.setgoal(goal)
    self.taskpause()
  def correctivepush(self):
    self.status()
    self.grippertobox()
    self.gripperpush()
    self.grippermoveaway()
  def status(self):
    p1= self.getpos(5) # box
    p2= (228,153) # ideal position
    diff=(p2[0]-p1[0],p2[1]-p1[1])
    print p1,p2,diff
  def grippertobox(self):
    goal=(self.getpos(0)[0],self.getpos(0)[1]-30)
    self.setgoal(goal)
    self.taskpause()
  def gripperpush(self):
    goal=(self.getpos(0)[0]+35,self.getpos(0)[1])
    self.setgoal(goal)
    self.taskpause()
  def movetorightside(self):
    self.moveoverup()
    self.moveoverright()
    self.fromrightinit()
    self.fromrightgrasp()
    self.fromrightrotate()
    self.fromrightrelease()
  def moveoverup(self):
    self.gripperopen(-1)
    self.setangle(90)
    self.taskpause()
    self.setgoal((100,45))
    self.taskpause()
  def moveoverright(self):
    self.setgoal((300,45))
    self.taskpause()
    self.setangle(180)
    self.taskpause()
    self.setangle(270)
    self.taskpause()
  def fromrightinit(self):
    self.setgoal((350,120))
    self.gripperopen(1)
    self.taskpause()
  def fromrightgrasp(self):
    self.setgoal((295,120))
    self.taskpause()
    self.gripperopen(-.2) # slow close
    self.taskpause()
    self.setgoal((350,120))
    self.taskpause()
  def fromrightrotate(self):
    self.setangle(180+45)
    self.taskpause()
    self.setgoal((300,120))
    self.taskpause()
  def fromrightrelease(self):
    self.setangle(180)
    self.setgoal((270,120))
    self.taskpause()
    self.gripperopen(1)
    self.taskpause()
     
class GUI(BehaviorTree):
  def __init__(self):
    super(GUI, self).__init__()
    pygame.init()
    self.window = pygame.display.set_mode(self.screen,HWSURFACE|DOUBLEBUF|RESIZABLE)
    self.clock = pygame.time.Clock()
    self.mouse = (0, 0)
    self.control = "manuel1"
  def updateGUI(self):
    #self.clock.tick(self.fps)  
    pygame.time.wait(1000/self.fps)
    self.window.fill((220, 220, 220))
    self.inputhandling()
    self.paintobjects()
  def paintobjects(self):
    text = str(self.control)
    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.control == "manuel1":
          self.goal = self.mouse
      if event.type == pygame.MOUSEBUTTONUP:
        pass
      if event.type == pygame.KEYDOWN:
        if event.key == pygame.K_1: self.setangle(self.angle-22.5)
        if event.key == pygame.K_2: self.setangle(self.angle+22.5)
        if event.key == pygame.K_3: self.gripperopen(-1)
        if event.key == pygame.K_4: self.gripperopen(1)
        if event.key == pygame.K_a: self.BTstart("start")
        if event.key == pygame.K_RIGHT: self.moveslow("right")
        if event.key == pygame.K_LEFT: self.moveslow("left")
        if event.key == pygame.K_UP: self.moveslow("up")
        if event.key == pygame.K_DOWN: self.moveslow("down")
        
class Game: 
  def __init__(self):
    random.seed()
    self.myGUI = GUI()
    for step in range(10000000):
      self.myGUI.updateGUI()
      self.myGUI.updatePhysics()
      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