从零实现宇树Z1机械臂画方块：C++ SDK实战指南

第一次看到机械臂精准地画出几何图形时，那种程序与物理世界完美对接的震撼感至今难忘。作为Unitree Z1机械臂的入门开发者，你可能已经熟悉了基础SDK调用，但如何将代码转化为具体的动作轨迹？本文将以"画方块"这个可视化任务为切入点，带你深入掌握笛卡尔空间运动控制的精髓。不同于简单的API调用演示，我们将从坐标系转换、轨迹规划到异常处理，完整呈现一个工业级机械臂程序的思考过程。

1. 环境准备与SDK基础

在开始编写画方块程序前，需要确保开发环境与机械臂硬件正确对接。Unitree Z1机械臂采用模块化设计，其SDK封装了底层运动控制逻辑，开发者只需关注任务级编程。以下是准备工作的关键步骤：

1. 网络配置
   机械臂默认IP为 192.168.123.110 ，建议将开发机配置为同网段（如 192.168.123.100 ）。验证连通性：

   ping 192.168.123.110
   

2. SDK结构解析
   官方SDK包含两个核心组件：

   • z1_controller ：实时控制内核
   • z1_sdk ：开发者接口库

3. 双终端启动模式
   需要分别运行控制器和用户程序：

   # 终端1 - 启动控制器
   cd z1_controller/build
   ./z1_ctrl
   
   # 终端2 - 运行示例程序
   cd z1_sdk/build
   ./highcmd_basic
   

提示：首次运行时若出现 [warning] 提示属正常现象，待SDK完全启动后警告会自动消失。

2. 笛卡尔空间运动原理

机械臂的"画方块"本质是末端执行器在三维空间中的直线运动。Unitree Z1采用 MoveL 函数实现笛卡尔空间下的线性插补运动，其函数原型为：

bool MoveL(const Vec6& posture, double gripper_pos, double cartesian_speed)

参数说明：

参数名	类型	描述
posture	Vec6	末端位姿(roll,pitch,yaw,x,y,z)
gripper_pos	double	夹具开合位置(0.0~1.0)
cartesian_speed	double	运动速度(m/s)

方块轨迹规划 需要计算四个顶点坐标。假设我们要在XY平面绘制边长为0.3m的正方形，起始点为(0.45, 0, 0.2)，则运动序列应为：

1. 起点A(0.45, 0, 0.2)
2. 向左移动到B(0.45, -0.15, 0.2)
3. 向上移动到C(0.45, -0.15, 0.5)
4. 向右移动到D(0.45, 0.15, 0.5)
5. 向下返回A(0.45, 0.15, 0.2)

注意：Z1机械臂的工作空间有限，建议先在仿真环境中验证坐标点是否可达。

3. 完整代码实现与解析

下面是通过有限状态机(FSM)实现画方块运动的完整代码，新建文件 draw_square.cpp 保存到 z1_sdk/example 目录：

#include "unitree_arm_sdk/control/unitreeArm.h"
using namespace UNITREE_ARM;

class SquareDrawer : public unitreeArm {
public:
    SquareDrawer() : unitreeArm(true) {}
    
    void drawSquare() {
        Vec6 postures[4];  // 存储四个顶点位姿
        double speed = 0.3; // 保守速度设置
        
        // 初始化夹具位置
        double gripperPos = 0.0;
        
        // 定义四个顶点
        postures[0] << 0, 0, 0, 0.45, 0.00, 0.2;  // 起点A
        postures[1] << 0, 0, 0, 0.45, -0.15, 0.2; // 左移B
        postures[2] << 0, 0, 0, 0.45, -0.15, 0.5; // 上移C 
        postures[3] << 0, 0, 0, 0.45, 0.15, 0.5;  // 右移D
        
        // 执行方块绘制
        MoveL(postures[0], gripperPos, speed);  // 移动到起点
        for(int i=1; i<4; i++) {
            MoveL(postures[i], gripperPos, speed);
        }
        MoveL(postures[0], gripperPos, speed);  // 闭合方块
    }
};

int main() {
    SquareDrawer arm;
    arm.sendRecvThread->start();  // 启动通信线程
    
    arm.backToStart();    // 回归初始位置
    arm.drawSquare();     // 执行绘制
    arm.backToStart();    // 返回初始位置
    
    arm.setFsm(ArmFSMState::PASSIVE);
    arm.sendRecvThread->shutdown();
    return 0;
}

关键实现细节：

1. 位姿容器 ：使用 Vec6 类型存储6自由度位姿参数
2. 运动序列 ：通过 MoveL 依次连接各顶点形成闭合路径
3. 速度控制 ：保守的0.3m/s速度确保运动平稳
4. 状态管理 ：严格遵循启动-执行-收尾的线程生命周期

4. 编译运行与调试技巧

将新程序加入编译系统需要修改 CMakeLists.txt ：

add_executable(draw_square 
    example/draw_square.cpp
)
target_link_libraries(draw_square
    unitree_arm_sdk
)

常见问题及解决方案：

Q1: 出现 [ERROR] MoveL posture has no inverse kinematics

• 检查坐标点是否超出工作空间
• 确保每次运动只改变一个轴向的坐标值
• 分段验证各点可达性

Q2: 机械臂运动不流畅

• 降低 cartesian_speed 参数值
• 在关键点增加延时：

  #include <chrono>
  #include <thread>
  
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  

Q3: 末端执行器抖动

• 检查机械臂各关节是否紧固
• 确认网络延迟在合理范围内
• 尝试降低控制频率

5. 进阶优化方向

基础功能实现后，可以考虑以下增强方案：

1. 轨迹平滑处理
   在顶点间插入过渡点实现圆弧转角：

   // 在B点前插入过渡点
   Vec6 transition;
   transition << 0,0,0, 0.45,-0.1,0.25;
   MoveL(transition, gripperPos, speed);
   

2. 动态参数调整
   通过命令行参数实时修改方块尺寸：

   if(argc > 1) {
       double size = atof(argv[1]);
       // 应用size参数到坐标计算
   }
   

3. 可视化反馈
   集成ROS的RViz工具实时显示目标轨迹与实际轨迹对比。

4. 异常恢复机制
   添加运动学求解失败时的回退策略：

   try {
       MoveL(posture, gripperPos, speed);
   } catch (const std::runtime_error& e) {
       std::cerr << "Movement failed: " << e.what() << std::endl;
       backToStart();
   }
   

机械臂编程最迷人的地方在于，你能亲眼看到算法转化为物理世界的精确运动。当第一次成功画出完美方块时，建议用手机慢动作记录下这个瞬间——那些精准的直线运动背后，是无数行代码与数学计算的结晶。