Software modifications and upgrades of Hexy

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 :smiley:.

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

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

...
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()

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.

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? :slight_smile:.
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.

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)

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.

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:

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”.

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:

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()

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 :astonished:).
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


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

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 .

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

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:

I hate to sweer but this is f****** amazing. You are the man Michal. The offset settings alone are gr8.

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 :neutral_face:
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.

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:

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

def poll(self):
      ...
     self.DrawingControls.Loop()
     ...

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:

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

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

[quote=“TooManySecrets”]Stop doing this m8, everytime i get on the forum i have my yaw drop :slight_smile:

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

Ad first - get used to it :smiley:
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: [i]init/i and [i]Loop/i)

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 :slight_smile:

It works perfect now.

This is exactly what I was afraid of. It’s difficult to help you if you don’t know how to download and install a library for python.
I’m using Python 2.7.3 by the way. I will not give you direct link, because this depends on the OS you are using. I have Win XP, 32bit.

Have all the stuff instaled now. Python 2.7 and the needed libraries. Try to open the pomoco.py and a window opens and closes. Cant even see what it says since its very fast.

For debugging it is better to open PoCoMo in IDLE (right click on *.py and choose “Edit with IDLE”) there you can run the code and see all error messages.

Michal, just started looking at your code. One of my concerns is from your second post on this thread that your binary communication protocol might preclude using any “old PoMoCo” code and the ASCII based serial interface. Is this the case?

I’ve had a look at your Servotor32.cpp, which seems to suggest that it’ll understand either communication protocol. Could you please confirm before I have to remember how to reflash Hexy’s brain?

You got a website or a blog somewhere with a Donate button? Feels like this level of Hexy awesomeness deserves a coffee/ beer bought for you.

[quote=“rpcook”]Michal, just started looking at your code. One of my concerns is from your second post on this thread that your binary communication protocol might preclude using any “old PoMoCo” code and the ASCII based serial interface. Is this the case?

I’ve had a look at your Servotor32.cpp, which seems to suggest that it’ll understand either communication protocol. Could you please confirm before I have to remember how to reflash Hexy’s brain?

You got a website or a blog somewhere with a Donate button? Feels like this level of Hexy awesomeness deserves a coffee/ beer bought for you.[/quote]

You are right, my version of Servotor32 also supports the old way of communication. I use the binary communication only for IK control, but this can be disabled if necessary. The only thing I messed up is the ping() function is Servotor32, because I’m playing with it now and I forgot to reverse these changes before upload.
Ad donation - I don’t do this for money, but it feels good to be appreciated :slight_smile:

Yeah, I’m going to have to find a good way to package all of this up in such a way that people aren’t going to have to install a bajillion plugins. PySerial seems to cause enough support headaches, I can’t imagine them having to install all the above as well.

In terms of a package for general use, might I suggest rolling in the binary communication protocol (those 30fps videos blow my mind), and then use some implementation of some IK demos (like the body rotation/ translation), but perhaps implemented in native Python code so that users don’t have to install NumPy. Not requiring extra libraries for the IK code will also make it easier to port to other platforms (Android?).

I’ve not had a chance to investigate in much detail yet, but I think that if you skip out the fancy graphics and joystick controls you could lose the other two plugins (matplotlib and pygame). Hopefully you should be able to implement something with the stock Python controllers (sliders etc) that will give you something of this level of control (of Hexy only, no mirroring on the screen)…

A lot of numpy commands can be done by math, but I’ve no experience with math and arrays/matrices.
matplotlib is only for the virtual robot, but I find this very useful tool, especially for beginners because you can see what you do to the robot with your experiments without actually damaging it.
pygame is indeed optional and can be easily removed from the code

Thanks for the tips Michal. My IK code uses the math library only, but doesn’t quite finish the job to include body rotations like Michal’s. I’m happy to go about and port what’s implemented in NumPy into math if that’d help the community.

Joe, what would be the most useful from your point of view? It’d be really nice if we can work on something that will benefit lots of Hexy users.

Michal,

I’ve had a bit of a chance to play with this code now. Firmware on Hexy’s microcontroller compiled fine and still works a treat, well done.

Your updated PoMoCo seemed to run fine after I’d tweaked your servotorComm.py to include the updates (around line 95) that allow it to automatically connect to the Bluetooth comm port under linux (Ubuntu). I noticed that your code had commented out that whole section… Any particular reason?

The GUI worked fine and seemed very slick. It might be my low screen resolution (only 600 lines) or possibly the Ubuntu OS, but the virtual Hexy didn’t seem to render properly. I got the grid, but not all the bits of limbs are in the right location, looks like a failing at the Z-axis scaling to me.

One bug bear though: when I quit the GUI I get the following error at the console from which I launched Python.

invalid command name "188452132callit" while executing "188452132callit" ("after" script)
Problem with this is that it hangs up the Python instance and I have to manually kill the process. The “stock” PoMoCo doesn’t show this behaviour. Any clues?