【Unitree Go1】Low Level制御の概要について

初めに
環境
１．Low Level制御の概要
２．実例：角度制御によるstand upプログラム
おわりに
関連記事

初めに

本記事では、Low Level制御における概要を解説させていただきます。
角度制御によるstand upプログラムの実例も交えた解説になります。

環境

環境は、次のようなものを想定しています。

機種: R&D(Edu), R&D+(Edu+)（Proではないもの）
unitree_legged_sdk: v3.5.1以上（本記事ではv3.8.6を使用）

※ Low Level制御を行う場合、付属のハンガーキャスターを使用し、ハンガーキャスターによって十分に安全を確認できるまでは絶対にハンガーキャスターを使用してください

１．Low Level制御の概要

Low Levelの制御方法

Low Level制御でロボットに指令を送る方法は３通りあります。
１．角度指令
角度指令を与えてロボットを制御する方法で、example_positionがこれに当たります。この場合、角度剛性Kpもセットで指定する必要があります。
２．速度指令
速度指令を与えてロボットを制御する方法で、example_velocityがこれに当たります。この場合、速度剛性Kdもセットで指定する必要があります。
３．トルク指令
モーターのトルクで制御する方法で、example_torqueがこれに当たります。セットとなる剛性はなく、一番難易度が高い制御になると思います。

物理パラメーターについて

Go1の物理パラメータについては、以下のgithubの情報が最も詳しいです：
unitree_ros/robots/go1_description/xacro/const.xacro – github

Unitree社の提供している歩行パターンについて

デフォルトの走行パターンは、バイナリデータとなっており、参考にできない形になっています。
しかし、unitree_guide – githubでは、別にlow level制御による歩行パターンの例があり、歩行パターン生成の参考になります。（ただし安定性は非常に低い走行です）

Low Levelの実行環境について

十分に安全を確認できるまでは、ハンガーキャスターに吊るした状態で行ってください。
また、L1+L2+startを押して、デフォルトの走行モードをOFFにしてから行ってください。

２．実例：角度制御によるstand upプログラム

実際に弊社で作成したプログラムは下記のようなものになります：

１．最初の姿勢　→　伏せの姿勢　（10秒かけて）
２．伏せの姿勢　→　立ちの姿勢　（10秒かけて）
という流れのプログラムになります。
※最初にやる場合は、ハンガーキャスターから吊るした状態で行ってください

今回使用したコード（クリックすると開きます）

/*****************************************************************
 Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved.
 Modified by TechShare.inc in 2023.
******************************************************************/

#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include <math.h>
#include <iostream>
#include <stdio.h>
#include <stdint.h>

using namespace std;
using namespace UNITREE_LEGGED_SDK;

const double PI=3.141592653589;

class Custom
{
public:
    Custom(uint8_t level) : safe(LeggedType::Go1),
                            udp(level, 8090, "192.168.123.10", 8007)
    {
      udp.InitCmdData(cmd);
    }
    void UDPRecv();
    void UDPSend();
    void RobotControl();

    Safety safe;
    UDP udp;
    LowCmd cmd = {0};
    LowState state = {0};
    double time_consume = 0;
    int motiontime = 0;
    int rate_count = 0;
    float dt = 0.002;     // 0.001~0.01

    double posCmd[4][3];
    const double P_STIFF = 80.0;
    double posStiffCmd[4][3] = {{P_STIFF, P_STIFF, P_STIFF}, {P_STIFF, P_STIFF, P_STIFF}, {P_STIFF, P_STIFF, P_STIFF}, {P_STIFF, P_STIFF, P_STIFF}};
    double init_pos[4][3];

    void set_position(double position[4][3])
    {
        cmd.motorCmd[FR_0].q = position[0][0];
        cmd.motorCmd[FR_1].q = position[0][1];
        cmd.motorCmd[FR_2].q = position[0][2];
        cmd.motorCmd[FL_0].q = position[1][0];
        cmd.motorCmd[FL_1].q = position[1][1];
        cmd.motorCmd[FL_2].q = position[1][2];
        cmd.motorCmd[RR_0].q = position[2][0];
        cmd.motorCmd[RR_1].q = position[2][1];
        cmd.motorCmd[RR_2].q = position[2][2];
        cmd.motorCmd[RL_0].q = position[3][0];
        cmd.motorCmd[RL_1].q = position[3][1];
        cmd.motorCmd[RL_2].q = position[3][2];
        return;
    }

    void set_posStiffness(double posS[4][3])
    {
        cmd.motorCmd[FR_0].Kp = posS[0][0];
        cmd.motorCmd[FR_1].Kp = posS[0][1];
        cmd.motorCmd[FR_2].Kp = posS[0][2];
        cmd.motorCmd[FL_0].Kp = posS[1][0];
        cmd.motorCmd[FL_1].Kp = posS[1][1];
        cmd.motorCmd[FL_2].Kp = posS[1][2];
        cmd.motorCmd[RR_0].Kp = posS[2][0];
        cmd.motorCmd[RR_1].Kp = posS[2][1];
        cmd.motorCmd[RR_2].Kp = posS[2][2];
        cmd.motorCmd[RL_0].Kp = posS[3][0];
        cmd.motorCmd[RL_1].Kp = posS[3][1];
        cmd.motorCmd[RL_2].Kp = posS[3][2];
        return;
    }

    void get_position(double (&position)[4][3])
    {
        udp.GetRecv(state);
        position[0][0] = state.motorState[FR_0].q;
        position[0][1] = state.motorState[FR_1].q;
        position[0][2] = state.motorState[FR_2].q;
        position[1][0] = state.motorState[FL_0].q;
        position[1][1] = state.motorState[FL_1].q;
        position[1][2] = state.motorState[FL_2].q;
        position[2][0] = state.motorState[RR_0].q;
        position[2][1] = state.motorState[RR_1].q;
        position[2][2] = state.motorState[RR_2].q;
        position[3][0] = state.motorState[RL_0].q;
        position[3][1] = state.motorState[RL_1].q;
        position[3][2] = state.motorState[RL_2].q;
        return;
    }

    double Fuse[4][3] ={{-0.02, 1.3, -2.8}, {-0.02, 1.3, -2.8}, {-0.02, 1.3, -2.8}, {-0.02, 1.3, -2.8}};

    double Tachi[4][3] ={{-0.27, 0.8, -1.6}, {-0.27, 0.8, -1.6}, {-0.27, 0.8, -1.6}, {-0.27, 0.8, -1.6}};
};

void Custom::UDPRecv()
{  
    udp.Recv();
}

void Custom::UDPSend()
{  
    udp.Send();
}

double jointCurveInterpolation(double initPos, double targetPos, double rate)
{
    double p;
    rate = std::min(std::max(rate, 0.0), 1.0);
    // p = initPos * (1 - rate) + targetPos * rate;
    double theta = (1 + cos(rate*PI))/2;
    p = initPos * theta + targetPos * (1 -theta);
    return p;
}

void positionCurve(double position[4][3], double initPosition[4][3], double targetPosition[4][3], double rate)
{
    for (int i = 0; i < 4; i++){
        for (int j = 0; j < 3; j++){
            position[i][j] = jointCurveInterpolation(initPosition[i][j], targetPosition[i][j], rate);
        }
    }
    return;
}

void printPosition(double Pos[4][3])
{
    for (int i = 0; i < 4; i++){
        for (int j = 0; j < 3; j++){
            printf("%f, ", Pos[i][j]);
        }
    }
    printf("\n");
    return;
}

void substitution(double (&Pos0)[4][3], double (&Pos1)[4][3])
{
    std::copy(&Pos1[0][0], &Pos1[0][0] + 4*3, &Pos0[0][0]);
    return;
}

void Custom::RobotControl() 
{
    motiontime += 2;
    udp.GetRecv(state);

    double pos[4][3];
    get_position(pos);

    if( motiontime < 500){
        get_position(init_pos);
        if(motiontime%100 == 0){
          printf("init pos: ");
          printPosition(pos);
        }
    }
    else{
        if(motiontime < 1000){
            substitution(posCmd, init_pos);
        }
        else if(motiontime < 11000){
            rate_count += 2;
            double rate = rate_count/10000.0 ;
            positionCurve(posCmd, init_pos, Fuse, rate);
        }
        else if(motiontime < 11100){
            rate_count = 0;
        }
        else if(motiontime < 21100){
            rate_count += 2;
            double rate = rate_count/10000.0 ;
            positionCurve(posCmd, Fuse, Tachi, rate);
        }
        else{
            printf("end move");
        }

        if(motiontime%100 == 0){
          printf("current pos: ");
          printPosition(pos);
          printf("command pos: ");
          printPosition(posCmd);
        }
    
        set_position(posCmd);
        set_posStiffness(posStiffCmd);
    }

    udp.SetSend(cmd);
}

int main(int argc, char const *argv[])
{
    std::cout << "Communication level is set to LOW-level." << std::endl
              << "WARNING: Make sure the robot is hung up." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();

    Custom custom(LOWLEVEL);
    // InitEnvironment();
    LoopFunc loop_control("control_loop", custom.dt,    boost::bind(&Custom::RobotControl, &custom));
    LoopFunc loop_udpSend("udp_send",     custom.dt, 3, boost::bind(&Custom::UDPSend,      &custom));
    LoopFunc loop_udpRecv("udp_recv",     custom.dt, 3, boost::bind(&Custom::UDPRecv,      &custom));

    loop_udpSend.start();
    loop_udpRecv.start();
    loop_control.start();

    while(1){
        sleep(10);
    };

    return 0; 
}

なお、角度剛性Kpとは、NM/radで表現される値であり、目標のモーター角と現在のモーター角の差分[rad]からトルク[NM]を算出するものとなります。
example_positionでは、5.0NM/radに設定されていますが、本プログラムでは80.0NM/radに設定しています。

おわりに

Low Level制御における概要を見てきました。
この記事が少しでも役に立てたのなら幸いです。

【Unitree Go1】外部PC上から無線で unitree_ros_to_real を実行する：ローレベル制御

初めに

環境