本文详细介绍了在ROSNoetic环境下安装UnitreeZ1机器人所需的依赖，包括仿真环境配置、键盘控制方法和ROS集成步骤，重点展示了如何在ROS包中集成Z1_SDK库进行高级功能操作。
目录
参考：
https://github.com/unitreerobotics/z1_ros/blob/noetic/doc/setup.md
我的环境：ros noetic，亲测有效
一、仿真环境安装
1、安装依赖环境
安装依赖：
sudo apt install -y libboost-all-dev libeigen3-dev liburdfdom-dev
sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen
sudo ln -s /usr/include/eigen3/unsupported /usr/local/include/unsupported安装pinocchio：
git clone --recursive https://github.com/stack-of-tasks/pinocchio
cd pinocchio && mkdir build && cd build
cmake .. \
-DCMAKE_BUILD_TYPE=Release \
-DCMAKE_INSTALL_PREFIX=/usr/local \
-DBUILD_PYTHON_INTERFACE=OFF \
-DBUILD_TESTING=OFF
make
sudo make install修改bashrc ：
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH
export CMAKE_PREFIX_PATH=/usr/local:$CMAKE_PREFIX_PATH
export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:$PKG_CONFIG_PATH安装pybind：
git clone https://github.com/pybind/pybind11.git
cd pybind11
mkdir build && cd build
cmake .. -DPYBIND11_TEST=OFF
make -j
sudo make install2、下载z1_sdk与z1_controller并编译
cd
git clone https://github.com/unitreerobotics/z1_controller.git
cd z1_controller
mkdir build
cd build
cmake ..
make
cd
git clone https://github.com/unitreerobotics/z1_sdk.git
cd z1_sdk
mkdir build
cd build
cmake ..
make3、 组织unitree_ws工作空间
cd
mkdir unitree_ws
cd unitree_ws
mkdir src
cd src
git clone --recursive https://github.com/unitreerobotics/unitree_ros.git
cd ..
rosdep install --from-paths src --ignore-src -yr --rosdistro noetic
catkin_make --pkg unitree_legged_msgs
catkin_make
echo "source ~/unitree_ws/devel/setup.bash">>~/.bashrc
source ~/.bashrc 如果无法运行rosdep步请参考：https://zhuanlan.zhihu.com/p/546426618
二、键盘控制/示例动作
1、打开gazebo仿真模型
roslaunch unitree_gazebo z1.launch2、键盘控制
cd
cd z1_controller/build
./sim_ctrl k可以实现键盘控制，键盘控制方法：键盘控制 · unitree-docs
比如先按键2，再长按Q或者A
3、示例动作
cd
cd z1_controller/build
./sim_ctrl另一个终端
cd
cd z1_sdk/build
./highcmd_basic可以实现机械臂的示例动作
注意：z1_controller中的文件比较底层，做开发要看z1_sdk里面的示例，有很多接口可以使用
三、ROS集成
1、需要ROS集成的场合
如果我们想在z1_sdk中订阅一个关于机械臂目标点位置的话题，用这个信息控制机械臂运动，我们就需要用到ros/ros.h，但是官方提供的z1_sdk不是ros的文件管理方式。
2、方法
（1）我们可以自己建立一个ros包（比如命名mission_sim）
（2）然后使用z1_sdk为我们提供的第三方库。请参考如何在ros中用第三方库。a）需要把libZ1_SDK_x86_64.so或者libZ1_SDK_aacrch64.so库（可以从z1_sdk/lib中拷贝，用哪个取决于系统架构）放到/usr/local/lib和mission_sim/lib下。b）修改mission_sim的cmakelist文件，需要在target_link_libraries加入libZ1_SDK_x86_64.so，参考下面的代码。c）之后就可以在我们的ros包里通过#include "unitree_arm_sdk/control/unitreeArm.h"用一些接口了，比如MOVEJ之类的。
（3）在mission_sim/include中放unitree_arm_sdk文件（可以从z1_sdk/include中拷贝），里面是一些.h文件。
（4）其中mission_sim/src中的程序可以参考z1_sdk/examples中为我们提供的几个范例程序修改（比如highcmd_basic.cpp）。
3、代码
cmakelist的编写参考以下代码：
cmake_minimum_required(VERSION 3.0.2)
project(mission_sim)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
controller_manager
genmsg
joint_state_controller
robot_state_publisher
std_msgs
gazebo_ros
visualization_msgs
tf
geometry_msgs
)
catkin_package()
include_directories(
${catkin_INCLUDE_DIRS}
/usr/local/include
)
link_directories(
${catkin_LIBRARY_DIRS}
/usr/local/lib
)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_executable(mission_vision src/mission_vision.cpp)
target_link_libraries(mission_vision
${catkin_LIBRARIES}
libZ1_SDK_x86_64.so
)
add_executable(mission_vision2 src/mission_vision2.cpp)
target_link_libraries(mission_vision2
${catkin_LIBRARIES}
libZ1_SDK_x86_64.so
)
add_executable(highcmd_basic src/highcmd_basic.cpp)
target_link_libraries(highcmd_basic
${catkin_LIBRARIES}
libZ1_SDK_x86_64.so
)
add_executable(mission_design src/mission_design.cpp)
target_link_libraries(mission_design
${catkin_LIBRARIES}
libZ1_SDK_x86_64.so
)
add_executable(try src/try.cpp)
target_link_libraries(try
${catkin_LIBRARIES}
libZ1_SDK_x86_64.so
)
为什么想到用了libZ1_SDK_x86_64.so这个库，是因为官方提供的单独的z1_controller中有一个名称叫做sim的ros包，它用到了这个库：
target_link_libraries(sim_ctrl
libZ1_${CMAKE_HOST_SYSTEM_PROCESSOR}.so
${catkin_LIBRARIES}
)四、不同架构怎么修改
可以用uname -m指令查架构类型
有两处需要修改
（1）如果是用arm架构下，自建的ROS包需要在cmakelist中依赖libZ1_SDK_aarch64.so库；如果是x86架构就是libZ1_SDK_x86.so
（2）z1_ros\unitree_ros\unitree_legged_control包，注意修改cmakelist中的依赖，是libunitree_joint_control_tool.so还是libunitree_joint_control_tool_arm64.so
五、接口函数
/*
* 功能：将z1_ctrl状态切换到fsm，等待切换完成
* 输入：ArmFSMState
* 输出：是否成功切换到fsm
* 备注：例如：只有State_Passive状态可以切换到State_LowCmd状态
*/
bool setFsm(ArmFSMState fsm);
/*
* 功能：将机械臂移到归零位置
* 等待到达归零位置后，切换到State_JointCtrl状态
* 输入：无
* 输出：无
*/
void backToStart();
/*
* 功能：将机械臂移到指定位置
* 等待到达指定位置后，切换到State_JointCtrl状态
* 输入：label
* 标签名，应存在于z1_controller/config/saveArmStates.csv中。
* 标签名字符数不能超过10个（char name[10]）
* 输出：无
*/
void labelRun(std::string label);
/*
* 功能：将当前位姿保存为标签，保存到saveArmStates.csv中
* 完成后切换到State_JointCtrl状态
* 输入：label
* 要保存的标签名，不能在z1_controller/config/saveArmStates.csv中已存在。
* 标签名字符数不能超过10个（char name[10]）
* 输出：无
*/
void labelSave(std::string label);
/*
* 功能：将当前位姿保存为标签，保存到saveArmStates.csv中
* 完成后切换到State_JointCtrl状态
* 输入：label
* 要保存的标签名，不能在z1_controller/config/saveArmStates.csv中已存在。
* 标签名字符数不能超过10个（char name[10]）
* 输出：无
*/
void teach(std::string label);
/*
* 功能：切换到State_Teach状态
* 输入：label
* 教学轨迹将保存为Traj_label.csv，保存在z1_controller/config/目录中
* 标签名字符数不能超过10个（char name[10]）
* 输出：无
*/
void teachRepeat(std::string label);
/*
* 功能：标定电机，使当前位姿成为原点位置
* 输入：无
* 输出：无
*/
void calibration();
/*
* 功能：通过关节路径移动机器人
* 输入：posture: 目标位置（roll pitch yaw x y z），单位：米
* maxSpeed: 机器人运动时的最大关节速度，单位：弧度/秒
* 范围：[0, pi]
* 输出：是否成功完成逆运动学计算
*/
bool MoveJ(Vec6 posture, double maxSpeed);
/*
* 功能：通过关节路径移动机器人，并同时控制机械臂夹爪
* 输入：posture: 目标位置（roll pitch yaw x y z），单位：米
* gripperPos: 夹爪目标角度，单位：弧度
* 范围：[-pi/2, 0]
* maxSpeed: 机器人运动时的最大关节速度，单位：弧度/秒
* 范围：[0, pi]
* 输出：是否成功完成逆运动学计算
*/
bool MoveJ(Vec6 posture, double gripperPos, double maxSpeed);
/*
* 功能：通过直线路径移动机器人
* 输入：posture: 目标位置（roll pitch yaw x y z），单位：米
* maxSpeed: 机器人运动时的最大关节速度，单位：米/秒
* 输出：是否成功完成逆运动学计算
*/
bool MoveL(Vec6 posture, double maxSpeed);
/*
* 功能：通过直线路径移动机器人，并同时控制机械臂夹爪
* 输入：posture: 目标位置（roll pitch yaw x y z），单位：米
* gripperPos: 夹爪目标角度，单位：弧度
* 范围：[-pi/2, 0]
* maxSpeed: 机器人运动时的最大关节速度，单位：米/秒
* 输出：是否成功完成逆运动学计算
*/
bool MoveL(Vec6 posture, double gripperPos, double maxSpeed);
/*
* 功能：通过圆形路径移动机器人
* 输入：middlePosture: 用于确定圆形路径形状的中间位置
* endPosture: 目标位置（roll pitch yaw x y z），单位：米
* maxSpeed: 机器人运动时的最大关节速度，单位：米/秒
* 输出：是否成功完成逆运动学计算
*/
bool MoveC(Vec6 middlePosture, Vec6 endPosture, double maxSpeed);
/*
* 功能：通过圆形路径移动机器人，并同时控制机械臂夹爪
* 输入：middlePosture: 用于确定圆形路径形状的中间位置
* endPosture: 目标位置（roll pitch yaw x y z），单位：米
* gripperPos: 夹爪目标角度，单位：弧度
* 范围：[-pi/2, 0]
* maxSpeed: 机器人运动时的最大关节速度，单位：米/秒
* 输出：是否成功完成逆运动学计算
*/
bool MoveC(Vec6 middlePosture, Vec6 endPosture, double gripperPos, double maxSpeed);
/*
* 功能：通过关节空间命令(q&qd命令)或笛卡尔空间姿态命令控制机器人
* 输入：fsm: ArmFSMState::JOINTCTRL 或 ArmFSMState::CARTESIAN
* 输出：是否成功完成逆运动学计算
* 描述：1. ArmFSMState::JOINTCTRL，
* 如果运行startTrack(ArmFSMState::JOINTCTRL)函数，
* 首先会设置以下参数：
* q : <---- lowstate->getQ()
* qd: <---- lowstate->getQd()
* gripperQ: <---- lowstate->getGripperQ()
* gripperQd: <---- lowstate->getGripperQd()
* 然后可以更改这些参数来控制机器人
* 2. ArmFSMState::CARTESIAN，
* 如果运行startTrack(ArmFSMState::CARTESIAN)函数，
* 首先会设置以下参数：
* twist：机器人末端执行器的速度
* pose：机器人当前姿态（位置和方向）
* 然后可以更改这些参数来控制机器人在笛卡尔空间中的移动
*/
bool startTrack(ArmFSMState fsm);
/*
* 功能：发送UDP消息到z1_ctrl并接收来自它的UDP消息
* 输入：无
* 输出：无
* 描述：sendRecvThread将以500Hz的频率运行sendRecv()函数
* ctrlcomp.sendRecv()在unitreeArm.sendRecv()中被调用，
* 并自动将命令参数设置到unitreeArm的lowcmd中
* 如果想在JOINTCTRL、CARTESIAN或LOWCMD模式下控制机器人，
* 而不是使用MoveJ、MoveL、MoveC等函数，
* 推荐创建自己的线程来处理命令参数
* （参考startTrack()函数的描述）
* 并在线程结束时执行sendRecv()。
*/
void sendRecv();
/*
* 功能：是否等待命令完成
* 输入：true 或 false
* 输出：无
* 描述：例如，MoveJ函数将向z1_controller发送轨迹命令，然后
* 使用usleep()等待轨迹执行完成。
* 如果将[wait]设置为false，MoveJ只会发送命令，用户需要判断
* 命令是否完成。
* 方法1：if(_ctrlComp->recvState.state != fsm)
* 轨迹完成后，FSM将自动切换到ArmFSMState::JOINTCTRL状态
* 方法2：if((lowState->endPosture - endPostureGoal).norm() < error)
* 检查当前姿态是否达到目标位置
* 相关函数：
* MoveJ(), MoveL(), MoveC(), backToStart(), labelRun(), teachRepeat()
*/
void setWait(bool Y_N);
/*
* 功能：根据输入参数自动设置q和qd命令
* 输入：directions: 移动方向 [包括夹爪]，范围：[ -1, 1 ]
* J1, J2, J3, J4, J5, J6, gripper
* jointSpeed: 关节速度，范围：[0, pi]
* 输出：无
* 描述：该函数通常用于通过键盘或操控杆控制机器人
* 当按下一个键时，directions[i] 设置为1或-1，函数将自动执行以下命令：
* qd = directions * jointSpeed
* q += qd * _ctrlComp->dt
* 如果directions == 0，机器人将停止移动
*/
void jointCtrlCmd(Vec7 directions, double jointSpeed);
/*
* 功能：根据输入参数自动设置空间速度命令
* 输入：directions: 移动方向 [包括夹爪]，范围：[ -1, 1 ]
* roll, pitch, yaw, x, y, z, gripper
* oriSpeed: 方向速度，范围：[0, 0.6]
* posSpeed: 位置速度，范围：[0, 0.3]
* 夹爪关节速度设置为1.0
* 输出：无
* 描述：该函数通常用于通过键盘或操控杆控制机器人
* 当按下一个键时，directions[i] 设置为1或-1，函数将自动执行以下命令：
* postureDelta = directions * speed
* postureGoal = postureDelta + posturePast
* Tgoal = postureToHomo(postureGoal)
* Tpast = postureToHomo(posturePast)
* omega = so3ToVec(MatrixLog3( Tpast.Rot.Transpose * Tgoal.Rot ))
* v = Tdelta.t
* twist = (omega, v)
* 如果directions == 0，机器人将停止移动
*/
void cartesianCtrlCmd(Vec7 directions, double oriSpeed, double posSpeed);
/*
* 功能：设置六个关节的命令到lowcmd类
* 输入：q: 关节角度
* qd: 关节速度
* tau: 关节力矩（仅在State_LOWCMD中使用）
* 输出：无
*/
void setArmCmd(Vec6 q, Vec6 qd, Vec6 tau = Vec6::Zero())
{
lowcmd->setQ(q);
lowcmd->setQd(qd);
lowcmd->setTau(tau);
}
/*
* 功能：设置夹爪的命令到lowcmd类
* 输入：q: 夹爪角度
* qd: 夹爪速度
* tau: 夹爪力矩（仅在State_LOWCMD中使用）
* 输出：无
*/
void setGripperCmd(double gripperPos, double gripperW, double gripperTau = 0.)
{
lowcmd->setGripperQ(gripperPos);
lowcmd->setGripperQd(gripperW);
lowcmd->setGripperTau(gripperTau);
}
扫一扫
2945
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
