纳博特科技开放平台

Appearance

使用 servo_move() 来进行跟踪运动 ​

servo_move(SOCKETFD socketFd,ServoMovePara servoMove) 可以接收一组点，然后将接收到的点位根据 timeStamp做平滑处理后下发到伺服运动。

cpp
struct ServoMovePara {
  ///< 传输参数
  bool clearBuffer;                         ///< 是否清除之前发送的，未开始插补计算的点位
  int targetMode;                           ///< 0-独立点 1-连续轨迹
  int sendMode;                             ///< 0-一次传输完全部轨迹 1-一次传输部分点位
  int runMode;                              ///< 0-接收完再运动 1-边接受边运动
  int sum;                                  ///< 总传输次数
  int count;                                ///< 当前是第几次传输
  ///< 运动参数
  int coord;                                ///< 0-关节 1-直角
  int size;                                 ///< 本次传输的点位数
  std::vector<std::vector<double>> pos;     ///< 二维数组，一维表示本次传输的点位数，二维长度为7，各个关节角度或笛卡尔坐标
  std::vector<std::vector<double>> axisvel; ///< 二维数组，一维表示本次传输的点位数，二维长度为7，各个轴的速度
  std::vector<std::vector<double>> axisacc; ///< 二维数组，一维表示本次传输的点位数，二维长度为7，各个轴的加速度
  std::vector<double> timeStamp;            ///< 长度为本次传输的点位数，表示到达该点位的时间，单位ms
};

使用前提：需要在示教器上运行外部点的指令 ​

启动运行这个作业文件

示例一：使用连续点位轨迹，每次传输部分点位来进行遥操作 ​

cpp
#include <iostream>
#include <vector>
#include <chrono>
#include "cpp_interface/nrc_api.h"


int main() {
  ServoMovePara param;


  // 目标点位数据
  double axis1_pos = 1;
  double axis2_pos = 2;
  
  // 查询运动前坐标
  std::vector<double> pos(7);
  get_current_position(fd, 0, pos);
  std::cout << "关节运动前坐标：" << pos[0] << " " << pos[1] << " " << pos[2] << " " 
            << pos[3] << " " << pos[4] << " " << pos[5] << " "  << pos[6] << std::endl;
  
  for (int k = 0; k < 4000; k++) {
      // 生成目标点位数据
      for (int i = 0; i < 2; i++) {
          param.pos.push_back({pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]});      //目标点位数据
          std::cout << "关节角=" << param.pos[i][6] << std::endl;
  
          if (k < 100) {
              pos[5] += 0.05;
          } else if (k >= 100 && k < 200) {
              pos[5] -= 0.05;
          } else if (k >= 200 && k < 300) {
              pos[5] += 0.05;
          } else {
              pos[5] -= 0.05;
          }
  
          std::cout << "k=" << k << std::endl;
      }
  
      std::cout << "大小=" << param.pos.size() << std::endl;
  
      // 设置每个轴的速度
      for (int i = 0; i < param.pos.size(); i++) {
          param.axisvel.push_back({1, 1, 1, 1, 1, 50, 1});     //到达目标点位的每个轴的速度，单位度每秒
      }
  
      // 设置每个轴的加速度
      for (int i = 0; i < param.pos.size(); i++) {
          param.axisacc.push_back({1, 1, 1, 1, 1, 50, 1});       //到达目标点位的每个轴的加速度
      }
  
      // 设置时间戳
      double times = 5;     //运行到该点位时，相对于起始点的时间
      for (int i = 0; i < param.pos.size(); i++) {
          param.timeStamp.push_back(times);
          times += 5;        //假设后续每一个点位都相较于上一个点多5ms,最终timeStamp数组是{5,10,15,20...}
      }
  
      // 设置参数
      param.clearBuffer = true;  // 使用连续运动时，clearBuffer，targetMode，sendMode，runMode需要按照这个固定格式赋值
      param.targetMode = 0;   // 连续轨迹
      param.sendMode = 0;     // 一次传输完全部轨迹点位
      param.runMode = 0;
      param.coord = 0;        // 关节坐标
      param.sum = 100000;        // 因为需要持续发送点位，我们假设总的发送次数很大
      param.count = 100000;        // 因为需要持续发送点位，我们假设总的发送次数很大
      param.extMove = 0;
      param.size = param.pos.size();
  
      // 调用运动接口
      auto t_start = std::chrono::high_resolution_clock::now();
      std::cout << "servo_move return: " << servo_move(fd, param) << std::endl;
  
      auto t_stop = std::chrono::high_resolution_clock::now();
      auto t_duration = std::chrono::duration<double>(t_stop - t_start);
      std::cout << "t_duration: " << t_duration.count() << std::endl;
  
      // 延时补偿
      if (t_duration.count() < times / 1000 + 0.01) {
          std::this_thread::sleep_for(std::chrono::duration<double>(times / 1000 + 0.01 - t_duration.count()));
      }
  
      // 清空参数
      param.pos.clear();
      param.axisvel.clear();
      param.axisacc.clear();
      param.timeStamp.clear();
  }
  return 0;
}

示例二：使用独立点位轨迹，，一次性传输完全部轨迹点位（没有平滑） ​

cpp
#include <iostream>
#include <vector>
#include <chrono>
#include "cpp_interface/nrc_api.h"


int main() {
  ServoMovePara param;


// 目标点位数据
double axis1_pos = 1;
double axis2_pos = 2;


// 查询运动前坐标
std::vector<double> pos(7);
get_current_position(fd, 0, pos);
std::cout << "关节运动前坐标：" 
          << pos[0] << " " << pos[1] << " " << pos[2] << " " 
          << pos[3] << " " << pos[4] << " " << pos[5] << " " 
          << pos[6] << std::endl;


for (int k = 0; k < 4000; k++) {
    // 生成目标点位数据
    for (int i = 0; i < 2; i++) {
        param.pos.push_back({pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]});      //目标点位数据
        std::cout << "关节角=" << param.pos[i][6] << std::endl;


        if (k < 100) {
            pos[5] += 0.05;
        } else if (k >= 100 && k < 200) {
            pos[5] -= 0.05;
        } else if (k >= 200 && k < 300) {
            pos[5] += 0.05;
        } else {
            pos[5] -= 0.05;
        }


        std::cout << "k=" << k << std::endl;
    }


    std::cout << "大小=" << param.pos.size() << std::endl;


    // 设置每个轴的速度
    for (int i = 0; i < param.pos.size(); i++) {
        param.axisvel.push_back({1, 1, 1, 1, 1, 50, 1});     //到达目标点位的每个轴的速度，单位度每秒
    }


    // 设置每个轴的加速度
    for (int i = 0; i < param.pos.size(); i++) {
        param.axisacc.push_back({1, 1, 1, 1, 1, 50, 1});       //到达目标点位的每个轴的加速度
    }


    // 设置时间戳
    double times = 5;     //运行到该点位时，相对于起始点的时间
    for (int i = 0; i < param.pos.size(); i++) {
        param.timeStamp.push_back(times);
        times += 5;        //假设后续每一个点位都相较于上一个点多5ms,最终timeStamp数组是{500,550,600,650...}
    }


    // 设置参数
    param.robotNum = 1;
    param.clearBuffer = true;  // 使用连续运动时，clearBuffer，targetMode，sendMode，runMode需要按照这个固定格式赋值
    param.targetMode = 0;   // 独立点位
    param.sendMode = 0;     // 一次传输完全部轨迹点位
    param.runMode = 0;
    param.coord = 0;        // 关节坐标
    param.size = param.pos.size();
    // 调用运动接口
    std::cout << "servo_move return: " << servo_move(fd, param) << std::endl;
    // 延时5ms
    std::this_thread::sleep_for(std::chrono::milliseconds(5));


    // 清空参数
    param.pos.clear();
    param.axisvel.clear();
    param.axisacc.clear();
    param.timeStamp.clear();
}
  return 0;
}