Update: Robot-Control-System

Seit der letzten Version ist schon etwas Zeit vergangen und es ist Zeit die neue Version vorzustellen. Es gab zwei Veränderungen. Einmal wurden die Motion Primitive leicht verändert, und zum Zweiten gibt es jetzt ein Menü. Das ganze wird in der Computeranimation als „Interactive Control“ bezeichnet. Der Vorteil ist, dass man darüber die benötigte FSM leichter erstellen und testen kann. Der Quellcode besteht wie immer aus Python Code.

'''
titel: Circle control
date: April 23, 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 = (600, 338)
    self.ppm = 100.0  # pixels per meter 100
    self.fric = 1.0  # 0.3 friction
    self.torque = 100
    self.fps = 30  # 60 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
  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, 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 createcircle(self,p,r):
    self.body.append(self.myworld.CreateDynamicBody(
      position=p,
      fixtures=b2FixtureDef(
        shape=b2CircleShape(radius=r), 
        restitution=0.5, density=1.0, 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 = 5000 # 500
    positionIterations = 100 # 10
    self.myworld.Step(timestep, velocityIterations, positionIterations)

class Gripper(Physics):
  def __init__(self):
    super(Gripper, self).__init__()
    self.createbox("static",(3, 3.3),(3, .02)) # bottom
    self.createbox("static",(3, 0.1),(3, .02)) # top
    self.createbox("static",(0.05, 2),(.02, 2)) # left
    self.createbox("static",(5.8, 2),(.02, 2)) # right
    
    self.createbox("dynamic",(3, 1),(.2, .1)) # robottop
    self.createcircle((1,1),0.1)
    self.createbox("dynamic",(3, 1),(.2, .1)) # robottop2
    self.createcircle((1,1),0.1)
    self.createbox("dynamic",(3, 1),(.2, .1)) # robottop3
    self.createcircle((3,1),0.1)
    self.createbox("dynamic",(2, 3),(.3, .3)) # box1
    self.createbox("dynamic",(3.5, 3),(.3, .3)) # box2
    self.createbox("static",(5.0, 3),(.02, 1)) # right2
    
    self.createprismaticjoint(1,4,1,0) # arm1 left/right
    self.createprismaticjoint(4,5,0,1) # arm1 up/down
    # arm2 left/right
    self.joint.append(self.myworld.CreatePrismaticJoint(
        bodyA=self.body[1],
        bodyB=self.body[6],
        localAnchorA=(0, .25),
        localAnchorB=(0, 0),
        axis=(1, 0),
        lowerTranslation=1,
        upperTranslation=3.2,
        enableMotor=True,
        enableLimit=False,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
    ))
    self.createprismaticjoint(6,7,0,1) # arm2 up/down
    # arm3 left/right
    self.joint.append(self.myworld.CreatePrismaticJoint(
        bodyA=self.body[1],
        bodyB=self.body[8],
        localAnchorA=(0, .5),
        localAnchorB=(0, 0),
        axis=(1, 0),
        lowerTranslation=1,
        upperTranslation=3.2,
        enableMotor=True,
        enableLimit=False,
        maxMotorForce=self.torque,
        motorSpeed=0.0,
    ))
    self.createprismaticjoint(8,9,0,1) # arm3 up/down
    
class Position(Gripper):
  def __init__(self):
    super(Position, self).__init__()
    self.speed=20.0
    self.pos = []
    # jointid, boxid, goal
    self.pos.append((0,4,300)) # x
    self.pos.append((1,5,100)) # y
    self.pos.append((2,6,400)) # x2
    self.pos.append((3,7,100)) # y2
    self.pos.append((4,8,500)) # x3
    self.pos.append((5,9,100)) # y3
  def move(self,id,p):
    if id==0:
      self.pos[0]=(0,4,p[0])
      self.pos[1]=(1,5,p[1])
    if id==1:
      self.pos[2]=(2,6,p[0])
      self.pos[3]=(3,7,p[1])
    if id==2:
      self.pos[4]=(4,8,p[0])
      self.pos[5]=(5,9,p[1])
  def setfsmspeed(self,speed):
    self.speed=speed
  def setposition(self):
    # x
    posid=0
    joint = self.pos[posid][0]
    goal, current = self.pos[posid][2],self.getpos(self.pos[posid][1])[0]
    factor=1.0*self.speed
    dxy=(goal-current)/factor
    self.setspeed(joint, dxy)
    # y
    posid=1
    joint = self.pos[posid][0]
    goal, current = self.pos[posid][2],self.getpos(self.pos[posid][1])[1]
    factor=1.0*self.speed
    dxy=(goal-current)/factor
    self.setspeed(joint, dxy)
    # robot2 x
    posid=2
    joint = self.pos[posid][0]
    goal, current = self.pos[posid][2],self.getpos(self.pos[posid][1])[0]
    factor=1.0*self.speed
    dxy=(goal-current)/factor
    self.setspeed(joint, dxy)
    # robot2 y
    posid=3
    joint = self.pos[posid][0]
    goal, current = self.pos[posid][2],self.getpos(self.pos[posid][1])[1]
    factor=1.0*self.speed
    dxy=(goal-current)/factor
    self.setspeed(joint, dxy)
    # robot3 x
    posid=4
    joint = self.pos[posid][0]
    goal, current = self.pos[posid][2],self.getpos(self.pos[posid][1])[0]
    factor=1.0*self.speed
    dxy=(goal-current)/factor
    self.setspeed(joint, dxy)
    # robot3 y
    posid=5
    joint = self.pos[posid][0]
    goal, current = self.pos[posid][2],self.getpos(self.pos[posid][1])[1]
    factor=1.0*self.speed
    dxy=(goal-current)/factor
    self.setspeed(joint, dxy)
    
class BehaviorTree(Position):
  def __init__(self):
    super(BehaviorTree, self).__init__()
    self.taskname=""
  def BTstart(self,taskname):
    self.status="Running"
    self.taskname=taskname
    self.mythread = threading.Thread(target=self.taskmain)      
    self.mythread.daemon = True  # stops, if mainprogram ends
    self.mythread.start()
  def taskmain(self):
    if self.taskname=="start":
      self.tiltleft()
      self.task()
    if self.taskname=="startmenu":
      self.task2()
    self.status="Ready"
  def pause(self):
    pygame.time.wait(1000)
  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 Dictionay(BehaviorTree):
  def __init__(self):
    super(Dictionay, self).__init__()
    self.robot1id,self.robot2id,self.robot3id = 5,7,9
    self.objectid=10
    self.objectlist=(10,11)
  def task2(self):
    if self.menuselect[0]==0: self.init()
    if self.menuselect[0]==1: self.tiltleft()
    if self.menuselect[0]==2: self.tiltright()
    if self.menuselect[0]==3: self.pregrasp()
    if self.menuselect[0]==4: self.grasp()
    if self.menuselect[0]==5: self.under()
    if self.menuselect[0]==6: self.moveair((250,150))
    if self.menuselect[0]==7: self.moveair((450,140))
    if self.menuselect[0]==8: self.unload()
    if self.menuselect[0]==9: self.tilt2()
    if self.menuselect[0]==10: self.tilt2grasp()
  def task(self):
    self.setobject(0)
    self.pregrasp()
    self.grasp()
    self.under()
    self.moveair((200,150))
    self.moveair((400,140))
    self.moveair((450,140))
    self.unload()

    self.setobject(1)
    self.pregrasp()
    self.grasp()
    self.under()
    self.moveair((400,140))
    self.moveair((450,140))
    self.unload()

  def setobject(self,id):
    self.objectid=self.objectlist[id]
  def pregrasp(self):
    p1 = self.relativeobject(-50,70) # left over
    p2 = self.relativeobject(50,70) # right over
    p3 = self.relativeobject(80,0) # right
    self.move(0,p1)
    self.move(1,p2)
    self.move(2,p3)
    self.pause()
    p4 = self.relativeobject(-50,10) # left pregrasp
    p5 = self.relativeobject(50,10) # right pregrasp
    self.move(0,p4)
    self.move(1,p5)
    self.pause()
  def grasp(self):
    p1 = self.relativeobject(-40,10) # left contact
    p2 = self.relativeobject(40,10) # right contact
    p3 = self.relativeobject(-40,35) # left up
    p4 = self.relativeobject(40,35) # right up
    self.move(0,p1)
    self.move(1,p2)
    self.pause()
    self.move(0,p3)
    self.move(1,p4)
    self.pause()
  def under(self):
    p2 = self.relativeobject(0,-80) # airunder
    p3 = self.relativeobject(0,-40) # aircontact
    self.move(2,p2)
    self.pause()
    self.move(2,p3)
    self.pause()
  def moveair(self,goal):
    base = self.getpos(self.robot1id)
    radius = self.calcdistance(base,goal)
    angle = self.angle_between_two_points(base,goal)
    p0 = self.polarpoint(self.getpos(self.robot1id),angle,radius)
    p1 = self.polarpoint(self.getpos(self.robot2id),angle,radius)
    p2 = self.polarpoint(self.getpos(self.robot3id),angle,radius)
    self.setfsmspeed(50.0) # slow
    self.move(0,p0)
    self.move(1,p1)
    self.move(2,p2)
    self.pause()
    self.setfsmspeed(20.0) # normal
  def unload(self):
    p1 = self.relativeobject(80,10) # right away
    p2 = self.relativeobject(80,70) # right up
    self.move(1,p1)
    self.pause() 
    self.move(1,p2)
    self.pause()
  def tiltleft(self):
    p1 = self.relativeobject(-10,60) # over1
    p2 = self.relativeobject(-10,35) # over2
    p3 = self.relativeobject(-100,35) # left
    self.move(0,p1)
    self.pause() 
    self.move(0,p2)
    self.pause()
    self.move(0,p3)
    self.pause()
  def tiltright(self):
    p1 = self.relativeobject(10,60) # over1
    p2 = self.relativeobject(10,35) # over2
    p3 = self.relativeobject(100,35) # left
    self.move(0,p1)
    self.pause() 
    self.move(0,p2)
    self.pause()
    self.move(0,p3)
    self.pause()
  def init(self):
    p1 = (200,140)
    p2 = (250,140)
    p3 = (300,140)
    self.move(0,p1)
    self.move(1,p2)
    self.move(2,p3)
    self.pause()
  def tilt2(self):
    p1 = self.relativeobject(-25,40) # over
    p2 = self.relativeobject(-55,20) # tilt
    self.move(0,p1)
    self.pause() 
    self.move(0,p2)
    self.pause()
  def tilt2grasp(self):
    p1 = self.relativeobject(50,70) # rightoverobject
    p2 = self.relativeobject(50,10) #rightgraspobject
    p3 = self.relativeobject(40,10) # rightcontact
    self.move(1,p1)
    self.pause() 
    self.move(1,p2)
    self.pause()
    self.move(1,p3)
    self.pause()
    p1 = self.relativeobject(-50,70) #leftoverobject
    p2 = self.relativeobject(-50,10) #leftgraspobject
    p3 = self.relativeobject(-40,10) #leftcontact
    self.move(0,p1)
    self.pause() 
    self.move(0,p2)
    self.pause()
    self.move(0,p3)
    self.pause()

class GUI(Dictionay):
  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 = 0 # mode
    self.menu1=("init","tiltleft","tiltright","pregrasp","grasp","under","moveaircenter","moveairplace","unload","tilt2","tilt2grasp")
    self.menu2=("Object1","Object2")
    self.menuselect=(0,0)
    self.status="Ready"
  def updateGUI(self):
    #self.clock.tick(self.fps)  
    pygame.time.wait(1000/self.fps)
    self.window.fill((220, 220, 220))
    self.inputhandling()
    self.painttext()
    self.paintobjects()
  def painttext(self):
    myfont = pygame.font.SysFont("freesans", 16)
    text = str(self.control)
    text2 = " " + str(self.mouse)
    pygame.display.set_caption(text+text2)
    # status
    label = myfont.render(self.status, True, self.black)
    self.window.blit(label, (20,15))
    # menu
    height=18
    pos=40
    for i in range(len(self.menu1)):
      text1 = self.menu1[i]
      label = myfont.render(text1, True, self.black)
      self.window.blit(label, (20,pos+i*height))
      # select
      label = myfont.render(">", True, self.black)
      if self.menuselect[0]==i:
        self.window.blit(label, (10,pos+i*height)) 
    for i in range(len(self.menu2)):
      text1 = self.menu2[i]
      label = myfont.render(text1, True, self.black)
      self.window.blit(label, (100,pos+i*height))      
      # select
      label = myfont.render(">", True, self.black)
      if self.menuselect[1]==i:
        self.window.blit(label, (90,pos+i*height)) 
  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
    p = self.getpos(self.objectid) # selected object
    pygame.draw.circle(self.window, self.red, p, 7, 2)
  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 == 0: pass
        if self.control == 1: self.move(0,self.mouse)
        if self.control == 2: self.move(1,self.mouse)
        if self.control == 3: self.move(2,self.mouse)
      if event.type == pygame.MOUSEBUTTONUP:
        self.selectbox()
      if event.type == pygame.KEYDOWN:
        if event.key == pygame.K_1: self.control=0
        if event.key == pygame.K_2: self.control=1
        if event.key == pygame.K_3: self.control=2
        if event.key == pygame.K_4: self.control=3
        if event.key == pygame.K_a: self.BTstart("start")
        if event.key == pygame.K_b: pass
        if event.key == pygame.K_LEFT: 
          if self.menuselect[1]==0: 
            self.menuselect=(self.menuselect[0],1)
          elif self.menuselect[1]==1: 
            self.menuselect=(self.menuselect[0],0)
          self.setobject(self.menuselect[1])
        if event.key == pygame.K_RIGHT: self.BTstart("startmenu")
        if event.key == pygame.K_UP: 
          temp=self.menuselect[0]-1
          if temp==-1: temp=len(self.menu1)-1
          self.menuselect=(temp,self.menuselect[1])
        if event.key == pygame.K_DOWN:
          temp=self.menuselect[0]+1
          if temp==len(self.menu1): temp=0
          self.menuselect=(temp,self.menuselect[1])
  def selectbox(self):
    print self.mouse
    p1,radius,p2 = self.getpos(self.objectid), 50, self.mouse
    print self.incircle(p1,radius,p2)
    
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

Ein Gedanke zu “Update: Robot-Control-System

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