Software modifications and upgrades of Hexy

Post all your hardware and software upgrades/projects here.

Software modifications and upgrades of Hexy

Postby michal » Sun Jun 16, 2013 4:37 pm

Hi all, in this thread I will try to regularly post my software modifications of Hexy.
First of all I would like to state that I'm sticking to controlling Hexy via python since I try to improve my programming skills in this language and Hexy is just the right motivation for doing so :D.
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Software modifications and upgrades of Hexy

Postby michal » Sun Jun 16, 2013 5:34 pm

Binary communication

Recently I posted whole class which can be used for inverse-kinematic control of hexy (see here)
As many of you know and complained, when you send new servo update to Hexy, USB serial communication is interfering with servo callback and the servos start to shake (see e.g. here). This can be partially eliminated by using Bluetooth as I do.
But there is also a second problem which comes from slow servo update. When you want to send new servo positions for all 18 servos (e.g. during inverse kinematic control), then for each servo you send separate packet looking something like this "#20P1500\r" = 9 bytes of data. For 18 servos this means 18*9 = 162 bytes of data. Each cycle! 162 bytes * 8 bit /9600 baud = 140ms, which is also approximately the time between update of the first and the last servo. This causes a lot of problems for motion synchronization.

Therefore I modified servotorComm.py and Servotor32.cpp and added possibility to send servo update in binary values, all 19 servos at once.
I send in total 21 bytes which is transmitted in about 17ms and the servos are updated at the end of the transmission, all at once.

EDIT: I added head servo as well to the packet

servotorComm.py
Code: Select all
class Controller:
    def __init__(self,servos=32):
          self.Dict = [5,6,7, 9,10,11, 13,14,15, 16,17,18, 20,21,22, 24,25,26, 31]
          ....
   def sendBinary(self):
        toSend = '$'
        for i in range(18):
            servoPos = self.servos[self.Dict[i]].getPosuS() + self.servos[self.Dict[i]].getOffsetuS()
            if servoPos < 500:  servoPos = 500
            if servoPos > 2500: servoPos = 2500
            toSend += chr(int(round(servoPos/10.0)))
        toSend += '\r'
        self.serialHandler.sendLock.acquire()
        self.serialHandler.sendQueue.append(toSend)
        self.serialHandler.sendLock.release()

Servotor32.cpp
Code: Select all
...
boolean binary = false;
short inAllPos[19];
...
void Servotor32::processChar(char inChar){
   if (binary) {
     switch(inChar){
      case '\r':
      case '\n':
         binary = false;
         changeServo(5,inAllPos[0]);
         changeServo(6,inAllPos[1]);
         changeServo(7,inAllPos[2]);
         changeServo(9,inAllPos[3]);
         changeServo(10,inAllPos[4]);
         changeServo(11,inAllPos[5]);
         changeServo(13,inAllPos[6]);
         changeServo(14,inAllPos[7]);
         changeServo(15,inAllPos[8]);
         changeServo(16,inAllPos[9]);
         changeServo(17,inAllPos[10]);
         changeServo(18,inAllPos[11]);
         changeServo(20,inAllPos[12]);
         changeServo(21,inAllPos[13]);
         changeServo(22,inAllPos[14]);
         changeServo(24,inAllPos[15]);
         changeServo(25,inAllPos[16]);
         changeServo(26,inAllPos[17]);
         changeServo(31,inAllPos[18]);
        break;
      default:
         inAllPos[numCount] = byte(inChar)*10;
         numCount++;
         break;
      }
        }
   else {
     switch(inChar){
      case '$':
        binary = true;
        numCount = 0;
        break;
      case '#':
        servoCounting = true;
        ...
         ...
    }


Then when you want to send your servo commands in binary form, first update all servos as usual with setPos, but with move = False. And then you once call sendBinary(). Here is example from updated class kinematicRobot()
Code: Select all
for i in range(6):
           ...
           self.con.servos[self.Dict[i*3+0]].setPos(deg = int(-hipAngle/np.pi*180 + 90),     move = False)
           self.con.servos[self.Dict[i*3+1]].setPos(deg = int(-kneeAngle/np.pi*180 + 180), move = False)
           self.con.servos[self.Dict[i*3+2]].setPos(deg = int(ankleAngle/np.pi*180 - 180),  move = False)

self.con.sendBinary()


Effect is guaranteed! Even on USB connection I didn't observe almost any jitter. Here is video of Hexy running on 8 FPS as in old situation and 30 FPS when I used binary data transfer.
http://youtu.be/5ZAEujH8fjU

Remark: To update standard moves is a bit more complicated, because each move of each leg is called in separate thread which is not synchronized across all servos.
Last edited by michal on Sun Jul 07, 2013 12:48 pm, edited 1 time in total.
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Software modifications and upgrades of Hexy

Postby michal » Mon Jun 17, 2013 8:05 pm

Multi-point offset compensation

As was already described by rpcook on his blog, the servos we are using suffer from nonlinearity between the desired and real position. The recommended offset calibration helps to remove the main (static) position error, but there is also another - position dependent - error due to nonlinerity of used servo position sensor. Well, what would you expect for 3 bucks? :-).
The good news is that the error variation is usually limited to a few degrees, which is good enough for the basic moves.
The bad news is that when you want to do more advance control of Hexy (like inverse-kinematics, or writing), you need to calibrate the servos more thoroughly.

I use 7-point calibration for each servo. Calibration is done at -80, -60, -30, 0, 30, 60, and 80 degrees.
Image

On the print-screen of my offset calibration software you can see how much the offsets vary. Sometimes it is almost constant (see RF knee), sometimes it goes up and down (see e.g. RB knee)
Image

Then I use linear interpolation to calculate offset for any position. For that I slightly modified functions getOffsetDeg, getOffsetuS, setOffset, and move in class Servo in servotorComm.py. And I also created a new function calculateOffset where the linear interpolation is calculated. I also added two constants (CENTER = 1500, GAIN = 11.111), because I found that for Turnigy TGY-50090M servos (at least the batch I got) their values should be (CENTER = 1400, GAIN = 10.01).
offset[3] is for 0 deg, offset[0] is for -80 deg and offset[6] is for +80 deg.
Code: Select all
CENTER = 1500
GAIN = 11.111

class Servo:

    def __init__(self,servoNum,serHandler,servoPos=CENTER,active=False):
        self.serHandler = serHandler
        self.active = active
        self.servoNum = servoNum

        # Servo position and offset is stored in microseconds (uS)
        self.servoPos = servoPos
        self.offset = [0,0,0,0,0,0,0]


    def setPos(self, timing=None, deg=None, move=True):
        if timing != None:
            self.servoPos = timing
        if deg != None:
            self.servoPos = int(CENTER+float(deg)*GAIN)
        if move:
            self.active = True
            self.move()
            if debug: print "Moved ",self.servoNum
        if debug: print "Servo",self.servoNum,"set to",self.servoPos

    def getPosDeg(self):
        return (self.servoPos-CENTER)/GAIN

    def getPosuS(self):
        return self.servoPos

    def getActive(self):
        return self.active

    def getOffsetDeg(self, index = 3):
        return self.offset[index]/GAIN

    def getOffsetuS(self, index = 3):
        return self.offset[index]

    def setOffset(self, timing=None, deg=None,index = 3):
        if timing != None:
            self.offset[index] = timing
        if deg != None:
            self.offset[index] = int(float(deg)*GAIN)

    def calculateOffset(self, pos):
        if pos < (CENTER-80*GAIN):
            newServoPos = pos + self.offset[0]
        elif pos < (CENTER-60*GAIN):
            newServoPos = pos + int((self.offset[1]-self.offset[0])*(pos-(CENTER-80*GAIN))/(20*GAIN) + self.offset[0])
        elif pos < (CENTER-30*GAIN):
            newServoPos = pos + int((self.offset[2]-self.offset[1])*(pos-(CENTER-60*GAIN))/(30*GAIN) + self.offset[1])
        elif pos < CENTER:
            newServoPos = pos + int((self.offset[3]-self.offset[2])*(pos-(CENTER-30*GAIN))/(30*GAIN) + self.offset[2])
        elif pos < (CENTER+30*GAIN):
            newServoPos = pos + int((self.offset[4]-self.offset[3])*(pos-CENTER)/(30*GAIN) + self.offset[3])
        elif pos < (CENTER+60*GAIN):
            newServoPos = pos + int((self.offset[5]-self.offset[4])*(pos-(CENTER+30*GAIN))/(30*GAIN) + self.offset[4])
        elif pos < (CENTER+80*GAIN):
            newServoPos = pos + int((self.offset[6]-self.offset[5])*(pos-(CENTER+60*GAIN))/(20*GAIN) + self.offset[5])
        else:
            newServoPos = pos + self.offset[6]
   
        return newServoPos

    def reset(self):
        self.setPos(timing=CENTER)
        self.move()

    def kill(self):
        self.active = False
        toSend = "#%dL\r"%(self.servoNum)
        self.serHandler.sendLock.acquire()
        self.serHandler.sendQueue.append(toSend)
        self.serHandler.sendLock.release()
        if debug: print "Sending command #%dL to queue"%self.servoNum

    def move(self):
        if self.active:
            servoPos = self.calculateOffset(self.servoPos)
            # Auto-correct the output to bound within 500uS to 2500uS signals, the limits of the servos
            if servoPos < 500:
                servoPos = 500
            if servoPos > 2500:
                servoPos = 2500
               
            # Debug message if needed
            if debug: print "Sending command #%dP%dT0 to queue"%(self.servoNum,int(servoPos))

            # Send the message the serial handler in a thread-safe manner
            toSend = "#%dP%.4dT0\r"%(self.servoNum,int(servoPos))
            self.serHandler.sendLock.acquire()
            self.serHandler.sendQueue.append(toSend)
            self.serHandler.sendLock.release()
        else:
            try:
                toSend = "#%.4dL\r"%(self.servoNum,int(servoPos))
                self.serHandler.sendLock.acquire()
                self.serHandler.sendQueue.append(toSend)
                self.serHandler.sendLock.release()
                if debug: print "Sending command #%dL to queue"%self.servoNum
            except:
                pass


Then you probably also need modified saving and loading of the offsets in GUI.py:
Code: Select all
class App:
   ....
   def loadOffsets(self):
        # If there is one offset file in the folder, automatically load it
        off_files = []
        for filename in os.listdir(os.getcwd()):
            start, ext = os.path.splitext(filename)
            if ext == '.cfg':
                off_files.append(filename)

        if len(off_files) == 1:
            print "Opening",off_files[0]
            config = ConfigParser.ConfigParser()
            config.read(off_files[0])

            try:
                data = config.items('offsets')
                for line in data:
                    servoNum = int(line[0])
                    offset = line[1]
                    offset = string.strip(offset,"[] ")
                    offset = np.int_(string.split(offset,', '))
                    for servo in self.con.servos:
                        if self.con.servos[servo].servoNum == servoNum:
                            for j in range(len(offset)):   
                                self.con.servos[servo].setOffset(timing = offset[j], index = j)
                            break
                print "Automatically loaded offsets from",off_files[0]
            except:
                print "Automatic offset load failed, is there an offset file in the program directory?"

    def saveOffsets(self):
        cfgFile = asksaveasfile(filetypes = [('CFG', '*.cfg'),("All Files",".*")], defaultextension=".cfg")
        config = ConfigParser.ConfigParser()
        config.add_section("offsets")
        for i in range(len(self.con.servos)):
            offset = []
            for j in range(7):
                offset.append(int(self.con.servos[i].getOffsetuS(index = j)))
            config.set("offsets", "%.3d"%(i), offset)             
        config.write(cfgFile)
   ...


The user interface I created in GUI.py I present "as is".
Code: Select all
class App:
   def __init__(self, master, controller):
      ...
      self.Dict = [
            ("LF hip", 7),
            ("LF knee", 6),
            ("LF ankle", 5),
            ("LM hip", 11),
            ("LM knee", 10),
            ("LM ankle", 9),
            ("LB hip", 15),
            ("LB knee", 14),
            ("LB ankle", 13),
            ("RF hip", 24),
            ("RF knee", 25),
            ("RF ankle", 26),
            ("RM hip", 20),
            ("RM knee", 21),
            ("RM ankle", 22),
            ("RB hip", 16),
            ("RB knee", 17),
            ("RB ankle", 18),
            ("Head",31)
        ]
        self.OffsetControls = groupOffsetControl(self.frame, self.con, self.Dict, colX = 25, rowY = 0) # a new sub-frame is created at colX, rowY
        # to show the controls call: self.OffsetControls.show()
        # to hide the controls call:  self.OffsetControls.hide()


New class groupOffsetControl:
Code: Select all
class groupOffsetControl:
    def __init__(self, frame, con, Dict, rowY = 0, colX = 0):
        self.frame = Frame(frame,width = 1000, height = 500)#, bg = 'blue')
        self.con = con
        self.rowY = rowY
        self.colX = colX
        self.Dict = Dict           
        self.nrServo = IntVar(value = -1)
        self.nrOffset = IntVar(value = 3)
        self.activeServo = -1
        self.activeOffset = 3
        self.rangeOffsets = [-80,-60,-30,0,30,60,80]
 
        # radio buttons for servo selection
        self.addSpace([0,0])
        Radiobutton(self.frame, text = "", variable = self.nrServo, value = -1, command = self.pickServo).grid(row = 2, column = 2)
        Label(self.frame,text = "OFF", font = ("Tahoma", 10,"bold"), anchor = W).grid(row = 2, column = 1, sticky = W)
        self.addSpace([5,0])
        for i, (text, value) in zip(range(len(Dict)),Dict):
            Radiobutton(self.frame, text = "", variable = self.nrServo, value = i, command = self.pickServo).grid(row = i+6 + i/3, column = 2)
            Label(self.frame, text = text, font = ("Tahoma", 10,"bold"), anchor = W).grid(row = i+6 + i/3, column = 1, sticky = W)
        self.addSpace([9,0])
        self.addSpace([13,0])
        self.addSpace([17,0])
        self.addSpace([21,0])
        self.addSpace([25,0])
        self.addSpace([29,0])

        # radio buttons for offset selection
        for (j, text) in zip(range(7),self.rangeOffsets):
            Radiobutton(self.frame, text = "", variable = self.nrOffset, value = j, command = self.pickServo).grid(row = 4, column = j+4)
            Label(self.frame, text = str(text), font = ("Tahoma", 10,"bold"), width = 8).grid(row = 3, column = j+4)

        # labels
        self.labelOffsetVar = []
        self.labelOffset = []
        for i in range(len(Dict)):
            variables = []
            labels = []
            for j in range(7):
                var = StringVar()
                var.set(int(round(self.con.servos[self.Dict[i][1]].getOffsetDeg(index = j))))
                variables.append(var)
                l = Label(self.frame, textvariable = var, font = ("Tahoma", 10))
                l.grid(row = i+6 + i/3, column = j+4)
                labels.append(l)
            self.labelOffsetVar.append(variables)
            self.labelOffset.append(labels)                             

        self.addSpace([31,0])
       
        #offset plus
        Button(self.frame, text="+", font = ("Tahoma", 12, "bold"), width = 6, command=self.offsetInc).grid(row=0, column=6)

        #offset minus
        Button(self.frame, text="-", font = ("Tahoma", 12, "bold"), width = 6, command=self.offsetDec).grid(row=0, column=7)

    def addSpace(self, coords):
        Label(self.frame, text="\t\t", fg="red").grid(row=coords[0], column=coords[1])

    def pickServo(self):
        self.labelOffset[self.activeServo][self.activeOffset].config(font = ("Tahoma", 10), fg = "black")
        self.con.servos[self.Dict[self.activeServo][1]].kill()
       
        self.activeServo = self.nrServo.get()
        self.activeOffset = self.nrOffset.get()
       
        if self.activeServo <> -1:
            self.labelOffset[self.activeServo][self.activeOffset].config(font = ("Tahoma", 10,"bold"), fg = "red")
            self.con.servos[self.Dict[self.activeServo][1]].setPos(deg = self.rangeOffsets[self.activeOffset])

    def offsetInc(self):
        if self.activeServo <> -1:
            offset = self.con.servos[self.Dict[self.activeServo][1]].getOffsetDeg(index = self.activeOffset) + 1         
            self.labelOffsetVar[self.activeServo][self.activeOffset].set(int(offset))
            self.con.servos[self.Dict[self.activeServo][1]].setOffset(deg = offset, index = self.activeOffset)
            self.con.servos[self.Dict[self.activeServo][1]].move()

    def offsetDec(self):
        if self.activeServo <> -1:
            offset = self.con.servos[self.Dict[self.activeServo][1]].getOffsetDeg(index = self.activeOffset) - 1       
            self.labelOffsetVar[self.activeServo][self.activeOffset].set(int(offset))
            self.con.servos[self.Dict[self.activeServo][1]].setOffset(deg = offset, index = self.activeOffset)
            self.con.servos[self.Dict[self.activeServo][1]].move()
       

    def show(self):
        self.activeServo = -1
        self.activeOffset = 3
        self.nrServo.set(-1)
        self.nrOffset.set(3)
        self.frame.grid(row = self.rowY, column = self.colX, rowspan = 100, columnspan = 100, sticky = N+W)
       
    def hide(self):
        self.frame.grid_forget()
Last edited by michal on Sat Jun 22, 2013 11:12 am, edited 1 time in total.
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Software modifications and upgrades of Hexy

Postby michal » Thu Jun 20, 2013 7:52 pm

Virtual (kinematic) Hexy and modified GUI of PoCoMo

Today I'm going to present my virtual Hexy. I implemented this in PoCoMo GUI for ease of debugging (or when all my batteries are dead :o).
I made complete kinematic model of Hexy, which means calculation of the robot actual position/shape based on the servo setpoints.
This is including calculation of contact with the floor and possible robot tilt and also horizontal movement based on the information which legs are in contact with the floor.
Please, first see the image and the video which best explain the functionality
Image
http://youtu.be/dUgYIrFb5TM

EDIT: I made speed optimizations in the code and also and now it uses TKinter's canvas function for plotting. The consequence is that the plotting of the virtual robot is about 100x faster and it does not require matplotlib library anymore. The plotting function does not use any anti-aliasing so the picture looks a bit rough, but that's just a small price for the huge performance improvement and reduced dependency. Still, it requires numpy since I found it very difficult to get completely rid of arrays and fast operations provided by numpy

Here is the class virtualRobot which should be included in GUI.py
Code: Select all
import numpy as np

#########################################################################################################################
# create and plot Kinematic model of the robot
# by Michal G., 05-07-2013
# - updated, now without matplotlib and about 100x faster
#########################################################################################################################
class virtualRobot:
    def __init__(self, frame, con, Dict):
        self.con = con
        self.frame = frame
        self.canvas = Canvas(self.frame, width = 745, height = 550, bg = 'white')
        self.canvas.bind("<B1-Motion>", self.LeftClickMove)
        self.canvas.bind("<B3-Motion>", self.RightClickMove)
        self.canvas.bind("<Button-1>", self.MouseClick)
        self.canvas.bind("<Button-3>", self.MouseClick)
        self.canvas.bind("<ButtonRelease-1>", self.LeftClickRelease)
        self.canvas.bind("<ButtonRelease-3>", self.RightClickRelease)
        self.canvas.grid()

        self.plane = np.array([[-200,-200,0],[200,-200,0],[200,200,0],[-200,200,0]])
        # create floor
        self.planePlot = self.canvas.create_polygon((0,0,0,0,0,0), width = 2, fill ='#EAEAEA', outline = 'black')
        self.gridxPlots = []
        self.gridyPlots = []
        self.gridx = np.zeros((16,3))
        self.gridy = np.zeros((16,3))
        for i in range(8):
            self.gridxPlots.append(self.canvas.create_line((0,0,0,0), fill = '#555'))
            self.gridyPlots.append(self.canvas.create_line((0,0,0,0), fill = '#555'))
            self.gridx[i*2:i*2+2] = [[-200+i*50, -200, 0], [-200+i*50, 200 ,0]]
            self.gridy[i*2:i*2+2] = [[-200, -200+i*50, 0], [200, -200+i*50 ,0]]

        # create robot plot
        self.legPlots = []
        for i in range(6):
            self.legPlots.append(self.canvas.create_line((0,0,0,0), width = 8, fill = 'red'))
        self.bodyPlot = self.canvas.create_polygon((0,0,0,0), width = 8, outline = 'blue', fill = '')
        self.headPlot = self.canvas.create_polygon((0,0,0,0), width = 8, outline = 'darkgreen', fill = '')

        self.canvas.create_rectangle((3,3,745,550), width = 3, outline ='black')

        self.cameraAngle = [-0.5,4.0]
        self.cameraAngleOld = [-0.5,4.0]
        self.cameraZoom = 2500
        self.cameraZoomOld = 2500
        self.mousePos = [0,0]
           
        self.oldCenterPlot = 0
       
        self.bodyRadius = 100
        self.hip = 26
        self.thigh = 49
        self.foot = 55
        self.bodyAngle = 50.0/180*np.pi
        self.bodyAngles = [np.pi/2 - self.bodyAngle, np.pi/2, np.pi/2 + self.bodyAngle,
                          3*np.pi/2 - self.bodyAngle, 3*np.pi/2, 3*np.pi/2 + self.bodyAngle]

        d = self.bodyRadius
        self.body = np.array([[d*np.cos(self.bodyAngles[0]),d*np.sin(self.bodyAngles[0]),0],
                             [d*np.cos(self.bodyAngles[1]),d*np.sin(self.bodyAngles[1]),0],
                             [d*np.cos(self.bodyAngles[2]),d*np.sin(self.bodyAngles[2]),0],
                             [d*np.cos(self.bodyAngles[3]),d*np.sin(self.bodyAngles[3]),0],
                             [d*np.cos(self.bodyAngles[4]),d*np.sin(self.bodyAngles[4]),0],
                             [d*np.cos(self.bodyAngles[5]),d*np.sin(self.bodyAngles[5]),0]]).T
       
        self.head = np.array([[20,20,0],[20,20,40],[20,-20,40],[20,-20,0]]).T
       
        if legsMirrored:
            self.sign = [1,1,1,-1,-1,-1]
        else:
            self.sign = [1,1,1,1,1,1]

        self.Dict = self.reverseDict(Dict)

        self.oldContactB = [False,False,False,False,False,False]
        self.oldContactP = []
        self.oldXYRzVector = np.array([0,0,0], dtype = 'float')


        # possible stable leg combinations (3 legs on the ground)
        self.legCombi = [[0,1,3],[0,1,4],[0,1,5],
                         [0,2,3],[0,2,4],[0,2,5],
                         [1,2,3],[1,2,4],[1,2,5],
                         [0,3,4],[1,3,4],[2,3,4],
                         [0,3,5],[1,3,5],[2,3,5],
                         [0,4,5],[1,4,5],[2,4,5]]

        self.complementaryLegCombi = []
        for i in range(len(self.legCombi)):
            self.complementaryLegCombi.append(filter(lambda x: x not in self.legCombi[i], range(6)))
       
    # internal - reverse dictionary   
    def reverseDict(self,Dict):
        outDict = []
        for i in range(9):    # left side from front to back
            outDict.append(Dict[i][1])

        for i in range(3):     # right side from back to front
            outDict.append(Dict[15-3*i][1])
            outDict.append(Dict[16-3*i][1])
            outDict.append(Dict[17-3*i][1])
           
        outDict.append(Dict[18][1])  # head   
        return outDict

    # internal - check if a point P is inside trinagle P1,P2,P3
    def inTriangle(self, p0, p1, p2, p = [0,0]):
        a = p1[0]-p0[0]
        b = p2[0]-p0[0]
        c = p1[1]-p0[1]
        d = p2[1]-p0[1]
        e = p[0]-p0[0]
        f = p[1]-p0[1]
        det = a*d-b*c
        if det == 0:
            return False
        else:
            x = (e*d-f*b)/det
            y = (a*f-c*e)/det
            return -0.01 <= x <= 1 and -0.01 <= y <= 1 and x + y <= 1

    # internal - find which robot legs are touching the floor and how is the body tilted
    def findZRxRyPlane(self,legs):
        lowestCombi = []
        for (legCombi, compLegCombi) in zip(self.legCombi, self.complementaryLegCombi):          # go through 3-legs combinations
            p0 = legs[legCombi[0]][3,:]
            p1 = legs[legCombi[1]][3,:]
            p2 = legs[legCombi[2]][3,:]
            if self.inTriangle(p0,p1,p2):       # inTrinagle = body center of gravity (point 0,0) is in trinagle -> stable combination of legs
                v0 = p1-p0
                v1 = p2-p0
                normal = np.cross(v0,v1)        # calculate normal vector of the plane given by 3 leg endpoints (feets)
                d = np.dot(normal,p0)                                                       
                for i in compLegCombi:                                  # go through all remaining legs and check if any is lower than this combination
                        dist = np.dot(normal,legs[i][3,:])
                        if dist < d:
                            break
                if dist >= d :                                      # if no other leg is lower than combination of these 3, you found it
                    lowestCombi = legCombi
                   
        v0 = legs[lowestCombi[1]][3,:]-legs[lowestCombi[0]][3,:]
        v1 = legs[lowestCombi[2]][3,:]-legs[lowestCombi[0]][3,:]
        normal = np.cross(v0,v1)                                    # once more calculate normal vector, now of the plane given by 3 lowest legs
        angleX = math.atan(normal[1]/normal[2])                     # get tilt of the body in Rx and Ry
        angleY = math.atan(-normal[0]/normal[2])
        zDist = np.dot(normal,legs[lowestCombi[0]][3,:])/np.sqrt(np.dot(normal,normal))     # get body height above the floor
        return zDist, angleX, angleY

    # internal - find how the robot moved in horizontal plane since the last situation
    def updateXYRzMotionVector(self,legs):
        xDist = 0
        yDist = 0
        angleZ = 0

        points = np.array(map(lambda leg: leg[3,:], legs))
       
        newContactB = (points < 2)[:,2]  # which legs are max 2mm above ground ~ is on the the ground

        counter = 0
        for i in range(6):
            if self.oldContactB[i] and newContactB[i]:  # leg was and still is on the ground, calculated displacement
                counter += 1
                [x,y] = (points[i,0:2] - self.oldContactP[i,0:2])   #difference between previous and new position)
                xDist -= x
                yDist -= y

                r = points[i,0:2]
                angleZ += math.atan(np.dot([r[1],-r[0]],[x,y])/np.dot(r,r))

        # body displacement = average displacement of all legs on the ground
        if counter > 0:
            xDist /= counter
            yDist /= counter
            angleZ /= counter

        # prevention of rounding errors - if the calculated displacement is less than 1 mm or 0.01 rad, say it was none
        if np.abs(xDist) < 1:
            xDist = 0
        if np.abs(yDist) < 1:
            yDist = 0
        if np.abs(angleZ) < 0.01:
            angleZ = 0

        # the reference world displacement (x,y) is obtained from the robot local coordinated (xDist,yDist) rotated by the robot orientation
        x,y,t = self.RotZ([xDist,yDist,0], self.oldXYRzVector[2] + angleZ/2)
           
        self.oldXYRzVector += [x,y,angleZ]  # store actual robot position
        self.oldContactP = points
        self.oldContactB = newContactB
        return self.oldXYRzVector, (xDist, yDist, angleZ)
       
    def RotX(self,pos,angle):
        Rx = [[1,0,0],
              [0,math.cos(angle),-math.sin(angle)],
              [0,math.sin(angle),math.cos(angle)]]
        return np.dot(Rx,pos)

    def RotY(self,pos,angle):
        Ry = [[math.cos(angle),0,math.sin(angle)],
              [0,1,0],
              [-math.sin(angle),0,math.cos(angle)]]     
        return np.dot(Ry,pos)

    def RotZ(self,pos,angle):
        Rz = [[math.cos(angle),-math.sin(angle),0],
              [math.sin(angle),math.cos(angle),0],
              [0,0,1]]
        return np.dot(Rz,pos)

    def RotXYZ(self,pos,a):
        Rxyz = [[math.cos(a[2])*math.cos(a[1]), math.cos(a[2])*math.sin(a[1])*math.sin(a[0])-math.sin(a[2])*math.cos(a[0]), math.cos(a[2])*math.sin(a[1])*math.cos(a[0])+math.sin(a[2])*math.sin(a[0])],
                [math.sin(a[2])*math.cos(a[1]), math.sin(a[2])*math.sin(a[1])*math.sin(a[0])+math.cos(a[2])*math.cos(a[0]), math.sin(a[2])*math.sin(a[1])*math.cos(a[0])-math.cos(a[2])*math.sin(a[0])],
                [-math.sin(a[1])              , math.cos(a[1])*math.sin(a[0])                                             , math.cos(a[1])*math.cos(a[0])]]
        return np.dot(Rxyz,pos)

    def RotZX(self, pos, a = [0,0]):
        Rzx = [[math.cos(a[1]),-math.sin(a[1])                             , 0],
              [math.cos(a[0])*math.sin(a[1]),math.cos(a[0])*math.cos(a[1]), -math.sin(a[0])],
              [math.sin(a[0])*math.sin(a[1]),math.sin(a[0])*math.cos(a[1]), math.cos(a[0])]]
        return np.dot(Rzx,pos)

    # internal - build body and perform rotation and translation
    def buildBody(self, pos = [0,0,0], angle = [0,0,0]):
        body = pos + self.RotXYZ(self.body,angle).T       
        return body

    # internal - build one leg and perform rotation and translation
    def leg(self, pos = [0,0,0], angle = [0,0,0], legAngle = 0, servoAngles = [0,0,0]):
        h = [self.hip,0,0]
        t = self.RotY([self.thigh,0,0], servoAngles[1])
        f = self.RotY([self.foot,0,0], servoAngles[1] + servoAngles[2])

        d = np.vstack((h,t,f))
        d = self.RotZ(d.T, legAngle + servoAngles[0])
        d = self.RotXYZ(d, angle).T
       
        knee = pos + d[0]
        ankle = knee + d[1]
        tip = ankle + d[2]

        return knee, ankle, tip

    # internal - build legs and perform rotation and translation
    def legs(self, body, angle = [0,0,0]):

        legs = []
        for i in range(6):
            # get actual servo angles
            servoAngles = [self.con.servos[self.Dict[i*3+0]].getPosDeg(),
                           self.con.servos[self.Dict[i*3+1]].getPosDeg()*self.sign[i],
                           ankleOffset - self.con.servos[self.Dict[i*3+2]].getPosDeg()*self.sign[i]]
            servoAngles = map(lambda x: math.radians(x), servoAngles)

            knee, ankle, tip = self.leg(body[i], angle, self.bodyAngles[i], servoAngles)
            leg = np.vstack((body[i], knee, ankle, tip))
            legs.append(leg)

        return legs   
       
    # internal - build head and perform rotation and translation
    def buildHead(self, pos = [0,0,0], angle = [0,0,0], servoAngle = 0):
        d = self.RotZ(self.head, servoAngle)
        d = self.RotXYZ(d, angle).T + pos
        head = np.vstack((pos, d))
        return head

    # internal - build whole body and perform rotation and translation
    def buildRobot(self, pos = [0,0,0], angle = [0,0,0], withHead = False):
   
        body = self.buildBody(pos, angle)
        legs = self.legs(body, angle)
        if withHead:
            neck = (body[0,:]+body[5,:])/2
            head = self.buildHead(neck, angle, self.con.servos[self.Dict[18]].getPosDeg()/180*np.pi)
            return body, legs, head
        else:
            return body, legs

    # internal - rotate the scene according to camera and transpofr it to 2D
    def Projection3Dto2D(self, data3D, angle = [0,0]):
        D = self.RotZX(data3D.T, angle)
        data2DX = self.cameraZoom*D[0]/(D[1]-2000)+372
        data2DY = self.cameraZoom*D[2]/(D[1]-2000)+300
        return data2DX, data2DY

    # internal - mouse event handling (for scene camera rotation)
    def MouseClick(self, event):
        self.mousePos = [event.x, event.y]

    def LeftClickMove(self, event):
        self.cameraAngle[0] = self.cameraAngleOld[0] + (self.mousePos[1] - event.y)/100.0
        self.cameraAngle[1] = self.cameraAngleOld[1] - (self.mousePos[0] - event.x)/100.0

    def LeftClickRelease(self, event):
        self.cameraAngleOld[0] = self.cameraAngle[0]
        self.cameraAngleOld[1] = self.cameraAngle[1]

    def RightClickMove(self, event):
        self.cameraZoom = self.cameraZoomOld + (self.mousePos[1] - event.y)*2

    def RightClickRelease(self, event):
        self.cameraZoomOld = self.cameraZoom

    # plot the virtual robot                     
    def plot(self, centerPlot):

        # first build the robot with intial position and orientation (0,0,0)
        body, legs = self.buildRobot()

        # find which legs are touching the ground and how is the robot tilted
        zDist, angleX, angleY = self.findZRxRyPlane(legs)
       
        # build the robot again, now at known height zDist and tilt angleX, angleY
        body, legs = self.buildRobot(pos = [0, 0, -zDist], angle = [angleX, angleY, 0])

        # if the robot should be kept in the middle of the plot, reset movement vector
        if self.oldCenterPlot and not(centerPlot):
            self.oldXYRzVector[0] = 0
            self.oldXYRzVector[1] = 0

        # find how the robot moved in horizontal plane since the last situation   
        (xDist,yDist,angleZ), (dX, dY, dR) = self.updateXYRzMotionVector(legs)
   
        if centerPlot:
            # build robot with known rotation and height, but centered in the plot
            body, legs, head = self.buildRobot(pos = [0, 0, -zDist], angle = [angleX, angleY, angleZ], withHead = True)
            xOff = -int(xDist)%50 - 200
            yOff = -int(yDist)%50 - 200
            # move the grid under the robot instead of robot
            for i in range(8):
                self.gridx[i*2:i*2+2] = [[xOff+i*50, -200, 0], [xOff+i*50, 200 ,0]]
                self.gridy[i*2:i*2+2] = [[-200, yOff+i*50, 0], [200, yOff+i*50 ,0]]
           
        else:
            # build robot with known position and rotation
            body, legs, head = self.buildRobot(pos = [xDist, yDist, -zDist], angle = [angleX, angleY, angleZ], withHead = True)
       
        # stack data to one array
        data3D = body
        for i in range(6):
            data3D = np.vstack((data3D,legs[i]))
        data3D = np.vstack((data3D, head, self.plane, self.gridx, self.gridy))
        # 3D to 2D transformation
        data2DX, data2DY = self.Projection3Dto2D(data3D, self.cameraAngle)
       
        # plot the robot - update plot data
        self.canvas.coords(self.bodyPlot,data2DX[0],data2DY[0],data2DX[1],data2DY[1],data2DX[2],data2DY[2],data2DX[3],data2DY[3],data2DX[4],data2DY[4],data2DX[5],data2DY[5])
        for i in range(6):
            self.canvas.coords(self.legPlots[i],data2DX[i*4+6],data2DY[i*4+6],data2DX[i*4+7],data2DY[i*4+7],data2DX[i*4+8],data2DY[i*4+8],data2DX[i*4+9],data2DY[i*4+9])
        i = 30
        self.canvas.coords(self.headPlot,data2DX[i],data2DY[i],data2DX[i+1],data2DY[i+1],data2DX[i+2],data2DY[i+2],data2DX[i+3],data2DY[i+3],data2DX[i+4],data2DY[i+4])
        i = 35
        self.canvas.coords(self.planePlot,data2DX[i],data2DY[i],data2DX[i+1],data2DY[i+1],data2DX[i+2],data2DY[i+2],data2DX[i+3],data2DY[i+3])
        for i in range(8):
            self.canvas.coords(self.gridxPlots[i],data2DX[i*2+39],data2DY[i*2+39],data2DX[i*2+40],data2DY[i*2+40])
            self.canvas.coords(self.gridyPlots[i],data2DX[i*2+55],data2DY[i*2+55],data2DX[i*2+56],data2DY[i*2+56])

        self.oldCenterPlot = centerPlot

Then you need to create a sub-frame in App.__init__. And create an instance of virtual robot .
Code: Select all
class App:
    def __init__(self, master, controller):
        ...
        # Setup names and servo assignment ###############################################################
        self.Dict = [
            ("LF hip", 7),
            ("LF knee", 6),
            ("LF ankle", 5),
            ("LM hip", 11),
            ("LM knee", 10),
            ("LM ankle", 9),
            ("LB hip", 15),
            ("LB knee", 14),
            ("LB ankle", 13),
            ("RF hip", 24),
            ("RF knee", 25),
            ("RF ankle", 26),
            ("RM hip", 20),
            ("RM knee", 21),
            ("RM ankle", 22),
            ("RB hip", 16),
            ("RB knee", 17),
            ("RB ankle", 18),
            ("Head",31)
        ]
        ...
        # create second frame with plot window ##########################################################
        self.frame2 = Frame(self.frame,width = 745, height = 550)
        self.frame2.grid(row = counter/3+2, column = 1, rowspan = 100, columnspan = 100, sticky = N+W)
        self.centerPlot = IntVar(value = 1)
        Checkbutton(self.frame, text="Auto center plot", var=self.centerPlot).grid(row=counter/3, column=15, columnspan = 2)
                 
        # Create robot figure
        self.virtualRobot = virtualRobot(self.frame2, self.con, self.Dict)

The plot is updated in each call of App.poll
Code: Select all
class App:
    ...
    def poll(self):
        ...
        # draw the robot
        self.virtualRobot.plot(self.centerPlot.get())
       ...



That's all for now. Next time I will add a link to my complete PoCoMo code. Stay tuned for more... :ugeek:
Last edited by michal on Sun Jul 07, 2013 12:46 pm, edited 1 time in total.
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Software modifications and upgrades of Hexy

Postby TooManySecrets » Fri Jun 21, 2013 8:52 pm

I hate to sweer but this is f****** amazing. You are the man Michal. The offset settings alone are gr8.
Image
TooManySecrets
 
Posts: 79
Joined: Mon Jan 14, 2013 5:02 pm

Re: Software modifications and upgrades of Hexy

Postby michal » Sat Jun 22, 2013 9:08 am

Doodling with Hexy

This is one of the applications of the inverse-kinematic control I made. I must confess that I was inspired by rpcook's writing with Hexy. First I though that I will make Hexy write, but then I got an idea to draw using Hexy. The code is much simpler. Hexy just follows mouse cursor and whenever you press left button it goes a bit lower. For the pen I'm using pen refill which goes though the holes in Hexy. It is important that the pen is rigidly connected to the robot, otherwise the drawing accuracy will be low. See the video of doodling Hexy and please excuse my drawing skills :|
http://youtu.be/SWA_rEiLPGc

The code is pretty simple. It creates a separate frame and a canvas inside. And then proper responses to the mouse actions. It also creates a slider to adjust pen height.
Code: Select all
class groupDrawingControl:
    def __init__(self, frame, con, Dict, rowY = 0, colX = 0):
        self.frame = Frame(frame,width = 1000, height = 500)
        self.rowY = rowY
        self.colX = colX
        self.on = False                     
        self.iZ = IntVar(value = 50)
        self.pos = [0, 0, self.iZ.get() + 5]
        self.posOld = [0, 0, self.iZ.get() + 5]
        self.lastCursorX = 0
        self.lastCursorY = 0
       
        self.scaleZ = Scale(self.frame, from_= 100, to = 35, length = 300, showvalue = 0, var = self.iZ, command = self.calibrateHeight)
        self.scaleZ.grid(row = 2, column = 1, rowspan = 3, sticky = S)
        Label(self.frame, textvariable = self.iZ, width = 3, font=('Tahoma', 10,'bold')).grid(row = 3, column = 0)
        Label(self.frame, text = 'Pen height', font=('Tahoma', 10,'bold')).grid(row = 1, column = 0, columnspan = 2, sticky = S)

        self.canvas = Canvas(self.frame, width = 500, height = 500, cursor = 'crosshair', bg = 'white')
        self.canvas.create_rectangle(3,3,501,501, width = 2, outline = 'black')
        self.canvas.bind("<Motion>", self.MoveCursor)
        self.canvas.bind("<B1-Motion>", self.LeftClickMove)
        self.canvas.bind("<Button-1>", self.LeftClick)
        self.canvas.bind("<ButtonRelease-1>", self.LeftClickRelease)
        self.canvas.grid(row = 0, column = 2, rowspan = 5)

        Button(self.frame, text = 'Clear canvas', width = 15, command = self.ClearCanvas).grid(row = 8, column = 2)
       
        self.Robot = kinematicRobot(con, Dict)

    def Loop(self):
        if self.on:
            # if the cursor position changed, update robot
            if  (self.pos != self.posOld):
                self.Robot.updateRobot(self.pos)

            self.posOld[0:3] = self.pos[0:3]

    def MoveCursor(self, event):
        self.pos[0] = (250 - event.y)/8.0
        self.pos[1] = (250 - event.x)/8.0
     
    def LeftClick(self, event):
        self.canvas.configure(cursor = 'target')
        self.lastCursorX = event.x
        self.lastCursorY = event.y
        self.pos[2] = self.iZ.get() # pen down

    def LeftClickMove(self, event):
        self.pos[0] = (250 - event.y)/8.0
        self.pos[1] = (250 - event.x)/8.0
        self.canvas.create_line((self.lastCursorX, self.lastCursorY, event.x, event.y), width = 2)
        self.lastCursorX = event.x
        self.lastCursorY = event.y

    def LeftClickRelease(self, event):
        self.canvas.configure(cursor = 'crosshair')
        self.pos[2] = self.iZ.get() + 5  # pen up

    def ClearCanvas(self):
        self.canvas.delete('all')
        self.canvas.create_rectangle(3,3,501,501, width = 2, outline = 'black')

    def calibrateHeight(self, event):
        self.pos[2] = self.iZ.get() + 5  # pen up

    def show(self):
        self.frame.grid(row = self.rowY, column = self.colX, rowspan = 100, columnspan = 100, sticky = N+W)
        self.Robot.initRobot(self.pos[2])
        self.on = True
       
    def hide(self):
        self.frame.grid_forget()
        self.on = False


The usage is similar to the other classes I made:
Code: Select all
class App:

    def __init__(self, master, controller):
        ...
        self.Dict = [
            ("LF hip", 7),
            ("LF knee", 6),
            ("LF ankle", 5),
            ("LM hip", 11),
            ("LM knee", 10),
            ("LM ankle", 9),
            ("LB hip", 15),
            ("LB knee", 14),
            ("LB ankle", 13),
            ("RF hip", 24),
            ("RF knee", 25),
            ("RF ankle", 26),
            ("RM hip", 20),
            ("RM knee", 21),
            ("RM ankle", 22),
            ("RB hip", 16),
            ("RB knee", 17),
            ("RB ankle", 18),
            ("Head",31)
        ]
        self.DrawingControls = groupDrawingControl(self.frame, self.con, self.Dict, colX = 25, rowY = 0)
        # to show the controls call: self.DrawingControls.show()
        # to hide the controls call:  self.DrawingControls.hide()


Calling update in infinite loop
Code: Select all
def poll(self):
      ...
     self.DrawingControls.Loop()
     ...
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Software modifications and upgrades of Hexy

Postby michal » Sat Jun 22, 2013 12:45 pm

And here is what you all have been waiting for - My complete PoCoMo + Servotor software package:
https://www.dropbox.com/s/ug3apvacijevuzo/PoCoMo%202.1.zip

Enjoy!

PS: I just started cooking another 2 applications for Hexy, stay tuned for more! :ugeek:
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Software modifications and upgrades of Hexy

Postby TooManySecrets » Sat Jun 22, 2013 1:33 pm

Stop doing this m8, everytime i get on the forum i have my yaw drop :)

Does the code have any other req, other the the stuff the old code needed. I cant seam to run it.
Image
TooManySecrets
 
Posts: 79
Joined: Mon Jan 14, 2013 5:02 pm

Re: Software modifications and upgrades of Hexy

Postby michal » Sat Jun 22, 2013 1:51 pm

TooManySecrets wrote:Stop doing this m8, everytime i get on the forum i have my yaw drop :)

Does the code have any other req, other the the stuff the old code needed. I cant seam to run it.


Ad first - get used to it :-D
Ad second - it needs numpy, matplotlib, and pygame. The last one you can skip, but you need do disable all code referring to Joystick (in class groupKinematicControl: __init__() and Loop())
michal
 
Posts: 36
Joined: Thu Jun 06, 2013 7:01 pm
Location: Delft, NL

Re: Software modifications and upgrades of Hexy

Postby TooManySecrets » Sat Jun 22, 2013 3:50 pm

What python version are you using, could you pls maybe post direct links to the stuff we need. There is a lot of downloads on all of those pages.

Edit:

Was missing pyserial :)

It works perfect now.
Last edited by TooManySecrets on Sat Jun 22, 2013 7:13 pm, edited 1 time in total.
Image
TooManySecrets
 
Posts: 79
Joined: Mon Jan 14, 2013 5:02 pm

Next

Return to Projects and Upgrades

Who is online

Users browsing this forum: No registered users and 1 guest

cron