🤖 AI assistants / automation: start with AI Entry Page · Agent Development Guide · Pre-generation Checklist (zh); index: /llms.txt; SKILL: SKILL.md

servo_move()를 사용한 추적 운동

servo_move(SOCKETFD socketFd,ServoMovePara servoMove)는 포인트 그룹을 수신한 후 timeStamp에 따라 수신한 포인트를 평활 처리하여 서보 운동으로 전송할 수 있습니다.

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;     ///< 2차원 배열. 1차원은 이번 전송의 포인트 수를 나타내고, 2차원 길이는 7이며 각 관절 각도 또는 Cartesian 좌표를 나타냄
  std::vector<std::vector<double>> axisvel; ///< 2차원 배열. 1차원은 이번 전송의 포인트 수를 나타내고, 2차원 길이는 7이며 각 축의 속도를 나타냄
  std::vector<std::vector<double>> axisacc; ///< 2차원 배열. 1차원은 이번 전송의 포인트 수를 나타내고, 2차원 길이는 7이며 각 축의 가속도를 나타냄
  std::vector<double> timeStamp;            ///< 이번 전송의 포인트 수와 같은 길이이며, 해당 포인트에 도달하는 시간을 나타냄. 단위 ms
};

사용 전제: 작업 파일과 함께 사용해야 함

servo_move()는 작업 파일 없이 단독으로 사용할 수 없습니다. 컨트롤러 쪽에 실행 중인 작업이 있어야 하고, 그 작업에 외부 포인트(서보 추적) 명령이 포함되어야 상위 컴퓨터가 7000 포트로 전송한 포인트가 실행됩니다.

일반적인 사용 방식은 하나의 고정 공정 작업 안에 상위 컴퓨터가 실시간으로 생성하는 궤적을 삽입하는 것입니다. 작업 파일이 흐름의 골격(원점 복귀, 픽업/배치, IO 시퀀스 등 고정 동작)을 담당하고, 센서·비전 결과에 따라 달라져야 하는 구간은 상위 컴퓨터가 실시간으로 포인트를 생성해 servo_move()로 전송합니다.

티칭 펜던트에서 이 작업 파일을 실행한 뒤, 상위 컴퓨터에서 servo_move()로 포인트를 전송합니다.

매개변수 값(헤더 주석 기준)

매개변수	값	의미
targetMode	0 / 1	0-독립 포인트, 1-연속 궤적
sendMode	0 / 1	0-전체 궤적 포인트를 한 번에 전송, 1-포인트 일부를 나눠서 전송
runMode	0 / 1	0-모두 수신 후 운동, 1-수신하면서 운동
coord	0 / 1	0-관절, 1-직교
clearBuffer	true / false	이전에 전송했지만 보간 계산을 시작하지 않은 포인트를 삭제할지 여부

아래 두 예시는 모두 targetMode = 0(독립 포인트), sendMode = 0(이번 프레임의 전체 포인트를 한 번에 전송)을 사용합니다. 포인트 사이를 연속 궤적으로 연결하려면 targetMode를 1로 설정하십시오.

예시 1: 프레임 단위로 포인트를 계속 전송하여 원격 조작

#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;  // 이전 프레임에서 보간을 시작하지 않은 포인트를 삭제
      param.targetMode = 0;   // 0-독립 포인트, 1-연속 궤적
      param.sendMode = 0;     // 0-전체 궤적 포인트를 한 번에 전송, 1-포인트 일부를 나눠서 전송
      param.runMode = 0;      // 0-모두 수신 후 운동, 1-수신하면서 운동
      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;
}

예시 2: 독립 포인트 궤적을 사용하고, 전체 궤적 포인트를 한 번에 전송(평활화 없음)

#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;  // 이전 프레임에서 보간을 시작하지 않은 포인트를 삭제
    param.targetMode = 0;   // 0-독립 포인트, 1-연속 궤적
    param.sendMode = 0;     // 0-전체 궤적 포인트를 한 번에 전송, 1-포인트 일부를 나눠서 전송
    param.runMode = 0;      // 0-모두 수신 후 운동, 1-수신하면서 운동
    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;
}