A big thank you to the website Engineering Unplugged for including Cognitive Dissonance on their featured site listings.
You can check it out here.
A big thank you to the website Engineering Unplugged for including Cognitive Dissonance on their featured site listings.
You can check it out here.
Work on the Quadbot has taken a backseat as I have been working on an entry for the ChipE Remix Challenge on Thingiverse.
My main motivation was the 18 or so servos on the hexapod prize, which would be exactly what I need to continue with the build of the Quadbot! All the robots are interesting though, so any prize would be a win!
My entry to the challenge is … ChipElata! Hope you enjoy.
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]); } } } }
Following from the previous post, the Inverse Kinematics (IK) have been calculated, just in time for the test rig hardware, which is arriving this week.
I found a geometrical solution to the IK by breaking the 3D problem down into two 2D problems:
Looking at the leg from the back/front, only joints 1 & 5 determine the offset along the world Y axis. The joints are equalised so that the foot is always facing perpendicular to the ground.
Looking at the leg sideways, the rest of the joints 2, 3 & 4 are calculated using mostly acos/atan2 functions. Note that the leg link lengths need to be projected onto the zplane, which is why the a_{2p}, a_{3p} etc. notation is used.
The test program now has controls for positioning a target for the foot to follow, based on the IK. This was very helpful for testing and debugging. A test function was also added which simply moves the target in an elliptical motion for the foot to follow. It is a very simple way of making the leg walk, but could be used as the foundation for a simply fourlegged walking gait.
The next tasks are to build the physical test rig, learn how to use the Arbotix controller and port the IK code to Arduino.
The kinematics are written in Python/TKInter, and code can be found on GitHub.
The geometrical drawings were made using the browserbased GeoGebra app.
The Forward Kinematics for the left leg of the Quadbot have been formalised, using modified DenavitHartenberg parameters and axes conventions.
I also made a simple Python applet to verify the maths and visualise the leg’s poses. I used Tkinter and three Canvas widgets to show orthogonal views.
The reason I am testing the maths in a quick Python program is that I want to be able to port them easily over to Arduino, as my latest aim is to drop the Raspberry Pi and AStar 32U4 LV Pi expansion module (shown in some of the latest CAD models) in favour of trying out an ArbotiX controller. A benefit with the latter is that I wouldn’t need a DynamixeltoUSB converter (e.g. USB2AX) or separate motor power supply.
Next up will be to work out the Inverse Kinematics.
Link Twist 
Link Length 
Link Offset 
Joint Angle 

j  alpha_i1  a_i1  d_i  theta_i 
1  0  0  0  th_1 
2  pi/2  29.05  0  th_2 – 34 
3  0  76.919  0  th_3 + 67.5 
4  0  72.96  0  th_4 
5  pi/2  45.032  0  th_5 
DH Parameters
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:
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:
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!
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!