Close

Quadruped Walking and Gait Testing

A project log for CAN Controlled Dual Closed-Loop Motor Controller

SAMD21 powered dual micro-gearmotor controller with CAN bus interface

Jake WachlinJake Wachlin 10/19/2020 at 21:580 Comments

The last log covered some of the preparation for building a walking quadruped robot using four of the dual motor controllers, one for each leg. The parts for doing this have now arrived, and I have been working on getting the system walking.


Leg Trajectory Simulation

While I had previously shown the motor controller controlling a leg through a motion profile, this profile was a complete guess and not based on any research. Unsurprisingly, that motion profile did not work well to get the system moving. I therefore decided to write a Python kinematic simulator that could be used to rapidly evaluate motion profiles, see if they are in the leg's reachable area, and ensure my inverse and forward kinematics are correct. I set it up roughly as a mirror of the C code running on the leg controller to ensure that process is correct. I give it a set of keyframes. Each keyframe consists of an x and y cartesian space coordinate and a value that indicates at what part of the cycle period it should reach that point. All other desired positions are linearly interpolated between those points. These desired positions are fed first into the inverse kinematic solver to calculate joint angles, then the forward kinematic solver to calculate knee and foot positions. This step will fail if the desired position is outside of the reachable area. The positive X axis is "down" and the positive Y axis is "forward," with the origin at the hip. The blue line in the image below shows the leg, with circle markers at the hip, knee, and foot. The black dots are the foot positions throughout the trajectory, equally spaced in time. Therefore, they are more bunched together during slower motions and more spaced apart during fast movements.

Clearly, this is an odd and complication motion and it is unsurprising that it doesn't work well. 


Gaits 

With my motion profile guess not working well, I spent some time reviewing research into quadrupedal robots. One of the most well known is the MIT Cheetah. I had the pleasure of taking Professor Sangbae Kim's Bioinspired Robotics course at MIT, and it was a fascinating introduction to the topic in general and to the MIT Cheetah in particular. Researchers have long classified different gaits for walking animals, including the trot, gallop, pronk, and bound. The following video in particular shows these various actions on the MIT Cheetah. Even if you don't know these gait classification terms, you will recognize the motions.

These motions serve as a starting inspiration for the gaits I will implement on my robot. I started with the slow trot, shown at about 0:25 into that video. From the video, we can gain some insight into how this gait is implemented. The front right leg and rear left leg are in sync, and the front left leg is in sync with the rear right leg. Throughout the motion, the foot is on the ground roughly 3X longer than it is in the air. The trajectory is shifted rearwards of the hip, so that the front of the trajectory is just slightly forward of the hip. Although the trajectory is likely more complex, and is skewed due to the proprioceptive control used, it looks to my eye roughly to be a triangle with side lengths that are roughly equal. The image below shows the trajectory generated for this gait. It matches all these observed characteristics.

The next gait is called the fast trot at about 0:32 in that video. Again, this is not very precise, but we can extract some rough characteristics from what we see. As with the slow trot, the front right leg and rear left leg are in sync and the front left leg is in sync with the rear right leg. However, here the foot is on the ground roughly the same amount of time that it is in the air. Because of this, most of the time only two feet are touching the ground. In the slow trot, there is a lot of time with all four feet contacting. This motion profile is shifted more forward, with the rear just back of the hip. It looks roughly triangular, but shorter and longer. However, on ground contact, the foot does appear to be moving backwards already. The image below shows this proposed trajectory.

The bound gait looks more like a gait a rabbit or frog would use than one a dog would. However, the gait of a running cheetah or dog actually looks somewhat bound-like. In the example in the video at around 0:40, the front legs move together and the rear legs move together. Each foot is on the ground roughly the same amount of time that it is in the air. The motion is roughly centered about the hip. It follows sort of a sharp triangle motion. The image below shows the proposed profile.

The video also shows a gallop and pronk, but we are not yet ready to try that, and I'm not sure the hardware could do it yet. 

DISCLAIMER

Note that the MIT Cheetah does not actually use these profiles. They are simplified versions extracted by eye from videos. In fact, they use more complicated profiles. One of their academic papers shows the leg profiles as mostly rounded-rectangle-looking Bezier curves, and show that the back legs were given different profiles from the front legs. In addition, due to the proprioceptive control used, the "positional tracking" is actually quite poor, but poor on purpose.


Implementation

Since the Python code was set up to be very similar to the C implementation, bringing in the proposed motion profiles was simple. I now have 4 different motion primitives corresponding to these 4 gaits.

void motion_primitive_init(void)
 {
    // Leg straight, motors at zero is leg straight down +x. Forward is +y
    /*
    *                O-> +y
    *                | \
    *            +x \/   \
    *                     O
    *                     |
    *                     |
    *                     O    
    *                        
    */

    // t_part must be always increasing, never > 1.0. Must be a cyclical motion primitive

    // Slow trot walk
    // Triangular, roughly 3X longer on ground than in air, front slightly forward of hip
    primitives[0].num_keyframes = 3;
    primitives[0].tau = 2.0;
    primitives[0].t_offset = 0.0;
    primitives[0].invert = 0;
    primitives[0].time_reverse = 0;
    primitives[0].x_offset_m = 0;
    primitives[0].y_offset_m = 0;
    primitives[0].x_scale = 1.0;
    primitives[0].y_scale = 1.0;

    primitives[0].frames[0].t_part = 0.0;
    primitives[0].frames[0].x = 0.105;
    primitives[0].frames[0].y = 0.01;

    primitives[0].frames[1].t_part = 0.75;
    primitives[0].frames[1].x = 0.105;
    primitives[0].frames[1].y = -0.05;

    primitives[0].frames[2].t_part = 0.875;
    primitives[0].frames[2].x = 0.06;
    primitives[0].frames[2].y = -0.02;

    // Fast trot walk
    // Mostly triangular, in air roughly same as on ground, rear slightly backward of hip
    primitives[1].num_keyframes = 4;
    primitives[1].tau = 2.0;
    primitives[1].t_offset = 0.0;
    primitives[1].invert = 0;
    primitives[1].time_reverse = 0;
    primitives[1].x_offset_m = 0;
    primitives[1].y_offset_m = 0;
    primitives[1].x_scale = 1.0;
    primitives[1].y_scale = 1.0;

    primitives[1].frames[0].t_part = 0.0;
    primitives[1].frames[0].x = 0.09;
    primitives[1].frames[0].y = 0.06;

    primitives[1].frames[1].t_part = 0.5;
    primitives[1].frames[1].x = 0.09;
    primitives[1].frames[1].y = -0.01;

    primitives[1].frames[2].t_part = 0.75;
    primitives[1].frames[2].x = 0.05;
    primitives[1].frames[2].y = 0.02;

    primitives[1].frames[3].t_part = 0.95;
    primitives[1].frames[3].x = 0.08;
    primitives[1].frames[3].y = 0.065;

    // Bound
    // Mostly triangular, in air roughly same as on ground, centered on hip
    primitives[2].num_keyframes = 4;
    primitives[2].tau = 2.0;
    primitives[2].t_offset = 0.0;
    primitives[2].invert = 0;
    primitives[2].time_reverse = 0;
    primitives[2].x_offset_m = 0;
    primitives[2].y_offset_m = 0;
    primitives[2].x_scale = 1.0;
    primitives[2].y_scale = 1.0;

    primitives[2].frames[0].t_part = 0.0;
    primitives[2].frames[0].x = 0.105;
    primitives[2].frames[0].y = 0.03;

    primitives[2].frames[1].t_part = 0.5;
    primitives[2].frames[1].x = 0.105;
    primitives[2].frames[1].y = -0.03;

    primitives[2].frames[2].t_part = 0.7;
    primitives[2].frames[2].x = 0.08;
    primitives[2].frames[2].y = -0.015;

    primitives[2].frames[3].t_part = 0.9;
    primitives[2].frames[3].x = 0.095;
    primitives[2].frames[3].y = 0.04;

    // Pronk
    // Only vertical motion, slow down, fast down, fast up, fast to nominal
    primitives[3].num_keyframes = 5;
    primitives[3].tau = 2.0;
    primitives[3].t_offset = 0.0;
    primitives[3].invert = 0;
    primitives[3].time_reverse = 0;
    primitives[3].x_offset_m = 0;
    primitives[3].y_offset_m = 0;
    primitives[3].x_scale = 1.0;
    primitives[3].y_scale = 1.0;

    primitives[3].frames[0].t_part = 0.0;
    primitives[3].frames[0].x = 0.07;
    primitives[3].frames[0].y = 0.0;

    primitives[3].frames[1].t_part = 0.3;
    primitives[3].frames[1].x = 0.09;
    primitives[3].frames[1].y = 0.0;

    primitives[3].frames[2].t_part = 0.4;
    primitives[3].frames[2].x = 0.105;
    primitives[3].frames[2].y = 0.0;

    primitives[3].frames[3].t_part = 0.5;
    primitives[3].frames[3].x = 0.05;
    primitives[3].frames[3].y = 0.0;

    primitives[3].frames[4].t_part = 0.6;
    primitives[3].frames[4].x = 0.07;
    primitives[3].frames[4].y = 0.0;
 }

I then set up the main ESP32 robot controller to cycle through these gaits, using the appropriate leg pairings when it did. I noticed during the fast trot that the system was tilted back significantly (likely due to PID tuning), so as a quick fix I added some primitive offsets on those commands. Also the random short delays are because I am having some bug with CAN right now reducing reception rates.

#include <CAN.h>
#include "dual_motor_can.h"

#define CAN_RX_PIN      4
#define CAN_TX_PIN      5
#define LED2_PIN              16

#define FRONT_LEFT            (1)
#define FRONT_RIGHT           (2)
#define REAR_RIGHT            (3)
#define REAR_LEFT             (4)

#define MOTOR_0                (0)
#define MOTOR_1                (1)

#define PRIMITIVE_SLOW_TROT    (0)
#define PRIMITIVE_FAST_TROT    (1)
#define PRIMITIVE_BOUND        (2)
#define PRIMITIVE_PRONK        (3)

#define INVERT                (1)
#define NO_INVERT             (0)

#define REVERSE               (1)
#define NO_REVERSE            (0)

#define NUM_PRIMITIVES        (4)
#define PRIMITIVE_CHANGE_PERIOD_MS  (10000)

static uint8_t prim_index = 0;
static uint8_t primitives[NUM_PRIMITIVES] = {PRIMITIVE_SLOW_TROT, PRIMITIVE_FAST_TROT, PRIMITIVE_BOUND, PRIMITIVE_PRONK};

void set_legs_mode(uint8_t prim)
{
  if(prim == PRIMITIVE_SLOW_TROT)
  {
    send_primitive_command(FRONT_LEFT, prim, NO_INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_command(FRONT_RIGHT, prim, INVERT, NO_REVERSE, 1000, 500);
    delay(50);
    send_primitive_command(REAR_RIGHT, prim, INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_command(REAR_LEFT, prim, NO_INVERT, NO_REVERSE, 1000, 500);
    delay(50);
  }
  else if(prim == PRIMITIVE_FAST_TROT)
  {
    send_primitive_params(FRONT_LEFT, prim, -5, 0, 100, 100); // Shift front upwards 
    delay(50);
    send_primitive_command(FRONT_LEFT, prim, NO_INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_params(FRONT_RIGHT, prim, -5, 0, 100, 100); // Shift front upwards 
    delay(50);
    send_primitive_command(FRONT_RIGHT, prim, INVERT, NO_REVERSE, 1000, 500);
    delay(50);
    send_primitive_params(REAR_RIGHT, prim, 5, 0, 100, 100); // Shift rear downwards 
    delay(50);
    send_primitive_command(REAR_RIGHT, prim, INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_params(REAR_LEFT, prim, 5, 0, 100, 100); // Shift rear downwards 
    delay(50);
    send_primitive_command(REAR_LEFT, prim, NO_INVERT, NO_REVERSE, 1000, 500);
    delay(50);
  }
  else if(prim == PRIMITIVE_BOUND)
  {
    send_primitive_command(FRONT_LEFT, prim, NO_INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_command(FRONT_RIGHT, prim, INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_command(REAR_RIGHT, prim, INVERT, NO_REVERSE, 1000, 500);
    delay(50);
    send_primitive_command(REAR_LEFT, prim, NO_INVERT, NO_REVERSE, 1000, 500);
    delay(50);
  }
  else if(prim == PRIMITIVE_PRONK)
  {
    send_primitive_command(FRONT_LEFT, prim, NO_INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_command(FRONT_RIGHT, prim, INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_command(REAR_RIGHT, prim, INVERT, NO_REVERSE, 1000, 0);
    delay(50);
    send_primitive_command(REAR_LEFT, prim, NO_INVERT, NO_REVERSE, 1000, 0);
    delay(50);
  }
}

void setup() {
  Serial.begin(115200);

  pinMode(LED2_PIN, OUTPUT);
  digitalWrite(LED2_PIN, LOW);

  Serial.println("CAN position control demo");

  CAN.setPins(CAN_RX_PIN, CAN_TX_PIN);

  // start the CAN bus at 1000 kbps
  if (!CAN.begin(1000E3)) {
    Serial.println("Starting CAN failed!");
    while (1);
  }

  delay(4003);

  // sync at start
  for (int i = 1; i <= 4; i++)
  {
    delay(30);
    send_time_sync(i, millis());
  }

  // Track profiles
  set_legs_mode(PRIMITIVE_SLOW_TROT);

  digitalWrite(LED2_PIN, HIGH);
}

void loop() {
  static uint32_t last_mode_change = 0;
  delay(1000);
  digitalWrite(LED2_PIN, !digitalRead(LED2_PIN));
  // Keep synced
  for (int i = 1; i <= 4; i++)
  {
    delay(30);
    send_time_sync(i, millis());
  }

  if(millis() - last_mode_change > PRIMITIVE_CHANGE_PERIOD_MS)
  {
    last_mode_change = millis();
    prim_index++;
    if(prim_index >= NUM_PRIMITIVES)
    {
      prim_index = 0;
    }
    set_legs_mode(primitives[prim_index]);
  }
}

WALKING ROBOTS!!

Now the part you actually care about, videos of the gaits. First, the slow trot. Due to the high CG and no stability controller, this motion is somewhat unstable, but it does work!

Next, the fast trot. While this does work, it doesn't look quite like the MIT Cheetah's gait. The whole system leans back, and the rear legs drag as they come forward. This gait can use some improvement but it moves in the right direction.

Finally, the bounding gait. This works marginally. If the timing is right, the system does bound properly. If not, it doesn't really move. It's also worth noting it moves in the opposite direction of what was intended, so not sure I can call that a success just yet.


Summary

I now have a working quadruped robot that I can develop gaits, higher level stability controllers, and more!

Discussions