前提
场景说明
- 每个场景示例写了测试机型，若机型不同，请参考《xCore控制系统手册》，确认参数合适后再运行示例。
- 此文档描述的是C++SDK的场景使用示例
机器人的两种控制模式
实时模式
实时模式概念
概念
实时模式控制接口允许用户通过外部设备与机器人进行快速直接的底层双向连接。它能提供机器人的当前状态，并通过以太网连接的外部工作站PC实现其直接控制。使用控制接口以1 kHz的频率发送实时控制量
用户侧算法：目标位置 = (X+Δx, Y+Δy)每个毫秒用户程序执行该算法机器人不做插补
为什么会有实时模式
- 实际应用角度客户对机器人末端轨迹、本体在空间中的位置有精确度要求，比如在复杂环境中避障。常规的MoveL/J等指令不能完全按照期望轨迹运行
- 机器人目标位置不能提前预知，
- 需要实时规划科研角度高校课题，算法验证
实时模式-笛卡尔阻抗控制
针对AR5机型的7轴机器人，如需改动，请修改 rokae::xMateErProRobot 改成相对应的机型即可
此场景需要实机测试，虚拟测试不了阻抗控制
#include <iostream>
#include <cmath>
#include <thread>
#include <atomic>
#include "rokae/robot.h"
#include "rokae/utility.h"
using namespace rokae;
/**
* @brief 主程序 - 机器人笛卡尔阻抗控制示例
* 该程序演示了如何使用rokae机器人SDK实现笛卡尔空间阻抗控制，
* 使机器人在保持一定接触力的情况下沿特定轨迹运动
*/
int main() {
using namespace std;
try {
// ==================== 机器人初始化配置 ====================
std::string ip = "192.168.21.10";
std::error_code ec;
// 创建xMate 7轴机器人实例
// 参数1: 机器人IP地址，参数2: 本地IP
rokae::xMateErProRobot robot(ip, "192.168.21.150");
// 设置实时网络容差为50ms，确保实时控制的稳定性
robot.setRtNetworkTolerance(50, ec);
// 设置机器人操作模式为自动模式
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// 设置运动控制模式为实时命令模式，允许实时发送控制指令
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
// 上电使能机器人
robot.setPowerState(true, ec);
// 获取实时运动控制器
auto rtCon = robot.getRtMotionController().lock();
// ==================== 初始位置运动 ====================
// 定义目标关节角度 (弧度)，这个需要根据当前机型的设置关节角度
std::array<double,7> q_drag_xm7p = {0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0};
// 关节空间运动到初始位置，速度比例0.5
rtCon->MoveJ(0.5, robot.jointPos(ec), q_drag_xm7p);
// ==================== 力控参数设置 ====================
// 设置力控坐标系为工具坐标系
// 工具坐标系相对于法兰坐标系的变换矩阵
// 这个矩阵表示：Z轴指向法兰外侧，Y轴指向法兰上方
std::array<double, 16> toolToFlange = {
0, 0, 1, 0, // X轴在基坐标系中的方向
0, 1, 0, 0, // Y轴在基坐标系中的方向
-1, 0, 0, 0, // Z轴在基坐标系中的方向
0, 0, 0, 1 // 位置坐标 (无偏移)
};
rtCon->setFcCoor(toolToFlange, FrameType::tool, ec);
// 设置笛卡尔阻抗系数
// 参数顺序: {X方向刚度, Y方向刚度, Z方向刚度, Rx方向刚度, Ry方向刚度, Rz方向刚度}
// 单位: 刚度(N/m), 旋转刚度(Nm/rad)
// 这里X和Y方向设置较高刚度(1200 N/m)，Z方向设置为0以便力控
rtCon->setCartesianImpedance({1200, 1200, 0, 100, 100, 0}, ec);
// 设置期望的接触力/力矩
// 参数顺序: {Fx, Fy, Fz, Mx, My, Mz}
// 这里设置X方向3N力，Z方向3N力，其他方向为0
rtCon->setCartesianImpedanceDesiredTorque({3, 0, 3, 0, 0, 0}, ec);
// ==================== 获取初始位姿 ====================
// 获取机器人末端在基坐标系中的当前位姿
std::array<double, 16> init_position {};
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), init_position);
// ==================== 开始阻抗控制 ====================
// 启动笛卡尔阻抗控制模式
rtCon->startMove(RtControllerMode::cartesianImpedance);
// ==================== 实时控制循环设置 ====================
double time = 0; // 时间计数器
std::atomic<bool> stopManually {true}; // 线程安全的停止标志
/**
* @brief 实时控制回调函数
* 这个函数每1ms被调用一次，生成机器人的目标轨迹
*/
std::function<CartesianPosition(void)> callback = [&, rtCon]()->CartesianPosition{
time += 0.001; // 每次调用增加1ms
// 轨迹参数
constexpr double kRadius = 0.2; // 运动半径
double angle = M_PI / 4 * (1 - std::cos(M_PI / 2 * time)); // 平滑的角度变化
double delta_z = kRadius * (std::cos(angle) - 1); // Z轴位置变化量
// 创建输出位置指令
CartesianPosition output{};
output.pos = init_position; // 保持初始位置和姿态
output.pos[7] += delta_z; // 仅修改Z轴位置 (变换矩阵中的第8个元素)
// 运动结束条件：40秒后停止
if(time > 40){
std::cout << "运动结束" << std::endl;
output.setFinished(); // 标记运动完成
stopManually.store(false); // 通知主线程可以停止
}
return output;
};
// ==================== 启动控制循环 ====================
// 设置控制回调函数
rtCon->setControlLoop(callback);
// 启动实时控制循环，false表示非阻塞模式
rtCon->startLoop(false);
// ==================== 等待运动完成 ====================
// 主线程等待，直到控制循环设置停止标志
while(stopManually.load());
// 停止控制循环
rtCon->stopLoop();
// 等待2秒确保完全停止
std::this_thread::sleep_for(std::chrono::seconds(2));
} catch (const std::exception &e) {
// 异常处理
std::cerr << "程序异常: " << e.what() << std::endl;
}
return 0;
}
实时模式-笛卡尔空间S规划
程序适用机型：xMateER7 Pro，AR5机型，需要实机测试
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "../print_helper.hpp"
using namespace rokae;
/**
* @brief 机器人笛卡尔空间直线运动示例程序
* 该程序演示了如何使用rokae SDK控制机器人进行笛卡尔空间的直线插补运动
* 主要功能：从当前位置运动到拖拽位姿，然后沿直线移动TCP工具
*/
int main() {
using namespace std;
// 创建机器人对象 - xMateErProRobot是珞石机器人的型号
rokae::xMateErProRobot robot;
try {
// 连接机器人
// 参数1: 机器人IP地址 (192.168.0.160)
// 参数2: 本机IP地址 (192.168.0.100)
// 注意：两个设备需要在同一网段，且网络连接正常
robot.connectToRobot("192.168.0.160", "192.168.0.100");
} catch(const rokae::Exception &e) {
std::cerr << "连接失败 " << e.what();
return 0; // 连接失败，退出程序
}
std::error_code ec; // 用于接收错误码
// 设置机器人操作模式为自动模式
// 自动模式：机器人可以执行程序，接受外部控制命令
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// 设置运动控制模式为实时命令模式
// RtCommand模式：允许通过实时回调函数控制机器人运动
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
// 开启机器人电源（上伺服）
// 上电后机器人可以运动，下电后机器人处于抱闸状态
robot.setPowerState(true, ec);
try {
// 获取实时运动控制器
// lock()将weak_ptr转换为shared_ptr，确保控制器对象存在
auto rtCon = robot.getRtMotionController().lock();
// 开始接收机器人状态数据
// 参数1: 更新周期(1ms) - 每1毫秒更新一次机器人状态
// 参数2: 需要接收的数据字段 - 关节位置和TCP位姿
robot.startReceiveRobotState(std::chrono::milliseconds(1),
{RtSupportedFields::jointPos_m, RtSupportedFields::tcpPose_m});
// 定义变量用于存储位置数据
std::array<double, 16> init_pos{}, end_pos{}; // 初始和结束位姿(4x4齐次变换矩阵)
std::array<double,7> jntPos{}; // 7个关节的角度位置(弧度)
Eigen::Quaterniond rot_cur; // 当前姿态的四元数表示
Eigen::Matrix3d mat_cur; // 当前姿态的旋转矩阵
double delta_s; // 路径插补参数，范围[0,1]
// 更新机器人状态数据（获取当前状态）
robot.updateRobotState(std::chrono::milliseconds(1));
// 获取当前关节位置
robot.getStateData(RtSupportedFields::jointPos_m, jntPos);
// 定义目标拖拽位姿（7个关节的角度）
// 这是一个预设的关节空间位姿，机器人将先运动到这个位置
std::array<double,7> q_drag_xm7p = {0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0};
// 执行关节空间运动：从当前位置运动到拖拽位姿
// 参数1: 速度比例 0.5
// 参数2: 起始关节位置
// 参数3: 目标关节位置
rtCon->MoveJ(0.5, jntPos, q_drag_xm7p);
// 初始化标志位和时间变量
static bool init = true; // 标记是否第一次进入回调函数
double time = 0; // 运动时间累计
/**
* @brief 实时控制回调函数
* 这个函数会被实时循环调用，用于生成笛卡尔空间的运动轨迹
* 返回值: CartesianPosition 包含目标位姿的控制命令
*/
std::function<CartesianPosition()> callback = [&, rtCon]() {
time += 0.001; // 时间累计，按1ms周期递增
// 初始化阶段：获取起始和结束位姿
if(init) {
// 读取当前TCP工具位姿（4x4齐次变换矩阵）
robot.getStateData(RtSupportedFields::tcpPose_m, init_pos);
// 设置目标位姿：在Z轴方向向下移动0.2米
end_pos = init_pos;
end_pos[11] -= 0.2; // 齐次矩阵中第11个元素是Z轴位置
init = false; // 初始化完成
}
std::array<double, 16> pose_start = init_pos;
// 从齐次变换矩阵中提取位置向量
// 齐次矩阵格式: [R11, R12, R13, Tx, R21, R22, R23, Ty, R31, R32, R33, Tz, 0, 0, 0, 1]
Eigen::Vector3d pos_1(pose_start[3], pose_start[7], pose_start[11]); // 起始位置 (Tx, Ty, Tz)
Eigen::Vector3d pos_2(end_pos[3], end_pos[7], end_pos[11]); // 结束位置
// 计算位移向量和总距离
Eigen::Vector3d pos_delta = pos_2 - pos_1; // 位移向量
double s = pos_delta.norm(); // 总距离
Eigen::Vector3d pos_delta_vector = pos_delta.normalized(); // 单位方向向量
// 创建笛卡尔运动规划器
// 参数1: 最大速度(0.05 m/s)
// 参数2: 总距离
CartMotionGenerator cart_s(0.05, s);
cart_s.calculateSynchronizedValues(0); // 计算同步运动参数
// 从齐次矩阵中提取旋转矩阵
Eigen::Matrix3d mat_start, mat_end;
mat_start << pose_start[0], pose_start[1], pose_start[2], // 第一行: R11, R12, R13
pose_start[4], pose_start[5], pose_start[6], // 第二行: R21, R22, R23
pose_start[8], pose_start[9], pose_start[10]; // 第三行: R31, R32, R33
mat_end << end_pos[0], end_pos[1], end_pos[2],
end_pos[4], end_pos[5], end_pos[6],
end_pos[8], end_pos[9], end_pos[10];
// 将旋转矩阵转换为四元数（便于插值计算）
Eigen::Quaterniond rot_start(mat_start); // 起始姿态四元数
Eigen::Quaterniond rot_end(mat_end); // 结束姿态四元数
Eigen::Vector3d pos_cur; // 当前目标位置
CartesianPosition cmd; // 控制命令
// 计算期望的运动轨迹
// calculateDesiredValues返回false表示运动未完成，继续计算轨迹
if (!cart_s.calculateDesiredValues(time, &delta_s)) {
// 线性插值计算当前位置
// delta_s/s 是归一化的进度参数 [0,1]
pos_cur = pos_1 + pos_delta * delta_s / s;
// 球面线性插值(SLERP)计算当前姿态
// slerp提供平滑的姿态过渡
Eigen::Quaterniond rot_cur = rot_start.slerp(delta_s / s, rot_end);
mat_cur = rot_cur.normalized().toRotationMatrix(); // 转换回旋转矩阵
// 构建新的齐次变换矩阵
std::array<double, 16> new_pose = {
{mat_cur(0, 0), mat_cur(0, 1), mat_cur(0, 2), pos_cur(0), // 第一行: R11,R12,R13,Tx
mat_cur(1, 0), mat_cur(1, 1), mat_cur(1, 2), pos_cur(1), // 第二行: R21,R22,R23,Ty
mat_cur(2, 0), mat_cur(2, 1), mat_cur(2, 2), pos_cur(2), // 第三行: R31,R32,R33,Tz
0, 0, 0, 1}}; // 第四行: 0,0,0,1
cmd.pos = new_pose; // 设置控制命令的目标位姿
} else {
// 运动完成，设置完成标志
cmd.setFinished();
}
return cmd; // 返回控制命令
};
// 设置实时控制循环
// 参数1: 控制回调函数
// 参数2: 预留参数，通常设为0
// 参数3: useStateDataInLoop=true 表示在循环中使用机器人状态数据
rtCon->setControlLoop(callback, 0, true);
// 设置控制模式为笛卡尔空间位置控制
// 在此模式下，机器人根据笛卡尔空间的位置命令进行运动
rtCon->startMove(RtControllerMode::cartesianPosition);
// 开始实时控制循环
// 参数: true 表示阻塞执行，直到运动完成
rtCon->startLoop(true);
print(std::cout, "控制结束"); // 运动完成，输出提示信息
} catch (const std::exception &e) {
// 异常处理：捕获并打印异常信息
print(std::cerr, e.what());
}
return 0;
}
实时模式-点位跟随功能
此功能需要使用xMateModel模型库，请设置编译选项XCORE_USE_XMATE_MODEL=ON
#include <thread>
#include <atomic>
#include "rokae/robot.h"
#include "../print_helper.hpp"
// 全局变量定义
std::atomic_bool running = true; ///< 程序运行状态标志，使用原子操作保证线程安全
std::ostream &os = std::cout; ///< 输出流引用，用于打印到控制台
/// 不同型号机器人的拖拽位姿定义（关节空间位置，单位：弧度）
std::array<double, 7u> q_drag_xm7p = { 0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0 }; ///< xMateER Pro 7轴机器人拖拽位姿
std::array<double, 6u> q_drag_er3 = { 0, M_PI/6, M_PI/3, 0, M_PI/2, 0 }; ///< xMateER 6轴机器人拖拽位姿
std::array<double, 6u> q_drag_cr7 = { 0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0 }; ///< CR系列机器人拖拽位姿
std::array<double, 6u> q_drag_sr3 = { 0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0 }; ///< SR系列机器人拖拽位姿
// 前向声明：不同型号机器人的路径点函数
std::vector<std::array<double, 6>> points_xMateSR3();
std::vector<std::array<double, 6>> points_xMateER3();
std::vector<std::array<double, 6>> points_xMateCR();
std::vector<std::array<double, 7>> points_xMateERPro();
/**
* @brief 6轴机器人跟随位置控制示例
* @param robot 机器人对象引用
* @param start_jnt 起始关节位置
* @param points_list 要跟随的关节位置点列表
*
* 功能：控制6轴机器人按照预定义的关节位置序列进行跟随运动
* 运动模式：关节空间位置跟随
*/
void example_followPosition6(rokae::xMateRobot& robot,
const std::array<double, 6>& start_jnt,
const std::vector<std::array<double, 6>>& points_list) {
// 获取实时运动控制器
auto rtCon = robot.getRtMotionController().lock();
std::thread updater; // 更新线程，用于周期性发送位置命令
error_code ec; // 错误码对象
try {
// 第一步：将机器人移动到起始位置
// MoveJ: 关节空间点到点运动，最大速度30%，从当前位置到start_jnt
rtCon->MoveJ(0.3, robot.jointPos(ec), start_jnt);
std::this_thread::sleep_for(std::chrono::seconds(1)); // 等待1秒确保运动完成
// 第二步：初始化跟随位置控制器
auto model = robot.model(); // 获取机器人模型
rokae::FollowPosition follow_pose(robot, model); // 创建跟随位置控制对象
print(os, "开始跟随");
// 第三步：设置期望的末端工具坐标系变换
// bMe_desire: base到end的期望变换矩阵
Eigen::Transform<double, 3, Eigen::Isometry> bMe_desire = Eigen::Transform<double, 3, Eigen::Isometry>::Identity(); // 设置旋转：四元数(0.0, 0.0, 1.0, 0.0)对应欧拉角A-180°, B-0°, C-180°
// 四元数格式: (x, y, z, w)，这里表示绕Z轴旋转180度
bMe_desire.rotate(Eigen::Quaterniond(0.0, 0.0, 1.0, 0.0));
// 设置平移：X=0.464m, Y=0.136m, Z=0.364m
bMe_desire.pretranslate(Eigen::Vector3d(0.464, 0.136, 0.364));
// 启动跟随控制
follow_pose.start(bMe_desire);
// 第四步：创建更新线程，周期性发送位置命令
updater = std::thread([&]() {
std::this_thread::sleep_for(std::chrono::seconds(2)); // 等待2秒后开始
follow_pose.setScale(2); // 设置运动缩放因子为2（可能是速度或加速度缩放）
auto it = points_list.begin(); // 迭代器指向路径点列表开始
while (running) { // 主循环，直到running标志为false
// 正向运动：从第一个点运动到最后一个点
while (running) {
follow_pose.update(*it++); // 发送当前点位置，迭代器后移
std::this_thread::sleep_for(std::chrono::milliseconds(600)); // 每600ms更新一次
if (it == points_list.end()) { // 如果到达终点
it--; // 回退到最后一个点
break;
}
}
// 反向运动：从最后一个点运动回第一个点
while (running) {
follow_pose.update(*it--); // 发送当前点位置，迭代器前移
std::this_thread::sleep_for(std::chrono::milliseconds(600));
if (it == points_list.begin()) { // 如果回到起点
break;
}
}
}
});
// 第五步：创建控制台输入线程，监听用户输入
std::thread consoleInput([] {
while (getchar() != 'q'); // 等待用户输入'q'键
running = false; // 设置运行标志为false，停止所有运动
});
consoleInput.detach(); // 分离线程，使其独立运行
// 第六步：主线程等待，直到running变为false
while (running);
// 第七步：清理资源
follow_pose.stop(); // 停止跟随控制
updater.join(); // 等待更新线程结束
}
catch (const std::exception& e) {
// 异常处理
print(std::cerr, e.what());
if (updater.joinable()) {
running = false;
updater.join();
}
}
}
/**
* @brief 7轴机器人跟随位置控制示例
* @param robot 7轴机器人对象引用
* @param start_jnt 起始关节位置（7个关节）
* @param points_list 要跟随的关节位置点列表（7个关节）
*
* 功能：与6轴版本类似，但针对7轴机器人设计
*/
void example_followPosition7(rokae::xMateErProRobot &robot,
const std::array<double, 7> &start_jnt,
const std::vector<std::array<double, 7>> &points_list) {
auto rtCon = robot.getRtMotionController().lock(); // 获取运动控制器
std::thread updater; // 更新线程
error_code ec; // 错误码
try {
// 运动到起始位置
rtCon->MoveJ(0.3, robot.jointPos(ec), start_jnt);
std::this_thread::sleep_for(std::chrono::seconds(1));
// 初始化跟随控制器
auto model = robot.model();
rokae::FollowPosition follow_pose(robot, model);
print(os, "开始跟随");
// 设置期望的末端位姿
Eigen::Transform<double, 3, Eigen::Isometry> bMe_desire = Eigen::Transform<double, 3, Eigen::Isometry>::Identity();
bMe_desire.rotate(Eigen::Quaterniond(0.0, 0.0, 1.0, 0.0)); // 旋转
bMe_desire.pretranslate(Eigen::Vector3d(0.464, 0.136, 0.364)); // 平移
follow_pose.start(bMe_desire);
// 位置更新线程（与6轴版本逻辑相同）
updater = std::thread([&]() {
std::this_thread::sleep_for(std::chrono::seconds(2));
follow_pose.setScale(2);
auto it = points_list.begin();
while(running) {
// 正向运动
while (running) {
follow_pose.update(*it++);
std::this_thread::sleep_for(std::chrono::milliseconds (600));
if (it == points_list.end()) {
it--;
break;
}
}
// 反向运动
while (running) {
follow_pose.update(*it--);
std::this_thread::sleep_for(std::chrono::milliseconds (600));
if (it == points_list.begin()) {
break;
}
}
}
});
// 控制台输入监听
std::thread consoleInput([]{
while(getchar() != 'q'); // 按'q'键停止
running = false;
});
consoleInput.detach();
// 等待停止信号
while(running);
follow_pose.stop();
updater.join();
} catch (const std::exception &e) {
// 异常处理
print(std::cerr, e.what());
if(updater.joinable()) {
running = false;
updater.join();
}
}
}
/**
* @brief 主程序
* 功能：根据连接的机器人型号自动选择相应的控制程序
*/
int main() {
using namespace rokae;
using namespace std;
using namespace rokae::RtSupportedFields;
// 网络配置
string remoteIP = "192.168.0.160"; // 机器人IP地址
string localIP = "192.168.0.100"; // 本机IP地址
error_code ec;
std::thread updater;
// 机器人对象声明（支持6轴和7轴）
xMateRobot robot; // 6轴机器人
xMateErProRobot robot_pro; // 7轴机器人
try {
// 连接机器人
robot.connectToRobot(remoteIP, localIP);
} catch (const std::exception &e) {
std::cerr << e.what();
return 0;
}
// 获取机器人信息并设置控制参数
std::string robot_name = robot.robotInfo(ec).type; // 机器人型号名称
robot.setRtNetworkTolerance(20, ec); // 设置网络容错时间为20ms
robot.setMotionControlMode(rokae::MotionControlMode::RtCommand, ec); // 实时命令模式
robot.setOperateMode(rokae::OperateMode::automatic, ec); // 自动操作模式
robot.setPowerState(true, ec); // 上电
try {
// 初始化实时控制
auto rtCon = robot.getRtMotionController().lock();
// 开始接收机器人状态，1ms周期，只接收关节位置数据
robot.startReceiveRobotState(std::chrono::milliseconds(1), {jointPos_m});
} catch (const std::exception &e) {
std::cerr << e.what();
return 0;
}
/**
* 根据机器人型号选择相应的控制程序
* 注意：不同型号机器人的运动范围和结构不同，需要使用适配的点位数据
*/
if(robot_name.find("CR7") != std::string::npos || robot_name.find("CR12") != std::string::npos ) {
// CR系列机器人（6轴）
print(std::cout, "Run example for", robot_name);
example_followPosition6(robot, q_drag_cr7, points_xMateCR());
}
else if(robot_name.find("SR3") != std::string::npos || robot_name.find("SR4") != std::string::npos) {
// SR系列机器人（6轴）
print(std::cout, "Run example for", robot_name);
example_followPosition6(robot, q_drag_sr3, points_xMateSR3());
}
else if(robot_name.find("xMate3") != std::string::npos ){
// xMateER3/7系列机器人（6轴）
print(std::cout, "Run example for", robot_name);
example_followPosition6(robot, q_drag_er3, points_xMateER3());
}
else if (robot_name.find("Pro")!=std::string::npos){
// xMateER Pro系列机器人（7轴）
print(std::cout,"Run example for",robot_name);
example_followPosition7(robot_pro,q_drag_xm7p,points_xMateERPro());
print(std::cout, "跟随结束");
// 7轴机器人特殊处理：设置空闲模式并下电
robot_pro.setMotionControlMode(MotionControlMode::Idle, ec);
robot_pro.setPowerState(false, ec);
}
else {
// 不支持的机型
print(std::cerr, "示例程序中的点位尚未在该机型上使用过");
}
return 0;
}
// 以下为各型号机器人的预设路径点数据
// 这些点位数据通过示教或计算得到，确保机器人在安全范围内运动
/**
* @brief 适用于xMateCR7/12的关节角度路径点
* 每个点包含6个关节角度（弧度）
*/
std::vector<std::array<double, 6>> points_xMateCR() {
return {
{0.0,0.52359877559829882,-1.5707963267948966,0.0,-1.0471975511965976,0.0},
{0.035487243764309395,0.51814607597827012,-1.5806637828906964,2.2607167760800269e-06,-1.0427832278773319,0.035483868678714996},
// ... 更多路径点
{-7.7890503523341657e-07,0.51711158261127554,-1.5612157759668301,2.5489758866954378e-07,-1.0632642491857673,-9.773599037238323e-07}};
}
/**
* @brief 适用于SR3 & SR4的关节角度路径点
*/
std::vector<std::array<double, 6>> points_xMateSR3() {
return {
{-0.017938136876709584,0.49435159910311133,-1.6278070272570087,0.10223539561771972,-1.0233324615672779,-0.071357800901066984},
{-0.032594083928095562,0.46634275144052351,-1.6842348590894223,0.20686587914942534,-1.0065436700890904,-0.14486251700897126},
// ... 更多路径点
{-0.037080277744188789,0.3130583447962762,-1.9938135626505962,0.99639141933330022,-1.1477775002697621,-0.62289965112758638}
};
}
/**
* @brief 适用于xMateER3/7 Pro的关节角度路径点（7轴）
*/
std::vector<std::array<double, (unsigned short)7>> points_xMateERPro() {
return {
{1.1984224905356572e-06,0.52361854956939269, 0, 1.0472093756318377,8.9881686790174287e-07,1.5708220928784431,4.4940843395087144e-06},
{0.053236563559073066,0.52595866921924528, 0, 1.043601404881831,4.7936899621426287e-06,1.5720861290003356,0.053240518353291834},
// ... 更多路径点
{2.6365294791784457e-06,0.52364731170916556, 0, 1.0472302281831731,1.4980281131695715e-06,1.5708358747370843,1.2883041773258315e-05}};
}
/**
* @brief 适用于xMateER3/7的关节角度路径点（6轴）
*/
std::vector<std::array<double, 6>> points_xMateER3() {
return {
{1.1984224905356572e-06,0.52361854956939269,1.0472093756318377,8.9881686790174287e-07,1.5708220928784431,4.4940843395087144e-06},
{0.053236563559073066,0.52595866921924528,1.043601404881831,4.7936899621426287e-06,1.5720861290003356,0.053240518353291834},
// ... 更多路径点
{2.6365294791784457e-06,0.52364731170916556,1.0472302281831731,1.4980281131695715e-06,1.5708358747370843,1.2883041773258315e-05}};
}
实时模式-轴空间阻抗控制
程序适用机型xMateER7 Pro，实现机器人的关节空间阻抗控制，使机器人在关节空间内具有柔顺性，能够对外部力矩做出顺应性响应。
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "../print_helper.hpp"
using namespace rokae;
/**
* @brief 主程序 - 机器人关节空间阻抗控制示例
* 该程序演示了如何使用rokae机器人SDK实现关节空间阻抗控制，
* 使机器人在关节空间内实现柔顺运动
*/
int main() {
using namespace std;
try {
// ==================== 机器人初始化配置 ====================
std::string ip = "192.168.0.160"; // 机器人IP地址
std::error_code ec;
// 创建xMateErProRobot机器人实例
// 参数1: 机器人IP地址，参数2: 本机IP地址
rokae::xMateErProRobot robot(ip, "192.168.0.100");
// 设置机器人操作模式为自动模式
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// 设置运动控制模式为实时命令模式
// 此模式允许实时发送控制指令，适用于阻抗控制等高级控制策略
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
// 机器人上电使能
robot.setPowerState(true, ec);
// 获取实时运动控制器
// lock()确保在多线程环境下安全访问控制器
auto rtCon = robot.getRtMotionController().lock();
// ==================== 机器人状态数据接收设置 ====================
// 启动机器人状态数据接收，每1毫秒更新一次
// 只接收关节位置数据，减少网络带宽占用
robot.startReceiveRobotState(std::chrono::milliseconds(1), {
RtSupportedFields::jointPos_m // 关节位置信息（单位：弧度）
});
double time = 0; // 时间计数器，用于轨迹生成
// ==================== 获取当前状态并规划初始运动 ====================
std::array<double, 7> jntPos{}; // 存储关节位置的数组（7个关节）
// 获取当前关节位置数据
robot.getStateData(RtSupportedFields::jointPos_m, jntPos);
// 定义目标拖拽位姿（关节角度，单位：弧度）
// 这是一个预设的关节角度配置，各关节分别运动到不同角度
std::array<double,7> q_drag_xm7p = {
0, // 关节1: 0弧度
M_PI/6, // 关节2: 30度
0, // 关节3: 0弧度
M_PI/3, // 关节4: 60度
0, // 关节5: 0弧度
M_PI/2, // 关节6: 90度
0 // 关节7: 0弧度
};
// 从当前位置进行关节空间运动到拖拽位姿
// 参数说明：
// 0.5 - 速度比例
// jntPos - 起始关节位置
// q_drag_xm7p - 目标关节位置
rtCon->MoveJ(0.5, jntPos, q_drag_xm7p);
// ==================== 实时控制回调函数定义 ====================
/**
* @brief 关节空间阻抗控制回调函数
* 这个Lambda函数每1ms被调用一次，生成关节位置指令
* 捕获列表说明：
* [&jntPos] - 按引用捕获初始关节位置
* [rtCon] - 按值捕获运动控制器
* [&time] - 按引用捕获时间变量
*/
std::function<JointPosition(void)> callback = [&jntPos, rtCon, &time] {
time += 0.001; // 每次调用增加1ms
// 计算关节角度增量
// 使用余弦函数生成平滑的角度变化：
// - M_PI/20.0: 最大角度变化幅度（9度）
// - M_PI/4 * time: 角频率，控制变化速度
double delta_angle = M_PI / 20.0 * (1 - std::cos(M_PI/4 * time));
// 创建关节位置指令对象，7个关节
JointPosition cmd(7);
// 为每个关节计算目标位置
// 所有关节同步运动相同的角度增量
for(unsigned i = 0; i < cmd.joints.size(); ++i) {
cmd.joints[i] = jntPos[i] + delta_angle;
}
// 运动结束条件：60秒后停止
if(time > 60) {
cmd.setFinished(); // 设置完成标志，停止控制循环
}
return cmd;
};
// ==================== 阻抗控制参数设置 ====================
// 设置关节空间阻抗系数（单位：Nm/rad）
// 阻抗系数越大，关节刚度越高，抗干扰能力越强
// 阻抗系数越小，关节柔顺性越好，更容易受外力影响
std::array<double, 7> impedance_gains = {
500, // 关节1刚度：500 Nm/rad
500, // 关节2刚度：500 Nm/rad
500, // 关节3刚度：500 Nm/rad
500, // 关节4刚度：500 Nm/rad
50, // 关节5刚度：50 Nm/rad（较低刚度，更柔顺）
50, // 关节6刚度：50 Nm/rad
50 // 关节7刚度：50 Nm/rad
};
rtCon->setJointImpedance(impedance_gains, ec);
// ==================== 启动实时控制循环 ====================
// 设置控制回调函数
rtCon->setControlLoop(callback);
// 更新起始位置为当前位置（确保从正确的位置开始阻抗控制）
jntPos = robot.jointPos(ec);
// 开始关节空间阻抗运动模式
rtCon->startMove(RtControllerMode::jointImpedance);
// 启动阻塞式控制循环
// 参数true表示阻塞模式，程序会在此处等待直到控制循环结束
rtCon->startLoop(true);
// 控制循环结束后执行
print(std::cout, "控制结束");
} catch (const std::exception &e) {
// 异常处理：输出错误信息
std::cout << "程序异常: " << e.what();
}
return 0;
}
实时模式-轴角度控制
程序适用机型：xMateER3
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "../print_helper.hpp"
using namespace rokae;
using namespace std;
/**
* @brief 机器人关节空间正弦波运动示例程序
* 功能：控制机器人在关节空间进行正弦波轨迹运动，所有关节同步运动
* 特点：实时控制、平滑轨迹、安全停止
*/
int main() {
// 创建机器人对象 - xMateRobot代表珞石6轴机器人
rokae::xMateRobot robot;
std::error_code ec; // 错误码对象，用于接收函数调用结果
// 连接到机器人
try {
// 参数1: 机器人IP地址 (192.168.0.160)
// 参数2: 本机IP地址 (192.168.0.100)
// 注意：两个设备需要在同一网段，网络连接正常
robot.connectToRobot("192.168.0.160", "192.168.0.100");
} catch (const std::exception &e) {
// 连接失败处理
print(std::cerr, e.what());
return 0;
}
// 设置机器人工作状态（三步曲）
robot.setOperateMode(rokae::OperateMode::automatic, ec); // 设置为自动模式，可执行程序
robot.setMotionControlMode(MotionControlMode::RtCommand, ec); // 设置为实时命令模式，允许高频控制
robot.setPowerState(true, ec); // 上电（开启伺服），机器人可以运动
try {
// 获取实时运动控制器
// lock()将weak_ptr转换为shared_ptr，确保控制器对象存在
auto rtCon = robot.getRtMotionController().lock();
// 可选: 设置接收机器人状态数据
// 参数1: 更新周期1ms - 每1毫秒接收一次状态
// 参数2: 接收的数据字段 - 只接收关节位置信息
robot.startReceiveRobotState(std::chrono::milliseconds(1), {RtSupportedFields::jointPos_m});
// 定义和初始化运动控制变量
double time = 0; // 运动时间累计，单位：秒
std::array<double, 6> jntPos{}; // 当前关节位置数组，6个元素对应6个关节
// 拖拽位姿 - 一个安全的起始位置
std::array<double, 6> q_drag_xm3 = {0, M_PI/6, M_PI/3, 0, M_PI/2, 0};
/**
* @brief 实时控制回调函数
* 这个函数会被实时循环调用（约1ms周期），用于生成关节空间运动轨迹
* 返回值: JointPosition 包含6个关节目标位置的控制命令
*
* 运动原理：所有关节按照正弦波规律运动，但运动方向不同
* 关节0,1,3,5: 正向运动 (+delta_angle)
* 关节2,4: 反向运动 (-delta_angle)
*/
std::function<JointPosition()> callback = [&, rtCon](){
time += 0.001; // 时间累计，每次增加1ms（假设调用周期为1ms）
// 计算关节角度增量 - 使用余弦函数生成平滑的S曲线轨迹
// 公式: Δθ = (π/20) * [1 - cos(π/2.5 * t)]
// - π/20: 幅度系数，限制最大角度变化
// - 1-cos(): 生成从0开始平滑加速的运动曲线
// - π/2.5: 频率系数，控制运动速度
double delta_angle = M_PI / 20.0 * (1 - std::cos(M_PI / 2.5 * time));
// 构建控制命令 - 对每个关节应用角度增量
JointPosition cmd = {{
jntPos[0] + delta_angle, // 关节0: 正向运动
jntPos[1] + delta_angle, // 关节1: 正向运动
jntPos[2] - delta_angle, // 关节2: 反向运动
jntPos[3] + delta_angle, // 关节3: 正向运动
jntPos[4] - delta_angle, // 关节4: 反向运动
jntPos[5] + delta_angle // 关节5: 正向运动
}};
// 运动停止条件：60秒后结束运动
if(time > 60) {
cmd.setFinished(); // 设置完成标志，停止实时控制循环
}
return cmd; // 返回控制命令给机器人
};
// 第一步：从当前位置运动到安全的拖拽位姿
// MoveJ: 关节空间点到点运动
// 参数1: 速度比例 0.3
// 参数2: 起始位置 - 当前关节位置
// 参数3: 目标位置 - 预设的拖拽位姿
rtCon->MoveJ(0.3, robot.jointPos(ec), q_drag_xm3);
// 第二步：设置实时控制循环
// 注册回调函数，机器人将以约1ms周期调用此函数
rtCon->setControlLoop(callback);
// 第三步：获取当前关节位置作为运动起始点
// 注意：必须在设置回调后获取，确保使用最新的位置数据
jntPos = robot.jointPos(ec);
// 第四步：开始关节空间位置控制
// 设置控制模式为关节位置控制，在此模式下机器人跟踪关节角度命令
rtCon->startMove(RtControllerMode::jointPosition);
// 第五步：启动实时控制循环
// 参数true表示阻塞执行，程序会停在这里直到运动完成（60秒或异常）
rtCon->startLoop(true);
// 运动完成提示
print(std::cout, "控制结束");
// 第六步：安全退出 - 恢复机器人到安全状态
robot.setMotionControlMode(MotionControlMode::Idle, ec); // 设置为空闲模式
robot.setPowerState(false, ec); // 下电（关闭伺服），机器人进入抱闸状态
} catch (const std::exception &e) {
// 异常处理：确保在任何异常情况下都能安全停止机器人
print(std::cerr, e.what()); // 打印异常信息
// 安全恢复操作
robot.setMotionControlMode(MotionControlMode::Idle, ec);
robot.setPowerState(false, ec);
}
return 0;
}
实时模式-轴空间S规划
程序适用机型xMateER7 Pro
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "../print_helper.hpp"
using namespace rokae;
using namespace std;
/**
* @brief 机器人关节空间正弦波运动示例程序
* 功能：控制机器人在关节空间进行正弦波轨迹运动，所有关节同步运动
* 特点：实时控制、平滑轨迹、安全停止
*/
int main() {
// 创建机器人对象 - xMateRobot代表珞石6轴机器人
rokae::xMateRobot robot;
std::error_code ec; // 错误码对象，用于接收函数调用结果
// 连接到机器人
try {
// 参数1: 机器人IP地址 (192.168.0.160)
// 参数2: 本机IP地址 (192.168.0.100)
// 注意：两个设备需要在同一网段，网络连接正常
robot.connectToRobot("192.168.0.160", "192.168.0.100");
} catch (const std::exception &e) {
// 连接失败处理
print(std::cerr, e.what());
return 0;
}
// 设置机器人工作状态（三步曲）
robot.setOperateMode(rokae::OperateMode::automatic, ec); // 设置为自动模式，可执行程序
robot.setMotionControlMode(MotionControlMode::RtCommand, ec); // 设置为实时命令模式，允许高频控制
robot.setPowerState(true, ec); // 上电（开启伺服），机器人可以运动
try {
// 获取实时运动控制器
// lock()将weak_ptr转换为shared_ptr，确保控制器对象存在
auto rtCon = robot.getRtMotionController().lock();
// 可选: 设置接收机器人状态数据
// 参数1: 更新周期1ms - 每1毫秒接收一次状态
// 参数2: 接收的数据字段 - 只接收关节位置信息
robot.startReceiveRobotState(std::chrono::milliseconds(1), {RtSupportedFields::jointPos_m});
// 定义和初始化运动控制变量
double time = 0; // 运动时间累计，单位：秒
std::array<double, 6> jntPos{}; // 当前关节位置数组，6个元素对应6个关节
// 拖拽位姿 - 一个安全的起始位置
std::array<double, 6> q_drag_xm3 = {0, M_PI/6, M_PI/3, 0, M_PI/2, 0};
/**
* @brief 实时控制回调函数
* 这个函数会被实时循环调用（约1ms周期），用于生成关节空间运动轨迹
* 返回值: JointPosition 包含6个关节目标位置的控制命令
*
* 运动原理：所有关节按照正弦波规律运动，但运动方向不同
* 关节0,1,3,5: 正向运动 (+delta_angle)
* 关节2,4: 反向运动 (-delta_angle)
*/
std::function<JointPosition()> callback = [&, rtCon](){
time += 0.001; // 时间累计，每次增加1ms（假设调用周期为1ms）
// 计算关节角度增量 - 使用余弦函数生成平滑的S曲线轨迹
// 公式: Δθ = (π/20) * [1 - cos(π/2.5 * t)]
// - π/20: 幅度系数，限制最大角度变化
// - 1-cos(): 生成从0开始平滑加速的运动曲线
// - π/2.5: 频率系数，控制运动速度
double delta_angle = M_PI / 20.0 * (1 - std::cos(M_PI / 2.5 * time));
// 构建控制命令 - 对每个关节应用角度增量
JointPosition cmd = {{
jntPos[0] + delta_angle, // 关节0: 正向运动
jntPos[1] + delta_angle, // 关节1: 正向运动
jntPos[2] - delta_angle, // 关节2: 反向运动
jntPos[3] + delta_angle, // 关节3: 正向运动
jntPos[4] - delta_angle, // 关节4: 反向运动
jntPos[5] + delta_angle // 关节5: 正向运动
}};
// 运动停止条件：60秒后结束运动
if(time > 60) {
cmd.setFinished(); // 设置完成标志，停止实时控制循环
}
return cmd; // 返回控制命令给机器人
};
// 第一步：从当前位置运动到安全的拖拽位姿
// MoveJ: 关节空间点到点运动
// 参数1: 速度比例 0.3
// 参数2: 起始位置 - 当前关节位置
// 参数3: 目标位置 - 预设的拖拽位姿
rtCon->MoveJ(0.3, robot.jointPos(ec), q_drag_xm3);
// 第二步：设置实时控制循环
// 注册回调函数，机器人将以约1ms周期调用此函数
rtCon->setControlLoop(callback);
// 第三步：获取当前关节位置作为运动起始点
// 注意：必须在设置回调后获取，确保使用最新的位置数据
jntPos = robot.jointPos(ec);
// 第四步：开始关节空间位置控制
// 设置控制模式为关节位置控制，在此模式下机器人跟踪关节角度命令
rtCon->startMove(RtControllerMode::jointPosition);
// 第五步：启动实时控制循环
// 参数true表示阻塞执行，程序会停在这里直到运动完成（60秒或异常）
rtCon->startLoop(true);
// 运动完成提示
print(std::cout, "控制结束");
// 第六步：安全退出 - 恢复机器人到安全状态
robot.setMotionControlMode(MotionControlMode::Idle, ec); // 设置为空闲模式
robot.setPowerState(false, ec); // 下电（关闭伺服），机器人进入抱闸状态
} catch (const std::exception &e) {
// 异常处理：确保在任何异常情况下都能安全停止机器人
print(std::cerr, e.what()); // 打印异常信息
// 安全恢复操作
robot.setMotionControlMode(MotionControlMode::Idle, ec);
robot.setPowerState(false, ec);
}
return 0;
}
实时模式-运动规划
适合MoveJ & MoveL & MoveC
int main() {
using namespace std;
try {
// 网络配置
std::string ip = "192.168.21.10"; // 机器人控制器IP地址
std::error_code ec; // 错误码对象，用于接收函数调用结果
// 创建7轴机器人对象并连接
// 参数1: 机器人IP (192.168.21.10)
// 参数2: 本机IP (192.168.21.1) - 需要与机器人同一网段
rokae::xMateErProRobot robot(ip, "192.168.21.10"); // xMate 7轴机器人
// 设置机器人操作模式为自动模式
// 自动模式：机器人可以执行程序，接受外部控制命令
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// 设置实时网络容错时间为20ms
// 重要：若程序运行时控制器已是实时模式，需要先切换到非实时模式再设置此参数
// 网络容错时间指允许的网络延迟，超过此时间会触发安全保护
robot.setRtNetworkTolerance(20, ec);
// 设置运动控制模式为实时命令模式
// RtCommand模式：允许高频实时控制，适用于需要精确轨迹的应用
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
// 开启机器人电源（上伺服）
// 上电后机器人可以运动，下电后机器人处于抱闸状态
robot.setPowerState(true, ec);
// 获取实时运动控制器
// lock()将weak_ptr转换为shared_ptr，确保控制器对象存在
auto rtCon = robot.getRtMotionController().lock();
// 示例程序使用机型: xMateER7 Pro (7轴机器人)
/******************************************************************
* 1. 关节空间运动：从当前位置MoveJ运动到拖拽位置
******************************************************************/
// 定义拖拽位姿 - 7个关节的目标角度（单位：弧度）
// 这是一个预设的安全位置，便于后续的笛卡尔空间运动
std::array<double, 7> q_drag_xm7p = {
0, // 关节1: 0弧度
M_PI / 6, // 关节2: 30度
0, // 关节3: 0弧度
M_PI / 3, // 关节4: 60度
0, // 关节5: 0弧度
M_PI / 2, // 关节6: 90度
0 // 关节7: 0弧度
};
// 执行关节空间点到点运动
// 参数1: 速度比例 0.5
// 参数2: 起始位置 - 当前关节位置
// 参数3: 目标位置 - 拖拽位姿
rtCon->MoveJ(0.5, robot.jointPos(ec), q_drag_xm7p);
/******************************************************************
* 2. 圆弧运动 (在X-Y平面上)
******************************************************************/
// 定义圆弧运动的三个关键点：起点、辅助点、终点
CartesianPosition start, aux, target;
// 获取当前法兰盘在基坐标系下的位姿，并转换为齐次变换矩阵
// 法兰盘坐标系：机器人末端的坐标系，工具安装的位置
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), start.pos);
// 定义变量存储旋转矩阵和平移向量
Eigen::Matrix3d rot_start; // 起始点的旋转矩阵
Eigen::Vector3d trans_start; // 起始点的平移向量 [x, y, z]
Eigen::Vector3d trans_aux; // 辅助点的平移向量
Eigen::Vector3d trans_end; // 终点的平移向量
// 将齐次变换矩阵分解为旋转矩阵和平移向量
Utils::arrayToTransMatrix(start.pos, rot_start, trans_start);
// 初始化辅助点和终点坐标（基于起始点）
trans_end = trans_start;
trans_aux = trans_start;
// 设置辅助点坐标：X方向-0.28m, Y方向-0.05m
// 辅助点与起点、终点共同确定圆弧的曲率
trans_aux[0] -= 0.28; // X坐标减少0.28米
trans_aux[1] -= 0.05; // Y坐标减少0.05米
// 设置终点坐标：Y方向-0.15m
trans_end[1] -= 0.15; // Y坐标减少0.15米
// 将辅助点和终点的坐标转换回齐次变换矩阵格式
// 注意：旋转矩阵保持不变，只改变位置
Utils::transMatrixToArray(rot_start, trans_aux, aux.pos);
Utils::transMatrixToArray(rot_start, trans_end, target.pos);
// 执行圆弧插补运动
// 参数1: 速度比例 0.2
// 参数2: 起点位姿
// 参数3: 辅助点位姿
// 参数4: 终点位姿
// 运动轨迹：从start出发，经过aux点，到达target点，形成圆弧
rtCon->MoveC(0.2, start, aux, target);
/******************************************************************
* 3. 直线运动
******************************************************************/
// 重新获取当前位置作为直线运动的起点
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), start.pos);
Utils::arrayToTransMatrix(start.pos, rot_start, trans_start);
// 设置直线运动终点坐标
trans_end = trans_start;
// 沿X轴负方向移动0.1米，Y轴负方向移动0.3米，Z轴负方向移动0.25米
trans_end[0] -= 0.1; // X: -0.1m
trans_end[1] -= 0.3; // Y: -0.3m
trans_end[2] -= 0.25; // Z: -0.25m
// 将终点坐标转换回齐次变换矩阵
Utils::transMatrixToArray(rot_start, trans_end, target.pos);
// 打印运动信息用于调试
print(std::cout, "MoveL start position:", start.pos, "Target:", target.pos);
// 执行直线插补运动
// 参数1: 速度比例 0.3
// 参数2: 起点位姿
// 参数3: 终点位姿
// 运动轨迹：从start到target的直线路径，工具姿态保持不变
rtCon->MoveL(0.3, start, target);
/******************************************************************
* 4. 安全关闭：恢复机器人到安全状态
******************************************************************/
// 设置运动控制模式为非实时命令模式
// NrtCommand模式：适用于非实时、低速控制场景
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
// 设置操作模式为手动模式
// 手动模式：通常用于示教、调试，限制自动运行
robot.setOperateMode(rokae::OperateMode::manual, ec);
robot.setPowerState(false, ec); // 下电（关闭伺服），机器人进入抱闸状态
}
catch (const std::exception& e) {
// 异常处理：捕获并打印所有异常信息
std::cerr << e.what();
}
return 0;
}
实时模式-工业6轴机型支持位置控制
#include <cmath>
#include <thread>
#include <iterator>
#include "../print_helper.hpp"
#include "Eigen/Geometry"
#include "rokae/robot.h"
#include "rokae/utility.h"
using namespace rokae;
std::ostream &os = std::cout; ///< 输出流引用，用于打印到控制台
/**
* @brief 机器人多种运动模式演示程序
* 功能：展示标准机器人的关节运动、圆弧运动、直线运动及工具坐标系设置
* 特点：包含安全区域设置、工具坐标系变换等高级功能
*/
int main() {
using namespace std;
try {
// 网络配置和机器人初始化
std::string ip = "192.168.0.160"; // 机器人控制器IP
std::error_code ec; // 错误码对象
rokae::StandardRobot robot(ip, "192.168.0.180"); // 创建标准机器人对象并连接
// 设置机器人工作模式
robot.setOperateMode(rokae::OperateMode::automatic, ec); // 自动模式
robot.setRtNetworkTolerance(20, ec); // 设置实时网络容错20ms
robot.setMotionControlMode(MotionControlMode::RtCommand, ec); // 实时命令模式
robot.setPowerState(true, ec); // 上电
// 获取实时运动控制器
auto rtCon = robot.getRtMotionController().lock();
/******************************************************************
* 工具坐标系设置：重置末端坐标系与法兰盘重合
******************************************************************/
std::array<double, 16> _end{}; // 4x4齐次变换矩阵
// 将位姿{0,0,0,0,0,0}转换为齐次矩阵，表示无偏移和旋转
// 参数：{x, y, z, rx, ry, rz} - 位置和欧拉角
Utils::postureToTransArray({0,0,0,0,0,0}, _end);
// 设置末端工具坐标系，使其与法兰盘坐标系完全重合
rtCon->setEndEffectorFrame(_end, ec);
// 示例程序使用机型: XB7h-R707
/******************************************************************
* 1. 关节空间运动：从当前位置MoveJ运动到发货位置
******************************************************************/
// 使用MoveJ进行关节空间点到点运动
// 参数1: 速度比例 0.4
// 参数2: 起始位置 - 当前关节位置
// 参数3: 目标位置 - 将角度转换为弧度后的关节角度
// 目标位姿: {0°, -15°, 60°, 0°, 45°, 0°}
rtCon->MoveJ(0.4, robot.jointPos(ec), Utils::degToRad(std::array<double, 6>({0, -15, 60, 0, 45, 0})));
/******************************************************************
* 2. 圆弧运动 (在X-Y平面上)
******************************************************************/
CartesianPosition start, aux, target; // 定义圆弧运动的三个关键点
// 获取当前末端在参考坐标系下的位姿，作为圆弧起点
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::endInRef, ec), start.pos);
Eigen::Matrix3d rot_start; // 起始点的旋转矩阵
Eigen::Vector3d trans_start; // 起始点的平移向量 [x, y, z]
Eigen::Vector3d trans_aux; // 辅助点的平移向量
Eigen::Vector3d trans_end; // 终点的平移向量
// 将齐次变换矩阵分解为旋转矩阵和平移向量
Utils::arrayToTransMatrix(start.pos, rot_start, trans_start);
// 初始化终点和辅助点坐标
trans_end = trans_start;
trans_aux = trans_start;
// 设置辅助点坐标：X方向+0.28m, Y方向-0.05m
trans_aux[0] += 0.28; // X坐标增加0.28米
trans_aux[1] -= 0.05; // Y坐标减少0.05米
// 设置终点坐标：Y方向-0.15m
trans_end[1] -= 0.15; // Y坐标减少0.15米
// 将坐标转换回齐次变换矩阵格式（保持原有旋转）
Utils::transMatrixToArray(rot_start, trans_aux, aux.pos);
Utils::transMatrixToArray(rot_start, trans_end, target.pos);
// 执行圆弧插补运动
// 运动轨迹：从start出发，经过aux点，到达target点，在X-Y平面形成圆弧
rtCon->MoveC(0.2, start, aux, target);
/******************************************************************
* 3. 直线运动（使用欧拉角表示姿态）
******************************************************************/
// 获取当前末端在参考坐标系下的位姿（欧拉角表示）
auto _pose_start = robot.posture(rokae::CoordinateType::endInRef, ec);
auto _pose_target = _pose_start; // 复制起始位姿
// 设置目标位姿：沿Z轴+0.2m, 绕Y轴旋转+60°
_pose_target[2] += 0.2; // Z坐标增加0.2米
_pose_target[4] += Utils::degToRad(60); // 绕Y轴旋转60度
// 将欧拉角位姿转换为齐次变换矩阵
Utils::postureToTransArray(_pose_start, start.pos);
Utils::postureToTransArray(_pose_target, target.pos);
// 打印运动信息用于调试
print(os, "MoveL start position:", start.pos, "Target:", target.pos);
// 执行直线插补运动
// 运动轨迹：从start到target的直线路径，工具姿态平滑过渡
rtCon->MoveL(0.3, start, target);
/******************************************************************
* 4. 设置实时模式末端坐标系并执行运动
******************************************************************/
// 设置新的工具坐标系：在X方向偏移0.1m，绕Y轴旋转90°
// 新的工具坐标系相对于法兰盘的变换：{0.1, 0, 0, 0, π/2, 0}
Utils::postureToTransArray(std::array<double, 6>({0.1, 0, 0, 0, M_PI_2, 0}), _end);
rtCon->setEndEffectorFrame(_end, ec);
// 使用新的工具坐标系执行关节运动
rtCon->MoveJ(0.4, robot.jointPos(ec), Utils::degToRad(std::array<double, 6>({0, -15, 60, 0, 45, 0})));
// 重要说明：
// 实时模式的工具坐标系设置是独立的，不能使用robot.posture(CoordinateType::endInRef)接口获取位姿
// 因为该接口返回的是基于非实时模式工具坐标系的位姿
// 方法1：直接给出设置末端坐标系后的起始位姿
// 位姿格式：{x, y, z, rx, ry, rz}
Utils::postureToTransArray({0.1036, 0, 0.415, 0.042, -M_PI_2, -0.0424}, start.pos);
// 方法2：接收实时状态数据获取TCP位姿
//robot.getStateData(RtSupportedFields::tcpPose_m, start.pos);
target.pos = start.pos;
target.pos[3] += 0.35; // X坐标增加0.35米
print(os, "MoveL start position:", start.pos, "Target:", target.pos);
// 使用新的工具坐标系执行直线运动
rtCon->MoveL(0.3, start, target);
/******************************************************************
* 5. 安全关闭：恢复机器人到安全状态
******************************************************************/
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec); // 切换到非实时模式
robot.setOperateMode(rokae::OperateMode::manual, ec); // 设置为手动模式
} catch (const std::exception &e) {
// 异常处理
print(std::cerr, e.what());
}
return 0;
}
实时模式-直接力矩控制
条件：1. 此示例需要使用xMateModel模型库，请设置编译选项XCORE_USE_XMATE_MODEL=ON
- 需要实机测试
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "Eigen/Geometry"
#include "../print_helper.hpp"
#include "rokae/utility.h"
using namespace rokae;
/**
* @brief 力矩控制示例程序
* 重要注意事项:
* 1) 力矩值不要超过机型的限制条件(见手册);
* 2) 初次运行时请手握急停开关, 避免机械臂非预期运动造成碰撞
* 3) 力矩控制是底层控制模式，需要精确的动力学模型
*/
void torqueControl(xMateErProRobot &robot) {
using namespace RtSupportedFields;
// 获取实时运动控制器
auto rtCon = robot.getRtMotionController().lock();
auto model = robot.model(); // 获取机器人动力学模型
error_code ec;
// 定义拖拽位姿 - 7轴机器人的安全起始位置
std::array<double,7> q_drag = {0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0 };
// 重新配置机器人状态接收
// 停止之前的接收，重新开始以获取所需的状态数据
robot.stopReceiveRobotState();
// 开始接收机器人状态，1ms周期，接收关节位置、速度、加速度和TCP位姿
robot.startReceiveRobotState(std::chrono::milliseconds(1),
{jointPos_m, jointVel_m, jointAcc_c, tcpPose_m});
// 第一步：运动到拖拽位置（安全起始点）
rtCon->MoveJ(0.2, robot.jointPos(ec), q_drag);
// 第二步：设置控制模式为力矩控制
// 力矩控制模式下，直接向关节发送力矩指令而不是位置或速度指令
rtCon->startMove(RtControllerMode::torque);
/******************************************************************
* 阻抗控制参数设置
* 阻抗控制模拟弹簧-阻尼系统，使机器人具有柔顺性
******************************************************************/
// 刚度参数（弹簧系数）
const double translational_stiffness{200.0}; // 平移刚度 N/m
const double rotational_stiffness{5.0}; // 旋转刚度 N·m/rad
// 定义刚度和阻尼矩阵（6x6，对应6自由度：3平移 + 3旋转）
Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
stiffness.setZero();
// 设置平移刚度和旋转刚度对角矩阵
stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
damping.setZero();
// 临界阻尼系数计算：damping = 2 * sqrt(stiffness)
// 这里设为0表示无阻尼，机器人会持续振荡
damping.topLeftCorner(3, 3) << 0.0 * sqrt(translational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
damping.bottomRightCorner(3, 3) << 0.0 * sqrt(rotational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
// 存储初始位置（作为期望位置）
std::array<double, 16> init_position {};
Eigen::Matrix<double, 6, 7> jacobian; // 雅可比矩阵：6x7（任务空间自由度 x 关节数）
// 获取当前法兰盘在基坐标系下的位姿作为初始位置
Utils::postureToTransArray(robot.posture(rokae::CoordinateType::flangeInBase, ec), init_position);
/******************************************************************
* 实时力矩控制回调函数
* 这个函数会以1ms周期被调用，计算并返回7个关节的力矩指令
******************************************************************/
std::function<Torque(void)> callback = [&]{
using namespace RtSupportedFields;
static double time=0; // 时间累计，用于控制运动时长
time += 0.001; // 每次增加1ms
// 将初始位置转换为Eigen变换矩阵
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(init_position.data()).transpose());
Eigen::Vector3d position_d(initial_transform.translation()); // 期望位置
Eigen::Quaterniond orientation_d(initial_transform.linear()); // 期望姿态（四元数）
// 定义状态变量
std::array<double, 7> q{}, dq_m{}, ddq_c{}; // 关节位置、速度、加速度
std::array<double, 16> pos_m {}; // TCP位姿
// 从机器人状态中读取实时数据
robot.getStateData(jointPos_m, q); // 关节位置
robot.getStateData(jointVel_m, dq_m); // 关节速度
robot.getStateData(jointAcc_c, ddq_c); // 关节加速度
/******************************************************************
* 动力学计算部分
******************************************************************/
// 计算雅可比矩阵 - 描述关节速度到末端速度的映射
std::array<double, 42> jacobian_array = model.jacobian(q);
// 计算重力补偿力矩
std::array<double, 7> gravity_array = model.getTorque(q, dq_m, ddq_c, TorqueType::gravity);
// 计算摩擦力补偿力矩
std::array<double, 7> friction_array = model.getTorque(q, dq_m, ddq_c, TorqueType::friction);
// 将数组数据转换为Eigen矩阵，便于数学运算
Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(gravity_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> friction(friction_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 6>> jacobian_(jacobian_array.data());
jacobian = jacobian_.transpose(); // 转置为6x7矩阵
Eigen::Map<const Eigen::Matrix<double, 7, 1>> q_mat(q.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq_mat(dq_m.data());
// 获取当前TCP位姿
robot.getStateData(tcpPose_m, pos_m);
Eigen::Affine3d transform(Eigen::Matrix4d::Map(pos_m.data()).transpose());
Eigen::Vector3d position(transform.translation()); // 当前位置
Eigen::Quaterniond orientation(transform.linear()); // 当前姿态
/******************************************************************
* 误差计算部分
******************************************************************/
Eigen::Matrix<double, 6, 1> error; // 6维误差向量 [位置误差(3), 姿态误差(3)]
// 位置误差：当前位置 - 期望位置
error.head(3) << position - position_d;
// 姿态误差计算（使用四元数）
// 确保四元数在同一个半球（避免双重覆盖问题）
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
orientation.coeffs() << -orientation.coeffs();
}
// 计算姿态误差四元数
Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
// 将四元数误差转换为轴角表示的旋转向量
error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
// 转换到基坐标系
error.tail(3) << -transform.linear() * error.tail(3);
/******************************************************************
* 控制律计算部分 - 阻抗控制
* τ = J^T * [ -K * e - D * (J * dq) ]
* 其中：
* τ: 关节力矩
* J: 雅可比矩阵
* K: 刚度矩阵
* D: 阻尼矩阵
* e: 位姿误差
* dq: 关节速度
******************************************************************/
Eigen::VectorXd tau_d(7); // 7维关节力矩向量
// 笛卡尔空间阻抗计算并映射到关节空间
tau_d << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq_mat));
// 注意：这里没有加入重力补偿和摩擦力补偿
// 实际应用中通常需要：tau_total = tau_impedance + gravity + friction
// 构建控制命令
Torque cmd(7);
Eigen::VectorXd::Map(cmd.tau.data(), 7) = tau_d;
// 停止条件：30秒后结束
if(time > 30){
cmd.setFinished();
}
return cmd;
};
// 设置实时控制循环
// 参数useStateDataInLoop = true 表示在回调函数中读取状态数据
// 发送周期与startReceiveRobotState()设置的1ms周期一致
rtCon->setControlLoop(callback, 0, true);
// 启动实时控制循环（阻塞执行）
rtCon->startLoop(true);
}
/**
* @brief 发送0力矩测试函数
* 功能：在力控模型准确的情况下，机械臂应保持静止不动
* 这是一个简单的力矩控制测试，用于验证动力学模型的准确性
*/
template <unsigned short DoF>
void zeroTorque(Cobot<DoF> &robot) {
error_code ec;
std::array<double,7> q_drag = {0, M_PI/6, 0, M_PI/3, 0, M_PI/2, 0 };
auto rtCon = robot.getRtMotionController().lock();
// 运动到拖拽位置
rtCon->MoveJ(0.2, robot.jointPos(ec), q_drag);
// 控制模式为力矩控制
rtCon->startMove(RtControllerMode::torque);
// 创建零力矩命令
Torque cmd {};
cmd.tau.resize(DoF); // 根据自由度数量调整力矩向量大小
// 简单的回调函数，始终返回零力矩
std::function<Torque(void)> callback = [&]() {
static double time=0;
time += 0.001;
if(time > 30){
cmd.setFinished(); // 30秒后停止
}
return cmd; // 返回零力矩
};
rtCon->setControlLoop(callback);
rtCon->startLoop();
print(std::cout, "力矩控制结束");
}
/**
* @brief 主程序
* 功能：初始化机器人并执行力矩控制演示
*/
int main() {
try {
std::string ip = "192.168.0.160";
std::error_code ec;
// 创建7轴机器人对象并连接
rokae::xMateErProRobot robot(ip, "192.168.0.100");
// 设置机器人工作模式
robot.setOperateMode(rokae::OperateMode::automatic, ec);
robot.setMotionControlMode(MotionControlMode::RtCommand, ec);
robot.setPowerState(true, ec);
try {
// 执行力矩控制演示
torqueControl(robot);
} catch (const rokae::RealtimeMotionException &e) {
// 实时运动异常处理
print(std::cerr, e.what());
// 发生错误时，切换回非实时模式确保安全
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
}
// 正常退出：恢复机器人到安全状态
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
robot.setOperateMode(rokae::OperateMode::manual, ec);
} catch (const std::exception &e) {
// 全局异常处理
print(std::cerr, e.what());
}
return 0;
}
非实时模式
概要
与RL程序实现的效果一样
用户侧算法：
-
拿到终点坐标(X,Y,Z,a,b,c)，方式拍照、示教，等
-
决定行走方式：直线
-
发送目标点
-
等待机器人到达目标点
非实时运动指令
前提
根据机型和坐标系的不同, 各示例中的点位不一定可达, 仅供接口使用方法的参考
#include <iostream>
#include <thread>
#include <cmath>
#include "rokae/robot.h"
#include "rokae/utility.h"
#include "print_helper.hpp"
using namespace std;
using namespace rokae;
std::ostream &os = std::cout; ///< 输出到控制台
namespace Predefines {
// ****** 预定义拖拽位姿 ******
const std::vector<double> ErDragPosture = {0, M_PI/6, M_PI/3, 0, M_PI_2, 0}; ///< xMateEr3, xMateEr7 拖拽位姿
const std::vector<double> ErProDragPosture = {0, M_PI/6, 0, M_PI/3, 0, M_PI_2, 0}; ///< xMateEr3 Pro, xMateEr7 Pro 拖拽位姿
const std::vector<double> CrDragPosture {0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0}; ///< xMateCR 拖拽位姿
const std::vector<double> Cr5DragPostre = { 0, M_PI / 6, -M_PI_2, -M_PI / 3, 0}; ///< CR5轴构型拖拽位姿
Toolset defaultToolset; ///< 默认工具工件坐标系
}
/**
* @brief 打印运动执行信息
* @param info 运动事件信息
*/
void printInfo(const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey::MoveExecution;
// 提取运动执行信息并打印
print(std::cout, "[运动执行信息] ID:", std::any_cast<std::string>(info.at(ID)),
"Index:", std::any_cast<int>(info.at(WaypointIndex)),
"已完成: ", std::any_cast<bool>(info.at(ReachTarget)) ? "YES": "NO",
std::any_cast<error_code>(info.at(Error)),
std::any_cast<std::string>(info.at(Remark)));
// 如果设置了自定义信息，打印这个信息
if(info.count(CustomInfo)) {
auto custom_info = std::any_cast<std::string>(info.at(CustomInfo));
if(!custom_info.empty()) print(std::cout, "自定义信息: ",custom_info);
}
}
/**
* @brief 主程序
* 演示如何连接机器人并执行各种运动示例
*/
int main() {
try {
using namespace rokae;
// *** 1. 连接机器人 ***
std::string ip = "192.168.21.10";
std::error_code ec;
rokae::xMateRobot robot(ip); // 创建xMate 6轴机器人实例
// *** 2. 切换到自动模式并上电 ***
robot.setOperateMode(OperateMode::automatic, ec);
robot.setPowerState(true, ec);
// *** 3. 设置默认运动参数 ***
robot.setMotionControlMode(MotionControlMode::NrtCommand, ec);
robot.setDefaultZone(50, ec); // 设置默认转弯区50mm
robot.setDefaultSpeed(200, ec); // 设置默认速度200mm/s
// 可选：设置运动事件回调函数
robot.setEventWatcher(Event::moveExecution, printInfo, ec);
// *** 4. 安全关闭 ***
robot.setPowerState(false, ec);
robot.disconnectFromRobot(ec);
} catch (const std::exception &e) {
print(std::cerr, e.what());
}
return 0;
}
查询机器人当前运行状态
通过接口operationState循环查询机器人的状态 ，如下是代码的简单实现
- 通过查询路径ID及路点序号是否已完成的方式
- 通过查询机械臂是否处于运动中的方式
/**
* @brief 等待运动结束 - 通过查询路径ID及路点序号是否已完成的方式
* @param robot 机器人对象
* @param traj_id 轨迹ID
* @param index 路点序号
*/
void waitForFinish(BaseRobot &robot, const std::string &traj_id, int index){
using namespace rokae::EventInfoKey::MoveExecution;
error_code ec;
while(true) {
// 查询运动执行事件信息
auto info = robot.queryEventInfo(Event::moveExecution, ec);
auto _id = std::any_cast<std::string>(info.at(ID));
auto _index = std::any_cast<int>(info.at(WaypointIndex));
// 检查是否有错误
if(auto _ec = std::any_cast<error_code>(info.at(Error))) {
print(std::cout, "路径", _id, ":", _index, "错误:", _ec.message());
return;
}
// 检查指定路径和路点是否完成
if(_id == traj_id && _index == index) {
if(std::any_cast<bool>(info.at(ReachTarget))) {
print(std::cout, "路径", traj_id, ":", index, "已完成");
}
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
}
/**
* @brief 等待运动结束 - 通过查询机械臂是否处于运动中的方式
* @param robot 机器人对象
* @param running 运行状态标志
*/
void waitRobot(BaseRobot &robot, bool &running) {
running = true;
while (running) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
error_code ec;
auto st = robot.operationState(ec);
// 如果机器人处于空闲或未知状态，则认为运动结束
if(st == OperationState::idle || st == OperationState::unknown){
running = false;
}
}
}
运动中报错
运动过程中如果出现错误，比如超限位、奇异点等，可以用如下方式接收到：
void printInfo(const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey::MoveExecution;
print(std::cout, "[运动执行信息] ID:", std::any_cast<std::string>(info.at(ID)), "Index:", std::any_cast<int>(info.at(WaypointIndex)),
"已完成: ", std::any_cast<bool>(info.at(ReachTarget)) ? "YES": "NO", std::any_cast<error_code>(info.at(Error)),
std::any_cast<std::string>(info.at(Remark)));
}
// 设置监听运动执行信息
robot.setEventWatcher(Event::moveExecution, printInfo, ec);
confdata的用法
conf的数据只适用于示教的点位，上层应用规划出来的点位没有这个数据，除非先运动到A点，再通过接口获取A点的笛卡尔坐标，才会有此数据
- 对于示教的点位，cartPosture()函数返回的结果是包含confdata的，之后下发的运动指令也要带上。
- 对于非示教点，confdata无法得知，那么需要告诉控制器不要用confdata计算逆解：
// 关闭使用conf
robot.setDefaultConfOpt(false, ec);
//开启conf的数据
CartesianPosition cartesian_position1({0.786, 0, 0.431, M_PI, 0.98, M_PI});
robot.setDefaultConfOpt(true, ec);
//如下数据一定是示教出来的点位才有的，配置conf数据
cartesian_position1.confData = {-1,1,-1,0,1,0,0,2};
/**
* @brief 严格遵循轴配置数据(conf data)的直线运动。点位适用机型xMateER3
* 演示如何使用轴配置数据来解决逆运动学多解问题
* @param robot 机器人对象
*/
void moveWithForcedConf(xMateRobot &robot) {
Toolset default_toolset;
error_code ec;
bool running;
std::string id;
// 设置工具工件坐标系
robot.setToolset(default_toolset, ec);
robot.setDefaultSpeed(200,ec);
robot.setDefaultZone(5, ec);
// 运动到拖拽位姿
print(std::cout, "运动到拖拽位姿");
robot.executeCommand({MoveAbsJCommand(Predefines::ErDragPosture)}, ec);
waitRobot(robot, running);
// 定义两个笛卡尔位置
CartesianPosition cartesian_position0({0.786, 0, 0.431, M_PI, 0.6, M_PI});
CartesianPosition cartesian_position1({0.786, 0, 0.431, M_PI, 0.98, M_PI});
// 创建运动指令
MoveJCommand j0({cartesian_position0}), j1({cartesian_position1});
MoveLCommand l0({cartesian_position0}), l1({cartesian_position1});
// 第一阶段：不严格遵循轴配置数据，MoveL & MoveJ 逆解选取当前轴角度最近解
print(std::cout, "开始MoveJ - 不遵循轴配置");
robot.moveAppend({j0, j1}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 1);
// 第二阶段：遵循轴配置数据，使用conf计算逆解
robot.setDefaultConfOpt(true, ec); // 开启轴配置选项
print(std::cerr, ec);
// 设置特定的轴配置数据
cartesian_position1.confData = {-1,1,-1,0,1,0,0,2};
l1.target = cartesian_position1;
j1.target = cartesian_position1;
print(std::cout, "开始MoveJ - 遵循轴配置");
robot.moveAppend({j0, j1}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 1);
// 第三阶段：测试MoveL指令
print(std::cout, "运动到拖拽位姿");
robot.executeCommand({MoveAbsJCommand(Predefines::ErDragPosture)}, ec);
waitRobot(robot, running);
print(std::cout, "开始MoveL - 遵循轴配置");
robot.moveAppend({l0, l1}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 1);
// 恢复默认设置
robot.setDefaultConfOpt(false, ec);
}
再HMI的新建笛卡尔点可以看到conf数据
配置轴数据逆解
注意点位必须是示教出来的
/**
* @brief 示例 - 设置轴配置数据(confData)处理多逆解问题, 点位适用机型xMateCR7
* 演示如何使用关节配置数据来获得期望的逆运动学解
* @param robot 机器人对象
*/
void multiplePosture(xMateRobot &robot) {
error_code ec;
std::string id;
// 使用默认工具工件坐标系
Toolset defaultToolset;
robot.setToolset(defaultToolset, ec);
// 设置使用confdata来计算逆解
robot.setDefaultConfOpt(true, ec);
// 创建关节运动指令
MoveJCommand moveJ({0.2434, -0.314, 0.591, 1.5456, 0.314, 2.173});
// 同样的末端位姿，不同的confData会产生不同的关节角度解
moveJ.target.confData = {-67, 100, 88, -79, 90, -120, 0, 0};
moveJ.jointSpeed = 0.1; // 设置关节速度百分比为10%
robot.moveAppend({moveJ}, id, ec);
moveJ.target.confData = {-76, 8, -133, -106, 103, 108, 0, 0};
robot.moveAppend({moveJ}, id, ec);
moveJ.target.confData = {-70, 8, -88, 90, -105, -25, 0, 0};
robot.moveAppend({moveJ}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
// 恢复默认设置
robot.setDefaultConfOpt(false, ec);
}
工具工件坐标的设定
通常机器人法兰末端会安装工具，从而工具坐标，也会设置外部参考坐标系（工件坐标系）
/**
* @brief 示例 - 使用工具工件坐标系，点位适用机型xMateCR7
* 演示如何在不同的工具工件坐标系中运动
* @param robot 机器人对象
*/
void moveInToolsetCoordinate(BaseRobot &robot) {
error_code ec;
std::string id;
// 使用默认的工具工件坐标系
robot.setToolset(Predefines::defaultToolset, ec);
// 运动到初始位置
MoveAbsJCommand moveAbs({0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0});
robot.moveAppend({moveAbs}, id, ec);
// 在默认坐标系中运动
MoveLCommand movel1({0.563, 0, 0.432, M_PI, 0, M_PI}, 1000, 100);
MoveLCommand movel2({0.33467, -0.095, 0.51, M_PI, 0, M_PI}, 1000, 100);
robot.moveAppend({movel1, movel2}, id, ec);
robot.moveStart(ec);
bool moving = true;
waitRobot(robot, moving);
// 切换到自定义工具工件坐标系
Toolset toolset1;
toolset1.ref = {{0.1, 0.1, 0}, {0, 0, 0}}; // 外部参考坐标系偏移：X+0.1m, Y+0.1m
toolset1.end = {{ 0, 0, 0.01}, {0, M_PI/6, 0}}; // 末端坐标系偏移：Z+0.01m, Ry+30°
#if 0
toolset1.load.mass = 2; // 设置负载2kg
#endif
robot.setToolset(toolset1, ec);
#if 0
// 方式2：使用已创建的工具工件
robot.setToolset("tool1", "wobj1", ec);
#endif
// 在新坐标系中运动
MoveLCommand movel3({0.5, 0, 0.4, M_PI, 0, M_PI}, 1000, 100);
robot.moveAppend({movel3}, id, ec);
robot.moveStart(ec);
waitRobot(robot, moving);
}
奇异规避
只针对非实时模式有效，实时模式无效
打开/关闭奇异点规避功能。只适用于部分机型
- 四轴锁定: 支持工业六轴，xMateCR 和 xMateSR 六轴机型；
- 牺牲姿态: 支持所有六轴机型；允许的姿态误差, 范围 (0, PI*2], 单位弧度
- 轴空间插补: 支持工业六轴机型 规避半径, 范围[0.005, 10], 单位米
/**
* @brief 锁定4轴奇异规避方式。示例适用机型xMateCR7
*/
void avoidSingularityMove_Lock4(rokae::xMateRobot &robot) {
error_code ec;
std::string id;
bool running;
robot.setToolset(Predefines::defaultToolset, ec);
// 先运动到起始位姿
robot.executeCommand({MoveAbsJCommand({0.453,0.539,-1.581,0.0,0.026,0})}, ec);
waitRobot(robot, running);
std::vector<rokae::MoveLCommand> cmds = {
MoveLCommand({0.66675437164302165, -0.23850040314585069, 0.85182031,-3.1415926535897931, 1.0471975511965979, 3.01151}),
MoveLCommand({0.66675437164302154, 0.15775146321850292, 0.464946,-3.1415926535897931, 1.0471975511965979, -2.6885547129789127})
};
// 不打开奇异规避模式, 会报错超出运动范围
robot.setAvoidSingularity(AvoidSingularityMethod::lockAxis4, false, 0, ec);
robot.moveAppend(cmds, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, (int)cmds.size() - 1);
// 打开奇异点规避模式，点位可达
// 注意，运动重置时会关闭所有奇异规避功能
robot.moveReset(ec);
robot.setAvoidSingularity(AvoidSingularityMethod::lockAxis4, true, 0, ec);
std::cerr << ec;
print(std::cout, "四轴锁定奇异规避功能", robot.getAvoidSingularity(AvoidSingularityMethod::lockAxis4, ec) ? "打开" : "关闭");
robot.moveAppend(cmds, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, (int)cmds.size() - 1);
robot.setAvoidSingularity(AvoidSingularityMethod::lockAxis4, false, 0, ec);
}
笛卡尔点位设置偏移 & 运动中暂停与继续
点位适用机型xMateEr7 Pro，演示如何设置位置偏移和在运动过程中控制机器人
void cartesianPointWithOffset(BaseRobot &robot) {
error_code ec;
// 定义基础位置
std::array<double, 6> pos = {0.631, 0, 0.38, M_PI, 0, M_PI};
std::array<double,6> offset_z = {0, 0, 0.2, 0, 0, 0}; // Z轴偏移0.2m
// 创建直线运动指令
MoveLCommand moveL1(pos, 500, 5), moveL2(pos, 800, 0);
// 设置空间旋转速度为100°/s（转换为弧度）
moveL1.rotSpeed = 100 / 180.0 * M_PI;
// 相对工件坐标系Z+偏移0.2m
moveL2.offset = { CartesianPosition::Offset::offs, offset_z};
// 创建关节运动指令
MoveJCommand moveJ1(pos, 200, 0), moveJ2(pos, 1000, 80);
// 相对工具坐标系Z+偏移0.2m
moveJ2.offset = {CartesianPosition::Offset::relTool, offset_z};
// 执行运动序列
robot.executeCommand({MoveAbsJCommand(Predefines::ErProDragPosture)}, ec); // 先到拖拽位姿
robot.executeCommand({moveL1, moveL2}, ec); // 执行直线运动
robot.executeCommand({moveJ1, moveJ2}, ec); // 执行关节运动
// 创建键盘输入线程，用于运动中控制
std::thread input([&]{
int c{};
print(os, "[p]暂停 [c]继续 [q]退出");
while(c != 'q') {
c = getchar();
switch(c) {
case 'p':
robot.stop(ec); // 暂停运动
print(std::cerr, ec); break;
case 'c':
robot.moveStart(ec); // 继续运动
print(std::cerr, ec); break;
default: break;
}
}
});
input.join();
robot.moveReset(ec); // 运动重置
}
发生碰撞恢复
不需要特别的恢复方法，等待保存诊断数据后上电 继续运行即可
模拟发生碰撞后等待20秒上电并继续运行
/**
* @brief 事件处理 - 模拟发生碰撞后等待20秒上电并继续运行
* 发生碰撞后，机器人控制器会立即记录诊断数据，需要10秒左右，记录完毕中才能开始运动。
* @param robot 机器人对象
* @param info 事件信息
*/
void recoverFromCollision(BaseRobot &robot, const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey;
bool isCollided = std::any_cast<bool>(info.at(Safety::Collided));
print(std::cout, "Collided:", isCollided);
if(isCollided) {
// 等待20秒让控制器记录诊断数据
std::this_thread::sleep_for(std::chrono::seconds(20));
error_code ec;
// 重新上电并继续运动
robot.setPowerState(true, ec);
robot.moveStart(ec);
print(std::cout, "Recovered from collision");
}
}
七轴冗余运动 & 发生碰撞检测后恢复运动
/**
* @brief 示例 - 七轴冗余运动 & 发生碰撞检测后恢复运动, 点位适用机型xMateER3 Pro
* 演示七轴机器人的冗余特性使用和碰撞恢复
* @param robot 机器人对象
*/
void redundantMove(xMateErProRobot &robot) {
error_code ec;
std::string id;
// 设置工具工件和运动参数
Toolset defaultToolset;
robot.setToolset(defaultToolset, ec);
robot.setDefaultSpeed(500, ec);
robot.setDefaultZone(0, ec); // fine模式
// 设置碰撞检测事件回调函数
robot.setEventWatcher(Event::safety, [&](const EventInfo &info){
recoverFromCollision(robot, info);
}, ec);
// ** 1) 变臂角运动 **
MoveAbsJCommand moveAbsj({0, M_PI/6, 0, M_PI/3, 0, M_PI_2, 0});
MoveLCommand moveL1({0.562, 0, 0.432, M_PI, 0, -M_PI});
moveL1.target.elbow = 1.45; // 设置臂角
robot.moveAppend({moveAbsj}, id, ec);
robot.moveAppend({moveL1}, id, ec);
moveL1.target.elbow = -1.51; // 改变臂角
robot.moveAppend({moveL1}, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
// ** 2) 60°臂角圆弧 **
CartesianPosition circle_p1({0.472, 0, 0.342, M_PI, 0, -M_PI}),
circle_p2({0.602, 0, 0.342, M_PI, 0, -M_PI}),
circle_a1({0.537, 0.065, 0.342, M_PI, 0, -M_PI}),
circle_a2({0.537, -0.065, 0.342, M_PI, 0, -M_PI});
// 所有点设置相同的60°臂角
circle_p1.elbow = M_PI/3;
circle_p2.elbow = M_PI/3;
circle_a1.elbow = M_PI/3;
circle_a2.elbow = M_PI/3;
MoveLCommand moveL2(circle_p1);
robot.moveAppend({moveL2}, id, ec);
MoveCCommand moveC1(circle_p2, circle_a1), moveC2(circle_p1, circle_a2);
std::vector<MoveCCommand> movec_cmds = {moveC1, moveC2};
robot.moveAppend(movec_cmds, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, (int)movec_cmds.size() - 1);
}
运动速度调节
/**
* @brief 示例 - 运动中调整运动速率
* 演示如何在线调整机器人的运动速度
* @param robot 机器人对象
*/
void adjustSpeed(BaseRobot &robot) {
error_code ec;
std::string id;
double scale = 0.5;
// 设置起始速度比例 50%
robot.adjustSpeedOnline(scale, ec);
// 创建往复运动序列
rokae::MoveAbsJCommand cmd1({0, 0, 0, 0, 0, 0}),
cmd2({1.5, 1.5,1.5,1.5,1.5,1.5});
robot.moveAppend({cmd1, cmd2,cmd1,cmd2,cmd1,cmd2,cmd1,cmd2}, id, ec);
robot.moveStart(ec);
bool running = true;
// 创建键盘输入线程用于速度调整
std::thread readInput([&]{
while(running) {
auto ch = std::getchar();
if(ch == 'a') {
if(scale < 0.1) {
print(std::cerr, "已达到1%");
continue;
}
scale -= 1e-1; // 减小10%
} else if(ch == 'd'){
if(scale > 1) {
print(std::cerr, "已达到100%");
continue;
}
scale += 1e-1; // 增加10%
} else {
continue;
}
// 应用速度调整
robot.adjustSpeedOnline(scale, ec);
print(os, "调整为", scale);
}
});
print(os, "机器人开始运动, 请按[a]减小速度 [d]增大速度, 步长为10%");
// 等待运动结束
waitRobot(robot, running);
readInput.join();
}
螺旋线运动MoveSPCommand
/**
* @brief 螺旋线运动，适用机型: xMate3
* 演示螺旋线轨迹生成和运动
* @param robot 机器人对象
*/
void spiralMove(rokae::xMateRobot &robot) {
error_code ec;
std::string id;
rokae::Toolset default_toolset = {};
robot.setToolset(default_toolset, ec);
// 定义螺旋线终点姿态（只用到rpy, xyz值任意）
rokae::CartesianPosition cart_target({0, 0, 0, 2.967, -0.2, 3.1415}),
cart_target1({0, 0, 0, -2.787577,0.1639,-2.9});
// 定义初始关节位置
rokae::MoveAbsJCommand absjcmd({0.0,0.22150561307150393,1.4779577696969546,0.0,1.2675963456219013,0.0});
// 螺旋线1: 初始半径0.01m, 半径变化步长0.0005m/rad, 逆时针旋转720°，速度v500
rokae::MoveSPCommand spcmd1({cart_target, 0.01, 0.0005, M_PI * 4, false, 500}),
// 螺旋线2: 初始半径0.05m, 半径变化步长0.001m/rad, 顺时针旋转360°，速度v100
spcmd2({cart_target1, 0.05, 0.001, M_PI * 2, true, 100});
std::vector<rokae::MoveSPCommand> spcmds = {spcmd1, spcmd2};
// 执行运动序列
robot.moveAppend({absjcmd}, id, ec); // 先到初始位置
robot.moveAppend(spcmds, id, ec); // 执行螺旋线运动
robot.moveStart(ec);
waitForFinish(robot, id, (int)spcmds.size() - 1);
}
全圆运动MoveCFCommand
/**
* @brief 示例 - 全圆运动，点位适配机型XMC20
* 演示三种不同类型的全圆运动
* @param robot 机器人对象
*/
void fullCircleMove(xMateRobot &robot) {
error_code ec;
// 设置默认工具工件
Toolset defaultToolset;
robot.setToolset(defaultToolset, ec);
// 起始关节角度
std::array<double, 6> start_angle = {0, 0.557737,-1.5184888, 0,-1.3036738, 0};
auto robot_model = robot.model();
// 计算起始角度对应的笛卡尔位姿
auto cart_pose = robot_model.calcFk(start_angle, ec);
// 关节绝对运动到起始位置
MoveAbsJCommand abs_j({start_angle[0], start_angle[1], start_angle[2],
start_angle[3], start_angle[4], start_angle[5]}, 1000, 0);
// 全圆指令，执行360度圆周运动
MoveCFCommand move_cf1(cart_pose, cart_pose, M_PI * 2, 100, 10);
// 设置辅助点偏移
move_cf1.auxOffset.type = CartesianPosition::Offset::offs;
move_cf1.auxOffset.frame.trans[1] = 0.01; // Y+10mm
move_cf1.targetOffset.type = CartesianPosition::Offset::offs;
move_cf1.targetOffset.frame.trans[0] = 0.005; // X+5mm
move_cf1.targetOffset.frame.trans[1] = -0.01; // Y-10mm
// 创建三种不同旋转类型的全圆运动
MoveCFCommand move_cf2 = move_cf1, move_cf3 = move_cf1;
move_cf1.rotType = MoveCFCommand::constPose; // 保持姿态不变
move_cf2.rotType = MoveCFCommand::rotAxis; // 绕旋转轴旋转
move_cf3.rotType = MoveCFCommand::fixedAxis; // 绕固定轴旋转
std::string id;
// 分别执行三种全圆运动
robot.moveAppend({abs_j}, id, ec);
robot.moveAppend({ move_cf1 }, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
robot.moveAppend({abs_j}, id, ec);
robot.moveAppend({ move_cf2 }, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
robot.moveAppend({abs_j}, id, ec);
robot.moveAppend({ move_cf3 }, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, 0);
}
运动停留指令MoveWaitCommand
可插在两条运动指令之间，前一条运动到位后，等待一段时间，再执行下一条。该指令执行完不会有信息反馈，停留时长, 最小有效时长1ms
void spiralMove(rokae::xMateRobot &robot) {
error_code ec;
std::string id;
rokae::Toolset default_toolset = {};
robot.setToolset(default_toolset, ec);
// 螺旋线终点姿态, 只用到rpy, xyz值任意
rokae::CartesianPosition cart_target({0, 0, 0, 2.967, -0.2, 3.1415}),
cart_target1({0, 0, 0, -2.787577,0.1639,-2.9});
rokae::MoveAbsJCommand absjcmd({0.0,0.22150561307150393,1.4779577696969546,0.0,1.2675963456219013,0.0});
// 螺旋线1: 初始半径0.01m, 半径变化步长0.0005m/rad, 逆时针旋转720°，速度v500
rokae::MoveSPCommand spcmd1({cart_target, 0.01, 0.0005, M_PI * 4, false, 500}),
// 螺旋线2: 初始半径0.05m, 半径变化步长0.001m/rad, 顺时针旋转360°，速度v100
spcmd2({cart_target1, 0.05, 0.001, M_PI * 2, true, 100});
auto duration1 = 100ms; // 100 毫秒
MoveWaitCommand waittime(duration1);
std::vector<rokae::MoveSPCommand> spcmds = {spcmd1, spcmd2};
robot.moveAppend({absjcmd}, id, ec);
robot.moveAppend({ waittime }, id, ec);//运动指令等待100ms再执行下一条指令
robot.moveAppend(spcmds, id, ec);
robot.moveStart(ec);
waitForFinish(robot, id, (int)spcmds.size() - 1);
}
外部轴/导轨控制
/**
* @brief 示例 - 带导轨运动。点位适配机型xMateSR4
* 演示如何控制带导轨的机器人系统
* @param robot 机器人对象指针
*/
template <WorkType wt, unsigned short dof>
void moveWithRail(Robot_T<wt, dof> *robot) {
error_code ec;
bool is_rail_enabled;
// 检查导轨功能是否已开启
robot->getRailParameter("enable", is_rail_enabled, ec);
if(!is_rail_enabled) {
print(os, "未开启导轨");
return;
}
// 设置导轨参数（需要重启控制器生效）
robot->setRailParameter("enable", true, ec); // 打开导轨功能
robot->setRailParameter("maxSpeed", 1, ec); // 设置最大速度1m/s
robot->setRailParameter("softLimit", std::vector<double>({-0.8, 0.8}), ec); // 设置软限位为+-0.8m
robot->setRailParameter("reductionRatio", 1.0, ec); // 设置减速比
auto curr = robot->BaseRobot::jointPos(ec);
print(os, "当前轴角度", robot->BaseRobot::jointPos(ec));
// *** Jog导轨示例 ***
// 切换到手动模式并上电
robot->setOperateMode(OperateMode::manual, ec);
robot->setPowerState(true, ec);
std::vector<double> soft_limit;
robot->getRailParameter("softLimit", soft_limit, ec);
// 在软限位内Jog运动
double step = (curr.back() - soft_limit[0] > 0.1 ? 0.1 : (curr.back() - soft_limit[0])) * 1000.0;
// 确定导轨轴的索引（对于6轴机器人，外部轴索引为6）
int ex_jnt_index = robot->robotInfo(ec).joint_num;
// 导轨轴空间负向运动100mm
robot->startJog(JogOpt::jointSpace, 0.6, step, ex_jnt_index, false, ec);
// 等待Jog结束
while(true) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
if(robot->operationState(ec) != OperationState::jogging) break;
}
robot->stop(ec);
// *** 带导轨的运动指令示例 ***
CartesianPosition p0({0.56, 0.136, 0.416, M_PI, 0, M_PI}),
p1({0.56, 0.136, 0.3, M_PI, 0, M_PI});
p0.external = { 0.02 }; // 导轨位置0.02m
p1.external = { -0.04 }; // 导轨位置-0.04m
MoveAbsJCommand abs_j_command({0, M_PI/6, -M_PI_2,0, -M_PI/3, 0 });
abs_j_command.target.external = { 0.1 }; // 导轨运动到0.1m
MoveJCommand j_command(p0);
MoveLCommand l_command(p1);
MoveCCommand c_command(p1, p0);
// 添加自定义信息，将在运动信息反馈中返回
l_command.customInfo = "hello";
std::string id;
robot->moveAppend(abs_j_command, id, ec);
robot->moveAppend(j_command, id, ec);
robot->moveAppend(l_command, id, ec);
robot->moveAppend(abs_j_command, id, ec);
robot->moveAppend(c_command, id, ec);
robot->moveStart(ec);
waitForFinish(*robot, id, 0);
}
力控指令操作
#include <thread>
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace rokae;
/**
* @brief 等待机器人运动完成
* @param robot 机器人指针
*/
void waitRobot(BaseRobot *robot) {
bool running = true;
while (running) {
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 每100ms检查一次
error_code ec;
auto st = robot->operationState(ec); // 获取机器人操作状态
// 当机器人处于空闲或未知状态时，认为运动完成
if(st == OperationState::idle || st == OperationState::unknown){
running = false;
}
}
}
/**
* @brief 示例 - 笛卡尔控制力控。适用机型：xMateCR
* 功能：在笛卡尔空间进行力控制，适用于需要精确力控制的场景
* Example - cartesian space force control
*/
void fcCartesianControl(xMateRobot &robot) {
error_code ec;
// 设置手持工具坐标系，绕Y轴旋转90°
// 工具坐标系定义工具末端相对于法兰盘的位置和姿态
Toolset toolset1;
toolset1.end.rpy[1] = M_PI_2; // 绕Y轴旋转90度
robot.setToolset(toolset1, ec);
// 获取力控制接口
auto fc = robot.forceControl();
// 力控初始化，使用工具坐标系
// FrameType::tool 表示在工具坐标系下进行力控制
fc.fcInit(FrameType::tool, ec);
// 设置控制类型为笛卡尔控制模式 (1=笛卡尔控制, 0=关节控制)
fc.setControlType(1, ec);
// 设置笛卡尔刚度。本示例用的是工具坐标系，所以工具坐标系的x方向0阻抗，其余方向阻抗较大
// 参数格式: {x, y, z, rx, ry, rz} - 对应6个自由度的刚度
// x方向刚度=0: 在工具x方向完全柔顺，可以自由移动
// 其他方向高刚度: 保持姿态稳定
fc.setCartesianStiffness({0, 1000, 1000, 500, 500, 500}, ec);
// 开始力控
print(std::cout , "开始笛卡尔模式力控");
fc.fcStart(ec);
#if 0
// 设置负载, 请根据实际情况设置，确保安全
// 注释掉的负载设置代码，实际使用时需要根据工具重量设置
Load load;
load.mass = 1; // 负载质量1kg
fc.setLoad(load, ec);
#endif
// 设置期望力 - 在工具坐标系的Z轴方向施加1N的力
// 参数格式: {fx, fy, fz, tx, ty, tz} - 力和力矩
fc.setCartesianDesiredForce({0, 0, 1, 0, 0, 0}, ec);
// 按回车结束力控
// 等待用户输入回车键来停止力控
while(getchar() != '\n');
fc.fcStop(ec);
}
/**
* @brief 示例 - 关节模式力控。适用机型：xMateCR
* 功能：在关节空间进行力控制，直接控制关节力矩
*/
template <unsigned short DoF>
void fcJointControl(ForceControl_T<DoF> &fc) {
error_code ec;
// 力控初始化，使用基坐标系
fc.fcInit(FrameType::base, ec);
// 设置控制类型为关节控制模式
fc.setControlType(0, ec);
// 设置各轴刚度。2轴4轴小阻抗，其余轴阻抗较大
// 小刚度的关节更容易被外力推动，大刚度的关节保持位置
fc.setJointStiffness({1000, 10, 1000, 5, 50, 50}, ec);
print(std::cout, "开始关节模式力控");
fc.fcStart(ec);
// 设置期望力矩 - 对各关节施加不同的期望力矩
// 参数: 6个关节的期望力矩值 (Nm)
fc.setJointDesiredTorque({1, 1, 3, 0, 0, 0}, ec);
// 按回车结束力控
while(getchar() != '\n');
fc.fcStop(ec);
}
/**
* @brief 示例 - 搜索运动 & 力控监控。测试机型：xMateER3
* 功能：在力控基础上叠加搜索运动，用于寻找特定位置或轮廓跟踪
*/
void fcOverlay(xMateRobot &robot){
error_code ec;
// 设置手持工具坐标系，绕Y轴旋转90°
Toolset toolset1;
toolset1.end.rpy[1] = M_PI_2;
robot.setToolset(toolset1, ec);
auto fc = robot.forceControl();
/******************************************************************
* 力控监控参数设置 - 安全保护
* 设置各种极限值，超过这些值会触发保护
******************************************************************/
// 设置最大关节速度 (rad/s)
fc.setJointMaxVel({3.0, 3.0, 3.5, 3.5, 3.5, 4.0}, ec);
// 设置最大关节动量 (kg·m²/s)
fc.setJointMaxMomentum({0.1, 0.1, 0.1, 0.055, 0.055, 0.055}, ec);
// 设置最大关节动能 (J)
fc.setJointMaxEnergy({250, 250, 250, 150, 150, 100}, ec);
// 设置笛卡尔空间最大速度 (m/s, rad/s)
fc.setCartesianMaxVel({1.0, 1.0, 1.0, 2.5, 2.5, 2.5}, ec);
// 开始监控
fc.fcMonitor(true, ec);
/******************************************************************
* 搜索运动设置
******************************************************************/
// 力控初始化
fc.fcInit(FrameType::tool, ec);
// 搜索运动必须为笛卡尔阻抗控制
fc.setControlType(1, ec);
// 设置绕Z轴的正弦搜索运动 (因为前面指定了力控坐标系为工具坐标系，所以这里是工具Z轴)
// 参数: 运动轴(2=Z轴), 频率(6Hz), 幅值类型(1), 幅值(π), 相位(1)
fc.setSineOverlay(2, 6, 1, M_PI, 1, ec);
// 开始力控
fc.fcStart(ec);
// 叠加XZ平面莉萨如搜索运动 - 产生复杂的轨迹
// 莉萨如曲线：两个垂直方向正弦运动的合成
fc.setLissajousOverlay(1, 5, 1, 10, 5, 0, ec);
// 开始搜索运动
print(std::cout, "开始搜索运动");
fc.startOverlay(ec);
#if 0
// 暂停和重新开始搜索运动 (示例代码，实际未执行)
fc.pauseOverlay(ec);
fc.restartOverlay(ec);
#endif
// 按回车结束力控
while(getchar() != '\n');
fc.stopOverlay(ec);
// 监控参数恢复到默认值
fc.fcMonitor(false, ec);
// 停止力控
fc.fcStop(ec);
}
/**
* @brief 示例 - 设置力控终止条件。测试机型：xMateER3
* 功能：设置力或位置的终止条件，当满足条件时自动停止
*/
void fcCondition(xMateRobot &robot) {
auto fc = robot.forceControl();
error_code ec;
// 设置工具坐标系，在Z方向偏移0.1m
Toolset toolset;
toolset.ref.trans[2] = 0.1;
robot.setToolset(toolset, ec);
// 力控初始化，使用世界坐标系
fc.fcInit(FrameType::world, ec);
fc.setControlType(1, ec);
fc.fcStart(ec);
// 设置力限制条件
// 参数: {fx_min, fx_max, fy_min, fy_max, fz_min, fz_max}, 使能, 超时时间
// 当测量力超出设定范围时触发终止
fc.setForceCondition({-20, 20, -15, 15, -15, 15}, true, 20, ec);
// 设置长方体区域限制条件
// isInside=false 代表在这个区域内时终止等待
// 长方体所在的坐标系，会叠加外部工件坐标系
Frame supvFrame;
supvFrame.trans[2] = -0.1; // 坐标系在Z方向偏移-0.1m
// 长方体范围: {x_min, x_max, y_min, y_max, z_min, z_max}
fc.setPoseBoxCondition(supvFrame, {-0.6, 0.6, -0.6, 0.6, 0.2, 0.3}, false, 20, ec);
// 阻塞等待满足终止条件
print(std::cout, "开始等待");
fc.waitCondition(ec);
print(std::cout, "等待结束，停止力控");
fc.fcStop(ec);
}
/**
* @brief 读取末端力矩信息
* 功能：读取并显示当前的力矩传感器数据
*/
template <unsigned short DoF>
void readTorqueInfo(ForceControl_T<DoF> &fc) {
error_code ec;
std::array<double, DoF> joint_torque{}, external_torque{};
std::array<double, 3> cart_force{}, cart_torque{};
// 读取当前力矩信息
// 从法兰盘坐标系读取力矩数据
fc.getEndTorque(FrameType::flange, joint_torque, external_torque, cart_torque, cart_force, ec);
// 打印力矩信息
print(std::cout, "末端力矩");
print(std::cout, "各轴测量力 -", joint_torque); // 关节测量力矩
print(std::cout, "各轴外部力 -", external_torque); // 外部作用力矩
print(std::cout, "笛卡尔力矩 -", cart_torque); // 笛卡尔空间力矩
print(std::cout, "笛卡尔力 -", cart_force); // 笛卡尔空间力
}
/**
* @brief 示例-力矩传感器标定
* 功能：对机器人的力矩传感器进行标定，提高力控精度
*/
void example_CalibrateForceSensor(xMateRobot &robot) {
error_code ec;
// 标定全部轴 - 对所有关节的力矩传感器进行标定
robot.calibrateForceSensor(true, 0, ec);
// 单轴(4轴)标定 - 只对指定关节进行标定
robot.calibrateForceSensor(false, 3, ec); // 3表示第4个关节（从0开始）
}
/**
* @brief 主程序
* 功能：演示机器人的各种力控制功能
*/
int main() {
error_code ec;
xMateRobot robot;
// 连接机器人
try {
robot.connectToRobot("192.168.0.160");
} catch(const Exception &e) {
std::cerr << e.what();
return 1;
}
// 获取力控制接口并读取力矩信息
auto fc = robot.forceControl();
readTorqueInfo(fc);
// 机器人上电和模式设置
robot.setOperateMode(rokae::OperateMode::automatic, ec);
robot.setPowerState(true, ec);
// 先运动到拖拽位姿, 注意选择正确的机型
// 不同机型的拖拽位姿不同，需要根据实际机型选择
std::vector<double> drag_cr = {0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0}, // CR系列拖拽位姿
drag_er = {0, M_PI/6, M_PI/3, 0, M_PI/2, 0}; // ER系列拖拽位姿
// 执行绝对关节运动到拖拽位姿
robot.executeCommand({MoveAbsJCommand(drag_cr)}, ec);
// 等待机器人运动完成
waitRobot(&robot);
// 运行示例程序
fcCartesianControl(robot); // 笛卡尔力控
fcJointControl(fc); // 关节力控
// 安全关闭
robot.setPowerState(false, ec);
robot.setOperateMode(OperateMode::manual, ec);
return 0;
}
读取末端按键状态
#include <iostream>
#include <cmath>
#include <thread>
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace rokae;
using namespace std;
/**
* @brief 机器人末端按键状态读取示例程序
* 功能：演示如何实时读取机器人末端按键的状态
* 应用场景：用于机器人示教、手动控制、状态监控等
*/
int main() {
try {
// 网络配置
std::string ip = "192.168.0.160"; // 机器人控制器IP地址
std::error_code ec; // 错误码对象，用于接收函数调用结果
// 创建机器人对象并连接
// 参数1: 机器人IP地址 (192.168.0.160)
// 参数2: 本机IP地址 (192.168.0.100) - 需要与机器人同一网段
// 注意：由于本示例使用实时状态数据，必须设置本机地址
rokae::xMateRobot robot(ip, "192.168.0.100");
/******************************************************************
* 1. 读取按键状态 - 非实时方式
* 使用getKeypadState接口一次性读取当前按键状态
******************************************************************/
KeyPadState state = robot.getKeypadState(ec);
// 打印当前所有末端按键的状态
// 通常机器人末端有多个按键，用于示教、模式切换等功能
std::cout << "当前末端按键的状态, "
<< "key1: " << state.key1_state
<< ", key2: " << state.key2_state
<< ", key3: " << state.key3_state
<< ", key4: " << state.key4_state
<< ", key5: " << state.key5_state
<< ", key6: " << state.key6_state
<< ", key7: " << state.key7_state
<< std::endl;
/******************************************************************
* 2. 配置实时状态接收
* 设置机器人以1ms周期发送状态数据，专门接收按键状态
******************************************************************/
// startReceiveRobotState: 开始接收机器人状态数据
// 参数1: 发送周期 - 1毫秒，机器人每1ms发送一次状态数据
// 参数2: 接收的数据字段 - 只接收按键状态数据，减少网络负载
robot.startReceiveRobotState(std::chrono::milliseconds(1),
{ RtSupportedFields::keypads });
/******************************************************************
* 3. 初始化按键状态数组
* 创建数组用于存储7个按键的实时状态
******************************************************************/
std::array<bool, 7> keypad{}; // 7个布尔值，对应7个末端按键
// 首次读取按键状态到数组
robot.getStateData(RtSupportedFields::keypads, keypad);
/******************************************************************
* 4. 创建实时读取线程
* 在新线程中持续读取按键状态，避免阻塞主线程
******************************************************************/
int count = 50; // 循环读取50次
// 创建线程用于实时读取按键状态
std::thread readKeyPad([&] {
// 循环50次，每次读取一次按键状态
while (count--) {
/******************************************************************
* 实时状态更新流程：
* 1. updateRobotState: 更新机器人状态缓存
* 2. getStateData: 从缓存中读取特定数据
******************************************************************/
// 第一步：更新机器人状态
// 从机器人接收最新的状态数据并更新内部缓存
// 参数：超时时间1ms，如果1ms内没有收到数据会返回错误
robot.updateRobotState(std::chrono::milliseconds(1));
// 第二步：从状态缓存中读取按键数据
// 将按键状态数据读取到keypad数组中
robot.getStateData(RtSupportedFields::keypads, keypad);
// 第三步：打印当前所有按键状态
std::cout << "当前末端按键的状态, "
<< "key1: " << keypad[0]
<< ", key2: " << keypad[1]
<< ", key3: " << keypad[2]
<< ", key4: " << keypad[3]
<< ", key5: " << keypad[4]
<< ", key6: " << keypad[5]
<< ", key7: " << keypad[6]
<< std::endl;
// 注意：这里没有延时，会以尽可能快的速度读取
// 实际应用中可能需要添加适当的延时
}
});
/******************************************************************
* 5. 等待线程结束
* 主线程等待读取线程完成50次读取后继续执行
******************************************************************/
readKeyPad.join(); // 阻塞主线程，直到readKeyPad线程执行完毕
}
catch (const std::exception& e) {
// 异常处理：捕获并打印所有异常
std::cout << "程序执行出错: " << e.what() << std::endl;
}
return 0;
}
透传协议读写控制末端工具
不同商家末端工具发送数据不同，请参照demo更改数据结构，
#include <iostream>
#include <cmath>
#include <thread>
#include <chrono>
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace rokae;
/**
* @brief DH电爪参数命名空间
* 包含所有与DH电爪通信相关的参数和数据结构
*/
namespace DhGripParams {
// Modbus RTU通信相关参数
// 初始化电爪的原始Modbus数据帧
std::vector<uint8_t> send_data = { 0x01,0x06,0x01,0x00,0x00,0xA5,0x48,0x4D };
// 接收电爪返回数据的缓冲区
std::vector<uint8_t> rev_data = { 0,0,0,0,0,0,0,0 };
// 电爪初始化参数
std::vector<int> init_set = { 0xA5 };
// 获取初始化状态的参数
std::vector<int> init_get = { 100 };
// 获取夹持状态的参数
std::vector<int> grip_status_get = { 0 };
// 力矩设置参数 (范围: 20-100)
std::vector<int> trq_set = { 100 };
// 速度设置参数 (范围: 1-100)
std::vector<int> vel_set = { 2 };
// 位置设置参数 (范围: 0-1000)
std::vector<int> pos_set = { 100 };
// 力矩读取缓冲区
std::vector<int> trq_get = { 0 };
// 速度读取缓冲区
std::vector<int> vel_get = { 0 };
// 位置读取缓冲区
std::vector<int> pos_get = { 0 };
// 实时位置读取缓冲区
std::vector<int> pos_now_get = { 0 };
}
/**
* @brief DH电爪初始化函数
* 功能：通过Modbus RTU协议初始化DH电爪
* 原理：发送特定的初始化指令序列到电爪控制器
* @param robot 机器人对象引用
*/
void DHGripInit(xMateRobot &robot) {
error_code ec;
// 方法1: 使用原始数据帧初始化（注释掉的代码）
// std::vector<uint8_t> send_data = { 0x01,0x06,0x01,0x00,0x00,0xA5,0x48,0x4D };
// robot.XPRS485SendData(send_data.size(), rev_data.size(), send_data, DhGripParams::rev_data, ec);
// 方法2: 使用Modbus RTU寄存器写操作初始化
// 参数说明：
// 0x01 - 从站地址 (电爪设备地址)
// 0x06 - 功能码 (写单个寄存器)
// 0x0100 - 寄存器地址 (初始化命令寄存器)
// "int16" - 数据类型 (16位有符号整数)
// 1 - 数据长度
// init_set - 初始化数据 {0xA5}
// false - 非阻塞模式
std::vector<int> init_set = { 0xA5 };
robot.XPRWModbusRTUReg(1, 0x06, 0x0100, "int16", 1, init_set, false, ec);
std::cout << "查询DH初始化执行结果: " << ec << std::endl;
}
/**
* @brief DH电爪运动控制函数
* 功能：设置电爪的力矩、速度和目标位置，并执行运动
* @param robot 机器人对象引用
* @param trq_set 力矩设置值 (20-100)
* @param vel_set 速度设置值 (1-100)
* @param pos_set 位置设置值 (0-1000)
*/
void DHGripMove(xMateRobot &robot, int& trq_set, int& vel_set, int& pos_set) {
error_code ec;
// 设置力矩参数
std::vector<int> trq_set_vec;
trq_set_vec.push_back(trq_set);
// 寄存器地址0x0101: 力矩设置寄存器
robot.XPRWModbusRTUReg(0x01, 0x06, 0x0101, "int16", 0x01, trq_set_vec, false, ec);
std::cout << "设置力执行结果: " << ec << std::endl;
// 设置速度参数
std::vector<int> vel_set_vec;
vel_set_vec.push_back(vel_set);
// 寄存器地址0x0104: 速度设置寄存器
robot.XPRWModbusRTUReg(0x01, 0x06, 0x0104, "int16", 0x01, vel_set_vec, false, ec);
std::cout << "设置速度执行结果: " << ec << std::endl;
// 设置位置参数
std::vector<int> pos_set_vec;
pos_set_vec.push_back(pos_set);
// 寄存器地址0x0103: 位置设置寄存器
robot.XPRWModbusRTUReg(0x01, 0x06, 0x0103, "int16", 0x01, pos_set_vec, false, ec);
std::cout << "设置位置执行结果: " << ec << std::endl;
}
/**
* @brief 获取DH电爪初始化状态
* 功能：读取电爪的初始化状态，确认初始化是否完成
* @param robot 机器人对象引用
* @param init_get 初始化状态输出参数
* 0: 初始化完成
* 1: 初始化中
* 2: 初始化失败
*/
void DHGripGetInitStatus(xMateRobot& robot, int& init_get) {
error_code ec;
std::vector<int> init_get_vec = { -1 }; // 初始化为无效值
// 寄存器地址0x0200: 初始化状态寄存器
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0200, "int16", 0x01, init_get_vec, false, ec);
init_get = init_get_vec[0];
std::cout << "获取初始化状态执行结果: " << ec << std::endl;
}
/**
* @brief 获取DH电爪夹持状态
* 功能：读取电爪当前的夹持状态
* @param robot 机器人对象引用
* @param grip_status_get 夹持状态输出参数
* 0-3: 不同的夹持状态
*/
void DHGripGetStatus(xMateRobot& robot, int& grip_status_get) {
error_code ec;
std::vector<int> grip_status_get_vec = { -1 }; // 初始化为无效值
// 寄存器地址0x0201: 夹持状态寄存器
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0201, "int16", 0x01, grip_status_get_vec, false, ec);
grip_status_get = grip_status_get_vec[0];
std::cout << "获取夹持状态执行结果: " << ec << std::endl;
}
/**
* @brief 获取DH电爪运行信息
* 功能：读取电爪当前的力矩、速度和位置信息
* @param robot 机器人对象引用
* @param trq_get 力矩读取输出参数
* @param vel_get 速度读取输出参数
* @param pos_get 位置读取输出参数
*/
void DHGripGetInfo(xMateRobot& robot, int& trq_get, int& vel_get, int& pos_get) {
error_code ec;
// 获取力矩信息
std::vector<int> trq_get_vec = { 0 };
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0101, "int16", 0x01, trq_get_vec, false, ec);
trq_get = trq_get_vec[0];
std::cout << "获取力执行结果: " << ec << std::endl;
// 获取速度信息
std::vector<int> vel_get_vec = { 0 };
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0104, "int16", 0x01, vel_get_vec, false, ec);
vel_get = vel_get_vec[0];
std::cout << "获取速度执行结果: " << ec << std::endl;
// 获取位置信息
std::vector<int> pos_get_vec = { 0 };
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0103, "int16", 0x01, pos_get_vec, false, ec);
pos_get = pos_get_vec[0];
std::cout << "获取位置执行结果: " << ec << std::endl;
}
/**
* @brief 获取DH电爪实时位置
* 功能：读取电爪的实时位置信息
* @param robot 机器人对象引用
* @param pos_now_get 实时位置输出参数
*/
void DHGripGetNewPos(xMateRobot& robot, int& pos_now_get) {
error_code ec;
std::vector<int> pos_now_get_vec = { 0 };
// 寄存器地址0x0202: 实时位置寄存器
robot.XPRWModbusRTUReg(1, 0x03, 0x0202, "int16", 1, pos_now_get_vec, false, ec);
pos_now_get = pos_now_get_vec[0];
std::cout << "获取实时位置执行状态: " << ec << std::endl;
}
/**
* @brief 线圈测试接口（针对吸盘等设备）
* 功能：测试Modbus RTU线圈读写功能
* 线圈用于控制开关量设备，如电磁阀、继电器等
*/
void CoilTest(xMateRobot& robot) {
error_code ec;
// 测试数据定义
std::vector<bool> bool_data_len1 = { 0 };
std::vector<bool> bool_data_len4 = { 0,0,0,1 };
std::vector<bool> bool_data_len10 = { 1,1,1,1,1,1,1,1,1,1 };
// 功能码0x01: 读线圈
robot.XPRWModbusRTUCoil(0x01, 0x01, 0x0001, 1, bool_data_len1, false, ec);
std::cout << "coiltest 0x01 code:" << ec << std::endl;
// 功能码0x02: 读离散输入
robot.XPRWModbusRTUCoil(0x01, 0x02, 0x0001, 1, bool_data_len1, false, ec);
std::cout << "coiltest 0x02 code:" << ec << std::endl;
// 功能码0x05: 写单个线圈
robot.XPRWModbusRTUCoil(0x01, 0x05, 0x0001, 1, bool_data_len4, false, ec);
std::cout << "coiltest 0x05 code:" << ec << std::endl;
// 功能码0x0F: 写多个线圈
robot.XPRWModbusRTUCoil(0x01, 0x0F, 0x0001, 4, bool_data_len4, false, ec);
std::cout << "coiltest 0x10 code:" << ec << std::endl;
robot.XPRWModbusRTUCoil(0x01, 0x0F, 0x0001, 10, bool_data_len10, false, ec);
std::cout << "coiltest 0x10 code:" << ec << std::endl;
}
/**
* @brief 寄存器功能码测试接口
* 功能：测试Modbus RTU寄存器读写功能
*/
void RegFuncCodeTest(xMateRobot& robot) {
error_code ec;
std::vector<int> int_data_len1 = { 0 };
std::vector<int> int_data_len3 = { 0,0,1 };
std::vector<int> int_data_len4 = { 0,0,0,1 };
// 功能码0x03: 读保持寄存器
robot.XPRWModbusRTUReg(0x01, 0x03, 0x0001, "int16", 1, int_data_len1, false, ec);
std::cout << "regtest 0x03 code:" << ec << std::endl;
// 功能码0x04: 读输入寄存器
robot.XPRWModbusRTUReg(0x01, 0x04, 0x0001, "int16", 1, int_data_len1, false, ec);
std::cout << "regtest 0x04 code:" << ec << std::endl;
// 功能码0x06: 写单个寄存器
robot.XPRWModbusRTUReg(0x01, 0x06, 0x0001, "int16", 1, int_data_len3, false, ec);
std::cout << "regtest 0x06 code:" << ec << std::endl;
// 功能码0x10: 写多个寄存器
robot.XPRWModbusRTUReg(0x01, 0x10, 0x0001, "int16", 1, int_data_len3, false, ec);
std::cout << "regtest 0x10 code:" << ec << std::endl;
robot.XPRWModbusRTUReg(0x01, 0x10, 0x0001, "int32", 1, int_data_len1, false, ec);
std::cout << "regtest 0x10 code:" << ec << std::endl;
}
/**
* @brief 寄存器数据类型测试接口（针对RM电爪）
* 功能：测试不同数据类型的寄存器读写
*/
void RegDataTypeTest(xMateRobot& robot) {
error_code ec;
std::vector<int> int16_data_len1 = { 0 };
std::vector<int> int16_data_len2 = { 0 };
std::vector<int> int32_data_len1_1 = { 65535 };
std::vector<int> int32_data_len1_2 = { 255 };
// int16数据类型测试
robot.XPRWModbusRTUReg(1, 0x03, 0x0200, "int16", 1, int16_data_len1, false, ec);
std::cout << "regdate type test int16:" << ec << std::endl;
// uint16数据类型测试
robot.XPRWModbusRTUReg(1, 0x03, 0x0200, "uint16", 1, int16_data_len1, false, ec);
std::cout << "regdate type test uint16:" << ec << std::endl;
// int32数据类型测试
robot.XPRWModbusRTUReg(1, 0x10, 0x0200, "int32", 1, int32_data_len1_1, false, ec);
std::cout << "regdate type test int32:" << ec << std::endl;
robot.XPRWModbusRTUReg(1, 0x10, 0x0200, "int32", 1, int32_data_len1_2, false, ec);
std::cout << "regdate type test int32:" << ec << std::endl;
// uint32数据类型测试
robot.XPRWModbusRTUReg(1, 0x10, 0x0200, "uint32", 1, int32_data_len1_1, false, ec);
std::cout << "regdate type test uint32:" << ec << std::endl;
robot.XPRWModbusRTUReg(1, 0x10, 0x0200, "uint32", 1, int32_data_len1_2, false, ec);
std::cout << "regdate type test uint32:" << ec << std::endl;
}
/**
* @brief 原始数据传输测试接口
* 功能：测试直接发送原始Modbus数据帧
*/
void RWData_Test(xMateRobot& robot) {
error_code ec;
// 使用原始数据帧初始化DH电爪
std::vector<uint8_t> send_data = { 0x01,0x06,0x01,0x00,0x00,0xA5,0x48,0x4D };
std::vector<uint8_t> rev_data = {};
robot.XPRS485SendData((int)send_data.size(), (int)rev_data.size(), send_data, DhGripParams::rev_data, ec);
std::cout << "裸传方式DH电爪初始化执行结果: " << ec << std::endl;
}
/**
* @brief 主程序
* 功能：演示DH电爪的完整控制流程
*/
int main() {
try {
// 网络配置
std::string ip = "192.168.21.10";
std::error_code ec;
// 创建机器人对象并连接
rokae::xMateRobot robot(ip, "192.168.21.10");
std::cout << "建立与Robot的连接" << std::endl;
// 测试函数（注释掉的代码，可根据需要启用）
// CoilTest(robot); // 线圈接口测试
// RegFuncCodeTest(robot); // 寄存器功能码测试
// RegDataTypeTest(robot); // 寄存器数据类型测试
// -------------------------- DH电爪控制主流程 --------------------------
// 步骤1: 打开末端xPanel，提供24V电源和RS485通信
std::cout << "设置xpanel中......" << std::endl;
// 参数说明：
// supply24v - 提供24V电源
// true - 启用RS485通信
robot.setxPanelRS485(xPanelOpt::Vout::supply24v, true, ec);
std::cout << "设置xpanel的结果: " << ec << std::endl;
// 步骤2: DH电爪初始化
DHGripInit(robot);
// 步骤3: 等待初始化完成（DH电爪初始化需要约7秒）
int i = 14; // 14次 * 0.5秒 = 7秒
while (i--) {
int init_status = -1;
DHGripGetInitStatus(robot, init_status);
std::cout << "获取初始化状态: "<< init_status << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
// 步骤4: 设置电爪参数并执行运动
int trq_set = 100; // 力矩设置 (20-100)
int vel_set = 2; // 速度设置 (1-100)
int pos_set = 0; // 位置设置 (0-1000)
DHGripMove(robot, trq_set, vel_set, pos_set);
// 步骤5: 读取电爪运行信息
int trq_get = 0;
int vel_get = 0;
int pos_get = 0;
DHGripGetInfo(robot, trq_get, vel_get, pos_get);
std::cout << "获取到的力: " << trq_get << std::endl;
std::cout << "获取到的速度: " << vel_get << std::endl;
std::cout << "获取到的位置: " << pos_get << std::endl;
// 步骤6: 持续监控电爪状态
while (true) {
int grip_status_get = -1;
DHGripGetStatus(robot, grip_status_get);
std::cout << "获取夹爪夹持状态: "<< grip_status_get << std::endl;
int pos_now_get = -1;
DHGripGetNewPos(robot, pos_now_get);
std::cout << "获取实时位置: " << pos_now_get << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
}
catch (const std::exception& e) {
std::cout << "程序异常: " << e.what() << std::endl;
}
return 0;
}
路径录制和回放
适合协作机器人，工业机器人不使用
注意：HMI和sdk的拖动示教保存的路径不一样，不能混合使用
#include <iostream>
#include <thread>
#include <unordered_map>
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace std;
using namespace rokae;
char parseInput(std::string &str);
void WaitRobot(BaseRobot *robot);
void printHelp();
/**
* @brief 打印运动执行信息
* 当机器人运动状态发生变化时，这个回调函数会被自动调用
* @param info 包含运动执行详细信息的结构体
*/
void printInfo(const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey::MoveExecution;
// 提取并打印运动执行的关键信息
print(std::cout, "[运动执行信息] ID:", std::any_cast<std::string>(info.at(ID)),
"Index:", std::any_cast<int>(info.at(WaypointIndex)),
"已完成: ", std::any_cast<bool>(info.at(ReachTarget)) ? "YES": "NO",
std::any_cast<error_code>(info.at(Error)),
std::any_cast<std::string>(info.at(Remark)));
}
/**
* @brief 主程序 - 机器人拖动示教与路径回放系统
* 该程序实现了一个完整的机器人拖动示教功能，包括：
* 1. 手动拖动录制路径
* 2. 路径保存和管理
* 3. 路径回放执行
* 4. 机器人状态监控
*/
int main() {
try {
// ==================== 机器人初始化连接 ====================
std::string ip = "192.168.21.10"; // 机器人控制器IP地址
error_code ec; // 错误码对象，用于接收函数调用结果
std::vector<std::string> paths; // 存储已保存路径名称的列表
// 创建xMate 6轴机器人实例并连接
xMateRobot robot(ip);
// 设置运动控制模式为非实时命令模式(NrtCommand)
// 这种模式适用于示教、路径回放等非实时性要求高的场景
robot.setMotionControlMode(MotionControlMode::NrtCommand, ec);
// 设置运动事件监听器，当运动状态变化时自动调用printInfo函数
robot.setEventWatcher(Event::moveExecution, printInfo, ec);
// 打印使用说明
printHelp();
// ==================== 主命令循环 ====================
char cmd = ' '; // 命令字符
while(cmd != 'q') { // 'q'命令退出循环
std::string str; // 存储用户输入的完整字符串
// 从控制台读取用户输入
getline(std::cin, str);
// 解析输入，提取命令字符和参数
cmd = parseInput(str);
// 根据命令字符执行相应操作
switch(cmd) {
case 'p': // 电源控制命令
if(str == "on") {
robot.setPowerState(true, ec);
std::cout << "* 机器人上电\n";
} else {
robot.setPowerState(false, ec);
std::cout << "* 机器人下电\n";
}
if(ec) break; // 如果有错误，跳出switch执行错误处理
continue; // 继续等待下一条命令
case 'm': // 模式切换命令
if(str == "manual") {
robot.setOperateMode(OperateMode::manual, ec);
std::cout << "* 手动模式\n";
} else {
robot.setOperateMode(OperateMode::automatic, ec);
std::cout << "* 自动模式\n";
}
if(ec) break;
continue;
case 'd': // 拖动功能控制
// 打开拖动的前置条件：手动模式且机器人下电状态
if(str == "on") {
// 启用笛卡尔空间自由拖动模式
robot.enableDrag(DragParameter::cartesianSpace, DragParameter::freely, ec);
cout << "* 打开拖动\n";
} else {
robot.disableDrag(ec);
std::cout << "* 关闭拖动\n";
}
if(ec) break;
continue;
case 'a': // 开始录制路径
// 开始录制路径，参数30表示最大录制时长30秒
robot.startRecordPath(30, ec);
std::cout << "* 开始录制路径\n";
if(ec) break;
continue;
case 'b': // 停止录制路径
robot.stopRecordPath(ec);
std::cout << "* 停止录制路径\n";
if(ec) break;
continue;
case 's': // 保存录制的路径
// str包含用户指定的路径名称
robot.saveRecordPath(str, ec);
std::cout << "* 保存路径为: " << str << endl;
if(ec) break;
continue;
case 'c': // 取消录制（不保存）
robot.cancelRecordPath(ec);
cout << "* 取消录制\n";
if(ec) break;
continue;
case 'u': // 查询所有已保存的路径
paths = robot.queryPathLists(ec);
if(paths.empty())
cout << "* 没有已保存的路径\n";
else {
cout << "* 已保存的路径: ";
for(auto p : paths)
cout << p << ", ";
cout << endl;
}
if(ec) break;
continue;
case 'v': // 删除指定路径
cout << "* 删除路径\"" << str << "\"\n";
robot.removePath(str, ec);
if(ec) break;
continue;
case 'r': { // 回放指定路径
// 回放路径，1.0表示100%速度回放
robot.replayPath(str, 1.0, ec);
if (ec) break;
// 开始执行回放运动
robot.moveStart(ec);
if (ec) break;
cout << "* 开始回放路径\"" << str << "\", 速率100%\n";
// 等待回放运动完成
WaitRobot(&robot);
cout << "* 回放结束\n";
continue;
}
case 'z': // 重置运动缓存
robot.moveReset(ec);
cout << "* 重置运动缓存\n";
if(ec) break;
continue;
case 'h': // 显示帮助信息
printHelp();
continue;
case 'q': // 退出程序
std::cout << " --- Quit --- \n";
continue;
default: // 无效命令
std::cerr << "无效输入\n";
continue;
}
// 错误处理：如果执行命令时发生错误，打印错误信息
cerr << "! 错误信息: " << ec.message() << endl;
}
// ==================== 程序退出清理 ====================
// 断开与机器人的连接
robot.disconnectFromRobot(ec);
} catch (const rokae::Exception &e) {
// 捕获并打印所有rokae库抛出的异常
std::cerr << "程序异常: " << e.what();
}
return 0;
}
// ==================== 全局变量和函数定义 ====================
/**
* @brief 命令映射表
* 将用户输入的命令字符串映射为单个字符，便于switch语句处理
*/
static const std::unordered_map<std::string, char> ConsoleInput = {
{"quit", 'q'}, // 退出程序
{"power", 'p'}, // 电源控制
{"drag", 'd'}, // 拖动控制
{"mode", 'm'}, // 模式切换
{"start", 'a'}, // 开始录制
{"stop", 'b'}, // 停止录制
{"save", 's'}, // 保存路径
{"cancel", 'c'}, // 取消录制
{"query", 'u'}, // 查询路径
{"remove", 'v'}, // 删除路径
{"reset", 'z'}, // 重置运动
{"replay", 'r'}, // 回放路径
{"help", 'h'} // 显示帮助
};
/**
* @brief 打印使用说明
* 显示所有可用的命令及其用法
*/
void printHelp() {
cout << " --- 拖动与路径回放使用示例 --- " << endl
<< "格式 <命令>[:参数] 例如 save:track0" << endl << endl
<< "命令 | 参数" << endl
<< "power 机器人上下电 | on|off" << endl
<< "mode 手/自动模式 | manual|auto" << endl
<< "drag 打开关闭拖动 | on|off" << endl
<< "start 开始录制路径 |" << endl
<< "stop 结束录制路径 |" << endl
<< "save 保存路径 | 路径名称" << endl
<< "cancel 取消录制 |" << endl
<< "query 查询已保存路径 |" << endl
<< "remove 删除路径 | 路径名称" << endl
<< "reset 重置运动缓存 |" << endl
<< "replay 路径回放 | 路径名称" << endl
<< "quit 结束\n" << endl
<< "使用示例:" << endl
<< " power:on - 机器人上电" << endl
<< " mode:manual - 切换到手动模式" << endl
<< " drag:on - 开启拖动功能" << endl
<< " start - 开始录制路径" << endl
<< " stop - 停止录制" << endl
<< " save:my_path - 保存路径为my_path" << endl
<< " replay:my_path - 回放路径my_path" << endl;
}
/**
* @brief 解析用户输入
* 将输入字符串解析为命令字符和参数
* @param str 用户输入的字符串（会被修改，参数部分保留在str中）
* @return 命令字符，如果无效返回' '
*/
char parseInput(std::string &str) {
size_t delimiter;
std::string cmd(str); // 默认整个字符串都是命令
// 查找分隔符':'
if((delimiter = str.find(':')) != std::string::npos) {
// 分离命令和参数
cmd = str.substr(0, delimiter); // 命令部分
str = str.substr(delimiter + 1); // 参数部分（修改原字符串）
}
// 在命令映射表中查找对应的命令字符
if(ConsoleInput.count(cmd))
return ConsoleInput.at(cmd);
else
return ' '; // 无效命令
}
/**
* @brief 等待机器人运动结束
* 通过轮询机器人操作状态来检测运动是否完成
* @param robot 机器人对象指针
*/
void WaitRobot(BaseRobot *robot) {
bool running = true; // 运动状态标志
while (running) {
// 每100毫秒检查一次状态，避免过度占用CPU
std::this_thread::sleep_for(std::chrono::milliseconds(100));
error_code ec;
auto st = robot->operationState(ec);
// 如果机器人处于空闲或未知状态，认为运动结束
if(st == OperationState::idle || st == OperationState::unknown){
running = false;
}
}
}
读取机器人状态数据
jointPos与posture的区别
//机器人当前轴角度, 机器人本体+外部轴, 单位: 弧度, 外部轴导轨,单位米
std::vector<double> rokae::BaseRobot::jointPos (error_code & ec);
//获取机器人法兰或末端的当前位姿,返回： double 数组，[X , Y , Z , Rx , Ry , Rz ]，其中平移量单位为米旋转量单位为弧度
std::array< double,6>rokae::BaseRobot::posture(CoordinateType ct, error_code & ec);
#include <thread>
#include <atomic>
#include <fstream>
#include "rokae/utility.h"
#include "rokae/robot.h"
#include "print_helper.hpp"
using namespace std;
using namespace rokae;
/**
* @brief 等待机器人运动完成
* @param robot 机器人指针
* 功能：轮询机器人操作状态，直到机器人进入空闲状态
*/
void WaitRobot(BaseRobot *robot) {
bool checking = true;
while (checking) {
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 每100ms检查一次
error_code ec;
auto st = robot->operationState(ec); // 获取机器人操作状态
// 当机器人处于空闲或未知状态时，认为运动完成
if(st == OperationState::idle || st == OperationState::unknown){
checking = false;
}
}
}
/**
* @brief 机器人状态数据记录示例程序
* 功能：在机器人运动过程中实时记录末端位姿和关节角度数据到CSV文件
* 应用场景：运动轨迹分析、性能测试、数据采集等
*/
int main() {
try {
using namespace RtSupportedFields;
/******************************************************************
* 1. 机器人初始化和连接
******************************************************************/
// 创建机器人对象并建立连接
// 参数1: 机器人IP地址 (192.168.0.160)
// 参数2: 本机IP地址 (192.168.0.100) - 需要与机器人同一网段
xMateRobot robot("192.168.0.160", "192.168.0.100");
error_code ec;
std::ostream &os = std::cout;
// 设置运动控制模式为非实时命令模式
// NrtCommand模式适用于非实时、低速控制场景
robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
/******************************************************************
* 2. 配置状态数据接收
******************************************************************/
// 开始接收机器人状态数据，设置发送间隔为1秒
// 接收的数据字段：
// - tcpPoseAbc_m: 末端位姿 (ABC欧拉角表示)
// - tau_m: 关节力矩
// - jointPos_m: 关节角度
//jointVel_m:关节速度
robot.startReceiveRobotState(chrono::seconds(1), {tcpPoseAbc_m, tau_m, jointPos_m,jointVel_m});
// 定义数据存储数组
std::array<double, 6> tcpPose{}; // 存储末端位姿 [x, y, z, A, B, C]
std::array<double, 6> arr6{}; // 存储关节角度 [j1, j2, j3, j4, j5, j6]
// 原子布尔变量，用于线程间同步
std::atomic_bool running{true};
/******************************************************************
* 3. 清空状态数据队列
******************************************************************/
// 接收状态数据的队列不会自动覆盖旧数据
// 通过循环读取的方法清除缓冲区中的旧数据，确保从最新数据开始记录
while (robot.updateRobotState(chrono::steady_clock::duration::zero()));
/******************************************************************
* 4. 创建数据记录文件
******************************************************************/
// 创建CSV文件，文件名包含时间戳以确保唯一性
std::ofstream ofs;
string filename = "read_" +
std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
std::chrono::steady_clock::now().time_since_epoch()).count()) +
".csv";
ofs.open(filename, std::ios::out);
// 写入CSV文件表头（可选）
ofs << "TCP_X,TCP_Y,TCP_Z,TCP_A,TCP_B,TCP_C,,Joint1,Joint2,Joint3,Joint4,Joint5,Joint6" << std::endl;
/******************************************************************
* 5. 创建状态数据读取线程
******************************************************************/
std::thread readState([&] {
print(os, "开始记录机器人状态数据...");
while (running) {
/******************************************************************
* 状态数据读取流程：
* 1. updateRobotState: 更新机器人状态缓存（阻塞等待新数据）
* 2. getStateData: 从缓存中读取特定类型的数据
* 3. 数据写入文件
******************************************************************/
// 周期性获取当前状态数据，超时时间设置为1秒（与数据发送间隔一致）
// 如果1秒内没有收到新数据，函数会返回超时错误
robot.updateRobotState(chrono::seconds(1));
// 读取末端位姿数据 (TCP Pose in ABC Euler angles)
// 格式: [x, y, z, A, B, C]
// x,y,z: 位置坐标 (米)
// A,B,C: 欧拉角 (弧度)
robot.getStateData(tcpPoseAbc_m, tcpPose);
// 读取关节角度数据 (Joint Positions)
// 格式: [j1, j2, j3, j4, j5, j6] (弧度)
robot.getStateData(jointPos_m, arr6);
/******************************************************************
* 数据写入CSV文件
* 格式: TCP_X,TCP_Y,TCP_Z,TCP_A,TCP_B,TCP_C,,Joint1,Joint2,Joint3,Joint4,Joint5,Joint6
******************************************************************/
ofs << tcpPose[0] << "," << tcpPose[1] << "," << tcpPose[2] << ","
<< tcpPose[3] << "," << tcpPose[4] << "," << tcpPose[5] << ",,"
<< arr6[0] << "," << arr6[1] << "," << arr6[2] << ","
<< arr6[3] << "," << arr6[4] << "," << arr6[5] << std::endl;
}
print(os, "停止记录机器人状态数据");
});
/******************************************************************
* 6. 创建机器人运动线程
******************************************************************/
std::thread moveThread([&]{
print(os, "开始机器人运动...");
// 设置机器人操作模式为自动模式
robot.setOperateMode(rokae::OperateMode::automatic, ec);
// 开启机器人电源（上伺服）
robot.setPowerState(true, ec);
/******************************************************************
* 定义运动路径点
* 使用绝对关节运动命令 MoveAbsJCommand
******************************************************************/
// 点1: 所有关节归零位置
MoveAbsJCommand p1({0, 0, 0, 0, 0, 0});
// 点2: 预设的目标位置
// 关节角度: [0, 30°, 60°, 0, 90°, 0]
MoveAbsJCommand p2({0, M_PI/6, M_PI/3, 0, M_PI_2, 0});
// 执行运动序列
std::string id; // 运动ID，用于跟踪运动状态
robot.moveAppend({p1, p2}, id, ec); // 添加运动命令到队列
robot.moveStart(ec); // 开始执行运动
// 等待运动完成
WaitRobot(&robot);
print(os, "机器人运动完成");
});
/******************************************************************
* 7. 线程同步和资源清理
******************************************************************/
// 等待运动线程完成
moveThread.join();
// 设置运行标志为false，停止数据读取线程
running = false;
// 等待数据读取线程完成
readState.join();
// 停止接收机器人状态数据
robot.stopReceiveRobotState();
// 文件流会在析构时自动关闭
print(os, "数据记录完成，文件保存为: " + filename);
} catch(const std::exception &e) {
// 异常处理
print(std::cerr, "程序执行出错: ", e.what());
}
return 0;
}
加载和运行RL工程
执行的顺序：查询工程信息（info）->加载工程（load）->机器人上电（on）->程序指针指向main（main）->开始运行工程（start）->暂停运行（pause）
#include <iostream>
#include <unordered_map>
#include "rokae/robot.h"
using namespace std;
using namespace rokae;
void printHelp();
/**
* @brief 命令映射表
* 将用户输入的命令字符串映射为单个字符，便于switch语句处理
*/
static const std::unordered_map<std::string, char> ConsoleInput = {
{"on", '0'}, // 机器人上电
{"off", 'x'}, // 机器人下电
{"quit", 'q'}, // 退出程序
{"info", 'i'}, // 查询工程信息
{"load", 'l'}, // 加载工程
{"main", 'm'}, // 程序指针指向main
{"start", 's'}, // 开始运行工程
{"pause", 'p'}, // 暂停运行
{"tool", 't'}, // 查询工具信息
{"wobj", 'w'}, // 查询工件信息
{"opt", 'o'}, // 设置运行参数
{"help", 'h'} // 显示帮助
};
/**
* @brief RL程序执行回调函数
* 当RL程序执行时，这个函数会被自动调用，显示程序执行的行号信息
* @param info 包含RL程序执行信息的结构体
*/
void rlExecutionCb(const EventInfo &info) {
using namespace EventInfoKey::RlExecution;
// 从事件信息中提取RL程序执行详情
std::string task_name = std::any_cast<std::string>(info.at(TaskName)); // 任务名称
std::string lookahead_file = std::any_cast<std::string>(info.at(LookaheadFile)); // 预读文件
int lookahead_line = std::any_cast<int>(info.at(LookaheadLine)); // 预读行号
std::string execute_file = std::any_cast<std::string>(info.at(ExecuteFile)); // 执行文件
int execute_line = std::any_cast<int>(info.at(ExecuteLine)); // 执行行号
// 打印程序执行状态信息
std::cout << "Task Name: " << task_name
<< " , Lookahead: " << lookahead_file << ", " << lookahead_line
<< ", Executing: " << execute_file << ", " << execute_line
<< std::endl;
}
/**
* @brief 主程序 - RL工程运行管理器
* 该程序实现了对机器人RL（Rokae Language）工程的管理和控制，包括：
* 1. 工程加载和运行控制
* 2. 工具和工件信息查询
* 3. 程序执行状态监控
* 4. 运行参数配置
*/
int main() {
try {
// ==================== 机器人初始化连接 ====================
std::string ip = "192.168.0.160"; // 机器人控制器IP地址
std::error_code ec; // 错误码对象，用于接收函数调用结果
// 创建xMate 6轴机器人实例并连接
rokae::xMateRobot robot(ip);
// ==================== 设置RL任务控制模式 ====================
// 设置为非实时RL任务控制模式，适用于运行预先编写好的RL程序
robot.setMotionControlMode(MotionControlMode::NrtRLTask, ec);
// 设置RL程序执行事件监听器，当程序执行时会自动调用rlExecutionCb函数
// 这样可以实时监控程序的执行进度
robot.setEventWatcher(rokae::Event::rlExecution, rlExecutionCb, ec);
// ==================== 设置操作模式 ====================
// 设置为自动模式，允许运行RL工程程序
robot.setOperateMode(OperateMode::automatic, ec);
// ==================== 主命令循环 ====================
printHelp(); // 显示使用说明
char cmd = ' '; // 命令字符
while(cmd != 'q') { // 'q'命令退出循环
std::string str; // 存储用户输入的字符串
// 从控制台读取用户输入
getline(cin, str);
// 在命令映射表中查找对应的命令字符
if(ConsoleInput.count(str)){
cmd = ConsoleInput.at(str);
} else {
cmd = ' '; // 无效命令
}
// 根据命令字符执行相应操作
switch(cmd) {
case '0': // 机器人上电
robot.setPowerState(true, ec);
cout << "* 机器人上电\n";
if(ec) break;
continue;
case 'x': // 机器人下电
robot.setPowerState(false, ec);
cout << "* 机器人下电\n";
if(ec) break;
continue;
case 'i': { // 查询工程信息
cout << "* 查询工程信息:\n";
// 获取控制器中所有的工程信息
auto infos = robot.projectsInfo(ec);
if(infos.empty()) {
cout << "无工程\n";
} else {
// 遍历并显示所有工程信息
for(auto &info : infos) {
cout << "名称: " << info.name << " 任务: ";
// 显示工程中包含的所有任务
for(auto &t: info.taskList) {
cout << t << " ";
}
cout << endl;
}
}
if(ec) break;
} continue;
case 'l': { // 加载工程
cout << "* 加载工程, 请输入加载工程名称: ";
std::string name, line, task;
vector<string> tasks;
// 读取工程名称
getline(cin, name);
cout << "请输入要运行的任务,空格分割: ";
// 读取要运行的任务列表
getline(cin, line);
istringstream iss(line);
while (iss >> task)
tasks.push_back(task);
// 加载指定工程和任务
robot.loadProject(name, tasks, ec);
if(ec) break;
continue;
}
case 'm': // 程序指针指向main
robot.ppToMain(ec);
cout << "* 程序指针指向main\n";
if(ec) break;
continue;
case 's': // 开始运行工程
robot.runProject(ec);
cout << "* 开始运行工程\n";
if(ec) break;
continue;
case 'p': // 暂停运行
robot.pauseProject(ec);
cout << "* 暂停运行\n";
if(ec) break;
continue;
case 't': { // 查询工具信息
cout << "* 查询工具信息\n";
// 获取控制器中定义的所有工具信息
auto tools = robot.toolsInfo(ec);
if(tools.empty())
cout << "无工具\n";
else {
// 显示工具详细信息
for(auto &tool : tools) {
cout << "工具: " << tool.name
<< ", 质量: " << tool.load.mass
<< "kg" << endl;
}
}
if(ec) break;
continue;
}
case 'w': { // 查询工件信息
cout << "* 查询工件信息\n";
// 获取控制器中定义的所有工件信息
auto wobjs = robot.wobjsInfo(ec);
if(wobjs.empty())
cout << "无工件\n";
else {
// 显示工件详细信息
for(auto &wobj : wobjs) {
cout << "工件: " << wobj.name
<< ", 是否手持: " << boolalpha << wobj.robotHeld
<< endl;
}
}
if(ec) break;
continue;
}
case 'o': { // 设置运行参数
cout << "* 设置运行参数, 请依次输入运行速率和是否循环([0]单次/[1]循环), 空格分隔: ";
double rate;
bool isLoop;
string line;
// 读取运行参数
getline(cin, line);
istringstream iss(line);
iss >> rate >> isLoop;
// 设置工程运行参数
// rate: 运行速率（0.0-1.0，1.0表示100%速度）
// isLoop: 是否循环运行
robot.setProjectRunningOpt(rate, isLoop, ec);
if(ec) break;
continue;
}
case 'h': // 显示帮助
printHelp();
continue;
case 'q': // 退出程序
std::cout << " --- Quit --- \n";
continue;
default: // 无效命令
std::cerr << "无效输入\n";
continue;
}
// 错误处理：如果执行命令时发生错误，打印错误信息
cerr << "! 错误信息: " << ec.message() << endl;
}
} catch (const rokae::Exception &e) {
// 捕获并打印所有rokae库抛出的异常
std::cout << "程序异常: " << e.what();
}
return 0;
}
/**
* @brief 打印使用说明
* 显示所有可用的命令及其功能描述
*/
void printHelp() {
cout << " --- 运行RL工程示例 --- \n\n"
<< "RL（Rokae Language）是机器人专用的编程语言，类似于其他机器人的脚本语言\n\n"
<< " 命令 功能说明\n"
<< " --- --------\n"
<< "on 机器人上电\n"
<< "off 机器人下电\n"
<< "info 查询控制器中所有的工程列表\n"
<< "load 加载指定的工程和任务\n"
<< "main 将程序指针指向main函数（重置程序）\n"
<< "start 开始运行已加载的工程\n"
<< "pause 暂停当前运行的程序\n"
<< "opt 设置运行参数（速度和循环模式）\n"
<< "tool 查询控制器中定义的工具信息\n"
<< "wobj 查询控制器中定义的工件信息\n"
<< "help 查看所有命令说明\n"
<< "quit 退出程序\n\n"
<< "使用流程示例:\n"
<< "1. info - 查看可用工程\n"
<< "2. load - 加载工程（按提示输入工程名和任务）\n"
<< "3. on - 机器人上电\n"
<< "4. main - 程序指针复位\n"
<< "5. start - 开始运行程序\n"
<< "6. pause/start - 暂停/继续运行\n"
<< "7. off - 运行完成后下电\n";
}
工具/工件/基坐标系标定
使用N点法标定机器人工具或工件坐标系
/**
* @brief 工具/工件/基坐标系标定类
* 用于实现机器人坐标系的标定功能，支持N点标定法
*/
template<WorkType Wt, unsigned short DoF>
class CalibrateFrame {
public:
/**
* @brief 构造函数
* @param robot 已创建好的机器人类
* @param type 标定坐标系类型（工具、工件或基坐标系）
* @param point_num 标定点数量，对应N点标定法
* @param is_held 是否机器人手持（对于工具标定为true，工件标定根据实际情况）
* @param base_aux 基坐标系标定辅助点（仅基坐标系标定时需要）
*/
CalibrateFrame(Robot_T<Wt,DoF> &robot, FrameType type, int point_num, bool is_held, const std::array<double, 3> &base_aux = {})
: robot_(&robot), type_(type), point_list_(point_num), is_held_(is_held), base_aux_(base_aux) {}
/**
* @brief 设置标定点
* 记录当前机器人关节位置作为标定点
* @param point_index 标定点索引（从0开始）
*/
void setPoint(unsigned point_index) {
if(point_index >= point_list_.size()) {
// 自行添加异常处理
print(std::cerr, "标定点下标超出范围");
return;
}
error_code ec;
// 请确保在调用前机器人对象有效
point_list_[point_index] = robot_->jointPos(ec);
}
/**
* @brief 所有标定位置已确认，得到标定结果
* 当所有标定点都设置完成后调用此函数进行计算
* @param ec 标定结果错误码
* @return 标定结果，包含坐标系变换矩阵和误差信息
*/
FrameCalibrationResult confirm(error_code &ec) {
return robot_->calibrateFrame(type_, point_list_, is_held_, ec, base_aux_);
}
private:
Robot_T<Wt, DoF> *robot_; ///< 机器人对象指针
FrameType type_; ///< 标定坐标系类型
std::vector<std::array<double, DoF>> point_list_; ///< 标定点关节位置列表
bool is_held_; ///< 是否机器人手持
std::array<double, 3> base_aux_; ///< 基坐标系标定辅助点
};
/**
* @brief 示例 - 标定工具/工件坐标系
* 使用N点法标定机器人工具或工件坐标系
* @param robot 机器人对象指针
*/
template<WorkType Wt, unsigned short DoF>
void example_calibrateFrame(Robot_T<Wt, DoF> *robot) {
int point_count = 4; // 4点标定法
// 创建工具坐标系标定对象（true表示工具由机器人手持）
Workflow::CalibrateFrame calibrate_frame(*robot, FrameType::tool, point_count, true);
// 依次设置各个标定点
for(int i = 0; i < point_count; i++) {
print(std::cout, "将机器人Jog到标定点", i+1, "，按回车确认");
while(getchar() != '\n'); // 等待用户确认
calibrate_frame.setPoint(i); // 记录当前关节位置
}
// 计算标定结果
error_code ec;
FrameCalibrationResult calibrate_result = calibrate_frame.confirm(ec);
if(ec) {
print(std::cerr, "标定失败:", ec);
} else {
print(std::cout, "标定成功，结果 -", calibrate_result.frame, "\n偏差:", calibrate_result.errors);
}
}
机械臂超出软限位后Jog回到限位内
/**
* @brief 机械臂超出软限位后Jog回到限位内
* 安全恢复功能，当机器人关节超出软限位时自动将其移回安全范围
* @param robot 机器人对象指针
*/
template<WorkType Wt, unsigned short DoF>
void recoveryFromOverJointRange(Robot_T<Wt, DoF> *robot) {
error_code ec;
auto curr_joint = robot->jointPos(ec); // 获取当前关节位置
std::array<double[2], DoF> soft_limits {}; // 存储各关节的软限位 [下限, 上限]
// 读取当前软限位设置
robot->getSoftLimit(soft_limits, ec);
std::array<double, DoF> jog_steps {}; // 各关节需要Jog的角度（弧度）
bool outofRange = false; // 标记是否有关节超出限位
// 将超出限位的轴Jog到软限位内±0.08rad (约5度)的安全边界内
double margin = 0.08; // 安全边界
for(unsigned i = 0; i < DoF; ++i) {
if(curr_joint[i] > soft_limits[i][1]) { // 超过上限
jog_steps[i] = soft_limits[i][1] - curr_joint[i] - margin;
outofRange = true;
}
if(curr_joint[i] < soft_limits[i][0]) { // 超过下限
jog_steps[i] = soft_limits[i][0] - curr_joint[i] + margin;
outofRange = true;
}
}
if(!outofRange) {
print(std::cout, "当前轴角度处于软限位内，无需恢复");
return;
}
// 安全恢复流程
// 1. 下电后, 关闭软限位（避免Jog时被软限位阻止）
robot->setPowerState(false, ec);
robot->setOperateMode(OperateMode::manual, ec); // 切换到手动模式
robot->setSoftLimit(false, ec); // 临时关闭软限位
robot->setPowerState(true, ec); // 重新上电
// 2. 依次Jog各超出限位的关节
double rate = 0.2; // Jog速率（20%）
for(unsigned i = 0; i < DoF; ++i) {
if(jog_steps[i] != 0) {
// 开始Jog运动：关节空间，指定速率，角度（转换为度），关节索引，方向
robot->startJog(JogOpt::Space::jointSpace, rate, Utils::radToDeg(abs(jog_steps[i])), i,
jog_steps[i] > 0, ec);
// 等待Jog完成
bool running = true;
while (running) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto st = robot->operationState(ec);
print(std::cout, st);
if(st == OperationState::jog){
running = false;
}
}
}
}
// 3. 恢复安全设置
robot->stop(ec); // 停止Jog
robot->setPowerState(false, ec); // 下电
// 重新打开软限位保护
robot->setSoftLimit(true, ec);
}
}
基础的信息查询
- 机器人的基本信息查询，控制版本，sdk版本，机型信息
- 查询控制器日志：queryControllerLog，上限10条
/**
* @brief 示例 - 基础的信息查询
* 演示如何查询机器人的基本状态信息
*/
template <WorkType wt, unsigned short dof>
void example_basicOperation(Robot_T<wt, dof> *robot){
error_code ec;
// *** 查询机器人基本信息 ***
auto robotinfo = robot->robotInfo(ec);
print(os, "控制器版本号:", robotinfo.version, "机型:", robotinfo.type);
print(os, "xCore-SDK版本:", robot->sdkVersion());
// *** 获取机器人当前状态 ***
auto joint_pos = robot->jointPos(ec); // 轴角度 [rad]
auto joint_vel = robot->jointVel(ec); // 轴速度 [rad/s]
auto joint_torque = robot->jointTorque(ec); // 轴力矩 [Nm]
auto tcp_xyzabc = robot->posture(CoordinateType::endInRef, ec); // 末端位姿
auto flan_cart = robot->cartPosture(CoordinateType::flangeInBase, ec); // 法兰位姿
robot->baseFrame(ec); // 基坐标系
print(os, "末端相对外部参考坐标系位姿", tcp_xyzabc);
print(os, "法兰相对基坐标系 -", flan_cart);
// 查询最近5条错误级别控制器日志
print(os, "查询控制器日志内容");
auto controller_logs = robot->queryControllerLog(5, {LogInfo::error}, ec);
for(const auto &log: controller_logs) {
print(os, log.content);
}
}
- queryControllerLog与setEventWatcher主要区别
queryControllerLog上位机主动查询，并根据日志级别查询日志信息，setEventWatcher控制器主动上报的事件，包括碰撞状态，非实时运动指令执行信息，RLRL执行状态
打开关闭拖动功能
/**
* @brief 示例 - 打开关闭拖动功能
* 演示机器人拖动示教功能
*/
void example_drag(BaseCobot *robot) {
error_code ec;
// 拖动功能前置条件：手动模式下电状态
robot->setOperateMode(rokae::OperateMode::manual, ec);
robot->setPowerState(false, ec);
// 启用笛卡尔空间自由拖动
robot->enableDrag(DragParameter::cartesianSpace, DragParameter::freely, ec);
print(os, "打开拖动", ec, "按回车继续");
std::this_thread::sleep_for(std::chrono::seconds(2)); // 等待控制模式切换完成
while(getchar() != '\n');
// 关闭拖动功能
robot->disableDrag(ec);
std::this_thread::sleep_for(std::chrono::seconds(2)); // 等待控制模式切换完成
}
读写IO和寄存器
/**
* @brief 示例 - 读写IO和寄存器
* 演示数字输入输出和寄存器的操作
*/
void example_io_register(BaseRobot *robot) {
error_code ec;
// *** 数字输入输出操作 ***
print(os, "DO1_0当前信号值为:", robot->getDO(1,0,ec));
// 设置仿真模式以模拟DI信号（仅仿真模式下可以设置DI）
robot->setSimulationMode(true, ec);
robot->setDI(0, 2, true, ec); // 设置DI0_2为true
print(os, "DI0_2当前信号值:", robot->getDI(0, 2, ec));
robot->setSimulationMode(false, ec); // 关闭仿真模式
// 读取单个float类型寄存器
float val_f;
std::vector<float> val_af;
// 读第1个寄存器（索引0）
robot->readRegister("register0", 0, val_f, ec);
// 读第10个寄存器（索引9）
robot->readRegister("register0", 9, val_f, ec);
// 读取整个寄存器数组
robot->readRegister("register0", 9, val_af, ec);
// 读取int类型寄存器数组
std::vector<int> val_ai;
robot->readRegister("register1", 1, val_ai, ec);
// 写入bool类型寄存器
robot->writeRegister("register0", 0, true, ec);
// 写入bool类型寄存器数组
std::vector<bool> val_bool_array = { false,true,false,true,false,true,false };
robot->writeRegister("register2", 0, val_bool_array, ec);
}
Jog机器人
Jog前置条件：非实时命令模式 + 手动模式
/**
* @brief 示例 - Jog机器人
* 演示手动点动控制机器人
*/
void example_jog(BaseRobot *robot) {
error_code ec;
robot->setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
robot->setOperateMode(rokae::OperateMode::manual, ec); print(os, "准备Jog机器人, 需手动模式上电, 请确认已上电后按回车键");
// 对于有外接使能开关的情况，需要按住开关手动上电
robot->setPowerState(true, ec);
// 世界坐标系下沿Z+方向运动50mm
print(os, "-- 开始Jog机器人-- \n世界坐标系下, 沿Z+方向运动50mm, 速率50%，等待机器人停止运动后按回车继续");
robot->startJog(JogOpt::world, 0.5, 50, 2, true, ec);
while(getchar() != '\n');
// 关节空间6轴负向连续转动
print(os, "轴空间，6轴负向连续转动，速率5%，按回车停止Jog");
robot->startJog(JogOpt::jointSpace, 0.05, 5000, 5, false, ec);
while(getchar() != '\n'); // 按回车停止
robot->stop(ec); // jog结束必须调用stop()停止
}
奇异点规避Jog
适用于xMateSR、xMateCR系列机型
/**
* @brief 示例 - 奇异点规避Jog，
* 在Jog过程中自动规避奇异点
*/
void example_avoidSingularityJog(xMateRobot &robot) {
error_code ec;
robot.setOperateMode(rokae::OperateMode::manual, ec);
print(os, "准备Jog机器人, 需手动模式上电, 请确认已上电后按回车键");
robot.setPowerState(true, ec);
while(getchar() != '\n');
// 使用奇异规避模式进行Jog
print(os, "-- 开始Jog机器人-- \n奇异规避模式, 沿Y+方向运动50mm, 速率20%，等待机器人停止运动后按回车继续");
robot.startJog(JogOpt::singularityAvoidMode, 0.2, 50, 1, true, ec);
while(getchar() != '\n'); // 按回车停止
robot.stop(ec); // jog结束必须调用stop()停止
}
打开和关闭碰撞检测
/**
* @brief 示例 - 打开和关闭碰撞检测
* 设置机器人的碰撞检测参数
*/
template <unsigned short dof>
void example_setCollisionDetection(Cobot<dof> *robot) {
error_code ec;
// 设置各轴碰撞检测灵敏度，范围0.01 ~ 2.0（相当于1% ~ 200%）
// 参数：各轴灵敏度数组，停止等级，回退距离
std::array<double, dof> sensitivities;
if constexpr (dof == 6) {
sensitivities = {1.0, 1.0, 1.0, 2.0, 1.0, 1.0}; // 6轴机器人
} else if constexpr (dof == 7) {
sensitivities = {1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0}; // 7轴机器人
}
robot->enableCollisionDetection(sensitivities, StopLevel::stop1, 0.01, ec);
std::this_thread::sleep_for(std::chrono::seconds(2));
// 关闭碰撞检测
robot->disableCollisionDetection(ec);
}
设置碰撞检测事件回调函数
void recoverFromCollision(BaseRobot &robot, const rokae::EventInfo &info) {
using namespace rokae::EventInfoKey;
bool isCollided = std::any_cast<bool>(info.at(Safety::Collided));
print(std::cout, "Collided:", isCollided);
if(isCollided) {
std::this_thread::sleep_for(std::chrono::seconds(20));
error_code ec;
robot.setPowerState(true, ec);
robot.moveStart(ec);
print(std::cout, "Recovered from collision");
}
}
robot.setEventWatcher(Event::safety, [&](const EventInfo &info){
recoverFromCollision(robot, info);
}, ec);
急停复位
用户按下了急停按钮，并且急停按钮旋开了情况下，可使用急停恢复
/**
* @brief 急停复位
* 在急停事件后恢复机器人状态
*/
void example_emergencyStopReset(BaseRobot *robot) {
error_code ec;
print(os, "急停复位");
robot->recoverState(1, ec); // 参数1表示恢复等级
if (ec) {
print(os, "复位失败:", ec);
} else {
print(os, "复位成功");
}
}
常用正逆解计算
注意事项：
- 上层应用规划的轨迹不使用conf数据做正逆计算，通常需要调用接口关闭Conf robot.setDefaultConfOpt(false, ec);
- 机器人法兰末端一般会挂载工具，计算正逆解时，需要设置工具工件坐标
计算接口：
- 正解：calcFk
- 逆解：calcIk
/**
* @brief 示例 - 计算正逆运动学
* 演示机器人运动学计算功能
*/
template <WorkType wt, unsigned short dof>
void example_coordinateCalculation(Robot_T<wt, dof> *robot){
error_code ec;
// 获取当前末端在参考坐标系中的位姿
auto tcp_xyzabc = robot->posture(CoordinateType::endInRef, ec);
// 获取当前工具坐标系设置
Toolset toolset1;
toolset1 = robot->toolset(ec);
print(os, "从控制器读取的工具工件坐标系:", toolset1);
auto model = robot->model(); // 获取机器人模型接口
// 在当前设置的工具工件坐标系下计算逆解
model.calcIk(tcp_xyzabc, ec);
// 在指定工具工件坐标系下计算逆解
auto ik = model.calcIk(tcp_xyzabc, toolset1, ec);
// 在当前设置的工具工件坐标系下计算正解
model.calcFk(ik, ec);
// 在指定工具工件坐标系下计算正解
auto fk_ret = model.calcFk(ik, toolset1, ec);
print(os, "目前的运动学逆解：", ik);
print(os, "运动学正解：", fk_ret);
//*** 坐标系转换：末端相对于外部参考 & 法兰相对于基坐标 ***
// 查询基坐标设置
auto base_in_world = robot->baseFrame(ec);
// 坐标系转换计算
auto flan_in_base =Utils::EndInRefToFlanInBase(base_in_world, toolset1, tcp_xyzabc);
auto flan_pos = robot->posture(CoordinateType::flangeInBase, ec);
auto end_in_ref = Utils::FlanInBaseToEndInRef(base_in_world, toolset1, flan_pos);
print(os, "输入末端相对外部参考坐标系位姿", tcp_xyzabc);
print(os, "计算得到的末端相对外部参考坐标系位姿", end_in_ref);
print(os, "输入的法兰相对基坐标系位姿", flan_pos);
print(os, "计算得到法兰相对基坐标系位姿", flan_in_base);
}
模型库的正逆解
注意事项：
SDK为用户提供了xMate运动学与动力学计算的库，方便计算机器人正逆解和雅克比矩阵等，目前支持的机型有：
- ER：xMateER系列所有机型，
- CR：XMC7/12,
- SR： XMS3/4/5（XMS5属于新机型，需要升级特殊版本的机型文件后方可使用模型库）
- AR：需升级AR机型专用的控制器版本
主要的接口包含
- 正解接口：getCartPose
- 逆解的接口
模块的库的正逆解决接口相对于CalcIK和CalcFK计算速度更快，通常1ms以内返回计算结果
#include "rokae/robot.h"
#include "print_helper.hpp"
#include "rokae/utility.h"
using namespace std;
using namespace rokae;
ostream &os = std::cout; ///< 输出流引用，用于打印到控制台
/**
* @brief xMateER7 Pro 7轴机器人模型功能演示
* 功能：展示7轴机器人的运动学、动力学模型计算功能
* 包括：雅可比矩阵、正逆运动学、速度加速度计算、力矩计算等
*/
void xMateErPro7_model(xMateModel<7> &model) {
try {
// 定义测试数据 - 7轴机器人
std::array<double, 7> zeros = {0, 0, 0, 0, 0, 0, 0}, // 零位关节角度
// 输入关节位置 (角度转换为弧度)
jointPos_in = Utils::degToRad(array<double,7>({5, 46, -10, 91, 1, -105, 11})),
// 输入关节速度 (rad/s)
jointVel_in = {0.3, 0.2, 0.5, 0.4, 0.1, 0.1, 0.1},
// 输入关节加速度 (rad/s²)
jointAcc_in = {1.3, 3.1, 4.1, 1.5, 1.6, 4.1, 8.1},
// 逆运动学初始猜测位置
jointInit = Utils::degToRad(array<double,7>({6, 45, -9, 92, 0, -103, 10})),
// 输出数据缓冲区
array1 {}, array2 {}, array3 {}, array4 {};
// 定义坐标系变换矩阵 (4x4齐次变换矩阵)
// F_TO_EE: 法兰盘到末端执行器的变换
// EE_TO_K: 末端执行器到工具的变换
std::array<double, 16> F_TO_EE = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1},
EE_TO_K = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
/******************************************************************
* 1. 雅可比矩阵计算
* 雅可比矩阵描述关节速度到末端速度的映射关系
******************************************************************/
print(os, "Jacobian -", model.jacobian(zeros));
print(os, "Jacobian given EE -", model.jacobian(zeros, F_TO_EE, EE_TO_K, SegmentFrame::endEffector));
/******************************************************************
* 2. 力矩计算（无摩擦力）
* 分解计算惯性力、科里奥利力、重力的力矩分量
******************************************************************/
model.getTorqueNoFriction(zeros, zeros, zeros, array1, array2, array3, array4);
print(os, "TorqueNoFriction full -", array1); // 总力矩
print(os, "TorqueNoFriction inertia -", array2); // 惯性力矩
print(os, "TorqueNoFriction coriolis -", array3); // 科里奥利力矩
print(os, "TorqueNoFriction gravity -", array4); // 重力矩
/******************************************************************
* 3. 正运动学计算 (Forward Kinematics)
* 根据关节角度计算机器人末端位姿
******************************************************************/
auto pos = model.getCartPose(jointPos_in);
print(os, "Flange posture -", pos); // 法兰盘位姿
/******************************************************************
* 4. 设置工具坐标系并计算末端位姿
******************************************************************/
model.setTcpCoor(F_TO_EE, EE_TO_K); // 设置末端执行器坐标系
auto pos_e = model.getCartPose(jointPos_in, SegmentFrame::endEffector);
print(os, "EE posture -", pos_e); // 末端执行器位姿
/******************************************************************
* 5. 速度和加速度计算
* 关节空间与笛卡尔空间的相互转换
******************************************************************/
// 关节空间到笛卡尔空间的速度/加速度转换
auto cartVel_out = model.getCartVel(jointPos_in, jointVel_in);
auto cartAcc_out = model.getCartAcc(jointPos_in, jointVel_in, jointAcc_in);
print(os, "Cartesian velocity -", cartVel_out);
print(os, "Cartesian acceleration -", cartAcc_out);
// 笛卡尔空间到关节空间的速度/加速度转换
print(os, "Joint velocities -", model.getJointVel(cartVel_out, jointPos_in));
print(os, "Joint accelerations -", model.getJointAcc(cartAcc_out, jointPos_in, jointVel_in));
/******************************************************************
* 6. 逆运动学计算 (Inverse Kinematics)
* 根据末端位姿计算关节角度
******************************************************************/
double psi = Utils::degToRad(-7.543); // 冗余自由度参数 (7轴机器人特有)
array<int, 8> confData = {0, 0, -1, 1, 0, -2, 0, 1}; // 构型选择参数
auto ret = model.getJointPos(pos, psi, jointInit, array1);
print(os, "IK calculation -", array1, "| ret:", ret); // 逆运动学计算结果
/******************************************************************
* 7. 验证逆运动学结果
* 使用逆运动学计算的关节角度重新计算正运动学，验证准确性
******************************************************************/
pos = model.getCartPose(array1);
print(os, "FK calculation -", pos);
/******************************************************************
* 8. 设置负载参数
* 配置末端负载，用于精确的动力学计算
******************************************************************/
double load_mass = 1.0; // 负载质量 (kg)
std::array<double, 3> load_centre = {0.1, 0.1, 0.1}, // 负载质心位置 (m)
load_inertia = {3.0, 2.0, 5.0}; // 负载惯性矩 (kg·m²)
model.setLoad(load_mass, load_centre, load_inertia);
} catch (const std::exception &e) {
print(cerr, e.what());
}
}
/**
* @brief xMateER3 6轴机器人模型功能演示
* 功能：展示6轴机器人的运动学、动力学模型计算功能
*/
void xMateEr3_model(xMateModel<6> &model) {
try {
// 定义测试数据 - 6轴机器人
std::array<double, 6> zeros {},
jointPos_in = Utils::degToRad(array<double,6>({-20, 37, 70, 0, 71, -19})),
jointVel_in = {0.3, 0.2, 0.5, 0.4, 0.1, 0.1},
jointAcc_in = {1.3, 3.1, 4.1, 1.5, 1.6, 4.1},
jointInit = Utils::degToRad(array<double,6>({-21, 38, 71, 0, 70, -20})),
array1 {}, array2 {}, array3 {}, array4 {};
std::array<double, 16> F_TO_EE = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1},
EE_TO_K = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
/******************************************************************
* 与7轴版本相同的功能演示，但针对6轴机器人
******************************************************************/
print(os, "Jacobian -", model.jacobian(zeros));
print(os, "Jacobian given EE -", model.jacobian(zeros, F_TO_EE, EE_TO_K, SegmentFrame::endEffector));
model.getTorqueNoFriction(zeros, zeros, zeros, array1, array2, array3, array4);
print(os, "TorqueNoFriction full -", array1);
print(os, "TorqueNoFriction inertia -", array2);
print(os, "TorqueNoFriction coriolis -", array3);
print(os, "TorqueNoFriction gravity -", array4);
print(os, "Torque inertia -", model.getTorque(zeros, zeros, zeros, TorqueType::inertia));
auto pos = model.getCartPose(jointPos_in);
print(os, "Flange posture -", pos);
model.setTcpCoor(F_TO_EE, EE_TO_K);
auto pos_e = model.getCartPose(jointPos_in, SegmentFrame::endEffector);
print(os, "EE posture -", pos_e);
auto cartVel_out = model.getCartVel(jointPos_in, jointVel_in);
auto cartAcc_out = model.getCartAcc(jointPos_in, jointVel_in, jointAcc_in);
print(os, "Cartesian velocity -", cartVel_out);
print(os, "Cartesian acceleration -", cartAcc_out);
print(os, "Joint velocities -", model.getJointVel(cartVel_out, jointPos_in));
print(os, "Joint accelerations -", model.getJointAcc(cartAcc_out, jointPos_in, jointVel_in));
double psi = Utils::degToRad(-7.543);
array<int, 8> confData = {0, 0, -1, 1, 0, -2, 0, 1};
auto ret = model.getJointPos(pos, psi, jointInit, array1);
print(os, "IK calculation -", array1, "| ret:", ret);
pos = model.getCartPose(array1);
print(os, "FK calculation -", pos);
// 设置负载 (6轴版本使用更大的负载)
double load_mass = 2.0;
std::array<double, 3> load_centre = {0.1, 0.1, 0.1}, load_inertia = {3.0, 2.0, 5.0};
model.setLoad(load_mass, load_centre, load_inertia);
} catch (const std::exception &e) {
print(cerr, e.what());
}
}
/**
* @brief 主程序
* 功能：根据配置选择测试6轴或7轴机器人模型
*/
int main() {
bool test_Er3 = false; // 测试模式选择: false-7轴, true-6轴
try {
std::string ip = "192.168.21.10";
if(test_Er3) {
// 测试6轴机器人 xMateER3
rokae::xMateRobot robot(ip); // 创建6轴机器人对象
auto model = robot.model(); // 获取机器人模型 (xMateModel<6>)
xMateEr3_model(model); // 执行6轴模型功能演示
} else {
// 测试7轴机器人 xMateER7 Pro
rokae::xMateErProRobot robot(ip); // 创建7轴机器人对象
auto model = robot.model(); // 获取机器人模型 (xMateModel<7>)
xMateErPro7_model(model); // 执行7轴模型功能演示
}
} catch (const std::exception &e) {
print(cerr, e.what());
}
}
3. VS2022上如何使用SDK的库
1. 新建解决方案
2. 确认SDK的库位置
注意库的路径和解决方案保持同一目录下
3. 添加头文件和库
4. 运行生成exe,成功运行程序
把对应的dll拷贝到生成的exe目录下
4. qt环境下使用sdk库
前提
目前只有qt6的版本支持Cmakefile.txt文件，当前的项目基于如下环境创建的，
新建工程
按照qtcreator的新建工程普通的工程
Cmakefile.txt添加库和路径
注意当前构建的debug还是release版本，如搞混了，会出现链接库的问题
set(Robot_Include_DIR "${CMAKE_SOURCE_DIR}/rokae/include/rokae")
set(Robot_Lib_DIR "${CMAKE_SOURCE_DIR}/rokae/lib/Windows/Release/64bit")
set(Eigen_DIR "${CMAKE_SOURCE_DIR}/rokae/external")
# 1. 添加头文件搜索路径
include_directories(
${Robot_Include_DIR}/
${Eigen_DIR}/
${Eigen_DIR}/Eigen
)
# 2. 添加库文件搜索路径
link_directories(
${Robot_Lib_DIR}/
)
# 3. 设置库文件名称（根据平台）
find_library(robotsdk NAMES xCoreSDK PATHS ${CMAKE_SOURCE_DIR}/rokae/lib/Windows/Release/64bit)
target_link_libraries(RobotTest
PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
# PRIVATE ${robotsdk}
PRIVATE ${CMAKE_SOURCE_DIR}/rokae/lib/Windows/Release/64bit/xCoreSDK_static.lib
PRIVATE ${CMAKE_SOURCE_DIR}/rokae/lib/Windows/Release/64bit/xMateModel.lib
)
构建版本
