servoJoint 接口应用

1. 使用步骤

在使用 servoJoint 接口时,请严格按照下述步骤进行操作，若下述步骤中有某一步骤在操作中有问题，则不能使用 servoJoint 功能，请联系相关工程师进行排查解决；

1.1 检查应用环境

请使用网线直连，目的是减小网络延迟；
请使用实时系统，查看是否是实时系统，如果不是，则需要安装实时内核；
- 打开终端。
- 运行命令:
```
uname -r #此命令将显示当前正在运行的内核版本号
```
- 如果内核版本号的末尾包含 -rt (例如: 4.14.195-rt94 ),则表示该内核已经安装了实时补丁；如果没有 -rt 标记，则当前机器上没有实时补丁。

1.2 移动示教测试

在上电并释放抱闸后，请在示教器的移动示教界面进行操作，观察机器臂能否正常移动示教，并且运动过程中没有异响、抖动等现象；

1.3 运行工程测试

新建一个简单工程，运行后观察机器臂能否正常运动，并且运动过程中没有异响、抖动等异常现象；

1.4 运行servoj示例测试

使用 SDK 中提供的示例进行测试，观察进行 servoJoint 控制时机器臂能否正常运动，主要看运动过程中是否有抖动现象，如果有抖动现象，或者运动过程中会自动停止，请调参，适当增大 t 的值；

注意安全：进行 servoJoint 控制时，请提前把机器臂周围的障碍物移开；
SDK 示例在 SDK 的包里，包括 C++ 示例和 Python 示例；
SDK下载： https://download.aubo-robotics.cn/sdk/

注意：示例下载后不一定能直接运行，使用时请简单看下示例的逻辑并做适当修改，比如轨迹文件路径和调用的方法函数；

1.5 集成 SDK 示例代码逻辑

按照上面的测试步骤操作都没有问题时，接下来可按照示例代码逻辑把 servoJoint 功能集成到个人的项目中；关键逻辑如下：

使用 moveJoint 接口运动到轨迹中的第一个点(serovJoint控制的初始点);
```c++ // 接口调用: 关节运动到轨迹中的第一个点，否则容易引起较大超调 robot_interface->getMotionControl()->moveJoint(traj[0], 80 * (M_PI / 180),
```
                                        60 * (M_PI / 180), 0, 0);
```

// 阻塞 int ret = waitArrival(robot_interface); if (ret == 0) { std::cout << "关节运动到轨迹文件中的第一个路点成功" << std::endl; } else { std::cout << "关节运动到轨迹文件中的第一个路点失败" << std::endl; }


2. 使用 `setServoMode(true)` 开启 servoJoint 模式（只在开始控制时执行一次）;  

```c++
// 接口调用: 开启 servo 模式
cli->getRobotInterface(robot_name)->getMotionControl()->setServoMode(true);

// 等待进入 servo 模式
int i = 0;
while (!cli->getRobotInterface(robot_name)
       ->getMotionControl()
       ->isServoModeEnabled()) {
    if (i++ > 5) { // 若Servo 模式使能失败!可适当调大该判断条件值
        std::cout << "Servo 模式使能失败! 当前的 Servo 模式是 "
            << cli->getRobotInterface(robot_name)
            ->getMotionControl()
            ->isServoModeEnabled()
            << std::endl;
        return -1;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

调用 servoJoint 接口开始进行运动控制（控制过程中可执行多次）；

// 接口调用: 关节伺服运动
cli->getRobotInterface(robot_name)
    ->getMotionControl()
    ->servoJoint(traj[i], 0.1, 0.2, 0.06, 0., 0.);

// sleep的作用是模拟调用 servoJoint 的间隔时间，根据实际情况可删除该行
std::this_thread::sleep_for(std::chrono::milliseconds(5));

结束时使用setServoMode(false)退出 servoJoint 模式（只在结束时执行一次）;

// 接口调用: 关闭 servo 模式
cli->getRobotInterface(robot_name)->getMotionControl()->setServoMode(false);

// 等待结束 servo 模式
while (cli->getRobotInterface(robot_name)
       ->getMotionControl()
       ->isServoModeEnabled()) {
    if (i++ > 5) {
        std::cout << "Servo 模式失能失败! 当前的 Servo 模式是 "
            << cli->getRobotInterface(robot_name)
            ->getMotionControl()
            ->isServoModeEnabled()
            << std::endl;
        return -1;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

2. 常见问题解决

2.1 servoJoint 的接口参数含义

servoJoint 接口说明参考：https://docs.aubo-robotics.cn/aubo_sdk_docs/

2.2 servoJoint 相关日志太多

在使用 servoJoint 功能时，发现 servoJoint 相关日志记录的过多，其中包含了需要算法计算的相关日志，可以通过配置来选择是否记录 servoJoint 的算法相关日志，从而减少日志记录内容；

// 配置项如下：
debug = false // 表示关闭记录算法相关日志
debug = true // 表示开启记录算法相关日志

在 ~/arcs_ws/conf/aubo_control.conf 中进行配置：

效果：
配置前

配置后

2.3 自动退出 servoJoint 模式

ServoJ 功能有自动退出 servoJoint 模式的机制，退出 servoJoint 模式后，不能继续控制机器臂运动，只能重新进入 servoJoint 模式；

自动退出机制包括：

通过 SDK 调用 stopJoint/stopLine/abort 等接口，会自动退出 servo 模式；
当进入了拖动模式，会自动退出 servoJoint 模式；
长时间没有收到 servoJoint 指令（ t 的 5 倍）,会自动退出 servoJoint 模式；
当退出 servoJoint 模式后，要想继续使用 ServoJ 功能，必须使用 SDK 调用 setServoMode(true) 重新进入 servoJoint 模式；

自动退出时记录的日志如下:

2.4 servoJoint 运动失败

查看 aubo_control.log 日志，看是否是自动退出了 servoJoint 模式，可以适当增大参数 t 值进行测试；
查看示教器是否有弹窗报异常，比如碰撞等，可以根据示教器弹窗提示进行操作解决；

2.5 如何采集 servoJoint 运行的诊断数据

在进行 servoJoint 控制时，发现机器臂抖动，或者运行的轨迹不对等情况，如有需要，请使用以下接口采集诊断数据，可自行分析，也可发给相关研发人员；

参考代码示例：

// 调用 startRecord 接口开始记录诊断数据（只开始控制时执行一次），可将这条语句放在 setServoMode(true) 之后
robot_interface->getRobotManage()->startRecord("servoj_test.csv");

// 调用 servoJoint 接口开始进行运动控制
for (size_t i = 1; i < traj.size(); i++) {
    // 接口调用: 关节伺服运动
    cli->getRobotInterface(robot_name)
        ->getMotionControl()
        ->servoJoint(traj[i], 0.1, 0.2, 0.06, 0, 0);

    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

// 调用 stopRecord 接口结束记录诊断数据（只结束控制时执行一次），可将这条语句放在 setServoMode(false) 之前
robot_interface->getRobotManage()->stopRecord();

整个运动控制流程结束后，诊断数据会生成在~/arcs_ws/log/.trace 文件夹下：

注意：.trace文件夹是个可隐藏的文件夹；

53-servoJoint 接口应用