Unitree 机器狗 SDK 高级控制示例

#include 'unitree_legged_sdk/unitree_legged_sdk.h' #include <math.h> #include #include <unistd.h> #include <string.h>

using namespace UNITREE_LEGGED_SDK;

class Custom { public: Custom(uint8_t level) : safe(LeggedType::Go1), udp(level, 8899, '192.168.123.161', 8082)

{ udp.InitCmdData(cmd); } void UDPRecv(); void UDPSend(); void RobotControl();

Safety safe; UDP udp; HighCmd cmd = {0}; HighState state = {0}; int motiontime = 0; float dt = 0.002; // 0.001~0.01 int s = 0; };

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

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

void Custom::RobotControl() { motiontime += 1; s = motiontime; udp.GetRecv(state); // printf('%d %f ', motiontime, state.imu.quaternion[2]); cmd.mode = 0; // 0:idle, default stand 1:forced stand 2:walk continuously cmd.gaitType = 0; cmd.speedLevel = 0; cmd.footRaiseHeight = 0; cmd.bodyHeight = 0; cmd.euler[0] = 0; cmd.euler[1] = 0; cmd.euler[2] = 0; cmd.velocity[0] = 0.0f; cmd.velocity[1] = 0.0f; cmd.yawSpeed = 0.0f; cmd.reserve = 0; s=s%4000; if (s < 2000){ cmd.mode = 2; cmd.gaitType = 2; cmd.velocity[0] = 0.3f; cmd.velocity[1] = 0.05f; cmd.yawSpeed = 0.02f; cmd.footRaiseHeight = 0.01; std::cout << 0 << std::endl; } else{ cmd.mode = 2; cmd.gaitType = 2; cmd.velocity[0] = 0.3f; cmd.velocity[1] = 0.003f; cmd.yawSpeed = 0.015f; cmd.footRaiseHeight = 0.01; std::cout<<1<<std::endl; } std::cout << motiontime << std::endl; udp.SetSend(cmd); }

int main(void) { std::cout << 'Communication level is set to HIGH-level.' << std::endl << 'WARNING: Make sure the robot is standing on the ground.' << std::endl << 'Press Enter to continue...' << std::endl; std::cin.ignore();

Custom custom(HIGHLEVEL); 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; } 解释代码每条含义内容：1. 头文件引入：

#include 'unitree_legged_sdk/unitree_legged_sdk.h'
#include <math.h>
#include <iostream>
#include <unistd.h>
#include <string.h>

引入了Unitree机器狗SDK头文件、math库、iostream库、unistd库和string库。

定义Custom类：

class Custom
{
public:
  Custom(uint8_t level) : safe(LeggedType::Go1),
                          udp(level, 8899, '192.168.123.161', 8082)

  {
    udp.InitCmdData(cmd);
  }
  void UDPRecv();
  void UDPSend();
  void RobotControl();

  Safety safe;
  UDP udp;
  HighCmd cmd = {0};
  HighState state = {0};
  int motiontime = 0;
  float dt = 0.002; // 0.001~0.01
  int s = 0; 
};

定义了Custom类，包含了UDP通信、机器狗安全、机器狗控制、机器狗状态、运动时间、时间间隔、运动步数等成员变量。

实现Custom类中的UDPRecv()函数：

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

UDP接收函数，调用了UDP类中的Recv()函数，用于接收机器狗的状态信息。

实现Custom类中的UDPSend()函数：

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

UDP发送函数，调用了UDP类中的Send()函数，用于向机器狗发送控制指令。

实现Custom类中的RobotControl()函数：

void Custom::RobotControl()
{
  motiontime += 1;
  s = motiontime;
  udp.GetRecv(state);
  cmd.mode = 0; // 0:idle, default stand      1:forced stand     2:walk continuously
  cmd.gaitType = 0;
  cmd.speedLevel = 0;
  cmd.footRaiseHeight = 0;
  cmd.bodyHeight = 0;
  cmd.euler[0] = 0;
  cmd.euler[1] = 0;
  cmd.euler[2] = 0;
  cmd.velocity[0] = 0.0f;
  cmd.velocity[1] = 0.0f;
  cmd.yawSpeed = 0.0f;
  cmd.reserve = 0;
  s=s%4000;
  if (s < 2000){
    cmd.mode = 2;
    cmd.gaitType = 2;
    cmd.velocity[0] = 0.3f;
    cmd.velocity[1] = 0.05f;
    cmd.yawSpeed = 0.02f;
    cmd.footRaiseHeight = 0.01;
    std::cout << 0 << std::endl;
  }
  else{
    cmd.mode = 2;
    cmd.gaitType = 2;
    cmd.velocity[0] = 0.3f;
    cmd.velocity[1] = 0.003f;
    cmd.yawSpeed = 0.015f;
    cmd.footRaiseHeight = 0.01;
    std::cout<<1<<std::endl;
  }
  std::cout << motiontime << std::endl;
  udp.SetSend(cmd);
}

机器狗控制函数，包含了运动时间的累加、运动步数的计算、接收机器狗状态信息、设置机器狗的控制指令等操作。

主函数：

int main(void)
{
  std::cout << 'Communication level is set to HIGH-level.' << std::endl
            << 'WARNING: Make sure the robot is standing on the ground.' << std::endl
            << 'Press Enter to continue...' << std::endl;
  std::cin.ignore();

  Custom custom(HIGHLEVEL);
  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;
}

主函数中定义了Custom类的实例custom，创建了三个LoopFunc类的实例loop_control、loop_udpSend和loop_udpRecv，并分别调用了Custom类中的RobotControl()、UDPSend()和UDPRecv()函数。最后进入了一个死循环。