The hardware for the test rig has arrived, and the first leg has been set up!

The metal brackets are from Trossen Robotics and largely match the dimensions of their equivalent plastic parts from Robotis. Some parts do not have metal counterparts, so I used spare plastic parts from my Bioloid kit. I also ordered an ArbotiX-M controller (Arduino-based), which communicates with the PC via a spare SparkFun FT232RL I had. The test rig frame is made out of MakerBeam XL aluminium profiles.

Unfortunately, I thought I had more 3-pin cables than I actually did, so I can’t control all the motors just yet. However, I’ve got an Xbox One controller controlling the IK’s foot target position and a simple serial program on the ArbotiX-M which receives the resulting position commands for each motor.

The code for both the Python applet and the Arduino can be found on GitHub here.

Some of the more useful snippets of code are shown below:

The following code handles gamepad inputs and converts them to a natural-feeling movement, based on equations of motion. The gamepad input gets converted into a virtual force, which is opposed by a drag proportional to the current velocity. The angles resulting from the IK are sent to the controller via serial port:

class GamepadHandler(threading.Thread): def __init__(self, master): self.master = master # Threading vars threading.Thread.__init__(self) self.daemon = True # OK for main to exit even if instance is still running self.paused = False self.triggerPolling = True self.cond = threading.Condition() # Input vars devices = DeviceManager() self.target = targetHome[:] self.speed = [0, 0, 0] self.inputLJSX = 0 self.inputLJSY = 0 self.inputRJSX = 0 self.inputRJSY = 0 self.inputLJSXNormed = 0 self.inputLJSYNormed = 0 self.inputRJSXNormed = 0 self.inputRJSYNormed = 0 self.dt = 0.005 def run(self): while 1: with self.cond: if self.paused: self.cond.wait() # Block until notified self.triggerPolling = True else: if self.triggerPolling: self.pollInputs() self.pollIK() self.pollSerial() self.triggerPolling = False # Get joystick input try: events = get_gamepad() for event in events: self.processEvent(event) except: pass def pause(self): with self.cond: self.paused = True def resume(self): with self.cond: self.paused = False self.cond.notify() # Unblock self if waiting def processEvent(self, event): #print(event.ev_type, event.code, event.state) if event.code == 'ABS_X': self.inputLJSX = event.state elif event.code == 'ABS_Y': self.inputLJSY = event.state elif event.code == 'ABS_RX': self.inputRJSX = event.state elif event.code == 'ABS_RY': self.inputRJSY = event.state def pollInputs(self): # World X self.inputLJSYNormed = self.filterInput(-self.inputLJSY) self.target[0], self.speed[0] = self.updateMotion(self.inputLJSYNormed, self.target[0], self.speed[0]) # World Y self.inputLJSXNormed = self.filterInput(-self.inputLJSX) self.target[1], self.speed[1] = self.updateMotion(self.inputLJSXNormed, self.target[1], self.speed[1]) # World Z self.inputRJSYNormed = self.filterInput(-self.inputRJSY) self.target[2], self.speed[2] = self.updateMotion(self.inputRJSYNormed, self.target[2], self.speed[2]) with self.cond: if not self.paused: self.master.after(int(self.dt*1000), self.pollInputs) def pollIK(self): global target target = self.target[:] runIK(target) with self.cond: if not self.paused: self.master.after(int(self.dt*1000), self.pollIK) def pollSerial(self): if 'ser' in globals(): global ser global angles writeStr = "" for i in range(len(angles)): x = int( rescale(angles[i], -180.0, 180.0, 0, 1023) ) writeStr += str(i+1) + "," + str(x) if i < (len(angles) - 1): writeStr += "," else: writeStr += "\n" ser.write(writeStr) with self.cond: if not self.paused: self.master.after(int(5*self.dt*1000), self.pollSerial) # 10x slower than pollIK def filterInput(self, i): if (i > 3277) or (i < -3277): # ~10% if i > 3277: oldMax = 32767 elif i < -3277: oldMax = 32768 inputNormed = math.copysign(1.0, abs(i)) * rescale(i, 0, oldMax, 0, 1.0) else: inputNormed = 0 return inputNormed def updateMotion(self, i, target, speed): c1 = 10000.0 c2 = 10.0 mu = 1.0 m = 1.0 u0 = speed F = c1*i - c2*u0 # Force minus linear drag a = F/m t = self.dt x0 = target # Equations of motion u = u0 + a*t x = x0 + u0*t + 0.5*a*pow(t, 2) # Update self target = x speed = u return target, speed def rescale(old, oldMin, oldMax, newMin, newMax): oldRange = (oldMax - oldMin) newRange = (newMax - newMin) return (old - oldMin) * newRange / oldRange + newMin

The following Python code is the leg’s Forward Kinematics (click to expand):

def runFK(angles): global a global footOffset global T_1_in_W global T_2_in_W global T_3_in_W global T_4_in_W global T_5_in_W global T_F_in_W a = [0, 0, 29.05, 76.919, 72.96, 45.032] # Link lengths "a-1" footOffset = 33.596 s = [0, 0, 0, 0, 0, 0] c = [0, 0, 0, 0, 0, 0] for i in range(1,6): s[i] = math.sin( math.radians(angles[i-1]) ) c[i] = math.cos( math.radians(angles[i-1]) ) # +90 around Y T_0_in_W = np.matrix( [ [ 0, 0, 1, 0], [ 0, 1, 0, 0], [ -1, 0, 0, 0], [ 0, 0, 0, 1] ] ) T_1_in_0 = np.matrix( [ [ c[1], -s[1], 0, a[1]], [ s[1], c[1], 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1] ] ) T_2_in_1 = np.matrix( [ [ c[2], -s[2], 0, a[2]], [ 0, 0, -1, 0], [ s[2], c[2], 0, 0], [ 0, 0, 0, 1] ] ) T_3_in_2 = np.matrix( [ [ c[3], -s[3], 0, a[3]], [ s[3], c[3], 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1] ] ) T_4_in_3 = np.matrix( [ [ c[4], -s[4], 0, a[4]], [ s[4], c[4], 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1] ] ) T_5_in_4 = np.matrix( [ [ c[5], -s[5], 0, a[5]], [ 0, 0, -1, 0], [-s[5], -c[5], 1, 0], [ 0, 0, 0, 1] ] ) T_F_in_5 = np.matrix( [ [ 1, 0, 0, footOffset], [ 0, 1, 0, 0], [ 0, 0, 1, 0], [ 0, 0, 0, 1] ] ) T_1_in_W = T_0_in_W * T_1_in_0 T_2_in_W = T_1_in_W * T_2_in_1 T_3_in_W = T_2_in_W * T_3_in_2 T_4_in_W = T_3_in_W * T_4_in_3 T_5_in_W = T_4_in_W * T_5_in_4 T_F_in_W = T_5_in_W * T_F_in_5

The following Python code is the leg’s Inverse Kinematics (click to expand):

def runIK(target): # Solve Joint 1 num = target[1] den = abs(target[2]) - footOffset a0Rads = math.atan2(num, den) angles[0] = math.degrees(a0Rads) # Lengths projected onto z-plane c0 = math.cos(a0Rads) a2p = a[2]*c0 a3p = a[3]*c0 a4p = a[4]*c0 a5p = a[5]*c0 j4Height = abs(target[2]) - a2p - a5p - footOffset j2j4DistSquared = math.pow(j4Height, 2) + math.pow(target[0], 2) j2j4Dist = math.sqrt(j2j4DistSquared) # # Solve Joint 2 num = target[0] den = j4Height psi = math.degrees( math.atan2(num, den) ) num = pow(a3p, 2) + j2j4DistSquared - pow(a4p, 2) den = 2.0*a3p*j2j4Dist if abs(num) <= abs(den): phi = math.degrees( math.acos(num/den) ) angles[1] = - (phi - psi) # Solve Joint 3 num = pow(a3p, 2) + pow(a4p, 2) - j2j4DistSquared den = 2.0*a3p*a4p if abs(num) <= abs(den): angles[2] = 180.0 - math.degrees( math.acos(num/den) ) # # Solve Joint 4 num = pow(a4p, 2) + j2j4DistSquared - pow(a3p, 2) den = 2.0*a4p*j2j4Dist if abs(num) <= abs(den): omega = math.degrees( math.acos(num/den) ) angles[3] = - (psi + omega) # Solve Joint 5 angles[4] = - angles[0] runFK(angles)

The following Arduino code is the serial read code for the Arbotix-M (click to expand):

#include <ax12.h> int numOfJoints = 5; void setup() { Serial.begin(38400); } void loop() { } void serialEvent() { int id[numOfJoints]; int pos[numOfJoints]; while (Serial.available()) { for (int i = 0; i < numOfJoints; ++i) { id[i] = Serial.parseInt(); pos[i] = Serial.parseInt(); } if (Serial.read() == '\n') { for (int i = 0; i < numOfJoints; ++i) { if ( (id[i] > 0) && (0 <= pos[i]) && (pos[i] <= 1023 ) ) SetPosition(id[i], pos[i]); } } } }