Seems like it’s Quadbot’s turn today. Project featured on the Fusion 360 community gallery, which means I need to hurry up and get it built.
Tag Archives: quadruped
5DOF leg test rig build and gamepad control
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 ArbotiXM controller (Arduinobased), 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 3pin 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 ArbotiXM 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 naturalfeeling 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 "a1" 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[i1]) ) c[i] = math.cos( math.radians(angles[i1]) ) # +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 zplane 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 ArbotixM (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]); } } } }
Quadbot updates and sensor options
Updates
A few and bits and pieces have been added to the model, along with some updates: Longer lower leg and cover, battery and battery compartment in rear body, main electronics boards, foot base and plate, and two ideas for sensors.
The lower legs were extended as initially the “knee” and “ankle” joints were too close. I think the new arrangement gives the leg better overall proportions.
As the battery pack has significant size and weight, its best to include in the design as early as possible. Originally neglected, I have now added a Turnigy 2200 mAh battery, and updated the rear bumper and bracket to accommodate it. Heat dissipation may be an issue, but I’ll leave it like this for now.
I have also measured the placement of the actuators in order to start thinking about the maths for the kinematics.
Some images of current progress:
Sensors
I have tried two ideas for area scanners which could be the main “eyes” of the robot. One is the Kinect v2, and the other a Scanse Sweep.
The main advantages of the Sweep is that it is designed specifically for robotics, with a large range and operation at varying light levels. On its own it only scans in a plane by spinning 360°, however it can be turned into a spherical scanner with little effort. Added bonuses are a spherical scan mounting bracket designed specifically for Dynamixel servos, as well as ROS drivers! It is currently available only on preorder on SparkFun.
The Kinect has a good resolution and is focused on tracking human subjects, being able to track 6 complete skeletons and 25 joints per person. However it only works optimally indoors and at short ranges directly in front of it (adaptor to convert to the USB interface. It is however significantly cheaper than the Sweep.
Below is a table comparing the important specs:
XBOX Kinect v2 
Scanse Sweep 

Technology 
TimeofFlight 
LiDAR 
Dimensions (mm) 
249 x 66 x 67 
65 x 65 x 52.8 
Weight (kg) 
1.4 
0.12 
Minimum Range (m) 
0.5 
~0.1 
Maximum Range (m) 
4.5 
40 
Sample rate (Hz) 
30 
1000 
Rotation rate (Hz) 
N/A 
110 
FOV (°) 
70 x 60 
360 (planar) 
Resolution 
512 x 424, ~ 7 x 7 pixels per ° 
1 cm 
Price (£) 
280 
80 (32 for adaptor) 
Sources:
 Miscrosoft Kinect hardware
 Scanse Sense user’s manual and technical specifications
 Kinect V1 and Kinect V2 fields of view compared
WordPress tip: One thing I really like about WordPress.com is that there are always ways around doing things that initially only seem possible with WordPress.org. Need to add a table into your post? Use Open Live Writer, make the table then copypaste the table’s generated HTML source code!
Hardware costs
The current estimated hardware costs are quite high, at around £2100. However about half this budget (£955) is due to the fact that I calculated the costs for the custom 3D printed parts by getting quotes from Shapeways. Getting them printed on a homemade 3D printer would reduce the cost significantly. The other large cost is naturally the 22 AX actuators at £790.
For anyone interested, the preliminary BOM can be downloaded here:
Quadbot 17 BOM – 20170226.xlsx
That’s it for now!