Unitree Go1 机器人高层控制示例

#include 'unitree_legged_sdk/unitree_legged_sdk.h'
#include <math.h>
#include <iostream>
#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;
}

编译步骤

在编译时需要添加 unitree_legged_sdk 库的路径和链接选项。具体步骤如下：

1. 找到 unitree_legged_sdk 库的路径。

如果是在 ROS 中使用，可以使用 rospack 命令来查找路径：

rospack find unitree_legged_sdk

如果是在独立的 C++ 项目中使用，可以在 unitree_legged_sdk 的安装目录中找到库的路径。

2. 在 CMakeLists.txt 文件中添加库的路径和链接选项。

# 添加头文件搜索路径
include_directories(<unitree_legged_sdk路径>)

# 添加库搜索路径
link_directories(<unitree_legged_sdk路径>)

# 添加链接选项
target_link_libraries(<目标名> unitree_legged_sdk)

其中，<unitree_legged_sdk路径> 是上一步中找到的库的路径，<目标名> 是你的 C++ 程序的目标名。

代码说明

该示例程序主要包含以下功能：

• 创建一个 Custom 类，该类包含了与 Unitree Go1 机器人交互所需的各种成员变量和函数。
• 在 main 函数中创建 Custom 类的实例，并启动三个循环：
  • control_loop：负责控制机器人运动。
  • udp_send：负责将控制指令发送给机器人。
  • udp_recv：负责接收机器人状态信息。
• 在 RobotControl 函数中，根据时间设定机器人的运动指令，包括模式、步态、速度、转向等。

该示例程序只是一个简单的演示，可以根据需要进行修改和扩展。

其他

• 该示例程序假设机器人已经连接到网络，并且网络配置正确。
• 该示例程序使用 UDP 协议进行通信，需要在机器人和电脑之间建立 UDP 连接。
• 该示例程序使用 boost 库，需要在编译时链接 boost 库。

相关资源

• Unitree Go1 官方网站：https://www.unitree.com/
• Unitree Go1 SDK 文档：https://github.com/UNITREE-Robotics/UNITREE_LEGGED_SDK