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!
Hey good idea with the android device. But i might have a less complicated solution.
A Raspberry Pi can run linux and it can run python. Why not just use that to talk back and forth from the servator and ps2 controller.
A Raspberry Pi is also low power and lighter then a android phone.
I haven’t tried it yet, but I’m going on the assumption that something like this will work
barrettsprojects.wordpress.com/2 … nstration/
Serial in commands from the USB host module which interprets the PS3 controller commands.
I have compiled the hexapod code shown in the tutorial I posted with servotor32 file necessary to move servos and it fits on the board. I haven’t been able to try it yet to assess the quality of the motion performance, so you could be right about the speed of Hexy being limited.
I had some limited success with doing the IK on board.
It all works as expected, but Hexy jolts around as he moves. The actions are not as smooth as I had hoped.
I think this is due to my fundamental misunderstanding of how the other hexapod makers were achieving smooth gaits while doing the processing on board. From what I gather looking at the lynxmotion kits they always have a servo controller board that handles the the servo movement and interpolation/smoothing while the brain handles the IK calcs, which is essentially what the servotor32 and PC running PoMoCo do…
I’m messing around with trying to do some kind of interrupt driven servo interpolation, but I’m not confident in that solution.
I’ve ordered a MK808 Android TV Stick on which I plan to install PicUntu and just run Michal’s PoMoCo 2.1