Servo Control Motion Using star_servo_point_position_motion_control()

Servo Control Motion Mode Dispatches Points Directly to the Servo Each Communication Cycle for Motion

struct ServoPointMovePara
{
	bool end;                       ///< Whether to clear previously sent points that haven't started interpolation
	int sum;                         ///< Total number of frames to send
	int count;                           ///< Current frame number
	std::vector<std::vector<double>> pos;   ///< 2D array: first dimension = number of points in this transfer, second dimension = 12 for joint angles or Cartesian coordinates. Coordinates: robot body (first 7) + external axes (last 5)
};

Note: The sent data will only cause the robot to move when total frames sum == count. Therefore, when sum != 1, count must start from 1 and accumulate up to the value of sum before the robot begins moving according to the dispatched points. (See Example 2 for details: it demonstrates sum = 10 with count = 1 passed to servo_point_position_motion_control, then count = 2 passed to servo_point_position_motion_control, count = 3 ... all the way to count = 10 passed to servo_point_position_motion_control, at which point all dispatched points will begin executing.)

Example 1: Encapsulate the servo_point_position_motion_control() interface, sending a single point to the servo at a time

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};  // First 7 = robot body, last 5 = external axes
  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]);
  }
}

Example 2: Encapsulate the servo_point_position_motion_control() interface, sending target points to the servo for motion, up to 600 points per transfer

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]};  // First 7 = robot body, last 5 = external axes
  for (int i = 0; i < 10; i++)
  {
    for (int j = 0; j < 600; j++)        // Store up to 600 points
    {
      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 ++;
  }
}

Example 3: Encapsulate the servo_point_position_motion_control() interface for cases where dispatched points don't move or the buffer is full

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};  // First 7 = robot body, last 5 = external axes
    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;
}

Example: When dispatched points don't move or the buffer is full, send the following code

#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);     // Enable servo control mode
  test_servo_point_position_motion_control();
  // When points need to be cleared, call the following interface
  // test_clean_motion();
}