对Unitree宇树Z1机械臂进行SDK修改和调用示例

本文主要是介绍对Unitree宇树Z1机械臂进行SDK修改和调用示例，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

参考Unitree官方文档键盘控制 · unitree-docs

修改机械臂IP进行连接

ping机械臂的默认IP（192.168.123.110）

网络不可达，此时需要修改机械臂IP使其与PC在同一局域网内，PC端才可与机械臂正常通信

在这里修改为192.168.123.xxx网段

调用ifconfig查看机械臂修改后的IP，在这里是33端口

调用官方提供的SDK（z1_controller和z1_sdk）

在第一个Terminal中调用编译z1_controller，在该文件夹下创建build文件夹，执行./z1_ctrl。

因为此时还没有启动机械臂SDK与机械臂控制器通信，所以会出现[warning]

在第二个Terminal中打开z1_sdk文件夹，创建build文件并编译，执行示例代码/highcmd_basic

此时机械臂启动，并执行highcmd_basic.cpp文件的内容

调用官方库进行自主编写直线运动Linear代码（C++）

根据Cartesian坐标系，调整机械爪的姿态和位置 (roll pitch yaw x y z)

需要先确定好机械臂的运动范围，若超出则会出现以下ERROR

[ERROR] MoveL posture: (roll pitch yaw x y z) has no inverse kinematics.

直线运动Linear在官方库中对应"unitree_arm_sdk/control/unitreeArm.h"内被封装完毕的MoveL(posture, gripper_pos, cartesian_speed)函数。posture为位姿数组，gripper_pos为夹具位置初始化，cartesian_speed笛卡尔坐标系下各个轴的运行速度。

在这里实现机械臂画一个”口“字的.cpp文件，该文件放在z1_sdk的example下。

//#include <ncurses.h>//Linux

#include "unitree_arm_sdk/control/unitreeArm.h"

using namespace UNITREE_ARM;

class Z1ARM : public unitreeArm{

public:

Z1ARM():unitreeArm(true){};

~Z1ARM(){};

void armCtrlByFSM(); // 通过有限状态机(FSM)控制机械臂

void printState(); // 打印机械臂的状态。

double gripper_pos = 0.0; // 夹具的位置。

double joint_speed = 2.0; // 关节的速度。

double cartesian_speed = 0.5; // 笛卡尔坐标系下的速度。

};

//Finite State Machine

void Z1ARM::armCtrlByFSM() {

Vec6 posture[2]; // 姿势数组。

gripper_pos = 0.0; // 夹具位置初始化。

std::cout << "[TO STATE]" << std::endl;

labelRun("forward");//初始位置（0,0,0,0.45,0,0.2）

//直线运动Linear

//输入末端位姿(roll pitch yaw x y z)

    //向左

std::cout << "[MOVEL]" << std::endl;

posture[0] << 0,0,0,0.45,-0.15,0.2;

cartesian_speed = 0.5;

MoveL(posture[0], gripper_pos, cartesian_speed);

printState();



//向上

posture[0] << 0,0,0,0.45,-0.15,0.4;

cartesian_speed = 0.5;

MoveL(posture[0], gripper_pos, cartesian_speed);

printState();

    //向右

posture[0] << 0,0,0,0.45,0.15,0.4;

cartesian_speed = 0.5;

MoveL(posture[0], gripper_pos, cartesian_speed);

printState();

    //向下

posture[0] << 0,0,0,0.45,0.15,0.2;

cartesian_speed = 0.5;

MoveL(posture[0], gripper_pos, cartesian_speed);

printState();

}

void Z1ARM::printState(){

std::cout<<"------ joint State ------"<<std::endl;

std::cout<<"qState: "<<lowstate->getQ().transpose()<<std::endl;

std::cout<<"qdState: "<<lowstate->getQd().transpose()<<std::endl;

std::cout<<"tauState: "<<lowstate->getTau().transpose()<<std::endl;

std::cout<<"------ Endeffector Cartesian Posture ------"<<std::endl;

std::cout<<"roll pitch yaw x y z"<<std::endl;

std::cout<<lowstate->endPosture.transpose()<<std::endl;

}

int main() {

// initscr(); // 初始化 ncurses

// keypad(stdscr, TRUE); // 启用功能键

// noecho(); // 不回显按键

// curs_set(0); // 隐藏光标

std::cout << std::fixed << std::setprecision(3);

// 创建机械臂对象并启动控制线程

Z1ARM arm;

arm.sendRecvThread->start();

//所有电机返回初始位置

arm.backToStart();

arm.armCtrlByFSM();//运行

// 机械臂回到初始位置，设置为被动模式，关闭控制线程

arm.backToStart();

arm.setFsm(ArmFSMState::PASSIVE);

arm.sendRecvThread->shutdown();

//endwin(); // 结束 ncurses

return 0;

}