Joint Control with servoJ

API Reference

/**
 * @brief Open joint tracking mode
 * @param vmax Velocity constraint, unit: deg/s
 * @param amax Acceleration constraint, unit: deg/s^2
 * @param jmax Jerk constraint, unit: deg/s^3
 */
Result open_servoJ(SOCKETFD socketFd, std::vector<double> vmax, std::vector<double> amax, std::vector<double> jmax);


/**
 * @brief Send target joint positions
 * @param q Target position, unit: degrees
 */
Result set_servoJ_pos(SOCKETFD socketFd, std::vector<double> q);


/**
 * @brief Close joint tracking mode
 */
Result stop_servoJ(SOCKETFD socketFd);

Usage Example

int main()
{
    // Connect to port 6000, used for power-on control and querying the robot arm's current position
    SOCKETFD fd_6000 = connect_robot("192.168.1.13", "6000");
    // Connect to port 7000
    SOCKETFD fd_7000 = connect_robot("192.168.1.13", "7000");
    if (fd_6000 < 0 || fd_7000 < 0) 
    {
        return 0; // Connection failed, exit.
    }
    // Power on the robot
    std::cout << "Ready: " <<  set_servo_state(fd_6000, 1)<<std::endl;
    std::cout << "Power on: " << set_servo_poweron(fd_6000)<<std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(30));
    
    // Set motion constraints
    std::vector<double> vMax = {10, 10, 10, 10, 10, 10, 10};  // Velocity 10 deg/s
    std::vector<double> avMax = {1000, 1000, 1000, 1000, 1000, 1000, 1000}; // Acceleration 1000 deg/s^2
    std::vector<double> jMax = {1000, 1000, 1000, 1000, 1000, 1000, 1000}; // Jerk 1000 deg/s^3
    std::cout << "Open joint tracking: " << open_servoJ(fd_7000, vMax, avMax, jMax) << std::endl;


    int n = 100
    while(n > 0)
    {
        n--;
        std::cout << "Current position: " << get_current_position(fd_6000, 0, pos) << std::endl;
    
        // Only increment the first element pos[0] by 0.2, corresponding to axis 1 of the robot arm
        pos[0] += 0.2;
    
        std::cout << "Send joint tracking: " << set_servoJ_pos(fd_7000, pos) << std::endl;
        // Dispatch a new position every 10ms
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }


    // Stop servoJ mode
    stop_servoJ(fd_7000);
    return 0;
}