• Quadrobot project build log
• Kinematics and code repository
• Custom CAD files

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.

Trigonometry

Trigonometry based on front view

Trigonometry based on side view

---

Evolution of the Kinematics

Elliptical path test

Quad kinematics gait test

Gait foot pitch test original

Gait foot pitch test adjusted

This Arduino code allows the OpenCM9.04, together with OpenCM 458 Expansion Board, to receive commands via serial from the Python kinematics code, and in turn send the goal positions and speeds to the AX-12 motors.

LegTestRig_OpenCM.ino

#include <string.h>

#define DXL_BUS_SERIAL3 3  //Dynamixel on Serial3(USART3)  <- OpenCM 485 EXP

int numOfJoints = 22;
char cmdBuffer[256];
int bufferCount = 0;
Dynamixel Dxl(DXL_BUS_SERIAL3);


void setup()
{
  // Initialize the Dynamixel bus
  // Dynamixel 2.0 Baudrate -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps
  Dxl.begin(3);

  // Set joint mode
  for (int i = 0; i < numOfJoints; ++i)
    Dxl.jointMode(i);

  // Set compliance
  Dxl.complianceMargin(254, 1, 1);
  Dxl.complianceSlope(254, 50, 50);

  // Home motors
  Dxl.setPosition(254, 512, 100);

  // Setup serial
  SerialUSB.begin();
  SerialUSB.attachInterrupt(usbInterrupt);
}


void loop()
{
}


void usbInterrupt(byte* buffer, byte nCount){
  // USB max packet data is 64 bytes, so nCount cannot exceed 64 bytes
  for(unsigned int i=0; i < nCount; ++i)
  {
    char c = (char)buffer[i];
    cmdBuffer[bufferCount++] = c;
    if (c == '\n')
    {
      useBuffer(cmdBuffer);
      buffer[0] = NULL;
      bufferCount = 0;
    }
  }
}


void useBuffer(char *p)
{
  char * pEnd = p;
  int x;
  int count = 0;
  int id[numOfJoints];
  int pos[numOfJoints];
  int speed[numOfJoints];

  // String format: ID POS SPEED repeated until \n
  while ( x = strtol(pEnd, &pEnd, 10) )
  {
    id[count] = x;
    if ( x = strtol(pEnd, &pEnd, 10) )
      pos[count] = x;
    if ( x = strtol(pEnd, &pEnd, 10) )
      speed[count] = x;
    count++;
  }

  for (int i = 0; i < count; ++i)
  {
    //SerialUSB.println(id[i]);
    if ( (1 <= id[i]) && (id[i] <= numOfJoints) &&
         (0 <= pos[i]) && (pos[i] <= 1023) &&
         (0 <= speed[i]) && (speed[i] <= 1023) )
    {
      //SerialUSB.print("I got id: ");
      //SerialUSB.println(id[i]);
      //SerialUSB.print("I got pos: ");
      //SerialUSB.println(pos[i]);
      //SerialUSB.print("I got speed: ");
      //SerialUSB.println(speed[i]);

      Dxl.setPosition(id[i], pos[i], speed[i]);
      //SerialUSB.println("Sent to AX-12!");
    }
  }
}


/*
void useBuffer(char *p)
{
  char * pEnd;
  int id;
  int pos;
  int speed;

  // String format: ID POS SPEED\n
  id = strtol(p, &pEnd, 10);
  pos = strtol(pEnd, &pEnd, 10);
  speed = strtol(pEnd, NULL, 10);

  //SerialUSB.print("I got id: ");
  //SerialUSB.println(id);
  //SerialUSB.print("I got pos: ");
  //SerialUSB.println(pos);
  //SerialUSB.print("I got speed: ");
  //SerialUSB.println(speed);

  if ( (1 <= id) && (id <= numOfJoints) &&
       (0 <= pos) && (pos <= 1023) &&
       (0 <= speed) && (speed <= 1023) )
  {
     Dxl.setPosition(id, pos, speed);
     //SerialUSB.println("Sent to AX-12!");
  }
}
*/

Note: A (out-of-date) version of the code, targeting the Arbotix-M, can be found here: LegTestRig.ino.

Here is an example I figured out, of how to create a Python class to run a function in a separate thread, with a defined time interval, that is pausable/stoppable. It subclasses the Thread class, and uses the help of an Event object to handle the timing functionality. After creating an object of this class, you call its start() method, which invokes the run() method in a separate thread. The pause(), resume() and stop() methods perform the respective actions. The example is a stripped-down version of an input handler, which applies the equations of motion to user inputs, that I created as part of the kinematics code.

Input Handler Example

import threading
from time import time
import math
class InputHandlerExample(threading.Thread):
    def __init__(self):
        # Threading/timing vars
        threading.Thread.__init__(self)
        self.event = threading.Event()
        self.dt = 0.05  # 50 ms
        self.paused = False
        self.prevT = time()
        self.currT = time()
        self.target = 0.0
        self.speed = 0.0
 
        # Input vars
        self.inputNormalised = 0.0
 
    def run(self):
        while not self.event.isSet():
            if not self.paused:
                self.pollInputs()
            self.event.wait(self.dt)
 
    def pause(self):
        self.paused = True
 
    def resume(self):
        self.paused = False
 
    def stop(self):
        self.event.set()
 
    def pollInputs(self):
        # Upate current time
        self.currT = time()
 
        # Get user input somehow here, normalise value
        #self.inputNormalised = getInput()
 
        # Update target and speed
        self.target, self.speed = self.updateMotion(
            self.inputNormalised, self.target, self.speed)
 
        # Make use of the returned target and speed values
        #useResults(target, speed)
 
        # Update previous time
        self.prevT = self.currT
 
    def updateMotion(self, i, target, speed):
        m = 1.0
        u0 = speed
 
        # Force minus linear drag
        inputForceMax = 1000
        dragForceCoef = 5
        F = inputForceMax*i - dragForceCoef*u0
        a = F/m
        t = self.currT - self.prevT
 
        # Zero t if it's too large
        if t > 0.5:
            t = 0.0
        x0 = target
 
        # Equations of motion
        u = u0 + a*t
        x = x0 + u0*t + 0.5*a*math.pow(t, 2)
 
        return x, u