Catalogue of series articles

Tip: you can add the directories of all articles in the series here. You need to add the directories manually
TODO: finishing after writing

preface

Limited cognition, I hope you will forgive me. If you have any problems, I hope to communicate with you and grow together!

Firstly, this paper briefly introduces the conversion of remote control handle state command to robot trunk state input. The specific content will be more later. Other modules can refer to my other articles

Tip: the following is the main content of this article

1, Trunk state command type (position, attitude, angle, angular velocity) input by remote control handle

type

(1) Forward linear speed
(2) Transverse linear velocity
(3) Vertical linear velocity

(4) Torso centroid X position
(5) Torso centroid Y position
(6) Torso Z height position

(7) Torso left and right roll angular velocity WR
(8) Trunk fore-and-aft pitch angular velocity WP
(9) Torso yaw angular velocity WY

(10) Torso left and right roll angle R
(11) Trunk fore-and-aft pitch angle P [anti theft mark – hzj]
(12) Torso yaw angle Y
This is a relatively complete definition. Remote sensing is two-dimensional, and one dial is two data

code

/**
 * Function: the desired state of the remote control refers to the COM track data
 */
template <typename T>
struct DesiredStateData 
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  DesiredStateData() { zero(); }
  void zero();                            // Reset all data to zero
  Vec12<T> stateDes;                      // Instantaneous desired state command
  Vec12<T> pre_stateDes;                  //Last expected state command
  Eigen::Matrix<T, 12, 10> stateTrajDes;  // Desired future state trajectory (up to 10 time steps MPC)
};

.
.

2, State types required for quadruped robot trunk state

type

(1) Trunk forward linear speed
(2) Transverse linear velocity of trunk

(3) Torso centroid X position
(4) Torso centroid Y position [anti theft mark – box hzj]

(5) Torso yaw angular velocity WY
(6) Torso yaw angle Y

code

/**
 * Function: torso status reference COM trajectory planning command
 */
template <typename T>
class DesiredStateCommand 
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  //Initialize with Gamepad Command structure, fun (joystick command, handle parameter setting, robot control parameters, state estimator data)
  DesiredStateCommand(GamepadCommand* command, rc_control_settings* rc_command,
                      RobotControlParameters* _parameters,
                      StateEstimate<T>* sEstimate, float _dt) 
{
    gamepadCommand = command;     //
    rcCommand = rc_command;       //
    stateEstimate = sEstimate;    //
    parameters = _parameters;     //

    data.stateDes.setZero();      //
    data.pre_stateDes.setZero();  //
    leftAnalogStick.setZero();    //
    rightAnalogStick.setZero();   //

    dt = _dt;
  }

  void convertToStateCommands();
  void setCommandLimits(T minVelX_in, T maxVelX_in,
                        T minVelY_in, T maxVelY_in, T minTurnRate_in, T maxTurnRate_in);
  void desiredStateTrajectory(int N, Vec10<T> dtVec);
  void printRawInfo();
  void printStateCommandInfo();
  float deadband(float command, T minVal, T maxVal);

  // These should come from the interface
  T maxRoll = 0.4;
  T minRoll = -0.4;
  T maxPitch = 0.4;
  T minPitch = -0.4;
  T maxVelX = 3.0;
  T minVelX = -3.0;

  T maxVelY = 2.0;
  T minVelY = -2.0;

  T maxTurnRate = 2.5;
  T minTurnRate = -2.5;

  Vec2<float> leftAnalogStick;
  Vec2<float> rightAnalogStick;

  // Maintain the trajectory of instantaneous expected state and future expected state
  DesiredStateData<T> data;
  const rc_control_settings* rcCommand;
  const GamepadCommand* gamepadCommand;
  bool trigger_pressed = false;

private:
  StateEstimate<T>* stateEstimate;
  RobotControlParameters* parameters;
  Mat12<T> A;                 //Dynamic matrix of discrete time approximation
  T dt;                       //Control loop time step change
  T deadbandRegion = 0.075;   //Simulate stick deadband cutoff
  const T filter = 0.1;
  int printNum = 5;           //Select the frequency of printing information every n iterations / / N*(0.001s) in simulation time
  int printIter = 0;          // Tracks the number of iterations since the last information print
};

.
.

3, The remote control handle state command is converted to the robot trunk state type [core]

1. Principle

Calculate the desired trunk state track (12 elements, 6 elements in essence) through the handle target command + the current state of the state estimator [anti theft mark – hzj]

(0) first clear the last expected state trajectory data

(1) Calculate and set forward linear speed (main)

Set the [anti-theft mark] with the analog value limit input by the left upper and lower joystick of the remote control handle–Box Zijun hzj]

(2) Calculate and set lateral linear velocity (main)

Use the analog value input from the left and right joystick of the remote control handle to limit the setting

(3) Calculate and set vertical linear velocity

There is no vertical speed by default

(4) Calculate and set the body centroid X position (primary)

Formula: torso centroid X position=State estimator pair X Estimate of location+(Handle control cycle*Body with handle currently set X Location)

(5) Calculate and set the body centroid Y position (primary)

Formula: torso centroid Y position=State estimator pair Y Estimate of location+(Handle control cycle*Body with handle currently set Y Location)

(6) Calculate and set Z height position [anti theft mark – box hzj]

The height of the robot is given, and the default is 45 CM

(7) Calculate and set the left and right roll angular speed WR

The default left and right scroll speed is 0

(8) Calculate and set the front and rear pitch angular velocity WP [anti theft mark – hzj]

The default fore-and-aft pitch angular velocity is 0

(9) Calculate and set yaw rate WY (primary)

Set the current torso yaw speed through the handle limiting

(10) Calculate and set the left and right roll angle R

When the model is simplified, the left and right rolling angles are assumed=0

(11) Calculate and set the fore-and-aft pitch angle P

When the model is simplified, the fore-and-aft pitch angle is assumed=0

(12) Calculate and set yaw angle Y (primary)

Formula: yaw angle Yaw=Effect of state estimator on yaw angle Yaw Estimated value of+(Handle control cycle*Body yaw angle currently set by handle Yaw)

2. Code

/**
 * Function: generate the desired COM trajectory of the torso [handle target command + current state of the state estimator] and calculate the desired state trajectory (12 state variables)
 */
template <typename T>
void DesiredStateCommand<T>::convertToStateCommands() 
{
  data.zero();    //(0) first clear the last expected state trajectory data

  //(1) Set forward linear speed
  //     --Use the analog value input from the upper and lower left joystick of the remote control handle to set the amplitude limit
  data.stateDes(6) =
      deadband(gamepadCommand->leftStickAnalog[1], minVelX, maxVelX);//Maximum or minimum analog value dead band limit of left upper and lower joystick of remote control handle

  //(2) Set lateral linear speed
  //     --Use the analog value input from the left and right joystick of the remote control handle to limit the setting
  data.stateDes(7) =
      deadband(gamepadCommand->leftStickAnalog[0], minVelY, maxVelY);//Maximum or minimum analog value dead band limit of left and right joystick of remote control handle

  //(3) Set vertical linear speed
  //     --There is no vertical speed by default
  data.stateDes(8) = 0.0;

  //(4) Sets the body centroid X position
  //    --Formula: torso centroid x position = estimated value of x position by the state estimator + (handle control cycle * torso x position currently set by the handle) explanation: expected x position increment = handle control cycle * torso x position currently set by the handle. The principle is similar to that of proportional controller
  data.stateDes(0) = stateEstimate->position(0) + dt * data.stateDes(6);

  //(5) Sets the body centroid Y position
  //    --Formula: torso centroid y position = estimated value of Y position by the state estimator + (handle control cycle * torso y position currently set by the handle) explanation: expected y position increment = handle control cycle * torso y position currently set by the handle. The principle is similar to that of proportional controller
  data.stateDes(1) = stateEstimate->position(1) + dt * data.stateDes(7);

  //(6) Set Z height position
  //    --The height of the robot is given, and the default is 45CM
  data.stateDes(2) = 0.45;

  //(7) Set the left and right roll angular speed  
  //     --The default left and right rolling speed is 0. When simplifying the model, it is assumed that the left and right rolling speed = 0
  data.stateDes(9) = 0.0;

  //(8) Set fore-and-aft pitch angular velocity 
  //     --The default fore-and-aft pitch angular velocity is 0. When simplifying the model, it is assumed that the fore-and-aft tilt velocity = 0
  data.stateDes(10) = 0.0;

  //(9) Set yaw rate
  //    --Set the current torso yaw speed through the handle limiting
  data.stateDes(11) =
      deadband(gamepadCommand->rightStickAnalog[0], minTurnRate, maxTurnRate);

  //(10) Sets the left and right roll angle  
  //    --When simplifying the model, it is assumed that the left and right rolling angle = 0
  data.stateDes(3) = 0.0;

  //(11) Set the fore-and-aft pitch angle
  //    --When simplifying the model, it is assumed that the front and rear pitch angle = 0
  data.stateDes(4) = 
     deadband(gamepadCommand->rightStickAnalog[1], minPitch, maxPitch);

  //(12) Set yaw angle 
  //   --Formula: Yaw angle Yaw = estimated Yaw angle Yaw by the state estimator + (handle control cycle * trunk Yaw angle Yaw currently set by the handle) explanation: expected Yaw angle Yaw increment = handle control cycle * trunk Yaw angle Yaw currently set by the handle. The principle is similar to that of proportional controller
  data.stateDes(5) = stateEstimate->rpy(2) + dt * data.stateDes(11);
}

.
.

4, COM trace set 0 function expected by torso (used during initialization)

/**
 * Function: set the desired COM track of the torso to 0 (used during initialization)
 */
template <typename T>
void DesiredStateData<T>::zero() 
{
  stateDes = Vec12<T>::Zero();
  stateTrajDes = Eigen::Matrix<T, 12, 10>::Zero();
}


template struct DesiredStateData<double>;
template struct DesiredStateData<float>;

5, Numerical dead band limit function (used for linear velocity and angular velocity on remote control handle rocker)

/**
 *  Function: numerical dead band limit function, used on remote control handle rocker
 */
template <typename T>
float DesiredStateCommand<T>::deadband(float command, T minVal, T maxVal) 
{
  if (command < deadbandRegion && command > -deadbandRegion) //The trajectory is within a reasonable range
  {
    return 0.0;
  }
  else                                                       //The trajectory is in an unreasonable range
  {
    return (command / (2)) * (maxVal - minVal);//Correction formula
  }
}

6, Print relevant status in real time for debugging

(1) Real time printing status command of handle operation

/**
 *  Function: real time print status command of handle operation
 *   There is no call at the end
 */
template <typename T>
void DesiredStateCommand<T>::printRawInfo() 
{
  //(1) Incremental print iteration
  printIter++;
  // (2) Print at the required frequency
  if (printIter == printNum) 
  {
    std::cout << "[DESIRED STATE COMMAND] Printing Raw Gamepad Info...\n";
    std::cout << "---------------------------------------------------------\n";
    std::cout << "Button Start: " << gamepadCommand->start
              << " | Back: " << gamepadCommand->back << "\n";
    std::cout << "Button A: " << gamepadCommand->a
              << " | B: " << gamepadCommand->b << " | X: " << gamepadCommand->x
              << " | Y: " << gamepadCommand->y << "\n";
    std::cout << "Left Stick Button: " << gamepadCommand->leftStickButton
              << " | X: " << gamepadCommand->leftStickAnalog[0]
              << " | Y: " << gamepadCommand->leftStickAnalog[1] << "\n";
    std::cout << "Right Analog Button: " << gamepadCommand->rightStickButton
              << " | X: " << gamepadCommand->rightStickAnalog[0]
              << " | Y: " << gamepadCommand->rightStickAnalog[1] << "\n";
    std::cout << "Left Bumper: " << gamepadCommand->leftBumper
              << " | Trigger Switch: " << gamepadCommand->leftTriggerButton
              << " | Trigger Value: " << gamepadCommand->leftTriggerAnalog
              << "\n";
    std::cout << "Right Bumper: " << gamepadCommand->rightBumper
              << " | Trigger Switch: " << gamepadCommand->rightTriggerButton
              << " | Trigger Value: " << gamepadCommand->rightTriggerAnalog
              << "\n\n";
    std::cout << std::endl;

    //Reset iteration counter
    printIter = 0;
  }
}

(2) Print the status of the body track in real time

/**
 * Function: print the status of Torso Track in real time
 */
template <typename T>
void DesiredStateCommand<T>::printStateCommandInfo() 
{
  //Incremental print iteration
  printIter++;

  //Print at the required frequency
  if (printIter == printNum) {
    std::cout << "[DESIRED STATE COMMAND] Printing State Command Info...\n";
    std::cout << "---------------------------------------------------------\n";
    std::cout << "Position X: " << data.stateDes(0)
              << " | Y: " << data.stateDes(1) << " | Z: " << data.stateDes(2)
              << "\n";
    std::cout << "Orientation Roll: " << data.stateDes(3)
              << " | Pitch: " << data.stateDes(4)
              << " | Yaw: " << data.stateDes(5) << "\n";
    std::cout << "Velocity X: " << data.stateDes(6)
              << " | Y: " << data.stateDes(7) << " | Z: " << data.stateDes(8)
              << "\n";
    std::cout << "Angular Velocity X: " << data.stateDes(9)
              << " | Y: " << data.stateDes(10) << " | Z: " << data.stateDes(11)
              << "\n";
    std::cout << std::endl;
    std::cout << std::endl;

    //Reset iteration counter
    printIter = 0;
  }
}

template class DesiredStateCommand<double>;
template class DesiredStateCommand<float>;

summary

xy linear speed and yaw angular speed can be given directly by the remote control handle through limiting [anti theft mark - box type hzj]

xy position, yaw angle and attitude are calculated by the feedback of the position information of the corresponding velocity integral of the handle + the state estimator