Motion Loop

Motion Control contains the main Finite State Machine, with these states:

The main loop of the motion controller. It is a FSM (Finite State Machine) which governs the state transitions.

There are following states present:

motion_control.hpp:

typedef enum
{
  Unknown,
  Idle,
  Sitting,  // Sitting, torque off
  Standing_up,
  Standing,
  Walking,
  Sitting_down
} tState;

State: Unknown

Default state. Turn off servo motors. This is a save state that should never be reached.

motion_control.cpp:

// Loop in which the motions are coordinated
void MotionControl::motion_loop(void)
{
  switch (hex_state) {
    case Unknown:   // check in which state the robot is and go to idle
      servo_driver.free_servos();
      break;

State: Idle

Idle state is the default starting state.

State transitions

The robot waits for an input in form of the “start_cmd”. If the command is received the state changes to Sitting while the servos are turned on.

motion_control.cpp:

    case Idle:
      if (start_cmd) {
        next_hex_state = Sitting;
        servo_driver.turn_on_servos();
      }
      break;

State: Sitting

In the sitting state the servo motors move to the neutral sitting position with a reduced speed. This enables a smooth transition from unknown servo positions to the neutral sitting position. (In the visualization this step is unconstrained, meaning it will take 0 time to execute).

State transitions

The stand_up_cmd resets the servos to full speed again, while transitioning to the standing up state. If the start_cmd is reset the robot goes back to the idle state.

motion_control.cpp:

    case Sitting:
      current_height = sitting_height;
      servo_driver.set_servo_speed(0.2);  // reduce speed to prevent jerking motion
      for (int c = 0; c < NUMBER_OF_LEGS; c++) {
        feet.foot[c].position.x = neutral_foot_pos_x[c];
        feet.foot[c].position.y = neutral_foot_pos_y[c];
        feet.foot[c].position.z = current_height;
      }

      if (stand_up_cmd) {
        servo_driver.set_servo_speed(0);  // max speed
        next_hex_state = Standing_up;
      } else if (!start_cmd) {
        next_hex_state = Idle;
        servo_driver.free_servos();
      }
      break;

State: Standing_up

In the standing up state the height of the body is increased until it reaches the desired standing height.

State transitions

As soon as the standing height is reached the FSM transitions to the walking state.

motion_control.cpp:

    case Standing_up:
      current_height -= sit_step_height;
      if (current_height <= -walking_height) {
        next_hex_state = Walking;
        current_height = -walking_height;
      }

      for (int c = 0; c < NUMBER_OF_LEGS; c++) {
        // feet.foot[c].position.x = neutral_foot_pos_x[c];
        // feet.foot[c].position.y = neutral_foot_pos_y[c];
        feet.foot[c].position.z = current_height;
      }
      break;

State: Standing

In the standing state the robot only moves its body without moving the feet positions. This also serves as a proof of the inverse kinematics.

State transitions

If the stand_up_cmd is reset, the robot will sit down again. If the walking_cmd is set the robot goes to the Walking state.

motion_control.cpp:

    case Standing:
      body.orientation.pitch = 0.4 * cmd_vel_.linear.x;
      body.orientation.roll = 0.5 * cmd_vel_.linear.y;
      body.orientation.yaw = 0.5 * cmd_vel_.angular.z;

      if (!stand_up_cmd) {
        if (gait.is_standing()) {
          next_hex_state = Sitting_down;
        }
      }

      if (walking_cmd) {
        next_hex_state = Walking;
      }
      break;

State: Walking

This state simply invokes the gait_cycle, which is where it starts walking according to the user input.

State transitions

If the stand_up_cmd is reset, the robot will sit down again. If the walking_cmd is reset the robot goes to the Standing state.

motion_control.cpp:

    case Walking:
      gait.gait_cycle(cmd_vel_, &feet);
      /*
      std::cerr << "[          ]      feet(x,y,z) G1: (" <<
        feet.foot[0].position.x << ", " <<
        feet.foot[0].position.y << ", " <<
        feet.foot[0].position.z << ")" <<
        std::endl;
      */

      if (!stand_up_cmd) {
        if (gait.is_standing()) {
          next_hex_state = Sitting_down;
        }
      }
      if (!walking_cmd) {
        if (gait.is_standing()) {
          next_hex_state = Standing;
        }
      }
      break;

State: Sitting_down

This is the reversed motion from standing up.

motion_control.cpp:

    case Sitting_down:
      current_height += sit_step_height;
      if (current_height >= sitting_height) {
        current_height = sitting_height;
        next_hex_state = Sitting;
      }

      for (int c = 0; c < NUMBER_OF_LEGS; c++) {
        // feet.foot[c].position.x = neutral_foot_pos_x[c];
        // feet.foot[c].position.y = neutral_foot_pos_y[c];
        feet.foot[c].position.z = current_height;
      }
      break;

Transmitting the Results

After the State logic the new states are assigned:

motion_control.cpp:

hex_state = next_hex_state;

And the leg joint angles are translated into joint_states which are then transmitted to the servo driver:

motion_control.cpp:

// transmit the servo positions
if (hex_state != Idle) {
  if (inverse_kinematics.calculate_ik(feet, body, &legs)) {
    int i = 0;
    for (int leg_index = 0; leg_index < NUMBER_OF_LEGS; leg_index++) {
      joint_state_.position[i] = legs.leg[leg_index].coxa;
      i++;
      joint_state_.position[i] = legs.leg[leg_index].femur;
      i++;
      joint_state_.position[i] = legs.leg[leg_index].tibia;
      i++;
    }
    servo_driver.transmit_servo_positions(joint_state_);
  } else {
    RCLCPP_WARN(rclcpp::get_logger("Motion Control"), "Out of Range");
    feet = old_feet;  // Do not move any foot if one is out of range
  }
}

old_feet = feet;

Functions

Not all functions are listed here, there are some more in the code, some of which are unused, obsolete or used for testing.

Sit Down/Stand Up

Commands used to trigger transitions. Only works if the start_cmd flag is set.

motion_control.cpp:

void MotionControl::stand_up(void)
{
  if (start_cmd) {
    RCLCPP_INFO(rclcpp::get_logger("Motion Control"), "Stand up");
    stand_up_cmd = true;
  }
}

void MotionControl::sit_down(void)
{
  if (start_cmd) {
    RCLCPP_INFO(rclcpp::get_logger("Motion Control"), "Sit down");
    stand_up_cmd = false;
  }
}

Start & Stop Command

This command first checks if the hexapod is sitting, if yes it sets the flag to either turn the servo motors on or off.

motion_control.cpp:

void MotionControl::start_stop(void)
{
  if (stand_up_cmd) {return;}
  if (start_cmd) {
    RCLCPP_INFO(rclcpp::get_logger("Motion Control"), "Stop");
    start_cmd = false;
  } else {
    RCLCPP_INFO(rclcpp::get_logger("Motion Control"), "Start");
    start_cmd = true;
  }
}

Stand/Walk Command

This command sets the flag to either walk or move the body while standing.

motion_control.cpp:

void MotionControl::stand_walk(void)
{
  if (!stand_up_cmd) {return;}
  if (walking_cmd) {
    RCLCPP_INFO(rclcpp::get_logger("Motion Control"), "Stand");
    walking_cmd = false;
  } else {
    RCLCPP_INFO(rclcpp::get_logger("Motion Control"), "Walk");
    walking_cmd = true;
  }
}

Update Velocity Command

Sets a new velocity if the robot is in the standing position.

motion_control.cpp:

void MotionControl::update_vel(geometry_msgs::msg::Twist cmd_vel)
{
  // if the sitdown commands arrives the velocity is set to 0.
  // So the Hexapod can enter a standing state
  if (stand_up_cmd) {
    cmd_vel_ = cmd_vel;
  } else {
    cmd_vel_.linear.x = 0;
    cmd_vel_.linear.y = 0;
    cmd_vel_.angular.z = 0;
  }
}