star_servo_point_position_motion_control()을 사용한 서보 제어 운동
서보 제어 운동 모드는 각 통신 주기마다 포인트를 서보에 직접 전송하여 운동합니다
cpp
struct ServoPointMovePara
{
\tbool end; ///< 이전에 전송했지만 보간 계산을 시작하지 않은 포인트를 삭제할지 여부
\tint sum; ///< 총 전송 프레임 수
\tint count; ///< 현재 몇 번째 프레임인지
\tstd::vector<std::vector<double>> pos; ///< 2차원 배열. 1차원은 이번 전송의 포인트 수를 나타내고, 2차원 길이는 12이며 각 관절 각도 또는 Cartesian 좌표를 나타냄 좌표: 로봇(앞 7개) + 외부 축(뒤 5개)
};주의: 전송된 데이터는 총 프레임 수 sum = count일 때만 로봇이 동작합니다. 따라서 sum≠1인 경우 count를 1부터 전송하고 sum 값까지 계속 누적해야 로봇이 해당 전송 포인트에 따라 운동을 시작합니다(구체적인 내용은 예시 2 참조. 예시 2는 sum = 10인 경우로, count = 1을 인터페이스 servo_point_position_motion_control에 전달하고, count = 2를 servo_point_position_motion_control에 전달하며, count = 3 .......... count = 10을 인터페이스 servo_point_position_motion_control에 전달한 후에야 전송한 모든 포인트가 실행되기 시작합니다.
예시 1: servo_point_position_motion_control() 인터페이스를 캡슐화하여 목표 포인트를 서보에 전송하고 운동합니다. 매번 포인트 하나만 서보에 전송
cpp
void test_servo_point_position_motion_control()
{
ServoPointMovePara servoMove;
servoMove.end = 0;
servoMove.sum = 1;
servoMove.count = 1;
std::vector<double> pos = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; //앞 7개는 로봇 본체, 뒤 5개는 외부 축
for (int i = 0; i < 100000; i++)
{
servoMove.pos.push_back({pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
, pos[7], pos[8], pos[9], pos[10], pos[11]});
std::cout << "servo_point_position_motion_control return " << servo_point_position_motion_control(fd_7000, servoMove) << std::endl;
servoMove.pos.clear();
pos[0] += 0.001;
pos[8] += 0.001;
printf("pos : %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f", pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
, pos[7], pos[8], pos[9], pos[10], pos[11]);
}
}예시 2: servo_point_position_motion_control() 인터페이스를 캡슐화하여 목표 포인트를 서보에 전송하고 운동합니다. 매번 최대 600개의 포인트 전송 가능
cpp
void test_servo_point_position_motion_control()
{
ServoPointMovePara servoMove;
servoMove.end = 0;
servoMove.sum = 10;
servoMove.count = 1;
std::vector<double> tar_pos;
std::vector<double> pos_sync;
get_current_position(fd, 0, tar_pos);
get_current_extra_position(fd, pos_sync);
std::vector<double> pos = {tar_pos[0], tar_pos[1], tar_pos[2], tar_pos[3], tar_pos[4], tar_pos[5], tar_pos[6],
pos_sync[0], pos_sync[1], pos_sync[2], pos_sync[3], pos_sync[4]}; //앞 7개는 로봇 본체, 뒤 5개는 외부 축
for (int i = 0; i < 10; i++)
{
for (int j = 0; j < 600; j++) //최대 600개의 포인트 저장
{
servoMove.pos.push_back({pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
, pos[7], pos[8], pos[9], pos[10], pos[11]});
if (i < 100) {
pos[0] += 0.008;
pos[8] += 0.008;
} else if ( i >= 200 && i < 300) {
pos[0] -= 0.008;
pos[8] -= 0.008;
} else if ( i >= 300 && i < 400) {
pos[0] += 0.008;
pos[8] += 0.008;
} else {
pos[0] -= 0.008;
pos[8] -= 0.008;
}
}
std::cout << "servo_point_position_motion_control return " << servo_point_position_motion_control(fd_7000, servoMove) << std::endl;
servoMove.pos.clear();
if (servoMove.count == 10)
{
servoMove.count = 0;
}
servoMove.count ++;
}
}예시 3: servo_point_position_motion_control() 인터페이스를 캡슐화하여 포인트를 전송해도 운동하지 않거나 버퍼가 가득 찬 경우 전송
cpp
void test_clean_motion()
{
ServoPointMovePara servoMove;
servoMove.end = 1;
servoMove.sum = 1;
servoMove.count = 1;
std::vector<double> pos = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; //앞 7개는 로봇 본체, 뒤 5개는 외부 축
servoMove.pos.push_back({pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
, pos[7], pos[8], pos[9], pos[10], pos[11]});
std::cout << "servo_point_position_motion_control return " << servo_point_position_motion_control(fd_7000, servoMove) << std::endl;
}예시: 포인트를 전송해도 운동하지 않거나 버퍼가 가득 찬 경우 아래 코드를 전송
cpp
#include <iostream>
#include <vector>
#include <chrono>
#include "cpp_interface/nrc_interface.h"
int main()
{
int fd_7000;
connect(fd_7000, "192.168.1.13");
enable_servo_position_motion_control(fd_7000, 1); //서보 제어 모드 활성화
test_servo_point_position_motion_control();
//포인트를 삭제해야 할 때 아래 인터페이스를 호출
// test_clean_motion();
}