Thanks! Yes, I’m using Bluetooth + Python. I made a lot of hardware and software modifications. I will make more posts about it soon.
Michal, that’s some awesome work. I see that you’ve implemented body rotations as well as lateral translations with your fixed feet code. Excellent.
How did you choose the location for the feet to maximise the range of motion of Hexy’s body?
Hardware and software changes? Could you elaborate? I presume that the software changes were exclusively in the Python code as I can see the legs are moving through a series of fixed points, rather than a smooth motion through a path? What hardware changes were needed to get to this state?
Finally, can we have a go? Source code? 
Here is how to calculate inverse kinematics for Hexy. Inverse kinematics calculates required servo angles from desired body and leg position/orientation.
For the calculations, I’m using numpy library
import numpy as npYou start with defining your body:
Hlength = 26
Tlength = 49
Flength = 55
bodyRadius = 100.0
bodyAngle = 50.0/180*np.pi
bodyAngles = [np.pi/2 - bodyAngle, np.pi/2, np.pi/2 + bodyAngle,
3*np.pi/2 - bodyAngle, 3*np.pi/2, 3*np.pi/2 + bodyAngle]
d = bodyRadius
body = np.array([[d*np.cos(bodyAngles[0]),d*np.sin(bodyAngles[0]),0],
[d*np.cos(bodyAngles[1]),d*np.sin(bodyAngles[1]),0],
[d*np.cos(bodyAngles[2]),d*np.sin(bodyAngles[2]),0],
[d*np.cos(bodyAngles[3]),d*np.sin(bodyAngles[3]),0],
[d*np.cos(bodyAngles[4]),d*np.sin(bodyAngles[4]),0],
[d*np.cos(bodyAngles[5]),d*np.sin(bodyAngles[5]),0]])
Then you perform rotation and translation of the body to desired new coordinates. For that you need rotation matrix for X,Y,Z rotations. You can do the rotations separately or combine them in one big matrix as I did:
Remark: It DOES mater in which order you perform rotational transformation (matrix multiplication). For info see Wiki: http://en.wikipedia.org/wiki/Rotation_matrix
def RotXYZ(point,a):
Rxyz = [[np.cos(a[2])*np.cos(a[1]), np.cos(a[2])*np.sin(a[1])*np.sin(a[0])-np.sin(a[2])*np.cos(a[0]), np.cos(a[2])*np.sin(a[1])*np.cos(a[0])+np.sin(a[2])*np.sin(a[0])],
[np.sin(a[2])*np.cos(a[1]), np.sin(a[2])*np.sin(a[1])*np.sin(a[0])+np.cos(a[2])*np.cos(a[0]), np.sin(a[2])*np.sin(a[1])*np.cos(a[0])-np.cos(a[2])*np.sin(a[0])],
[-np.sin(a[1]), np.cos(a[1])*np.sin(a[0]), np.cos(a[1])*np.cos(a[0])]]
return np.dot(Rxyz,point)And you perform Rotation and Translation of the body:
Tr = [0, 0, 75]
Rot = [-0.2*np.pi/6, 0.2*np.pi/6, -0.1*np.pi/2]
b = Tr + RotXYZ(body.T, Rot).T # ".T" means transpose - due to my definition of X,Y,Z coordinates in columns instead of rows
Now I make a small trick to obtain an unique solution for hip angles. I need to create a second set of body points which are just slightly rotated in Rz at first.
def RotZ(pos,angle):
Rz = [[np.cos(angle),-np.sin(angle),0],
[np.sin(angle),np.cos(angle),0],
[0,0,1]]
return np.dot(Rz,pos)
b2 = RotZ(body.T, 0.001).T
b2 = Tr + RotXYZ(b2.T, Rot).TRemark: For clarity, the figure shows the Rz rotation 100x amplified
Now you define/obtain your desired/previous foot position:
f = [-50, -150, 0]
Next step is to calculate projection P of the foot point F to the plane defined by the body B. For that you need to calculate normal vector of the B-plane. For details see Wiki: http://en.wikipedia.org/wiki/Plane_(geometry) and then project the point to the plane, see e.g. http://stackoverflow.com/questions/8942950/how-do-i-find-the-orthogonal-projection-of-a-point-onto-a-plane
v0 = b[1] - b[0] # 2 vectors from any 3 points defining the plane B
v1 = b[2] - b[0]
normal = np.cross(v0, v1)
normal /= np.linalg.norm(normal)
p = f - np.dot(normal, f - b[4])*normal
Now you can calculate hip angle. For that you utilize the second set of body points from which we make a vector: B-B*. This vector is perpendicular to the hip servo zero angle. The second vector is from the projected foot point to to the body point: P-B. Now we calculate angle between those two vectors (see Wiki: http://en.wikipedia.org/wiki/Dot_product):
Remark: You might ask why I didn’t use the center body point instead of this cumbersome B*. You must realize that solution for an angle between two vectors is always in range of 0 to 180 degrees. For zero hip servo angle the solution would be exactly 180 deg and any other angle would always be smaller than 180 deg. However, you would not knot to which side to turn, because the sign is always positive. In my calculation, the solution gives 90 degree when hip servo angle is 0 and if you offset these 90 degrees you get the needed servo angle.
h0 = p - b[4]
h1 = b1[4] - b[4]
Hangle = (np.arccos(np.dot(h0,h1)/np.linalg.norm(h0)/np.linalg.norm(h1))) - np.pi/2 
Next step is to calculate knee servo position from know hip length (Hlength) and normalized vector h0
= distance Hlength from B[4] in direction h0
k = b[4] + Hlength*h0/np.linalg.norm(h0)
Now you need to calculate two lengths of K-F and B-F:
kf = np.linalg.norm(k - f)
bf = np.linalg.norm(b[4] - f)
The ankle and knee angles are then calculated using cosine law (see wiki: http://en.wikipedia.org/wiki/Cosine_law):
cos(gamma) = (a^2+b^2-c^2)/(2ab).
Ankle angle by using thigh and foot lengths (Tlength, Flength) and K-F length.
And knee angle as combination of 2 angles. And of course you have to offset them to obtain servo angles.
Aangle = np.pi - np.arccos((Tlength**2 + Flength**2 - kf**2)/(2*Tlength*Flength))
Kangle = np.pi - np.arccos((Hlength**2 + kf**2 - bf**2)/(2*Hlength*kf)) + np.arccos((Tlength**2 + kf**2 - Flength**2)/(2*Tlength*kf))
And so you get the solution:
Michal. Wow. That’s such an informative post!
I look forward to digesting this information in some detail this weekend. Love the graphs. Thanks!
Here is complete class I made for controlling the robot by inverse kimenatics. You can append this at the end of robot.py
EDIT:
- speed optimization (about 3x faster now), but still uses numpy for several operations. Due to speed reasons.
- uses binary serial communication (see here).
import numpy as np
#################################################################################################
# Inverse-kinematic robot Hexy
# by Michal G., 01-07-2013
# - speed optimization (about 3x faster now)
# - it uses binary serial communication for data transfer, see http://forum.arcbotics.com/viewtopic.php?f=21&t=481#p2979
#################################################################################################
class kinematicRobot():
def __init__(self, con, Dict):
self.con = con
self.Dict = self.reverseDict(Dict)
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]
self.CycleStep = 0 # counter for moving legs sychnronization
self.MovingLegs = 3 # how many legs are moving at once
self.FootPos = np.zeros((6,3)) # X,Y,Z position of all feet
self.footDist = 160 # nominal distance of foot from body center. Empirical value
for i in range(6): # reset feet position
self.FootPos[i] = [self.footDist*np.cos(self.bodyAngles[i]),self.footDist*np.sin(self.bodyAngles[i]),0]
d = self.bodyRadius
self.body = np.array([[d*math.cos(self.bodyAngles[0]),d*math.sin(self.bodyAngles[0]),0],
[d*math.cos(self.bodyAngles[1]),d*math.sin(self.bodyAngles[1]),0],
[d*math.cos(self.bodyAngles[2]),d*math.sin(self.bodyAngles[2]),0],
[d*math.cos(self.bodyAngles[3]),d*math.sin(self.bodyAngles[3]),0],
[d*math.cos(self.bodyAngles[4]),d*math.sin(self.bodyAngles[4]),0],
[d*math.cos(self.bodyAngles[5]),d*math.sin(self.bodyAngles[5]),0]]).T
self.body2 = self.RotZ(self.body,0.001)
if legsMirrored:
self.sign = [1,1,1,-1,-1,-1]
else:
self.sign = [1,1,1,1,1,1]
# internal - reverse servo 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 - Rotation X,Y,Z
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)
# internal - Rotation Z
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)
# internal - build body and perform translation
def buildBody(self, pos = [0,0,0], angle = [0,0,0]):
b = self.RotXYZ(self.body,angle).T + pos
b2 = self.RotXYZ(self.body2,angle).T + pos
return b, b2
# internal - shift all legs in X,Y,Rz
def setFeetPosInc(self, (x, y, Rz) = (0,0,0)):
self.FootPos = self.RotZ(self.FootPos.T, Rz*np.pi/180).T + [x, y, 0]
# set number of legs moving at once (1 to 3)
def setMovingLegs(self, iMovingLegs = 2):
if iMovingLegs > 0 and iMovingLegs < 4:
self.CycleStep = 0
self.MovingLegs = iMovingLegs
# initialization
def initRobot(self, z = 75):
# get body on the ground (z = 35 mm)
b = np.array([self.bodyRadius, 35])
k = np.array([self.bodyRadius + self.hip, 35])
f = np.array([self.footDist, 0])
kf = np.linalg.norm(k - f)
bf = np.linalg.norm(b - f)
ankleAngle = np.arccos((self.thigh**2 + self.foot**2 - kf**2)/(2*self.thigh*self.foot))
kneeAngle = np.arccos((self.hip**2 + kf**2 - bf**2)/(2*self.hip*kf)) + np.arccos((self.thigh**2 + kf**2 - self.foot**2)/(2*self.thigh*kf))
for i in range(6):
self.con.servos[self.Dict[i*3+1]].setPos(deg = int(-kneeAngle/np.pi*180 + 180)*self.sign[i], move = False)
self.con.servos[self.Dict[i*3+2]].setPos(deg = int(ankleAngle/np.pi*180 - 180 + ankleOffset)*self.sign[i], move = False)
self.con.sendBinary()
time.sleep(0.5)
# reset hip position
for i in range(6):
self.con.servos[self.Dict[i*3+0]].setPos(deg = 0)
self.con.sendBinary()
time.sleep(0.3)
# get body to nominal height z
b = np.array([self.bodyRadius, z])
k = np.array([self.bodyRadius + self.hip, z])
kf = np.linalg.norm(k - f)
bf = np.linalg.norm(b - f)
ankleAngle = np.arccos((self.thigh**2 + self.foot**2 - kf**2)/(2*self.thigh*self.foot))
kneeAngle = np.arccos((self.hip**2 + kf**2 - bf**2)/(2*self.hip*kf)) + np.arccos((self.thigh**2 + kf**2 - self.foot**2)/(2*self.thigh*kf))
for i in range(6):
self.con.servos[self.Dict[i*3+1]].setPos(deg = int(-kneeAngle/np.pi*180 + 180)*self.sign[i], move = False)
self.con.servos[self.Dict[i*3+2]].setPos(deg = int(ankleAngle/np.pi*180 - 180 + ankleOffset)*self.sign[i], move = False)
self.con.sendBinary()
# update robot position - the main calculation loop
def updateRobot(self, pos = (0,0,75), angle = (0,0,0), move = False, dPos = (0,0,0), send = True):
"""
The idea is that during move the body stays still, but the legs move in opposite direction
The body position and orientation is given by absolute pos and angle.
If move is enabled, the legs are shifted by dPos = (dx,dy,dRz) each time
"""
angle = map(lambda x: math.radians(x), angle) # angles from deg to rad
# build body with desired position and orientation
b, b2 = self.buildBody(pos, angle)
# if move, shift all feet position by dPos = (dx,dy,dRz) increment
if move:
self.setFeetPosInc(dPos)
# calculate body normal vector
v0 = b[1] - b[0]
v1 = b[2] - b[0]
normal = np.cross(v0, v1)
normal /= math.sqrt(np.dot(normal,normal))
doMove = False
"""
1 Cycle step = one leg move up or down
Cycle steps = 12/MovingLegs
if MovingLegs = 2
Cycle: Leg: Vertical move:
0 0 and 3 Up
1 0 and 3 Down
2 1 and 4 Up
3 1 and 4 Down
4 2 and 5 Up
5 2 and 5 Down
0 0 and 3 Up
...
"""
# if legs movement enabled or some legs in the air, update legs position
if move or (self.CycleStep % 2 == 1):
d = self.footDist
i = self.CycleStep/2
for j in range(self.MovingLegs): # reset the position of the legs which should move this cycle (1 to 3 legs)
self.FootPos[i + j*6/self.MovingLegs] = [d*math.cos(self.bodyAngles[i + j*6/self.MovingLegs]),d*math.sin(self.bodyAngles[i + j*6/self.MovingLegs]),0]
self.CycleStep += 1
self.CycleStep %= (12/self.MovingLegs)
if (self.CycleStep % 2 == 1): # go up with the legs
doMove = True
# calculate inverse kinematics for each leg
for i in range(6):
f = self.FootPos[i]
f_b = f - b[i]
p = f - np.dot(normal, f_b)*normal # x = (q - (n.(q-p))*n , q = out of plane, p = plane point, n = normal
h0 = p - b[i]
h1 = b2[i] - b[i]
h0norm = math.sqrt(np.dot(h0,h0))
h1norm = math.sqrt(np.dot(h1,h1))
hipAngle = math.acos(np.dot(h0,h1)/h0norm/h1norm)
k = b[i] + self.hip*h0/h0norm
k_f = k-f
kfnorm = math.sqrt(np.dot(k_f,k_f)) # norm
x = (self.thigh**2 + self.foot**2 - kfnorm**2)/(2*self.thigh*self.foot)
if -1 <= x <= 1:
ankleAngle = math.acos(x)
else:
ankleAngle = np.nan
bfnorm = math.sqrt(np.dot(f_b,f_b)) # norm
x = (self.hip**2 + kfnorm**2 - bfnorm**2)/(2*self.hip*kfnorm)
y = (self.thigh**2 + kfnorm**2 - self.foot**2)/(2*self.thigh*kfnorm)
if -1 <= x <= 1 and -1 <= y <= 1:
kneeAngle = math.acos(x) + math.acos(y)
else:
kneeAngle = np.nan
# move proper legs up
if doMove and (self.CycleStep / 2 == i % (6/self.MovingLegs)):
kneeAngle += np.pi/5.0
ankleAngle -= np.pi/5.0
# send updated angles to servos only if solution exists
if np.isnan((hipAngle, kneeAngle, ankleAngle)).any():
print "No solution for leg %d" % i
else:
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)*self.sign[i], move = False)
self.con.servos[self.Dict[i*3+2]].setPos(deg = int(ankleAngle/np.pi*180 - 180 + ankleOffset)*self.sign[i], move = False)
if send:
self.con.sendBinary()How to use it
In GUI.py, in init of class App you create a new variables
self.Dict = [ # Setup names and servo assignment
("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.Robot = kinematicRobot(con, self.Dict) # robot instance
self.pos = (0,0,75) # (x,y,z) position of body [mm]
self.angle = (0,0,0) # (Rx,Ry,Rz) orientation of body [deg]
self.dPos = (0,0,0) # (dx,dy,dRz) displacement of legs per one cycle
self.move = False # no move now
self.once = False # no position update nowThen when you want to initialize the position of the robot you call
then in pool you call updateRobot.
def poll(self):
...
if self.once or self.move:
self.Robot.updateRobot(self.pos, self.angle, self.move, self.dPos)
if self.move == False:
self.once = False
...
self.master.after(1000/FPS, self.poll) For FPS I recommend FPS= 20-30 as an optimal value between Hexy movement speed and reaction times of the servos. With binary communication and speed improvements you can achieve as much as about 50 FPS, but this might cause overload of Servotor32 serial buffer and stop response from the robot. I also recommend to use Bluetooth which by my experience causes less disturbance of the active servos.
If you only want to change body position/angle (with static legs) set
self.pos = (30, 0, 75) # example, lean forward
self.once = TrueIf you want to do the complete move (moving legs) set
self.dPos = (5, 0, 0) # example, move forward 5mm per cycle
self.move = TrueTo stop the movement, update the position once more such that all legs get to ground for sure:
self.move = False
self.once = TrueYou can also change the number of simultaneously moving legs (between 1 and 3, default is 2). Try it, it’s fun 
. See the video
self.Robot.setMovingLegs(iMovingLegs = 2)Thant’s it. Enjoy! 
Anyone up to connecting a ps3 controller or something for this?
I’m using Logitech Wireless Gamepad F710.
and pygame.joystick library to interface it in PoCoMo/GUI
It has 5 analog axes, 10 buttons and 1 hat which is more than enough for controlling hexy.
Can you pls post a complete zip or rar file with all the modified files you are using (i meed the modefied pomoco)
That would help a lot.
Be patient. There are several reasons why I didn’t do that yet:
- My version of PoCoMo is adjusted to the hardware modifications I made. So it wouldn’t work with your Hexy without modifications.
- I’m bad in making comments inside code. Hence it would backfire on me with many questions like “What does this function does?”, “My Hexy behaves strangely when I use your code, what am I doing wrong?”
- Where’s the fun if I give you all the solution immediately? I post here a lot of hints and even complete classes. If you don’t want to use your brain cells, buy a radio-controlled car instead.
I first want to walk you through my modifications in the thread I’m writing. Once I will describe all major changes I will share the complete code.
Thx Michal. Im sure im going to love all your changes and upgrades 
Michael,
Would you mind sharing the changes with me? I’d love to have them folded into mainline PoMoCo 
I just got samples of metal gears that do 180 degrees reasonably without jamming, and at a good price. It looks like we’ll be offering a metal gear Hexy within a month or so (probably $300 vs $250 for plastic), and a discount bulk upgrade pack for people who’ve already purchased a Hexy.
Michal,
Great example on the IK code to demonstrate the different sorts of walking gaits that are available with a hexapod. I’m very impressed. How long has this taken you, out of interest?
Thanks,
Rob
@roboalchemist - will do, soon, probably this weekend.
@rpcook - thanks! The basic IK code took me about 2 days + about 14 days of polishing it (API, GUI, Joystick control etc).
16 days to get to this level of polish!? I really hope you are a student with little else to do with your time, because if you fit in this much awesomeness around a full-time job I’m going to have to come up with a new word to describe this level of productivity!
Well, I made the virtual (kinematic) robot and multi-point offset calibration before I started IK project, so technically I’m busy with Hexy for a bit more than month. I’m not a student for some time , but I use my brain a lot at work which helps me staying sharp.
Great explanation of the inverse kinematics Michael. Very helpful.
I have been trying to implement inverse kinematics on the Servotor32 for Hexy for a couple of weeks now.
Main trouble I think I am having now is nonlinearity of servos and math functions taking too long to compute on the microcontroller.
Has anyone else got IK working on Hexy? Or is anyone else working on IK for the Servotor32?
When you say working on IK, is there a specific problem to solve?
klims,
servo nonlinearity is definitely an issue for accurate IK, but even one constant offset value per servo should be enough initially.
I agree that the math functions can be pretty demanding, but did you try to optimize the IK code? For example if you are trying to implement my IK code, than I would initially skip computation of body orientation - just keep it horizontal. Then you can store body points as a precomputed array. This already saves you about half of the computation time.
I’m working on something like what Michael has put together for controlling hexy using IK to calculate leg positions, but working entirely on the Servotor32. My aim is to connect a ps3 controller to hexy directly and control it as Michael does.
I’ve done the interesting task of solving the IK and doing the math, now I need to implement it properly which I would much rather grab someone else’s solution if it’s available.
Michael: I haven’t copied your IK code, I have tried to implement my own based around some IK/Computer vision tutorials.
Yep that’s right, I am taking the precomputed array approach but I think some of my point transformation functions are overly complex for what they need to do on hexy.
I have found http://blog.oscarliang.net/arduino-hexapod-robot/ tutorial very useful, and am considering just copying the code and making the necessary changes for Hexy.
klims, PS3 controlled Hexy is an awesome goal, good luck with it.
I’d schemed this as one of my projects with Hexy but I’ve not had enough time to have a go yet. Few problems that I foresaw were:
[ul]
[]I don’t know what the communication protocol is from the PS3 controller (apart from Bluetooth), but I’d be very surprised if it’s a low baud rate serial interface, which I think would preclude direct communication between the controller and the (current) Bluetooth module.[/]
[]Given that the stock build of software in the Servotor32 makes the onboard memory about half full. Implementation of full-robot IK code might be either too large to fit onboard, or the processor speed would limit the speed of the IK code such that Hexy’s speed would be limited.[/]
[]A way that I saw to get around these would have been to use an intermediate device (I’d schemed an Android device), which has enough resources to form two Bluetooth links (one to PS3 controller, one to Hexy) and do the difficult IK maths. Someone’s already posted a port of PoMoCo to Android, which might serve as a decent starting point.[/]
[/ul]
Of course, these are just my foreboding, ominous thoughts, and they might all be solvable. Just thought that other peeps might benefit from sharing these ideas.
Would really like to see a project like this work, as demonstrating Hexy at schools etc, the PoMoCo interface is a bit of an accessibility block. Having a games controller operate the robot will really boost the engagement of people with Hexy. Good luck to you!