Author Archives: dxydas

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 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
        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() = 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
                    if self.triggerPolling:
                        self.triggerPolling = False
            # Get joystick input
                events = get_gamepad()
                for event in events:

    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)[0], self.speed[0] = self.updateMotion(self.inputLJSYNormed,[0], self.speed[0])
        # World Y
        self.inputLJSXNormed = self.filterInput(-self.inputLJSX)[1], self.speed[1] = self.updateMotion(self.inputLJSXNormed,[1], self.speed[1])
        # World Z
        self.inputRJSYNormed = self.filterInput(-self.inputRJSY)[2], self.speed[2] = self.updateMotion(self.inputRJSYNormed,[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 =[:]
        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)
            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]


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

#include <ax12.h>

int numOfJoints = 5;

void setup()

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 ( == '\n')
      for (int i = 0; i < numOfJoints; ++i)       {         if ( (id[i] > 0) && (0 <= pos[i]) && (pos[i] <= 1023 ) )
          SetPosition(id[i], pos[i]);

Quadbot Inverse Kinematics

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 z-plane, which is why the a2p, a3p etc. notation is used.


Trigonometry based on front view.


Trigonometry based on side view.

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 four-legged walking gait.

Quadbot 17 Kinematics_003

Kinematics test program updated with target visual and sliders.


Test of IK using elliptical motion target which the foot (F) follows.

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 browser-based GeoGebra app.

Quadbot Forward Kinematics

The Forward Kinematics for the left leg of the Quadbot have been formalised, using modified Denavit-Hartenberg 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 A-Star 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 Dynamixel-to-USB converter (e.g. USB2AX) or separate motor power supply.

Next up will be to work out the Inverse Kinematics.

j alpha_i-1 a_i-1 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

D-H Parameters

Quadbot 17 Kinematics_001

Quadbot kinematics applet, zeroed position

Quadbot 17 Kinematics_002

Quadbot kinematics applet, test position using sliders

Quadbot updates and sensor options


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 pre-order 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




Dimensions (mm)

249 x 66 x 67

65 x 65 x 52.8

Weight (kg)



Minimum Range (m)



Maximum Range (m)



Sample rate (Hz)



Rotation rate (Hz)



FOV (°)

70 x 60

360 (planar)


512 x 424, ~ 7 x 7 pixels per °

1 cm

Price (£)


80 (32 for adaptor)


WordPress tip: One thing I really like about is that there are always ways around doing things that initially only seem possible with Need to add a table into your post? Use Open Live Writer, make the table then copy-paste 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 – 2017-02-26.xlsx

That’s it for now!

Quad-legged robot ideas

My humanoid robot updates have temporarily taken a backseat due to some other distractions, such as the FPV quadcopter, and now this! Updates on the Bioloid will still continue however, as I am far from done with it.

So as another side-project, I’ve been thinking about trying to build a custom robot from the ground-up, rather than basing it on an existing platform like the Bioloid.

CAD software

I have been using FreeCAD for the Bioloid project, which is a great free tool but somewhat hard to use and lacking many of the features found in modern CAD software.

As I need to design a large number of new parts, I’ve been looking around for a good CAD software with most or all of the following key features:

  • free, or at least licenced for hobbyist use
  • parametric modelling
  • export to STL
  • 3D printing support tools a plus, but not necessary
  • nice renderer?

Autodesk’s line of tools seem to meet these requirements very well. Autodesk 123D seemed like an excellent choice, with more advanced features than its sibling Tinkercad, a CAD program which runs inside a web browser. However, Autodesk has recently been restructuring its suite of tool according to this announcement, so this lead me to check out Fusion 360.

It has a very flexible licensing model, which means it can be used for free as a student, educator, start-up and most importantly, as an enthusiast (non-profit). I have only been using it for a few days, but so far it seems impressive. It works very similar to SolidWorks, and has a number of useful features such as direct integration with various 3D printing tools and services, and an easy-to-use renderer. One thing that may be seen as a downside is the fact that everything is stored on the cloud, but local backups are possible.


Components & 3D printing

The main design of the robot will be centred around the use of Dynamixel’s AX range of servos, as they are the most competitively priced motors I’ve found for the power and features they offer. Most other high-torque servos are prohibitively expensive, considering I will need about 16 and each costs over £100!

The exact model will probably be the AX-12A, which is an improved version of the AX-12+ used on the Bioloid. I might be able to stretch to the faster, more expensive AX-18A, however as their external design is identical, any frames used will be compatible with both.


For the basic servo joints I will be using a combination of the plastic frames in the Robotis range, as well as possibly some metal frames by Trossen Robotics. The rest of the robot will be designed with 3D printed parts in mind. Whether I go for an expensive online 3D printing service or try and revive my 3D printer remains to be seen.


These are a few images as well as designs by other hobbyists which I am using as inspiration: