Gait

The main function of the Gait is to cycle through the walking motion. This is done using a FSM with states dependent on the position of the foot.

If we look at one leg, animal or human gait consist of 2 phases:

1. Swing Phase: The foot is in the air, “swings” forward.

2. Stance Phase: The foot is on the ground, “pushing” back. Here the foot supports the weight while moving the body forward.

Stance Phase

This is where the leg moves the body, and it is actually the less complicated of the motions. It directly corresponds to the movement of the body and is therefore directly linked to the input.

Swing Phase

The swing phase is further divided into sub-phases, here I have chosen sub-phases which seemed to make to most sense to me:

Stance_to_Swing

Transition state where foot moves of the ground while keeping in line with the movement of the body. In case of soft or uneven ground this means the leg can lift itself before moving in the opposite direction.

Swing_Lift

Lifting the foot until it reaches a certain height.

Swing_Plateau

Moves the foot to an estimated suitable starting position according to the current movement direction.

Swing_Lower

Lowers the foot in preparation of the next stance phase

Swing_To_Stance

Opposite motion of the stance_to_swing phase.

Complete Motion

The planed motion looks like this:

1. Stance

2. Stance_to_Swing

3. Swing_Lift

4. Swing_Plateau

5. Swing_Lower

6. Swing_To_Stance

The gray line depicts the neutral position for the Coxa joint.

This motion is the desired steady state when the robot is walking without changing direction or speed.

Start Walking

Let us assume the feet are i a neutral position, this means all feet touch the ground and the Coxa joint is in a neutral position, as indicated by the gray line.

This means some legs have to move in Stance state while others have to transition to the swing state. The goal is to reach a steady state as soon as possible.

Foot 0 starts in stance, while foot 1 transitions directly to swing. With the current implementation a steady state is reached in 2 cycles.

Stop Walking

When the robot stops walking the legs should move into a neutral position, in order to be ready for further movements or sitting down.

This means that the legs cannot move anymore during stance phase, but have to reach the neutral point in the swing phase.

Walking Patterns

For stability reasons at least 3 legs have to touch the ground at any time. Otherwise there is no guarantee that the hexapod is stable.

This restriction leaves several possible gaits. Ants and other insects usually walk with a tripod gait:

Tripod Gait

Tripod gait means that 3 legs are in the stance phase while the other 3 are in swing state.

Code

One step cycle is the movement from a neutral point through stance & swing back to the neutral point. The step cycle is divided into a number of sub-steps. I have chosen 50 sub-steps for the stance as well as the swing phase, which results in at most 100 sub-steps per cycle.

Gait distinguishes between straight walking (angular velocity z = 0) and walking in circles.

Walking Straight

All legs on the ground have the same speed and direction, which is directly proportional to the input.

gait.cpp:

if (cmd_vel.angular.z == 0) {
  dx[leg_index] = -vx;
  dy[leg_index] = -vy;
}

dz[leg_index] = 0;  // feet stay on the ground

dx and dy are the relative distances each leg moves. This means the legs on the ground move directly opposite to the robot’s direction.

The swing phase is divided in states according to the description above:

Stance_to_Swing/Swing_To_Stance

Both states are pretty much the same.

gait.cpp:

if (cmd_vel.angular.z == 0) {
  dx[leg_index] = -transition_factor[0][transition_index] * vx;
  dy[leg_index] = -transition_factor[0][transition_index] * vy;
}

dz[leg_index] = transition_factor[1][transition_index] * height_per_step;

Where transition_factor is a hard coded transition sequence, where the foot is lifted/lowered slowly and moving another two sub-step in sync with the stance feet while moving off the ground:

gait.hpp:

std::vector<std::vector<double>> transition_factor =
{
  {1, 1, 0},  // x/y movement
  {0, 0.5, 1}   // z movement
};

Swing_Lift/Swing_Plateau/Swing_Lower

During this part of the swing the foot should reach a good position where the next stance phase can start. In a steady state situation the end position would be opposite of the start position. But the input can change any time, which complicates things.

In order to reach a steady state movement in any case, it has been decided that the swing phase has to pass through the neutral point. This prevents too large deviations from a steady state.

In order to achieve the pass through neutral point the swing phase has been further divided into move to neutral and move from neutral.

This is achieved using the reached_neutral flag:

gait.cpp:

// dx/dy movement to/from neutral, only for these 3 states.
// 1. Reach neutral point
// 1a if neutral point reached before steps/2 goto steps/2 to shorten the walking period
// 2. move from neutral to good starting point for next step
if (gait_state == Swing_lift ||
  gait_state == Swing_plateau ||
  gait_state == Swing_lower)
{
  if (!reached_neutral) {
    bool close_neutral = true;
    // check if we are close to the neutral point
    for (int leg_index : leg_groups[leg_group_index]) {
      if (std::abs(foot_pos_x[leg_index] - neutral_foot_pos_x[leg_index]) > stability_radius ||
        std::abs(foot_pos_y[leg_index] - neutral_foot_pos_y[leg_index]) > stability_radius)
      {
        close_neutral = false;
      }
    }

Checks if all feet are close to the neutral point. If this is the case the sub-step-index is moved ahead if necessary in order to shorten the step cycle. This happens in case of the start walking, where the initial position is close to neutral. If the step cycle would make use of the full number of steps in this case, the neutral position could not be reached fast enough and the steady state would be out of reach.

    // Skip a few steps if we are already close to the neutral point
    if (close_neutral) {
      // std::cerr << "Reached neutral point" << std::endl;
      reached_neutral = true;
      if (sub_step_index < SUB_STEPS / 2) {
        sub_step_index = SUB_STEPS / 2;
      }
    }
  }

The goal is to achieve the neutral point fast, but at a similar time for all legs. Because it does not help if some legs are faster than others. The motion is smoother if the legs reach the neutral point around the same time.

Therefore, we estimate the max. nr of steps the slowest foot requires to reach the target, on either the x or y axis:

gait.cpp:

  if (!reached_neutral) {
    // move to neutral point, try to do so before reaching mid cycle
    step_nr = 0;
    // find max nr of steps required to reach neutral point
    for (int leg_index : leg_groups[leg_group_index]) {
      step_nr = std::max(
        step_nr,
        std::abs(
          (foot_pos_x[leg_index] - neutral_foot_pos_x[leg_index]) /
          (distance_per_step * scaling_factor))
      );
      step_nr = std::max(
        step_nr,
        std::abs(
          (foot_pos_y[leg_index] - neutral_foot_pos_y[leg_index]) /
          (distance_per_step * scaling_factor))
      );
    }

This is reevaluated on each new sub-step.

Assign the dx/dy to move to the neutral point, but not faster than (distance_per_step * scaling_factor).

gait.cpp:

  for (int leg_index : leg_groups[leg_group_index]) {
    // move foot the fastet way to neutral position
    dx[leg_index] = -sgn((foot_pos_x[leg_index] - neutral_foot_pos_x[leg_index])) *
      std::min(
      std::abs((foot_pos_x[leg_index] - neutral_foot_pos_x[leg_index])) / step_nr,
      (distance_per_step * scaling_factor)
      );
    dy[leg_index] = -sgn((foot_pos_y[leg_index] - neutral_foot_pos_y[leg_index])) *
      std::min(
      std::abs((foot_pos_y[leg_index] - neutral_foot_pos_y[leg_index])) / step_nr,
      (distance_per_step * scaling_factor)
      );
  }

This concludes the move_to_neutral part of the swing state.

The move from neutral is considerably simpler, as the movement is simply in the opposite direction of the feet in stance phase.

gait.cpp:

  } else {
    // move from neutral point, as many steps as there are still left in the cycle
    double scaling_factor = (SUB_STEPS + transition_steps) / (SUB_STEPS - 2 * transition_steps);
    for (int leg_index : leg_groups[leg_group_index]) {
      if (cmd_vel.angular.z == 0) {
        dx[leg_index] = vx * scaling_factor;
        dy[leg_index] = vy * scaling_factor;
      }

The dz movement in swing is extremely simple: Swing_Lift: dz = height_per_step Swing_Plateau: dz = 0 Swing_Lower: dz = -height_per_step

Switch Between Swing and Stance

After a finished swing/stance phase the foot switches to stance/swing phase. All legs are grouped, and an index of each group dictates if they are in swing or stance phase, and when to switch:

gait.cpp:

  if (new_gait_interval) {
    sub_step_index = 0;
    leg_group_index = (leg_group_index + 1) % 2;
    leg_group_index1 = (leg_group_index + 1) % 2;

    // std::cerr << "[----------] Switch leg group " << std::endl;
  }

Assign Feet Positions

After the relative movements of all feet are calculated the values are assigned:

gait.cpp:

// Calculate the position for each foot
for (int leg_index = 0; leg_index < NUMBER_OF_LEGS; leg_index++) {
  next_foot_pos_x[leg_index] = foot_pos_x[leg_index] + dx[leg_index];
  next_foot_pos_y[leg_index] = foot_pos_y[leg_index] + dy[leg_index];
  next_foot_pos_z[leg_index] = foot_pos_z[leg_index] + dz[leg_index];
  // TODO(voserp): Test if z goes below ground

  feet->foot[leg_index].position.x = next_foot_pos_x[leg_index];
  feet->foot[leg_index].position.y = next_foot_pos_y[leg_index];
  // This way the floor is at height 0
  feet->foot[leg_index].position.z = next_foot_pos_z[leg_index] - walking_height;
}

Walking Curves

Walking in curved lines is considerably more complicated than walking in a straight line.

While performing a turn the robot should have a center point around which it turns. This center point is directly calculated from the input.

If we consider the robot walking around a given center point each leg has to move around this point. This means each leg has a different radius because each leg has a different distance to the center point:

If we consider the the center of the robot moves \(d\phi\) around the center point, each leg has to travel a different distance since the radii are different for each leg, as illustrated above.

Center Point

One of the first steps required is to calculate the center point (x0, y0) according to the input:

gait.cpp:

if (cmd_vel.angular.z != 0) {
  if (vx == 0 && vy == 0) {
    x0 = 0;
    y0 = 0;
  } else {
    // 2.5/exp(vz) linearizes the input and scales it to a radius from approx. 0.22m to 1.8m
    x0 = -vy / sqrt(pow(vy, 2) + pow(vx, 2)) *  std::cout << "title('Foot 1 y'); xlabel('y'), ylabel('z')" << std::endl;

int Plot_Start = 00;
int Plot_Stop = 400;
int Plot_Stop = 150;
int Plot_Steps = Plot_Stop - Plot_Start;

Gait gait;
      sgn(cmd_vel.angular.z) * (2.5 / exp(abs(cmd_vel.angular.z)) - 0.7);
    y0 = vx / sqrt(pow(vy, 2) + pow(vx, 2)) *
      sgn(cmd_vel.angular.z) * (2.5 / exp(abs(cmd_vel.angular.z)) - 0.7);
    // std::cerr << "x0/y0 = " << x0 << "/" << y0 << std::endl;
  }

If vx and vy are 0, the robot rotates without moving. therefore x0 and y0 are 0. Otherwise the center is calculated like this:

\[x_0 = \frac{-v_y}{\sqrt{v_x^2 + v_y^2}} * sign(v_z) * factor\]

Where the factor is used for linearizing and scaling of the center point position:

\[factor = \frac{2.5}{\exp{|v_z|}} - 0.7\]

Using the center point the distance of each foot is calculated:

gait.cpp:

for (int leg_index = 0; leg_index < NUMBER_OF_LEGS; leg_index++) {
  rho[leg_index] = sqrt(
    pow(foot_pos_x[leg_index] - x0, 2) +
    pow(foot_pos_y[leg_index] - y0, 2)
  );

Also the angle of each foot corresponding to the current position:

if ((foot_pos_x[leg_index] - x0) < 0) {
  phi[leg_index] = M_PI - asin((foot_pos_y[leg_index] - y0) / rho[leg_index]);
} else {
  phi[leg_index] = asin((foot_pos_y[leg_index] - y0) / rho[leg_index]);
}

Then we need to calculate how much the robot moves, in terms of angle difference dphi:

    if (vx == 0 && vy == 0) {
      dphi = cmd_vel.angular.z * dphi_max_standing;
    } else {
      dphi = sgn(cmd_vel.angular.z) * atan(sqrt(pow(vx, 2) + pow(vy, 2)) / max_rho);
      // dphi has to be limited so that the fastest foot is not faster than the allowed limit
    }

If we have the radius of each foot and dphi we can calculate the movement of the feet on the ground:

for (int leg_index : leg_groups[leg_group_index1]) {
  if (cmd_vel.angular.z == 0) {
    dx[leg_index] = -vx;
    dy[leg_index] = -vy;
  } else {
    dx[leg_index] = -foot_pos_x[leg_index] +
      rho[leg_index] * cos(phi[leg_index] - dphi) + x0;
    dy[leg_index] = -foot_pos_y[leg_index] +
      rho[leg_index] * sin(phi[leg_index] - dphi) + y0;
  }

  dz[leg_index] = 0;  // feet stay on the ground
}

If the angular velocity of z is 0 (walking straight), the the legs move directly as the input dictates. The rotation motion is calculated in the else clause. Movement in the swing phase can be the same for waling straight or walking in circles, since the legs don’t touch the ground in the swing phase.

This describes roughly the gait implementation. The detailed math might follow later if requested.