Unitree Z1机械臂编程实战：用C++ SDK实现一个画“口”字的完整项目

机械臂编程一直是机器人开发中的核心技能之一。对于已经掌握Unitree Z1机械臂SDK基础操作的开发者来说，如何将理论知识转化为实际项目能力是关键。本文将以"让机械臂末端画一个正方形（口字）"为目标，带你完整走一遍从需求分析、代码编写到调试运行的全流程。

1. 项目规划与准备工作

在开始编码之前，我们需要明确几个关键点：

• 运动轨迹规划 ：正方形由四条直线组成，需要确定四个顶点的坐标
• 机械臂工作空间限制 ：确保所有目标点都在机械臂可达范围内
• 姿态控制 ：保持末端执行器姿态一致，避免旋转影响绘图效果

首先创建一个新的C++项目文件 draw_square.cpp ，放在 z1_sdk/examples 目录下。基础代码结构如下：

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

class Z1DrawSquare : public unitreeArm {
public:
    Z1DrawSquare():unitreeArm(true){};
    ~Z1DrawSquare(){};
    
    void drawSquare();
    void printState();
    
    double gripper_pos = 0.0;
    double cartesian_speed = 0.3;
};

2. 笛卡尔空间路径规划

画正方形需要确定四个顶点的坐标。我们选择在XY平面绘制，Z轴保持固定高度。假设正方形边长为0.3米，中心点坐标为(0.45, 0, 0.3)。

四个顶点坐标计算如下：

顶点	X坐标	Y坐标	Z坐标
左下	0.45	-0.15	0.3
左上	0.45	-0.15	0.6
右上	0.45	0.15	0.6
右下	0.45	0.15	0.3

在代码中实现路径规划：

void Z1DrawSquare::drawSquare() {
    Vec6 posture[4]; // 存储四个顶点位姿
    
    // 初始化四个顶点
    posture[0] << 0,0,0,0.45,-0.15,0.3; // 左下
    posture[1] << 0,0,0,0.45,-0.15,0.6; // 左上
    posture[2] << 0,0,0,0.45,0.15,0.6;  // 右上
    posture[3] << 0,0,0,0.45,0.15,0.3;  // 右下
    
    // 移动到起点
    MoveL(posture[0], gripper_pos, cartesian_speed);
    printState();
    
    // 绘制四条边
    for(int i=1; i<=4; i++) {
        MoveL(posture[i%4], gripper_pos, cartesian_speed);
        printState();
        sleep(1); // 每个顶点暂停1秒
    }
}

3. 处理运动限制与错误

在实际运行中，可能会遇到两种常见错误：

1. 逆运动学无解错误 ： [ERROR] MoveL posture has no inverse kinematics
2. 关节限位错误 ：机械臂无法到达指定位置

解决方法：

• 逐步验证每个目标点 ：单独测试每个顶点是否可达
• 调整姿态角 ：保持roll/pitch/yaw为0简化运动学计算
• 缩小运动范围 ：如果出现错误，按比例缩小正方形尺寸

调试技巧：

// 调试单个顶点
void testSinglePoint(const Vec6& posture) {
    try {
        MoveL(posture, gripper_pos, cartesian_speed);
        std::cout << "Point reachable: " << posture.transpose() << std::endl;
    } catch (...) {
        std::cout << "ERROR: Cannot reach point " << posture.transpose() << std::endl;
    }
}

4. 完整项目集成与优化

将上述代码整合到完整项目中，并添加以下优化：

• 运动平滑处理 ：在顶点处添加短暂停顿
• 状态监控 ：实时打印机械臂状态
• 错误恢复 ：遇到错误自动回退到安全位置

完整main函数实现：

int main() {
    std::cout << std::fixed << std::setprecision(3);
    
    Z1DrawSquare arm;
    arm.sendRecvThread->start();
    
    // 初始化机械臂位置
    arm.backToStart();
    
    // 绘制正方形
    arm.drawSquare();
    
    // 返回初始位置并关闭
    arm.backToStart();
    arm.setFsm(ArmFSMState::PASSIVE);
    arm.sendRecvThread->shutdown();
    
    return 0;
}

最后，在 CMakeLists.txt 中添加新示例：

add_executable(draw_square examples/draw_square.cpp)
target_link_libraries(draw_square ${LIBRARY_NAME})

5. 高级技巧与扩展

掌握了基础的正方形绘制后，可以尝试以下扩展：

• 参数化绘制 ：将正方形尺寸、位置设为可调参数
• 速度曲线优化 ：实现加减速控制使运动更平滑
• 轨迹插值 ：在两点之间插入中间点提高精度
• 异常处理 ：添加更完善的错误检测和恢复机制

参数化绘制的改进示例：

void drawParametricSquare(double centerX, double centerY, double centerZ, double size) {
    double halfSize = size/2;
    Vec6 posture[4];
    
    posture[0] << 0,0,0, centerX-halfSize, centerY-halfSize, centerZ;
    posture[1] << 0,0,0, centerX-halfSize, centerY-halfSize, centerZ+size;
    posture[2] << 0,0,0, centerX+halfSize, centerY+halfSize, centerZ+size;
    posture[3] << 0,0,0, centerX+halfSize, centerY+halfSize, centerZ;
    
    // ... 其余绘制代码相同
}

实际项目中，建议先在小范围内测试运动轨迹，确认无误后再扩大运动范围。同时，记得定期保存机械臂状态数据，便于后续分析和优化。