Tuesday, 1 April 2014

6 DOF Biped Robot using Dynamixel AX-12A Servos and Arduino

Having mastered driving Robotis Dynamixel AX Servos with an Arduino, I wanted to do something practical with that knowledge. How about building a biped robot?

There are plenty of biped robot kits available, like the Lynxmotion BRAT and the Robotis Bioloid, but I wanted to build something from the parts I already had lying around.

Each of the six Dynamixel AX-12A Servos I recently purchased came supplied with a U shaped bracket and mounting plate. I decided that if I bolted these together I’d get a pretty decent pair of legs. This configuration would give me a total of six degrees of freedom (DOF) - three in each leg - hip and knee joints on the sagittal plane and an ankle joint on the coronal plane.

I cut and drilled a strip of aluminium to form a pelvis and tie the legs together. I had a sheet of 3mm HDPE (the same stuff plastic chopping boards are made from) lying around, so cut this into rectangles to form the feet.

For power, I reused a 5000mAH LiPo battery, which I cable tied to the aluminium pelvis. This placed the centre of gravity nice and high, which is actually useful. On top of that, I taped an Arduino mounted in a plastic case and finally added the CDS55xx Driver Board (to interface the Arduino to the servos).

It's not the prettiest robot, but that completed the mechanical build.

Next came the software part, which turned out to be pretty simple. The key to getting a biped to walk well is developing an efficient walking gait. There are two different ways to approach this:

Static gait - the centre of gravity is projected inside the polygon formed by the robot’s feet. This is the simplest form, although looks artificial.

Dynamic gait - the centre of gravity is not necessarily projected within the polygon of the robot’s feet, however, dynamic balance is maintained. This is far more complex, but results in more natural movement.

For my first experiment, I chose a static gait. As I only had to contend with 6 DOF I decided I could dispense with complex inverse kinematics calculations and do it by hand. And, by keeping the motion of each leg symmetrical it’s easier to keep the centre of gravity central.

I settled on a repeating pattern of four poses that would make up the gait:

- Rotate ankles clockwise, shifting centre gravity to the left and lifting right leg.
- Extend right leg forward and push backwards with left leg.
- Rotate ankles anticlockwise, shifting centre gravity to the right and lifting left leg.
- Extend left leg forward and push backwards with right leg.

Here is a video of the completed biped walking happily across my floor.


Here is the code. Please note, I reference the Dynamixel class as featured in my last blog post.

#include "Dynamixel.h"
#include "Wire.h"
Dynamixel servo;
int velocity = 90;
int centre = 511;
byte c = 0;
byte p = 0;
/* Offset array from centre position
int pos[5][6] = {
  {   0,   0,   0,   0,   0,   0 }, // Stand upright
  { -55, -55,  25, -55, -55,  25 }, // Right leg forward
  { -55, -55, -25, -55, -55, -25 }, // Lean right
  {  55,  55, -25,  55,  55, -25 }, // Left leg forward
  {  55,  55,  25,  55,  55,  25 }  // Lean left
void setup() {
void loop() {
  if (c < 4)
    if (p > 4)
      p = 1;
  else update(0);
void update(byte p)
  // Update each servo position.
  for (byte i = 0; i < 6; i++)
    servo.setPos(i + 1, centre + pos[p][i], velocity);
  // Wait for motion to complete.
Thanks for reading.

Sunday, 23 March 2014

Driving Robotis Dynamixel AX-12A Servos from an Arduino

In my continuing quest for knowledge about robotics I recently bought some Robotis Dynamixel AX-12A servos, with the intention of hooking up to an Arduino. These awesome little servos pack a real punch, with over 15 kg/cm torque. There are plenty of hobby servos that have similar torque, but what sets these apart is that they also have the ability to track and report their speed, temperature, shaft position, voltage, and load. This level of feedback is essential for building advanced robotics applications.

Robotis Dynamixal AX-12A
Unlike hobby servos, these servos operate using a serial half-duplex TTL RS232 protocol. This is actually good news, as your micro-controller doesn’t need to worry about generating individual PWM signals for each servo. Instead, all sensor management and position control is handled by the servo's built-in micro-controller. Position and speed can be controlled with a 1024 step resolution. Wiring is pretty simple with two connectors on each servo allowing a daisy chain to be constructed.

The main controller communicates with the Dynamixel servos by sending and receiving data packets. There are two types of packets; the Instruction Packet (sent from the main controller to the servos) and the Status Packet (sent from the servos to the main controller). By default the communication speed is 1Mbps. A factory fresh servo has a preset ID of 01, which can easily be updated if you intend to run more than one servo from the same micro-controller.

The next challenge was to hook these up to an Arduino. This is not quite as simple as a regular hobby servo, as you must convert the full-duplex signal coming from the Arduino RX/TX pins to the half-duplex required by the Dynamixel servos. Luckily, there is a simple piece of hardware that will do the job for you. I purchased the CDS55xx Driver Board from Robosavvy. This board integrates a half-duplex and a voltage regulator circuit and thus makes it possible to directly connect Dynamixel AX servos to an Arduino.

Next I needed some code to drive these units. If you’ve read my previous post, you’ll know I don’t like bloated software libraries, so instead I created a bare bones class that allows me to set the servo ID and read & write servo positions – enough for current needs. The code is below – hope you find it useful…

#include "Arduino.h"
// Registers
#define P_ID 3                   // ID {RD/WR}
#define P_GOAL_POSITION_L 30     // Goal Position L {RD/WR}
#define P_PRESENT_POSITION_L 36  // Present Position L {RD}
// Instructions
#define INST_READ 0x02           // Reading values in the Control Table.
#define INST_WRITE 0x03          // Writing values to the Control Table.
class Dynamixel{
  void setPos(byte id, int pos, int vel);
  void setID(byte id, byte newId);
  int getPos(byte id);
  void WriteHeader(byte id, byte length, byte type);

#include "Dynamixel.h"
#include "Wire.h"  
Dynamixel::Dynamixel() {}
/* id = servo ID [0 - 255]
** pos = new position [0 - 1023]
** vel = servo velocity [0 - 1023] */
void Dynamixel::setPos(byte id, int pos, int vel)
  int writeLength = 7;
  byte pos_h = pos / 255;
  byte pos_l = pos % 255; 
  byte vel_h = vel / 255;
  byte vel_l = vel % 255;
  // Write standard header.
  WriteHeader(id, writeLength, INST_WRITE);
  // Starting address of where the data is to be written.
  // Write position low byte.
  // Write position high byte.
  // Write velocity low byte.
  // Write velocity high byte.
  // Check Sum.  
  Serial.write((~(id + writeLength + INST_WRITE + P_GOAL_POSITION_L + pos_l + pos_h + vel_l + vel_h))&0xFF);
  // Wait for instruction to be processed.
  // Discard return data.
  while (Serial.read() >= 0){}
/* id = servo ID [0 - 255] */
int Dynamixel::getPos(byte id)
  int writeLength = 4;
  int readLength = 2;
  // Write standard header.
  WriteHeader(id, writeLength, INST_READ);
  // Starting address of where the data is to be read.
  // The length of data to read.
  // Check Sum.  
  Serial.write((~(id + writeLength + INST_READ + P_PRESENT_POSITION_L + readLength))&0xFF);
  // Wait for instruction to be processed.
  // Discard extra data.
  for (int i = 0; i < 5; i++) Serial.read();
  // Read low byte.
  int low_Byte = Serial.read();
  // Read low byte.
  int high_byte = Serial.read();
  // Discard returned checksum.
  // Return position.
  return (int)high_byte << 8 | low_Byte;
/* id = servo ID [0 - 255]
** newId = new servo ID [0 - 255] */
void Dynamixel::setID(byte id, byte newId)
  int writeLength = 4;
  // Write standard header.
  WriteHeader(id, writeLength, INST_WRITE);
  // Starting address of where the data is to be written.
  // New ID.
  // Check Sum.
  Serial.write((~(id + writeLength + INST_WRITE + P_ID + newId))&0xFF);
void Dynamixel::WriteHeader(byte id, byte length, byte type)

Thursday, 6 March 2014

Using an MPU-6050 Gyroscope & Accelerometer with an Arduino

I recently purchased a SparkFun (InvenSense) MPU-6050, six degrees of freedom Gyroscope & Accelerometer from Robosavvy. It's a great bit of kit, which combines a 3-axis gyroscope and a 3-axis accelerometer on the same board. It hooks up easily to an Arduino using the I2C bus. So far, so good...

I then begin searching the Internet for example code with these two devices working together. My search always led me to a huge, unwieldy library, which seemed very bloated, considering all I wanted to do was read some values from the board.

I began to dig deeper and experiment, which enabled me to create the code sample below. It relies on using default values, which are fine for my application - and it's light! The accelerometer channels are very twitchy, so the general advice is to incorporate a low pass filter, which I've done.

Hopefully, you will also find it useful for your projects.

Main Program
#include "Wire.h"
#include "MPU6050.h"
MPU6050 mpu;  
int ax, ay, az, gx, gy, gz;
float smooth_ax, smooth_ay, smooth_az;
void setup()
void loop()
  /* Read device to extract current
   accelerometer and gyroscope values. */
  mpu.read6dof(&ax, &ay, &az, &gx, &gy, &gz);
  /* Apply low pass filter to smooth
   accelerometer values. */
  smooth_ax = 0.95 * smooth_ax + 0.05 * ax;
  smooth_ay = 0.95 * smooth_ay + 0.05 * ay;
  smooth_az = 0.95 * smooth_az + 0.05 * az;
  /* Output to serial monitor. */

#include "Arduino.h"
#include "Wire.h"
#define MPU6050_DEVICE_ADDRESS   0x68
#define MPU6050_RA_ACCEL_XOUT_H  0x3B
#define MPU6050_RA_PWR_MGMT_1    0x6B
#define MPU6050_PWR1_SLEEP_BIT   6
class MPU6050 {
  void wakeup();
  void read6dof(int* ax, int* ay, int* az, int* gx, int* gy, int* gz);
  byte id;
  byte buffer[14];
  void readByte(byte reg, byte *data);
  void readBytes(byte reg, byte len, byte *data);
  void writeBit(byte reg, byte num, byte data);
  void writeByte(byte reg, byte data);

#include "MPU6050.h"
MPU6050::MPU6050() {
/* Wake up device and use default values for
 accelerometer (±2g) and gyroscope (±250°/sec). */
void MPU6050::wakeup() {
  writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0);
/* Read device memory to extract current
 accelerometer and gyroscope values. */
void MPU6050::read6dof(int* ax, int* ay, int* az, int* gx, int* gy, int* gz) {
  readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
  *ax = (((int)buffer[0]) << 8) | buffer[1];
  *ay = (((int)buffer[2]) << 8) | buffer[3];
  *az = (((int)buffer[4]) << 8) | buffer[5];
  *gx = (((int)buffer[8]) << 8) | buffer[9];
  *gy = (((int)buffer[10]) << 8) | buffer[11];
  *gz = (((int)buffer[12]) << 8) | buffer[13];
/* Read a single byte from specified register. */
void MPU6050::readByte(byte reg, byte *data) {
  readBytes(reg, 1, data);
/* Read multiple bytes starting at specified register. */
void MPU6050::readBytes(byte reg, byte len, byte *data) {
  byte count = 0;
  Wire.requestFrom(id, len);
  while (Wire.available()) data[count++] = Wire.read();
/* Write bit to specified register and location. */
void MPU6050::writeBit(byte reg, byte num, byte data) {
  byte b;
  readByte(reg, &b);
  b = (data != 0) ? (b | (1 << num)) : (b & ~(1 << num));
  writeByte(reg, b);
/* Write byte to specified register. */
void MPU6050::writeByte(byte reg, byte data) {

Wednesday, 15 January 2014

Cleaning Noisy Time Series Data – Low Pass Filter (C#)

When working with time series data, like stock market prices, values can often contain a lot of noise, obscuring a real trend. One of the best ways to remove this noise is to run the data through a low pass filter.

Methods like simple moving averages and exponential moving averages are quick to implement and do a relatively good job. However, the disadvantages of these methods is that they only “look back” and do not take into account future values. This results in smoothed data which is out of phase with the original data-set, leading to peaks and troughs occurring later than reality.

A way to get around these issues is to implement a better filter, such as a Fast Fourier Transform or a Savitzky–Golay filter. However, these methods can be fairly complex and heavy to implement.

A simple method I use is shown below. I’m not sure if it’s a recognised technique, but I like to think of it as a one dimensional radial basis function. It looks back and forward around a value’s nearest neighbours, taking a weighted average, which decays exponentially by distance. And, like all good vacuum cleaners, this method cleans right up to the edges, by adding inferred linear slopes to the beginning and ends of the clean data-set.

The graph below shows a very noisy sine wave and its cleaner equivalent.

Here's the code - I hope you find it useful.

using System;
using System.IO;
class Program
    static void Main(string[] args)
        int range = 5; // Number of data points each side to sample.
        double decay = 0.8; // [0.0 - 1.0] How slowly to decay from raw value.
        double[] noisy = NoisySine();
        double[] clean = CleanData(noisy, range, decay);
        WriteFile(noisy, clean);
    static private double[] CleanData(double[] noisy, int range, double decay)
        double[] clean = new double[noisy.Length];
        double[] coefficients = Coefficients(range, decay);
        // Calculate divisor value.
        double divisor = 0;
        for (int i = -range; i <= range; i++)
            divisor += coefficients[Math.Abs(i)];
        // Clean main data.
        for (int i = range; i < clean.Length - range; i++)
            double temp = 0;
            for (int j = -range; j <= range; j++)
                temp += noisy[i + j] * coefficients[Math.Abs(j)];
            clean[i] = temp / divisor;
        // Calculate leading and trailing slopes.
        double leadSum = 0;
        double trailSum = 0;
        int leadRef = range;
        int trailRef = clean.Length - range - 1;
        for (int i = 1; i <= range; i++)
            leadSum += (clean[leadRef] - clean[leadRef + i]) / i;
            trailSum += (clean[trailRef] - clean[trailRef - i]) / i;
        double leadSlope = leadSum / range;
        double trailSlope = trailSum / range;
        // Clean edges.
        for (int i = 1; i <= range; i++)
            clean[leadRef - i] = clean[leadRef] + leadSlope * i;
            clean[trailRef + i] = clean[trailRef] + trailSlope * i;
        return clean;
    static private double[] Coefficients(int range, double decay)
        // Precalculate coefficients.
        double[] coefficients = new double[range + 1];
        for (int i = 0; i <= range; i++)
            coefficients[i] = Math.Pow(decay, i);
        return coefficients;
    static private void WriteFile(double[] noisy, double[] clean)
        using (TextWriter tw = new StreamWriter("data.csv"))
            for (int i = 0; i < noisy.Length; i++)
                tw.WriteLine(string.Format("{0:0.00}, {1:0.00}", noisy[i], clean[i]));
    static private double[] NoisySine()
        // Create a noisy sine wave.
        double[] noisySine = new double[180];
        Random rnd = new Random();
        for (int i = 0; i < 180; i++)
            noisySine[i] = Math.Sin(Math.PI * i / 90) + rnd.NextDouble() - 0.5;
        return noisySine;

Monday, 6 January 2014

Extracting Plain Text from Web Page HTML (C#)

Natural Language processing solutions, like Athena, require a good supply of high quality text.

As well as loading in ad-hoc documents, I’ve given Athena free reign to browse the Internet as required. Its two main sources of information are Wikipedia and BBC News.

Wikipedia is great for providing domain knowledge and key facts, whilst the BBC News site is an excellent source of up to the minute current affairs.

Anybody who has attempted to extract plain text from real world HTML will know that what should be a simple task can quickly snowball into a mammoth project.

There have been many debates on sites like Stackoverflow on how best to do this. Most people start their journey by using regular expressions (regex) - but this is really only viable with well formed and simple HTML. Madness soon follows...

In the real world, HTML is not always well formed and in practice you will also want to ignore such things as adverts, menus and page navigation. To overcome this, you may consider creating a hybrid regex / imperative code parser. Suddenly, this is getting serious...

Luckily, if you’re using C#, you already have the perfect solution in your toolbox - the WebBrowser control in Windows Forms. This control already knows how to render web pages into text and is incredibly tolerant to badly formed HTML.

Using the HtmlDocument property in the WebBrowser control, you can easily navigate the document to find exactly the clean text portions you’re looking for. And, of course, just because this control sits into the System.Windows.Forms namespace, doesn't mean you can’t use it in other types of application - just be sure to add the relevant assembly reference. One complication is that the WebBrowser control needs to run in its own thread (which is easy to work around).

In the simple example below, I have created a console application that allows you to type in a search phrase on the command line, which is sent to Google, extracting links to the BBC News website and returning relevant, clean, plain text.

Sites like BBC News are very well structured, thanks to their content management system. Therefore, by reading the CSS classname associated with HTML tags, you can easily isolate the information you require.

using System;
using System.Text;
using System.Threading;
using System.Windows.Forms;
class Program
    private string _plainText;
    static void Main(string[] args)
        new Program();
    private Program()
        while (true)
            Console.Write("> ");
            string phrase = Console.ReadLine();
            if (phrase.Length > 0)
                Thread thread = new Thread(new ParameterizedThreadStart(GetPlainText));
    private void GetPlainText(object phrase)
        string uri = "";
        WebBrowser _webBrowser = new WebBrowser();
        _webBrowser.Url = new Uri(string.Format(@"http://www.google.com/search?as_q={0}&as_sitesearch=www.bbc.co.uk/news", phrase));
        while (_webBrowser.ReadyState != WebBrowserReadyState.Complete) Application.DoEvents();
        foreach (HtmlElement a in _webBrowser.Document.GetElementsByTagName("A"))
            uri = a.GetAttribute("href");
            if (uri.StartsWith("http://www.bbc.co.uk/news")) break;
        StringBuilder sb = new StringBuilder();
        WebBrowser webBrowser = new WebBrowser();
        webBrowser.Url = new Uri(uri);
        while (webBrowser.ReadyState != WebBrowserReadyState.Complete) Application.DoEvents();
        // Pick out the main heading.
        foreach (HtmlElement h1 in webBrowser.Document.GetElementsByTagName("H1"))
            sb.Append(h1.InnerText + ". ");
        // Select only the article text, ignoring everything else.
        foreach (HtmlElement div in webBrowser.Document.GetElementsByTagName("DIV"))
            if (div.GetAttribute("classname") == "story-body")
                foreach (HtmlElement p in div.GetElementsByTagName("P"))
                    string classname = p.GetAttribute("classname");
                    if (classname == "introduction" || classname == "")
                        sb.Append(p.InnerText + " ");
        _plainText = sb.ToString();

This is what the result looks like after searching for British Airways...

Happy screen scraping!