Inverse Kinetics

Hexy Questions, Problems and Suggestions
#1

found a nice write up about how to do this…http://www.engineer-this.com/IK.shtml

he even posted (mostly) working arduino code for a quadraped.

Anyone think they can tackle a port?

I know it’s been stated that the stated that the servotor32 doesn’t use the standard servo.h library –

Anyone know if it can use the servo.h? if not, might I ask what stops prevents that use?

#2

There has been some discussion along these lines already on this forum. I’ve written up some Python code that solves the IK problem for the lower two leg joints, plus some maths to cover a full leg solution.

My job for the next week or so is to convert the full leg maths (using a couple of co-ordinate transforms for each leg) into a complete IK model for Hexy. This should be sufficient for some exciting movements, time will tell :slight_smile:

#3

I’ve used the python code from rpcook and added functions to calculate the whole leg.

The coordinates I use might be a bit uncommon:

X+ goes left for the left legs and right for the right legs.
Y+ goes forward, into the direction of the head
Z+ goes downward

Here’s the Code:

import time
import math

# The ikLowerLeg function is intellectual property of robcook, aka rpcook
# http://robcook.eu/hexy/inverse-kinematics-part-2/
def ikLowerLeg(x, y):
    a = 48.0
    b = 51.0
    try:
        d = math.sqrt(x*x+y*y)
        k = (d*d-b*b+a*a)/(2*d)
        m = math.sqrt(a*a-k*k)
    except ZeroDivisionError:
        return
    except ValueError:
        return
    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]
    return returnAngles

#calculates the hypothenuse of the hip joint, resulting in the 'x' length to be passed to ikLowerLeg
def ikLeg(x,y,z):
    hip = math.degrees(math.atan2(float(y),float(x)))
    hyp = math.sqrt(x*x+y*y)
    print(hyp);
    knee, ankle = ikLowerLeg(hyp, z)
    returnAngles = [hip, knee, ankle]
    return returnAngles

# convenience function to move a leg
# example: moveLeg(hexy.LF, 30,0,85)
def moveLeg(legToMove, x,y,z):
    # if you want the right legs of the hexy to use negative x values for consistency, uncomment these two lines
    #if legToMove == hexy.RF or legToMove == hexy.RM or legToMove == hexy.RB:
    #    x *= -1
    
    # i want front to be positive y values on both sides
    if legToMove == hexy.LF or legToMove == hexy.LM or legToMove == hexy.LB:
        y *= -1
    
    # the middle legs are more outside, correct that, if you want, with these two lines
    #if legToMove == hexy.LM or legToMove == hexy.RM:
    #    x -= 30
        
    degHip, degKnee, degAnkle = ikLeg(x,y,z)
    if legToMove == hexy.LF or legToMove == hexy.RB:
        degHip += 50
    
    if legToMove == hexy.RF or legToMove == hexy.LB:
        degHip -= 50
    
    legToMove.hip(degHip)
    legToMove.knee(degKnee)
    legToMove.ankle(degAnkle)

moveLeg(hexy.LF, 30,10,85)
moveLeg(hexy.RF, 30,10,85)
moveLeg(hexy.LM, 30,0,85)
moveLeg(hexy.RM, 30,0,85)
moveLeg(hexy.LB, 30,-10,85)
moveLeg(hexy.RB, 30,-10,85)


time.sleep(0.5)
hexy.con.killAll()

time.sleep(2)

# test IK movement
moveLeg(hexy.RM, 30,0,80)

for y in range(0,-15,-1):
    moveLeg(hexy.RM, 30,y,80)

for y in range(-15,15,1):
    moveLeg(hexy.RM, 30,y,80)

time.sleep(1)
moveLeg(hexy.RM, 30,0,80)
time.sleep(1)
moveLeg(hexy.RM, 30,0,85)

time.sleep(0.5)
hexy.con.killAll()

Comments, Suggestions and spelling corrections are appreciated :slight_smile:

#4

FYI I’m totally happy for peeps to use the code that I post.

Looks like some maths in there. My task this weekend was a full robot solution, will try to catch up!