I have since implemented the maths in my previous post. After a little debugging, I had the expected joint angles returned by the code, which could then be sent to Hexy. Necessarily it didn't all work perfectly first time...
The Python code is as follows:
def ikLowerLeg(x, y):
print "IK function called. x=", x, "y=", y
a = 48.0
b = 51.0
d = math.sqrt(x*x+y*y)
k = (d*d-b*b+a*a)/(2*d)
m = math.sqrt(a*a-k*k)
print "Divide by Zero error. No valid joint solution."
print "Math function error. Probably square root of negative number. No valid joint solution."
theta = math.degrees(math.atan2(float(y),float(x))-math.atan2(m,k))
phi = math.degrees(math.atan2(m,k)+math.atan2(m,(d-k)))
returnAngles = [theta, -phi]
print "theta=", theta, "phi=", phi
Here's a video of Hexy trying to move its leg on a vertical path (e.g. constant x, stepped y):
Two problems with the motion as seen in this video:
- The motion is very jerky.
- The motion isn't purely vertical.
Apart from these two minor issues, it really was rather a success. So I went onto try a horizontal path:
To get a better understanding of the paths that the legs follow during these movements, I slowed the movement right down and took a few photos of the leg positions. These composite images are shown below.
So it appears that the vertical path really wasn't that, but the horizontal path was pretty close to correct. Given that the maths is of closed form the remaining sources of error is the physical implementation, e.g. the servos. Given that this is a low-cost hexapod kit, the servos are very low cost. This suggests that when you ask for x-degress you don't always get x-degrees.
A non-linear response from robot actuators is bad news for precise positioning. Hopefully some range of the positioners will be close enough to linear, or the servos will have a predictable response which can be measured and then compensated for. Although the complication here is that if you have to evaluate each servos' non-linear response, you'll spend all your time calibrating. This is a problem for another day.
Now to revisit that jerky motion. During the vertical IK movement, this effect is particularly strong. I'm told that these servos do not allow any modification to their closed-loop feedback parameters, so all one can do is pass a sensible set of demand positions at a sensible rate. The controller code (PoMoCo) run in python and sends commands via a serial link to the Servotor32 PCB in Hexy, which in turn sets the demand position on the individual servos.
Perhaps a future Servotor32 firmware might be able to handle paths rather than just absolute positions, which would make performing complex movements smoother, with less stress on the serial link between controller and Hexy... I must suggest this to the developers (I'm aware that I could do this myself as the whole thing's open source, but hey: collaboration and what-not).