Skip to content

GDK Robot Interface Documentation (C++)

Overview

The Robot module provides a unified robot control interface for the G02 robot, integrating capabilities such as robot body state retrieval, motion control, joint control, and end-effector control. Through the C++ interface, developers can conveniently implement comprehensive control of the robot, suitable for scenarios such as robot control, state monitoring, action execution, and path planning.

Interface Description

Robot Class

This class encapsulates the robot's main control interfaces, integrating the HAL (Hardware Abstraction Layer) and motion control functionality.

1. GetJointStates()

  • Note: Use motor_position and motor_velocity to get the motor position and velocity; position and velocity are fields reserved for low-speed motors and are not relevant in the current version
  • Function: Retrieves the robot's joint state information
  • Parameters:
Parameter Type Description
joint_states JointStates& Output parameter, joint state information object
  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success, and the joint_states parameter contains the joint state information

JointStates Object Details

The JointStates struct contains the following members:

Member Type Description Unit
nums size_t Number of joints None
states std::vector<JointState> List of joint states List of joint states
timestamp uint64_t Timestamp Nanoseconds 
struct JointStates {
  size_t nums{};                     ///< number of joint states
  std::vector<JointState> states{};  ///< joint states
  uint64_t timestamp{};              ///< joint states timestamp(ns)
};

JointState struct:

Member Type Description Unit
name std::string Joint name String
mode uint32_t Joint mode None
position double Joint position Radians
velocity double Joint velocity Radians/second
effort double Joint torque N·m
motor_position double Motor position Radians
motor_velocity double Motor velocity Radians/second
motor_current double Motor current A
error_code uint32_t Error code, 0 indicates normal None 
struct JointState {
  std::string name{};
  uint32_t mode{};
  double position{};
  double velocity{};
  double effort{};
  double motor_position{};
  double motor_velocity{};
  double motor_current{};
  uint32_t error_code{};
};
  • Example:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::JointStates joint_states;
    if (robot.GetJointStates(joint_states) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get joint states" << std::endl;
    } else {
        std::cout << "Number of joints: " << joint_states.nums << std::endl;
        std::cout << "Timestamp: " << joint_states.timestamp << std::endl;
        for (const auto& joint_state : joint_states.states) {
            std::cout << "Joint: " << joint_state.name
                      << ", position: " << joint_state.position
                      << ", velocity: " << joint_state.velocity
                      << ", torque: " << joint_state.effort
                      << ", error code: " << joint_state.error_code << std::endl;
        }
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

2. GetEndState()

  • Function: Retrieves end-effector state information
  • Parameters:
Parameter Type Description
end_state DualEndState& Output parameter, dual end-effector state information object
  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success, and the end_state parameter contains the end-effector state information

DualEndState Object Details

The DualEndState struct contains the following members:

Member Type Description Unit
left_end_state EndState Left end-effector state End-effector state
right_end_state EndState Right end-effector state End-effector state 
struct DualEndState {
  EndState left_end_state{};
  EndState right_end_state{};
};

EndState struct:

Member Type Description Unit
controlled bool Whether it is under control Boolean
type uint32_t End-effector type None
names std::vector<std::string> List of joint names List of strings
end_states std::vector<MotorState> List of motor states List of motor states 
struct EndState {
  bool controlled{};
  uint32_t type{};
  std::vector<std::string> names{};
  std::vector<MotorState> end_states{};
};

MotorState struct:

Member Type Description Unit
id uint32_t Motor ID None
enable bool Whether enabled Boolean
position double Motor position Radians
velocity double Motor velocity Radians/second
effort double Motor torque N·m
current float Motor current A
voltage float Motor voltage V
temperature float Motor temperature °C
status uint32_t Motor status None
err_code uint32_t Error code None 
struct MotorState {
  uint32_t id = 0;
  bool enable = false;
  double position = 0.0;
  double velocity = 0.0;
  double effort = 0.0;
  float current = 0.0f;
  float voltage = 0.0f;
  float temperature = 0.0f;
  uint32_t status = 0;
  uint32_t err_code = 0;
};
  • Example:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::DualEndState end_state;
    if (robot.GetEndState(end_state) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get end state" << std::endl;
    } else {
        std::cout << "Left end-effector state:" << std::endl;
        std::cout << "  Control state: " << (end_state.left_end_state.controlled ? "Yes" : "No") << std::endl;
        std::cout << "  Type: " << end_state.left_end_state.type << std::endl;
        for (const auto& motor_state : end_state.left_end_state.end_states) {
            std::cout << "  Motor ID: " << motor_state.id
                      << ", position: " << motor_state.position
                      << ", current: " << motor_state.current
                      << ", temperature: " << motor_state.temperature << std::endl;
        }

        std::cout << "Right end-effector state:" << std::endl;
        std::cout << "  Control state: " << (end_state.right_end_state.controlled ? "Yes" : "No") << std::endl;
        std::cout << "  Type: " << end_state.right_end_state.type << std::endl;
        for (const auto& motor_state : end_state.right_end_state.end_states) {
            std::cout << "  Motor ID: " << motor_state.id
                      << ", position: " << motor_state.position
                      << ", current: " << motor_state.current
                      << ", temperature: " << motor_state.temperature << std::endl;
        }
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

3. GetWholeBodyStatus()

  • Function: Retrieves the robot's whole-body state information
  • Parameters:
Parameter Type Description
whole_body_status WholeBodyStatus& Output parameter, whole-body state information object
  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success, and the whole_body_status parameter contains the whole-body state information

WholeBodyStatus Object Details

The WholeBodyStatus struct contains the following members:

Member Type Description Unit
right_arm_error uint32_t Right arm error code None
left_arm_error uint32_t Left arm error code None
right_arm_control bool Right arm control state Boolean
left_arm_control bool Left arm control state Boolean
right_arm_estop bool Right arm emergency-stop state Boolean
left_arm_estop bool Left arm emergency-stop state Boolean
right_end_error uint32_t Right end-effector error code None
left_end_error uint32_t Left end-effector error code None
right_end_model std::string Right end-effector model String
left_end_model std::string Left end-effector model String
waist_error uint32_t Waist error code None
lift_error uint32_t Lift error code None
neck_error uint32_t Neck error code None
chassis_error uint32_t Chassis error code None
timestamp uint64_t Timestamp Nanoseconds 
struct WholeBodyStatus {
  uint32_t right_arm_error{0};
  uint32_t left_arm_error{0};
  bool right_arm_control{false};
  bool left_arm_control{false};
  bool right_arm_estop{false};
  bool left_arm_estop{false};
  uint32_t right_end_error{0};
  uint32_t left_end_error{0};
  std::string right_end_model{};
  std::string left_end_model{};
  uint32_t waist_error{0};
  uint32_t lift_error{0};
  uint32_t neck_error{0};
  uint32_t chassis_error{0};
  uint64_t timestamp{};
};
  • Example:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::WholeBodyStatus whole_body_status;
    if (robot.GetWholeBodyStatus(whole_body_status) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get whole body status" << std::endl;
    } else {
        std::cout << "Whole-body state information:" << std::endl;
        std::cout << "Right arm error code: " << whole_body_status.right_arm_error << std::endl;
        std::cout << "Left arm error code: " << whole_body_status.left_arm_error << std::endl;
        std::cout << "Right arm control state: " << (whole_body_status.right_arm_control ? "Yes" : "No") << std::endl;
        std::cout << "Left arm control state: " << (whole_body_status.left_arm_control ? "Yes" : "No") << std::endl;
        std::cout << "Right arm emergency-stop state: " << (whole_body_status.right_arm_estop ? "Yes" : "No") << std::endl;
        std::cout << "Left arm emergency-stop state: " << (whole_body_status.left_arm_estop ? "Yes" : "No") << std::endl;
        std::cout << "Right end-effector model: " << whole_body_status.right_end_model << std::endl;
        std::cout << "Left end-effector model: " << whole_body_status.left_end_model << std::endl;
        std::cout << "Waist error code: " << whole_body_status.waist_error << std::endl;
        std::cout << "Lift error code: " << whole_body_status.lift_error << std::endl;
        std::cout << "Neck error code: " << whole_body_status.neck_error << std::endl;
        std::cout << "Chassis error code: " << whole_body_status.chassis_error << std::endl;
        std::cout << "Timestamp: " << whole_body_status.timestamp << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

4. GetMotionControlStatus()

  • Function: Retrieves the end-effector control status information
  • Parameters:
Parameter Type Description
status MotionControlStatus& Output parameter, end-effector control status object
  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success, and the status parameter contains the motion control status information

MotionControlStatus Object Details

The MotionControlStatus struct contains the following members:

Member Type Description Unit
frame_names std::vector<std::string> List of end-effector joint names List of strings
frame_poses std::vector<Pose> List of end-effector joint poses List of poses
collision_pairs_1 std::vector<std::string> Collision pair list 1 List of strings
collision_pairs_2 std::vector<std::string> Collision pair list 2 List of strings
mode uint8_t Motion mode None
error_code uint8_t Error code, 0 indicates normal None
error_msg std::string Error message String
twists std::vector<Twist> List of velocity information List of velocities
wrenches std::vector<Wrench> List of force/torque information List of forces/torques

Description of mode values:

Value Meaning
0 Stop
1 G1_Servo
2 Path planning
5 G2_Servo 
struct MotionControlStatus {
  std::vector<std::string> frame_names{};
  std::vector<Pose> frame_poses{};
  std::vector<std::string> collision_pairs_1{};
  std::vector<std::string> collision_pairs_2{};
  uint8_t mode{};
  uint8_t error_code{};
  std::string error_msg{};
  std::vector<Twist> twists{};
  std::vector<Wrench> wrenches{};
};
  • Example:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::MotionControlStatus status;
    if (robot.GetMotionControlStatus(status) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get motion control status" << std::endl;
    } else {
        std::cout << "End-effector control status:" << std::endl;
        std::cout << "Mode: " << status.mode << std::endl;
        std::cout << "Error code: " << status.error_code << std::endl;
        std::cout << "Error message: " << status.error_msg << std::endl;
        std::cout << "Number of joints: " << status.frame_names.size() << std::endl;
        for (size_t i = 0; i < status.frame_names.size(); i++) {
            std::cout << "Joint: " << status.frame_names[i]
                      << ", position: x=" << status.frame_poses[i].position.x
                      << ", y=" << status.frame_poses[i].position.y
                      << ", z=" << status.frame_poses[i].position.z << std::endl;
        }
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

5. GetChassisPowerState()

  • Function: Retrieves the chassis power state
  • Parameters:
Parameter Type Description
chassis_power_state ChassisPowerState& Chassis power state object
  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

ChassisPowerState Object Details

The ChassisPowerState struct contains the following members:

Member Type Description Unit
battery_main_power_switch_state uint8_t Battery main power switch state None
emergency_stop_pedal_state uint8_t Emergency stop pedal state None
battery_main_power_switch_fault_state uint8_t Battery main power switch fault state None
emergency_stop_pedal_fault_state uint8_t Emergency stop pedal fault state None
chassis_power_board_state uint8_t Chassis power board state None
chassis_left_traction_motor_power_state uint8_t Chassis left traction motor power state None
chassis_right_traction_motor_power_state uint8_t Chassis right traction motor power state None
chassis_left_steering_motor_power_state uint8_t Chassis left steering motor power state None
chassis_right_steering_motor_power_state uint8_t Chassis right steering motor power state None
chassis_lidar1_power_state uint8_t Chassis lidar 1 power state None
chassis_lidar2_power_state uint8_t Chassis lidar 2 power state None
chassis_ultrasonic_radar_power_state uint8_t Chassis ultrasonic radar power state None
chassis_tof_camera_power_state uint8_t Chassis ToF camera power state None
chassis_ethernet_switch_power_state uint8_t Chassis Ethernet switch power state None
chassis_external_power_output_state uint8_t Chassis external power output state None
battery_main_power_output_switch_state uint8_t Battery main power output switch state None
battery_states std::vector<BatteryState> List of battery states List of battery states
charge_plug_insert_state uint8_t Charging plug insertion state None
charge_plug_input_voltage float Charging plug input voltage V
charge_plug_input_current float Charging plug input current A
charge_plug_input_short_circuit_fault_state uint8_t Charging plug input short-circuit fault state None
charge_plug_input_open_circuit_fault_state uint8_t Charging plug input open-circuit fault state None
chassis_led_strip_power_state uint8_t Chassis LED strip power state None
chassis_power_board_temperature float Chassis power board temperature °C
power_48v_bus_power_on_fault_state uint8_t 48V bus power-on fault state None
power_poe_bus_power_on_fault_state uint8_t PoE bus power-on fault state None
chassis_board_12v_output_fault_state uint8_t Chassis board 12V output fault state None
chassis_board_5v_output_fault_state uint8_t Chassis board 5V output fault state None
chassis_power_board_fault_state uint32_t Chassis power board fault state None
timestamp uint64_t Timestamp Nanoseconds 
struct ChassisPowerState {
  uint8_t battery_main_power_switch_state{0};
  uint8_t emergency_stop_pedal_state{0};
  uint8_t battery_main_power_switch_fault_state{0};
  uint8_t emergency_stop_pedal_fault_state{0};
  uint8_t chassis_power_board_state{0};
  uint8_t chassis_left_traction_motor_power_state{0};
  uint8_t chassis_right_traction_motor_power_state{0};
  uint8_t chassis_left_steering_motor_power_state{0};
  uint8_t chassis_right_steering_motor_power_state{0};
  uint8_t chassis_lidar1_power_state{0};
  uint8_t chassis_lidar2_power_state{0};
  uint8_t chassis_ultrasonic_radar_power_state{0};
  uint8_t chassis_tof_camera_power_state{0};
  uint8_t chassis_ethernet_switch_power_state{0};
  uint8_t chassis_external_power_output_state{0};
  uint8_t battery_main_power_output_switch_state{0};
  std::vector<BatteryState> battery_states{};
  uint8_t charge_plug_insert_state{0};
  float charge_plug_input_voltage{0.0};
  float charge_plug_input_current{0.0};
  uint8_t charge_plug_input_short_circuit_fault_state{0};
  uint8_t charge_plug_input_open_circuit_fault_state{0};
  uint8_t chassis_led_strip_power_state{0};
  float chassis_power_board_temperature{0.0};
  uint8_t power_48v_bus_power_on_fault_state{0};
  uint8_t power_poe_bus_power_on_fault_state{0};
  uint8_t chassis_board_12v_output_fault_state{0};
  uint8_t chassis_board_5v_output_fault_state{0};
  uint32_t chassis_power_board_fault_state{0};
  uint64_t timestamp{0};
};

BatteryState struct:

Member Type Description Unit
battery_charging_status uint8_t Battery charging status None
battery_output_voltage float Battery output voltage V
battery_output_current float Battery output current A
battery_charging_current float Battery charging current A
battery_temperature float Battery temperature °C
battery_soc float Battery charge percentage (State of Charge) %
battery_soh uint8_t Battery health (State of Health) %
battery_short_circuit_fault_state uint8_t Battery short-circuit fault state None
battery_open_circuit_fault_state uint8_t Battery open-circuit fault state None
battery_other_fault_state uint8_t Battery other fault state None
battery_outside_output_voltage float Battery external output voltage V
battery_outside_connection uint8_t Battery external connection state None
battery_outside_open_circuit_fault_state uint8_t Battery external open-circuit fault state None
battery_switch_state uint8_t Battery switch state None
battery_unlock_state uint8_t Battery unlock state None
battery_input_fault_state uint8_t Battery input fault state None
battery_charging_mos_switch_state uint8_t Battery charging MOS switch state None 
struct BatteryState {
  uint8_t battery_charging_status{0};
  float battery_output_voltage{0.0};
  float battery_output_current{0.0};
  float battery_charging_current{0.0};
  float battery_temperature{0.0};
  float battery_soc{0.0};
  uint8_t battery_soh{0};
  uint8_t battery_short_circuit_fault_state{0};
  uint8_t battery_open_circuit_fault_state{0};
  uint8_t battery_other_fault_state{0};
  float battery_outside_output_voltage{0.0};
  uint8_t battery_outside_connection{0};
  uint8_t battery_outside_open_circuit_fault_state{0};
  uint8_t battery_switch_state{0};
  uint8_t battery_unlock_state{0};
  uint8_t battery_input_fault_state{0};
  uint8_t battery_charging_mos_switch_state{0};
};
  • Example:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::ChassisPowerState chassis_power_state;
    if (robot.GetChassisPowerState(chassis_power_state) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to get chassis power state" << std::endl;
    } else {
        std::cout << "Chassis power state information:" << std::endl;
        std::cout << "  Battery main power switch: " << (int)chassis_power_state.battery_main_power_switch_state << std::endl;
        std::cout << "  Emergency stop pedal state: " << (int)chassis_power_state.emergency_stop_pedal_state << std::endl;
        std::cout << "  Chassis power board state: " << (int)chassis_power_state.chassis_power_board_state << std::endl;
        std::cout << "  Chassis power board temperature: " << chassis_power_state.chassis_power_board_temperature << "°C" << std::endl;
        std::cout << "  Charging plug input voltage: " << chassis_power_state.charge_plug_input_voltage << "V" << std::endl;
        std::cout << "  Charging plug input current: " << chassis_power_state.charge_plug_input_current << "A" << std::endl;
        std::cout << "  Number of batteries: " << chassis_power_state.battery_states.size() << std::endl;

        for (size_t i = 0; i < chassis_power_state.battery_states.size(); i++) {
            const auto& battery = chassis_power_state.battery_states[i];
            std::cout << "  Battery " << i << ":" << std::endl;
            std::cout << "    Charge: " << battery.battery_soc << "%" << std::endl;
            std::cout << "    Health: " << (int)battery.battery_soh << "%" << std::endl;
            std::cout << "    Voltage: " << battery.battery_output_voltage << "V" << std::endl;
            std::cout << "    Current: " << battery.battery_output_current << "A" << std::endl;
            std::cout << "    Temperature: " << battery.battery_temperature << "°C" << std::endl;
        }

        std::cout << "  Timestamp: " << chassis_power_state.timestamp << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

6. GetChestPowerState()

  • Function: Retrieves the chest power state
  • Parameters:
Parameter Type Description
chest_power_state ChestPowerState& Output parameter, chest power state object
  • Return value: GDKRes, the operation result status code.

ChestPowerState Object Details

The ChestPowerState struct contains the following members:

Member Type Description Unit
power_onoff_req uint8_t Power on/off request None
emergency_stop_button_req uint8_t Emergency stop button request None
power_switch_fault_state uint8_t Power switch fault state None
emergency_stop_button_fault_state uint8_t Emergency stop button fault state None
power_full_low_req uint8_t Power full/low request None
chest_power_board_power_state uint8_t Chest power board power state None
domain_controller_power_state uint8_t Domain controller power state None
head_interactive_board_power_state uint8_t Head interactive board power state None
curved_screen_power_state uint8_t Curved screen power state None
head_yaw_motor_power_state uint8_t Head yaw motor power state None
head_pitch_motor_power_state uint8_t Head pitch motor power state None
head_roll_motor_power_state uint8_t Head roll motor power state None
waist_yaw_motor_power_state uint8_t Waist yaw motor power state None
head_motor_short_circuit_fault_state uint8_t Head motor short-circuit fault state None
waist_pitch_motor_power_state uint8_t Waist pitch motor power state None
leg_bending1_motor_power_state uint8_t Leg bending 1 motor power state None
leg_bending2_motor_power_state uint8_t Leg bending 2 motor power state None
leg_bending3_motor_power_state uint8_t Leg bending 3 motor power state None
waist_motor_short_circuit_fault_state uint8_t Waist motor short-circuit fault state None
left_arm_power_state uint8_t Left arm power state None
left_arm_motor_short_circuit_fault_state uint8_t Left arm motor short-circuit fault state None
left_arm_brake_enable_state uint8_t Left arm brake enable state None
right_arm_power_state uint8_t Right arm power state None
right_arm_motor_short_circuit_fault_state uint8_t Right arm motor short-circuit fault state None
right_arm_brake_enable_state uint8_t Right arm brake enable state None
fan_power_state uint8_t Fan power state None
chest_power_board_fan_fault_state uint8_t Chest power board fan fault state None
body_fan1_fault_state uint8_t Body fan 1 fault state None
body_fan2_fault_state uint8_t Body fan 2 fault state None
body_fan3_fault_state uint8_t Body fan 3 fault state None
body_fan4_fault_state uint8_t Body fan 4 fault state None
upper_body_led_strip_power_state uint8_t Upper body LED strip power state None
poe_power_state uint8_t PoE power state None
ipad_power_state uint8_t iPad power state None
chest_reserved_lidar_power_state uint8_t Chest reserved lidar power state None
chest_power_board_temperature float Chest power board temperature °C
chest_power_board_fault_state uint32_t Chest power board fault state None
timestamp uint64_t Timestamp Nanoseconds 
struct ChestPowerState {
  uint8_t power_onoff_req{0};
  uint8_t emergency_stop_button_req{0};
  uint8_t power_switch_fault_state{0};
  uint8_t emergency_stop_button_fault_state{0};
  uint8_t power_full_low_req{0};
  uint8_t chest_power_board_power_state{0};
  uint8_t domain_controller_power_state{0};
  uint8_t head_interactive_board_power_state{0};
  uint8_t curved_screen_power_state{0};
  uint8_t head_yaw_motor_power_state{0};
  uint8_t head_pitch_motor_power_state{0};
  uint8_t head_roll_motor_power_state{0};
  uint8_t waist_yaw_motor_power_state{0};
  uint8_t head_motor_short_circuit_fault_state{0};
  uint8_t waist_pitch_motor_power_state{0};
  uint8_t leg_bending1_motor_power_state{0};
  uint8_t leg_bending2_motor_power_state{0};
  uint8_t leg_bending3_motor_power_state{0};
  uint8_t waist_motor_short_circuit_fault_state{0};
  uint8_t left_arm_power_state{0};
  uint8_t left_arm_motor_short_circuit_fault_state{0};
  uint8_t left_arm_brake_enable_state{0};
  uint8_t right_arm_power_state{0};
  uint8_t right_arm_motor_short_circuit_fault_state{0};
  uint8_t right_arm_brake_enable_state{0};
  uint8_t fan_power_state{0};
  uint8_t chest_power_board_fan_fault_state{0};
  uint8_t body_fan1_fault_state{0};
  uint8_t body_fan2_fault_state{0};
  uint8_t body_fan3_fault_state{0};
  uint8_t body_fan4_fault_state{0};
  uint8_t upper_body_led_strip_power_state{0};
  uint8_t poe_power_state{0};
  uint8_t ipad_power_state{0};
  uint8_t chest_reserved_lidar_power_state{0};
  float chest_power_board_temperature{0.0};
  uint32_t chest_power_board_fault_state{0};
  uint64_t timestamp{0};
};
  • Example:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::ChestPowerState chest_state;
    if (robot.GetChestPowerState(chest_state) == agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Chest power state information:" << std::endl;
        std::cout << "  Power on/off request: " << (int)chest_state.power_onoff_req << std::endl;
        std::cout << "  Emergency stop button request: " << (int)chest_state.emergency_stop_button_req << std::endl;
        std::cout << "  Chest power board state: " << (int)chest_state.chest_power_board_power_state << std::endl;
        std::cout << "  Chest power board temperature: " << chest_state.chest_power_board_temperature << "°C" << std::endl;
        std::cout << "  Domain controller power: " << (int)chest_state.domain_controller_power_state << std::endl;
        std::cout << "  Left arm power: " << (int)chest_state.left_arm_power_state << std::endl;
        std::cout << "  Right arm power: " << (int)chest_state.right_arm_power_state << std::endl;
        std::cout << "  Left arm brake enable: " << (int)chest_state.left_arm_brake_enable_state << std::endl;
        std::cout << "  Right arm brake enable: " << (int)chest_state.right_arm_brake_enable_state << std::endl;
        std::cout << "  Head yaw motor power: " << (int)chest_state.head_yaw_motor_power_state << std::endl;
        std::cout << "  Head pitch motor power: " << (int)chest_state.head_pitch_motor_power_state << std::endl;
        std::cout << "  Head roll motor power: " << (int)chest_state.head_roll_motor_power_state << std::endl;
        std::cout << "  Fan power: " << (int)chest_state.fan_power_state << std::endl;
        std::cout << "  Timestamp: " << chest_state.timestamp << std::endl;
    } else {
        std::cout << "Failed to get chest power state" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

7. JointControl()

  • Function: Joint control interface (path-planning control); the interface returns after the target position is reached.
  • Parameters:
Parameter Type Description
joint_control_req const JointControlReq& Joint control request object
  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

JointControlReq Object Details

The JointControlReq struct contains the following members:

Member Type Description Unit
uuid std::string Unique request identifier String
life_time double Request lifetime Seconds
joint_names std::vector<std::string> List of joint names List of strings
joint_positions std::vector<double> List of joint positions Radians
joint_velocities std::vector<double> List of joint velocities Radians/second
detail std::string Detailed information String 
struct JointControlReq {
  double life_time{0.0};
  std::vector<std::string> joint_names{};
  std::vector<double> joint_positions{};
  std::vector<double> joint_velocities{};
  std::string uuid{};
  std::string detail{};
};

Joint Limit Values

The limit values for each joint (in radians) are as follows:

Joint Name Minimum Maximum
idx01_body_joint1 -1.082104 0.000174
idx02_body_joint2 -0.000174 2.652900
idx03_body_joint3 -1.919862 1.570970
idx04_body_joint4 -0.436332 0.436332
idx05_body_joint5 -3.045599 3.045599
idx11_head_joint1 -1.570970 1.570970
idx12_head_joint2 -0.349240 0.349240
idx13_head_joint3 -0.534773 0.534773
idx21_arm_l_joint1 -3.071796 3.071796
idx22_arm_l_joint2 -2.059505 2.059505
idx23_arm_l_joint3 -3.071796 3.071796
idx24_arm_l_joint4 -2.495838 1.012308
idx25_arm_l_joint5 -3.071796 3.071796
idx26_arm_l_joint6 -1.012308 1.012308
idx27_arm_l_joint7 -1.535907 1.535907
idx61_arm_r_joint1 -3.071796 3.071796
idx62_arm_r_joint2 -2.059505 2.059505
idx63_arm_r_joint3 -3.071796 3.071796
idx64_arm_r_joint4 -2.495838 1.012308
idx65_arm_r_joint5 -3.071796 3.071796
idx66_arm_r_joint6 -1.012308 1.012308
idx67_arm_r_joint7 -1.535907 1.535907
  • Example:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <random>
#include <sstream>

std::string generate_uuid() {
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_int_distribution<uint32_t> dis(0, 0xFFFFFFFF);
    std::stringstream ss;
    ss << std::hex << dis(gen) << "-" << dis(gen) << "-" << dis(gen) << "-" << dis(gen);
    return ss.str();
}

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    agibot::gdk::JointControlReq joint_control;
    joint_control.life_time = 5.0;
    joint_control.joint_names = {"idx21_arm_l_joint1","idx22_arm_l_joint2","idx23_arm_l_joint3"};
    joint_control.joint_positions = {0.0, 0.0, 0.0};
    joint_control.joint_velocities = {0.0, 0.0, 0.0};
    joint_control.detail = "Left arm joint control";

    if (robot.JointControl(joint_control) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to control joints" << std::endl;
    } else {
        std::cout << "Joint control command sent successfully" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

8. MoveHeadJoint()

  • Function: Head joint position planning control interface; the interface returns after the target position is reached.
  • Parameters:
Parameter Type Description
positions std::vector<double>& List of head joint positions; fill in the target joint angles in the order "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"
velocities const std::vector<double>& List of head joint velocities; fill in the target joint velocities in the order "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"

  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

  • Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    std::vector<double> head_positions = {0.0, 0.0, 0.0};  // Head joint positions
    std::vector<double> head_velocities = {0.3, 0.3, 0.3};  // Head joint velocities

    if (robot.MoveHeadJoint(head_positions, head_velocities) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move head position" << std::endl;
    } else {
        std::cout << "Head position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

9. MoveWaistJoint()

  • Function: Waist joint position planning control interface; the interface returns after the target position is reached.
  • Parameters:
Parameter Type Description
positions std::vector<double>& List of waist joint positions; fill in the target joint angles in the order "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3", "idx04_body_joint4", "idx05_body_joint5"
velocities const std::vector<double>& List of waist joint velocities; fill in the target joint velocities in the order "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3", "idx04_body_joint4", "idx05_body_joint5"

  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

  • Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    std::vector<double> waist_positions = {0.0, 0.0, 0.0, 0.0, 0.0};  // Waist joint positions
    std::vector<double> waist_velocities = {0.3, 0.3, 0.3, 0.3, 0.3};  // Waist joint velocities

    if (robot.MoveWaistJoint(waist_positions, waist_velocities) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move waist position" << std::endl;
    } else {
        std::cout << "Waist position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

10. MoveArmJoint()

  • Function: Arm joint position planning control interface; the interface returns after the target position is reached.
  • Parameters:
Parameter Type Description
positions std::vector<double>& List of arm joint positions; fill in the target joint angles in the order "idx21_arm_l_joint1", "idx22_arm_l_joint2", "idx23_arm_l_joint3",
"idx24_arm_l_joint4", "idx25_arm_l_joint5", "idx26_arm_l_joint6",
"idx27_arm_l_joint7", "idx61_arm_r_joint1", "idx62_arm_r_joint2",
"idx63_arm_r_joint3", "idx64_arm_r_joint4", "idx65_arm_r_joint5",
"idx66_arm_r_joint6", "idx67_arm_r_joint7"
velocities const std::vector<double>& List of arm joint velocities; fill in the target joint velocities in the order "idx21_arm_l_joint1", "idx22_arm_l_joint2", "idx23_arm_l_joint3",
"idx24_arm_l_joint4", "idx25_arm_l_joint5", "idx26_arm_l_joint6",
"idx27_arm_l_joint7", "idx61_arm_r_joint1", "idx62_arm_r_joint2",
"idx63_arm_r_joint3", "idx64_arm_r_joint4", "idx65_arm_r_joint5",
"idx66_arm_r_joint6", "idx67_arm_r_joint7"
control_group const int Control group: 0 controls the left arm, 1 controls the right arm, 2 controls both arms
- Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success
  • Example:
#include "gdk/gdk.h"
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    std::vector<double> arm_positions = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};  // Arm joint positions
    std::vector<double> arm_velocities = {0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3};  // Arm joint velocities

    if (robot.MoveArmJoint(arm_positions, arm_velocities, 2) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move arm position" << std::endl;
    } else {
        std::cout << "Arm position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

11. JointServoControl()

  • Function: Joint position servo control interface. It must be controlled at a 100 Hz control frequency and supports normal mode and low-latency mode; normal mode is used by default.
  • Note: Low-latency mode has no collision protection; use it with caution for safety. To simultaneously control the end-effector, use this interface to issue control commands — see the example below for details.
  • Parameters:
Parameter Type Description
joint_servo_control_req const JointServoControlReq& Joint position servo request object
enable_low_latency bool Whether to enable low-latency mode; defaults to false
  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

JointServoControlReq Object Details

The JointServoControlReq struct contains the following members:

Member Type Description Unit
control_period double Control period Seconds
joint_names std::vector<std::string> List of joint names List of strings
joint_positions std::vector<double> List of joint positions Radians
joint_velocities std::vector<double> List of joint velocities Radians/second 
struct JointServoControlReq {
  double control_period{0.0};
  std::vector<std::string> joint_names{};
  std::vector<double> joint_positions{};
  std::vector<double> joint_velocities{};
};

Parameter description: - control_period indicates the control period, in seconds; it is recommended to set it slightly larger than the current control frequency - The three lists joint_names, joint_positions, and joint_velocities must have the same length - The values in joint_positions must be within the corresponding joint's limit range (see the joint limit values below); otherwise, ErrorCode::kInvalidInput is returned - joint_names, joint_positions, and joint_velocities cannot be empty; otherwise, ErrorCode::kInvalidInput is returned - joint_velocities is a reserved parameter that is currently unused and may be left empty

Joint Limit Values

The limit values for each joint (in radians) are as follows:

Joint Name Minimum Maximum
idx01_body_joint1 -1.082104 0.000174
idx02_body_joint2 -0.000174 2.652900
idx03_body_joint3 -1.919862 1.570970
idx04_body_joint4 -0.436332 0.436332
idx05_body_joint5 -3.045599 3.045599
idx11_head_joint1 -1.570970 1.570970
idx12_head_joint2 -0.349240 0.349240
idx13_head_joint3 -0.534773 0.534773
idx21_arm_l_joint1 -3.071796 3.071796
idx22_arm_l_joint2 -2.059505 2.059505
idx23_arm_l_joint3 -3.071796 3.071796
idx24_arm_l_joint4 -2.495838 1.012308
idx25_arm_l_joint5 -3.071796 3.071796
idx26_arm_l_joint6 -1.012308 1.012308
idx27_arm_l_joint7 -1.535907 1.535907
idx61_arm_r_joint1 -3.071796 3.071796
idx62_arm_r_joint2 -2.059505 2.059505
idx63_arm_r_joint3 -3.071796 3.071796
idx64_arm_r_joint4 -2.495838 1.012308
idx65_arm_r_joint5 -3.071796 3.071796
idx66_arm_r_joint6 -1.012308 1.012308
idx67_arm_r_joint7 -1.535907 1.535907
End-effector (omnipicker)
idx31_gripper_l_inner_joint1 -0.785 0
idx71_gripper_r_inner_joint1 -0.785 0
End-effector (dahuan)
idx31_gripper_l_inner_joint1 0 0.025
idx71_gripper_r_inner_joint1 0 0.025
End-effector (ctek90d)
idx31_gripper_l_inner_joint1 -0.91 0
idx71_gripper_r_inner_joint1 -0.91 0
End-effector (o10_t2 dexterous hand, left)
idx31_hand_l_thumb_roll_joint -1.1213740444063567 0.029670597283903602
idx32_hand_l_thumb_abad_joint -0.04537856055185257 1.642354826126664
idx33_hand_l_thumb_mcp_joint -0.8415977653116657 0.0
idx36_hand_l_index_abad_joint 0.0 0.16406094968746698
idx37_hand_l_index_pip_joint 0.0 1.4835298641951802
idx39_hand_l_middle_pip_joint 0.0 1.4835298641951802
idx41_hand_l_ring_abad_joint -0.16929693744344995 0.0
idx42_hand_l_ring_pip_joint 0.0 1.4835298641951802
idx44_hand_l_pinky_abad_joint -0.1850049007113989 0.0
idx45_hand_l_pinky_pip_joint 0.0 1.4835298641951802
End-effector (o10_t2 dexterous hand, right)
idx71_hand_r_thumb_roll_joint -0.029670597283903602 1.1213740444063567
idx72_hand_r_thumb_abad_joint -1.642354826126664 0.04537856055185257
idx73_hand_r_thumb_mcp_joint 0.0 0.8415977653116657
idx76_hand_r_index_abad_joint -0.16406094968746698 0.0
idx77_hand_r_index_pip_joint 0.0 1.4835298641951802
idx79_hand_r_middle_pip_joint 0.0 1.4835298641951802
idx81_hand_r_ring_abad_joint 0.0 0.16929693744344995
idx82_hand_r_ring_pip_joint 0.0 1.4835298641951802
idx84_hand_r_pinky_abad_joint 0.0 0.1850049007113989
idx85_hand_r_pinky_pip_joint 0.0 1.4835298641951802
End-effector (o12_t2 dexterous hand, left)
idx31_hand_l_thumb_roll_joint -0.9425 0.0
idx32_hand_l_thumb_abad_joint 0.0 1.3875
idx33_hand_l_thumb_mcp_joint -0.8273 0.0
idx34_hand_l_thumb_pip_joint -1.2915 0.0
idx36_hand_l_index_abad_joint -0.2618 0.2618
idx37_hand_l_index_mcp_joint 0.0 1.3526
idx38_hand_l_index_pip_joint 0.0 1.5307
idx40_hand_l_middle_abad_joint -0.2618 0.2618
idx41_hand_l_middle_mcp_joint 0.0 1.3579
idx42_hand_l_middle_pip_joint 0.0 1.8151
idx44_hand_l_ring_mcp_joint 0.0 1.5359
idx47_hand_l_pinky_mcp_joint 0.0 1.5359
End-effector (o12_t2 dexterous hand, right)
idx71_hand_r_thumb_roll_joint 0.0 0.9425
idx72_hand_r_thumb_abad_joint -1.3875 0.0
idx73_hand_r_thumb_mcp_joint -0.8273 0.0
idx74_hand_r_thumb_pip_joint -1.2915 0.0
idx76_hand_r_index_abad_joint -0.2618 0.2618
idx77_hand_r_index_mcp_joint 0.0 1.3526
idx78_hand_r_index_pip_joint 0.0 1.5307
idx80_hand_r_middle_abad_joint -0.2618 0.2618
idx81_hand_r_middle_mcp_joint 0.0 1.3579
idx82_hand_r_middle_pip_joint 0.0 1.8151
idx84_hand_r_ring_mcp_joint 0.0 1.5359
idx87_hand_r_pinky_mcp_joint 0.0 1.5359
  • Error handling:
  • If joint_names or joint_positions is empty, ErrorCode::kInvalidInput is returned
  • If the values in joint_positions exceed the corresponding joint's limit range, ErrorCode::kInvalidInput is returned
  • If retrieving the configuration parser fails, ErrorCode::kRuntimeError is returned

  • For other error conditions, the corresponding error code is returned

  • Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <vector>
#include <algorithm>

using namespace agibot::gdk;

// Control parameters
const double CONTROL_PERIOD = 0.01;  // Control period (seconds)
const double RATE_HZ = 100.0;        // Send frequency (Hz)
const double DURATION = 5.0;         // Control duration (seconds)

class JointServoControlController {
private:
    Robot robot_;

    double getJointPositionByName(const JointStates& joint_states, const std::string& joint_name) {
        for (const auto& state : joint_states.states) {
            if (state.name == joint_name) {
                return state.motor_position;
            }
        }
        throw std::runtime_error("Joint name " + joint_name + " not found");
    }

    double interpolatePosition(double start_pos, double target_pos, double t) {
        return start_pos + t * (target_pos - start_pos);
    }

public:
    void executeJointServoControl(const std::vector<std::string>& target_joint_names,
                                   const std::vector<double>& target_positions) {
        std::this_thread::sleep_for(std::chrono::seconds(1));  // Wait 1 second

        // Get current joint states
        JointStates current_joint_states;
        if (robot_.GetJointStates(current_joint_states) != GDKRes::kSuccess) {
            std::cout << "Failed to get joint states" << std::endl;
            return;
        }
        std::cout << "Current number of joints: " << current_joint_states.nums << std::endl;

        // Get the starting position
        std::vector<double> start_positions;
        for (const auto& joint_name : target_joint_names) {
            try {
                double pos = getJointPositionByName(current_joint_states, joint_name);
                start_positions.push_back(pos);
                std::cout << "Joint " << joint_name << " current position: " << pos << " rad" << std::endl;
            } catch (const std::exception& e) {
                std::cout << "Error: " << e.what() << std::endl;
                return;
            }
        }

        // Calculate number of steps
        int n_steps = static_cast<int>(DURATION * RATE_HZ);
        std::cout << "Total steps: " << n_steps << ", duration: " << DURATION << " s" << std::endl;

        // Execute trajectory
        double dt = 1.0 / RATE_HZ;
        auto start_time = std::chrono::steady_clock::now();

        for (int i = 0; i < n_steps; i++) {
            double t = static_cast<double>(i) / (n_steps - 1);

            // Create joint position servo request
            JointServoControlReq joint_position_servo_req;
            joint_position_servo_req.control_period = CONTROL_PERIOD;

            // Compute current target position (linear interpolation)
            std::vector<double> current_positions;
            for (size_t j = 0; j < target_joint_names.size(); j++) {
                double interp_pos = interpolatePosition(
                    start_positions[j], target_positions[j], t
                );
                current_positions.push_back(interp_pos);
            }

            joint_position_servo_req.joint_names = target_joint_names;
            joint_position_servo_req.joint_positions = current_positions;

            // Use normal mode (enable_low_latency=false, the default)
            auto res = robot_.JointServoControl(joint_position_servo_req);
            // To use low-latency mode, pass enable_low_latency=true:
            // auto res = robot_.JointServoControl(joint_position_servo_req, true);
            if (res != GDKRes::kSuccess) {
                std::cout << "Failed to send control command, step: " << i << std::endl;
                return;
            }

            // Control the send rate
            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto expected_time = std::chrono::milliseconds(static_cast<int>((i + 1) * dt * 1000));
            auto sleep_time = expected_time - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }

        std::cout << "Joint position servo control complete" << std::endl;

        // Hold the final position
        std::cout << "Entering final position hold (Ctrl+C to stop)..." << std::endl;
        try {
            while (true) {
                JointServoControlReq joint_position_servo_req;
                joint_position_servo_req.control_period = CONTROL_PERIOD;
                joint_position_servo_req.joint_names = target_joint_names;
                joint_position_servo_req.joint_positions = target_positions;

                // Use normal mode (enable_low_latency=false, the default)
                auto res = robot_.JointServoControl(joint_position_servo_req);
                // To use low-latency mode, pass enable_low_latency=true:
                // auto res = robot_.JointServoControl(joint_position_servo_req, true);
                if (res != GDKRes::kSuccess) {
                    std::cout << "Failed to hold position" << std::endl;
                    break;
                }

                std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(dt * 1000)));
            }
        } catch (const std::exception& e) {
            std::cout << "Hold interrupted: " << e.what() << std::endl;
        }
    }
};

int main() {
    // Initialize the GDK system
    if (GDKInit() != GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // Get current joint states
    JointStates current_joint_states;
    if (robot.GetJointStates(current_joint_states) != GDKRes::kSuccess) {
        std::cout << "Failed to get joint states" << std::endl;
        GDKRelease();
        return -1;
    }
    std::cout << "Current number of joints: " << current_joint_states.nums << std::endl;

    // Define the joints to control (example: the first 3 joints of the left arm)
    std::vector<std::string> target_joint_names = {
        "idx21_arm_l_joint1",
        "idx22_arm_l_joint2",
        "idx23_arm_l_joint3"
    };

    // Get the current position as the starting position (for interpolation)
    std::vector<double> start_positions;
    for (const auto& joint_name : target_joint_names) {
        bool found = false;
        for (const auto& state : current_joint_states.states) {
            if (state.name == joint_name) {
                start_positions.push_back(state.motor_position);
                std::cout << "Joint " << joint_name << " current position: " << state.motor_position << " rad" << std::endl;
                found = true;
                break;
            }
        }
        if (!found) {
            std::cout << "Error: joint " << joint_name << " not found" << std::endl;
            GDKRelease();
            return -1;
        }
    }

    // Set target angles (directly specify the target angle values, in radians)
    std::vector<double> target_positions = {
        0.0,  // idx21_arm_l_joint1: target angle 0.0 rad
        0.0,  // idx22_arm_l_joint2: target angle 0.0 rad
        0.0   // idx23_arm_l_joint3: target angle 0.0 rad
    };

    std::cout << "\nTarget angles:" << std::endl;
    for (size_t i = 0; i < target_joint_names.size(); i++) {
        std::cout << "  " << target_joint_names[i] << ": " << target_positions[i] << " rad" << std::endl;
    }

    // Execute joint position servo control
    JointServoControlController controller;
    controller.executeJointServoControl(
        target_joint_names, target_positions);

    // Release GDK system resources
    if (GDKRelease() != GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}
  • Example of simultaneously controlling the arm and the end-effector:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <vector>

using namespace agibot::gdk;

const double CONTROL_PERIOD = 0.01;
const double RATE_HZ = 100.0;
const double DURATION = 3.0;
const double HOLD_DURATION = 0.5;    // Time to hold at -0.785 or 0 (seconds)
const int NUM_CYCLES = 3;            // Number of back-and-forth cycles

static const std::vector<std::string> ARM_L_JOINT_NAMES = {
    "idx21_arm_l_joint1", "idx22_arm_l_joint2", "idx23_arm_l_joint3",
    "idx24_arm_l_joint4", "idx25_arm_l_joint5", "idx26_arm_l_joint6",
    "idx27_arm_l_joint7",
};

// Extract the position list by joint name from the GetJointStates return value
std::vector<double> getArmPositionsByName(const JointStates& joint_states,
                                          const std::vector<std::string>& joint_names) {
    std::vector<double> positions;
    for (const auto& name : joint_names) {
        for (const auto& state : joint_states.states) {
            if (state.name == name) {
                positions.push_back(state.motor_position);
                break;
            }
        }
    }
    return positions;
}

// Extract the joint name list and position list for the specified end-effector side from the GetEndState return value
// If use_left is true, take left_end_state; if false, take right_end_state
void getEENamesAndPositions(const DualEndState& end_state,
                            bool use_left,
                            std::vector<std::string>& names,
                            std::vector<double>& positions) {
    const EndState& state = use_left ? end_state.left_end_state : end_state.right_end_state;
    names = state.names;
    positions.clear();
    for (const auto& motor : state.end_states) {
        positions.push_back(motor.position);
    }
}

int main() {
    if (GDKInit() != GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    Robot robot;
    std::this_thread::sleep_for(std::chrono::seconds(2));

    JointStates joint_states;
    if (robot.GetJointStates(joint_states) != GDKRes::kSuccess) {
        std::cout << "Failed to get joint states" << std::endl;
        GDKRelease();
        return -1;
    }

    DualEndState end_state;
    if (robot.GetEndState(end_state) != GDKRes::kSuccess) {
        std::cout << "Failed to get end-effector state" << std::endl;
        GDKRelease();
        return -1;
    }

    std::vector<std::string> ee_names;
    std::vector<double> ee_positions;
    getEENamesAndPositions(end_state, true, ee_names, ee_positions);  // Left end-effector

    std::vector<std::string> all_names = ARM_L_JOINT_NAMES;
    all_names.insert(all_names.end(), ee_names.begin(), ee_names.end());

    std::vector<double> arm_positions = getArmPositionsByName(joint_states, ARM_L_JOINT_NAMES);
    if (arm_positions.size() != ARM_L_JOINT_NAMES.size()) {
        std::cout << "Left arm joint count mismatch" << std::endl;
        GDKRelease();
        return -1;
    }

    // The arm performs small back-and-forth joint-space motion as before, while the end-effector joint moves back and forth between -0.785 and 0
    std::vector<double> arm_start = arm_positions;                    // Arm initial pose
    std::vector<double> arm_target = arm_positions;                   // Arm target pose (small offset)
    for (auto& v : arm_target) v += 0.05;
    const double ee_low = -0.785;                                     // End-effector lower limit
    const double ee_high = 0.0;                                       // End-effector upper limit

    std::cout << "Number of controlled joints: arm " << ARM_L_JOINT_NAMES.size()
              << " + end-effector " << ee_names.size() << " = " << all_names.size() << std::endl;
    std::cout << "The arm will move in joint space between arm_start and arm_target, and the end-effector joint will move back and forth between "
              << ee_low << " and " << ee_high << " for " << NUM_CYCLES << " cycles" << std::endl;

    int n_steps = static_cast<int>(DURATION * RATE_HZ);           // Number of steps for a single move
    int hold_steps = static_cast<int>(HOLD_DURATION * RATE_HZ);   // Number of steps for the hold phase
    double dt = 1.0 / RATE_HZ;

    // Helper function: send control command
    auto sendControlCommand = [&](const std::vector<double>& arm_vals, const std::vector<double>& ee_vals) -> bool {
        std::vector<double> current = arm_vals;
        current.insert(current.end(), ee_vals.begin(), ee_vals.end());
        JointServoControlReq req;
        req.control_period = CONTROL_PERIOD;
        req.joint_names = all_names;
        req.joint_positions = current;
        req.joint_velocities = std::vector<double>(current.size(), 0.0);
        return robot.JointServoControl(req) == GDKRes::kSuccess;
    };

    for (int cycle = 0; cycle < NUM_CYCLES; cycle++) {
        std::cout << "\n=== Cycle " << (cycle + 1) << "/" << NUM_CYCLES
                  << ": arm arm_start -> arm_target, end-effector " << ee_low << " -> " << ee_high << " ===" << std::endl;
        auto start_time = std::chrono::steady_clock::now();

        // Segment 1: arm arm_start -> arm_target, end-effector ee_low -> ee_high
        for (int i = 0; i < n_steps; i++) {
            double t = (n_steps > 1) ? static_cast<double>(i) / (n_steps - 1) : 1.0;
            std::vector<double> current_arm;
            for (size_t j = 0; j < arm_start.size(); j++) {
                current_arm.push_back(arm_start[j] + t * (arm_target[j] - arm_start[j]));
            }
            std::vector<double> current_ee(ee_names.size(), ee_low + t * (ee_high - ee_low));

            if (!sendControlCommand(current_arm, current_ee)) {
                std::cout << "Send failed, cycle=" << (cycle + 1) << ", phase=0->1, step=" << i << std::endl;
                GDKRelease();
                return -1;
            }

            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto expected_ms = static_cast<int>((i + 1) * dt * 1000);
            auto sleep_time = std::chrono::milliseconds(expected_ms) - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }

        // Segment 2: hold at arm_target / ee_high
        std::cout << "=== Cycle " << (cycle + 1) << "/" << NUM_CYCLES
                  << ": holding at arm_target / " << ee_high << " for " << HOLD_DURATION << " s ===" << std::endl;
        start_time = std::chrono::steady_clock::now();
        std::vector<double> current_arm = arm_target;
        std::vector<double> current_ee(ee_names.size(), ee_high);

        for (int i = 0; i < hold_steps; i++) {
            if (!sendControlCommand(current_arm, current_ee)) {
                std::cout << "Send failed, cycle=" << (cycle + 1) << ", phase=hold@1, step=" << i << std::endl;
                GDKRelease();
                return -1;
            }

            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto expected_ms = static_cast<int>((i + 1) * dt * 1000);
            auto sleep_time = std::chrono::milliseconds(expected_ms) - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }

        // Segment 3: arm arm_target -> arm_start, end-effector ee_high -> ee_low
        std::cout << "=== Cycle " << (cycle + 1) << "/" << NUM_CYCLES
                  << ": arm arm_target -> arm_start, end-effector " << ee_high << " -> " << ee_low << " ===" << std::endl;
        start_time = std::chrono::steady_clock::now();

        for (int i = 0; i < n_steps; i++) {
            double t = (n_steps > 1) ? static_cast<double>(i) / (n_steps - 1) : 1.0;
            std::vector<double> current_arm;
            for (size_t j = 0; j < arm_target.size(); j++) {
                current_arm.push_back(arm_target[j] + t * (arm_start[j] - arm_target[j]));
            }
            std::vector<double> current_ee(ee_names.size(), ee_high + t * (ee_low - ee_high));

            if (!sendControlCommand(current_arm, current_ee)) {
                std::cout << "Send failed, cycle=" << (cycle + 1) << ", phase=1->0, step=" << i << std::endl;
                GDKRelease();
                return -1;
            }

            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto expected_ms = static_cast<int>((i + 1) * dt * 1000);
            auto sleep_time = std::chrono::milliseconds(expected_ms) - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }

        // Segment 4: hold at arm_start / ee_low
        std::cout << "=== Cycle " << (cycle + 1) << "/" << NUM_CYCLES
                  << ": holding at arm_start / " << ee_low << " for " << HOLD_DURATION << " s ===" << std::endl;
        start_time = std::chrono::steady_clock::now();
        current_arm = arm_start;
        current_ee = std::vector<double>(ee_names.size(), ee_low);

        for (int i = 0; i < hold_steps; i++) {
            if (!sendControlCommand(current_arm, current_ee)) {
                std::cout << "Send failed, cycle=" << (cycle + 1) << ", phase=hold@0, step=" << i << std::endl;
                GDKRelease();
                return -1;
            }

            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto expected_ms = static_cast<int>((i + 1) * dt * 1000);
            auto sleep_time = std::chrono::milliseconds(expected_ms) - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }
    }

    std::cout << "\nArm arm_start<->arm_target, end-effector " << ee_low << "<->" << ee_high << " back-and-forth control finished" << std::endl;

    if (GDKRelease() != GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;
    return 0;
}

12. MoveHeadJointServo()

  • Function: Head joint position servo control interface. It must be controlled at a 100 Hz control frequency and supports normal mode and low-latency mode; normal mode is used by default.
  • Note: Low-latency mode has no collision protection; use it with caution for safety.
  • Parameters:
Parameter Type Description
positions const std::vector<double>& List of head joint positions (radians), in the order "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"
control_period const double Control period (seconds); recommended to be slightly larger than the control frequency
enable_low_latency const bool Whether to enable low-latency mode; defaults to false

Parameter description: - positions must have a length of 3 - The values in positions must be within the corresponding joint's limit range (see the joint limit values below); otherwise, ErrorCode::kInvalidInput is returned - control_period indicates the control period, in seconds; recommended to be slightly larger than the control frequency - enable_low_latency is used to select the control channel; low-latency mode is suitable for scenarios with higher real-time requirements

Joint Limit Values

The limit values for the head joints (in radians) are as follows:

Joint Name Minimum Maximum
idx11_head_joint1 -1.570970 1.570970
idx12_head_joint2 -0.349240 0.349240
idx13_head_joint3 -0.534773 0.534773

  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

  • Error handling:

  • If the length of positions is not 3, ErrorCode::kInvalidInput is returned
  • If the values in positions exceed the corresponding joint's limit range, ErrorCode::kInvalidInput is returned

  • For other error conditions, the corresponding error code is returned

  • Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <vector>
#include <algorithm>

using namespace agibot::gdk;

// Control parameters
const double CONTROL_PERIOD = 0.01;  // Control period (seconds)
const double RATE_HZ = 100.0;        // Send frequency (Hz)
const double DURATION = 5.0;         // Control duration (seconds)

class HeadJointServoController {
private:
    Robot robot_;

    double getJointPositionByName(const JointStates& joint_states, const std::string& joint_name) {
        for (const auto& state : joint_states.states) {
            if (state.name == joint_name) {
                return state.motor_position;
            }
        }
        throw std::runtime_error("Joint name " + joint_name + " not found");
    }

    double interpolatePosition(double start_pos, double target_pos, double t) {
        return start_pos + t * (target_pos - start_pos);
    }

public:
    void executeHeadJointServoControl(const std::vector<double>& target_positions) {
        std::this_thread::sleep_for(std::chrono::seconds(1));  // Wait 1 second

        // Get current joint states
        JointStates current_joint_states;
        if (robot_.GetJointStates(current_joint_states) != GDKRes::kSuccess) {
            std::cout << "Failed to get joint states" << std::endl;
            return;
        }

        // Get the starting position
        std::vector<std::string> head_joint_names = {
            "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"
        };
        std::vector<double> start_positions;
        for (const auto& joint_name : head_joint_names) {
            try {
                double pos = getJointPositionByName(current_joint_states, joint_name);
                start_positions.push_back(pos);
                std::cout << "Joint " << joint_name << " current position: " << pos << " rad" << std::endl;
            } catch (const std::exception& e) {
                std::cout << "Error: " << e.what() << std::endl;
                return;
            }
        }

        // Calculate number of steps
        int n_steps = static_cast<int>(DURATION * RATE_HZ);
        std::cout << "Total steps: " << n_steps << ", duration: " << DURATION << " s" << std::endl;

        // Execute trajectory
        double dt = 1.0 / RATE_HZ;
        auto start_time = std::chrono::steady_clock::now();

        for (int i = 0; i < n_steps; i++) {
            double t = static_cast<double>(i) / (n_steps - 1);

            // Compute current target position (linear interpolation)
            std::vector<double> current_positions;
            for (size_t j = 0; j < 3; j++) {
                double interp_pos = interpolatePosition(
                    start_positions[j], target_positions[j], t
                );
                current_positions.push_back(interp_pos);
            }

            // Use normal mode (enable_low_latency=false, the default)
            auto res = robot_.MoveHeadJointServo(
                current_positions, CONTROL_PERIOD);
            // To use low-latency mode, pass enable_low_latency=true:
            // auto res = robot_.MoveHeadJointServo(
            //     current_positions, target_velocities, CONTROL_PERIOD, true
            // );
            if (res != GDKRes::kSuccess) {
                std::cout << "Failed to send control command, step: " << i << std::endl;
                return;
            }

            // Control the send rate
            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto expected_time = std::chrono::milliseconds(static_cast<int>((i + 1) * dt * 1000));
            auto sleep_time = expected_time - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }

        std::cout << "Head joint position servo control complete" << std::endl;

        // Hold the final position
        std::cout << "Entering final position hold (Ctrl+C to stop)..." << std::endl;
        try {
            while (true) {
                // Use normal mode (enable_low_latency=false, the default)
                auto res = robot_.MoveHeadJointServo(
                    target_positions, CONTROL_PERIOD);
                // To use low-latency mode, pass enable_low_latency=true:
                // auto res = robot_.MoveHeadJointServo(
                //     target_positions, target_velocities, CONTROL_PERIOD, true
                // );
                if (res != GDKRes::kSuccess) {
                    std::cout << "Failed to hold position" << std::endl;
                    break;
                }

                std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(dt * 1000)));
            }
        } catch (const std::exception& e) {
            std::cout << "Hold interrupted: " << e.what() << std::endl;
        }
    }
};

int main() {
    // Initialize the GDK system
    if (GDKInit() != GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // Get current joint states
    JointStates current_joint_states;
    if (robot.GetJointStates(current_joint_states) != GDKRes::kSuccess) {
        std::cout << "Failed to get joint states" << std::endl;
        GDKRelease();
        return -1;
    }

    // Define target positions (example: head joints)
    std::vector<double> target_positions = {
        0.0,  // idx11_head_joint1: target angle 0.0 rad
        0.0,  // idx12_head_joint2: target angle 0.0 rad
        0.0   // idx13_head_joint3: target angle 0.0 rad
    };

    std::cout << "\nTarget angles:" << std::endl;
    std::vector<std::string> head_joint_names = {
        "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"
    };
    for (size_t i = 0; i < head_joint_names.size(); i++) {
        std::cout << "  " << head_joint_names[i] << ": " << target_positions[i] << " rad" << std::endl;
    }

    // Execute head joint position servo control
    HeadJointServoController controller;
    controller.executeHeadJointServoControl(target_positions);

    // Release GDK system resources
    if (GDKRelease() != GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

13. MoveWaistJointServo()

  • Function: Waist joint position servo control interface. It must be controlled at a 100 Hz control frequency and supports normal mode and low-latency mode; normal mode is used by default.
  • Note: Low-latency mode has no collision protection; use it with caution for safety.
  • Parameters:
Parameter Type Description
positions const std::vector<double>& List of waist joint positions (radians), in the order "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3", "idx04_body_joint4", "idx05_body_joint5"
control_period const double Control period (seconds); recommended to be slightly larger than the control frequency
enable_low_latency const bool Whether to enable low-latency mode; defaults to false

Parameter description: - positions must have a length of 5 - The values in positions must be within the corresponding joint's limit range (see the joint limit values below); otherwise, ErrorCode::kInvalidInput is returned - control_period indicates the control period, in seconds; recommended to be slightly larger than the control frequency - enable_low_latency is used to select the control channel; low-latency mode is suitable for scenarios with higher real-time requirements

Joint Limit Values

The limit values for the waist joints (in radians) are as follows:

Joint Name Minimum Maximum
idx01_body_joint1 -1.082104 0.000174
idx02_body_joint2 -0.000174 2.652900
idx03_body_joint3 -1.919862 1.570970
idx04_body_joint4 -0.436332 0.436332
idx05_body_joint5 -3.045599 3.045599

  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

  • Error handling:

  • If the length of positions is not 5, ErrorCode::kInvalidInput is returned
  • If the values in positions exceed the corresponding joint's limit range, ErrorCode::kInvalidInput is returned

  • For other error conditions, the corresponding error code is returned

  • Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <vector>
#include <algorithm>

using namespace agibot::gdk;

// Control parameters
const double CONTROL_PERIOD = 0.01;  // Control period (seconds)
const double RATE_HZ = 100.0;        // Send frequency (Hz)
const double DURATION = 5.0;         // Control duration (seconds)

class WaistJointServoController {
private:
    Robot robot_;

    double getJointPositionByName(const JointStates& joint_states, const std::string& joint_name) {
        for (const auto& state : joint_states.states) {
            if (state.name == joint_name) {
                return state.motor_position;
            }
        }
        throw std::runtime_error("Joint name " + joint_name + " not found");
    }

    double interpolatePosition(double start_pos, double target_pos, double t) {
        return start_pos + t * (target_pos - start_pos);
    }

public:
    void executeWaistJointServoControl(const std::vector<double>& target_positions) {
        std::this_thread::sleep_for(std::chrono::seconds(1));  // Wait 1 second

        // Get current joint states
        JointStates current_joint_states;
        if (robot_.GetJointStates(current_joint_states) != GDKRes::kSuccess) {
            std::cout << "Failed to get joint states" << std::endl;
            return;
        }

        // Get the starting position
        std::vector<std::string> waist_joint_names = {
            "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3",
            "idx04_body_joint4", "idx05_body_joint5"
        };
        std::vector<double> start_positions;
        for (const auto& joint_name : waist_joint_names) {
            try {
                double pos = getJointPositionByName(current_joint_states, joint_name);
                start_positions.push_back(pos);
                std::cout << "Joint " << joint_name << " current position: " << pos << " rad" << std::endl;
            } catch (const std::exception& e) {
                std::cout << "Error: " << e.what() << std::endl;
                return;
            }
        }

        // Calculate number of steps
        int n_steps = static_cast<int>(DURATION * RATE_HZ);
        std::cout << "Total steps: " << n_steps << ", duration: " << DURATION << " s" << std::endl;

        // Execute trajectory
        double dt = 1.0 / RATE_HZ;
        auto start_time = std::chrono::steady_clock::now();

        for (int i = 0; i < n_steps; i++) {
            double t = static_cast<double>(i) / (n_steps - 1);

            // Compute current target position (linear interpolation)
            std::vector<double> current_positions;
            for (size_t j = 0; j < 5; j++) {
                double interp_pos = interpolatePosition(
                    start_positions[j], target_positions[j], t
                );
                current_positions.push_back(interp_pos);
            }

            // Use normal mode (enable_low_latency=false, the default)
            auto res = robot_.MoveWaistJointServo(
                current_positions, CONTROL_PERIOD);
            // To use low-latency mode, pass enable_low_latency=true:
            // auto res = robot_.MoveWaistJointServo(
            //     current_positions, target_velocities, CONTROL_PERIOD, true
            // );
            if (res != GDKRes::kSuccess) {
                std::cout << "Failed to send control command, step: " << i << std::endl;
                return;
            }

            // Control the send rate
            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto expected_time = std::chrono::milliseconds(static_cast<int>((i + 1) * dt * 1000));
            auto sleep_time = expected_time - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }

        std::cout << "Waist joint position servo control complete" << std::endl;

        // Hold the final position
        std::cout << "Entering final position hold (Ctrl+C to stop)..." << std::endl;
        try {
            while (true) {
                // Use normal mode (enable_low_latency=false, the default)
                auto res = robot_.MoveWaistJointServo(
                    target_positions, CONTROL_PERIOD);
                // To use low-latency mode, pass enable_low_latency=true:
                // auto res = robot_.MoveWaistJointServo(
                //     target_positions, target_velocities, CONTROL_PERIOD, true
                // );
                if (res != GDKRes::kSuccess) {
                    std::cout << "Failed to hold position" << std::endl;
                    break;
                }

                std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(dt * 1000)));
            }
        } catch (const std::exception& e) {
            std::cout << "Hold interrupted: " << e.what() << std::endl;
        }
    }
};

int main() {
    // Initialize the GDK system
    if (GDKInit() != GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // Get current joint states
    JointStates current_joint_states;
    if (robot.GetJointStates(current_joint_states) != GDKRes::kSuccess) {
        std::cout << "Failed to get joint states" << std::endl;
        GDKRelease();
        return -1;
    }

    // Define target positions (example: waist joints)
    std::vector<double> target_positions = {
        0.0,  // idx01_body_joint1: target angle 0.0 rad
        0.0,  // idx02_body_joint2: target angle 0.0 rad
        0.0,  // idx03_body_joint3: target angle 0.0 rad
        0.0,  // idx04_body_joint4: target angle 0.0 rad
        0.0   // idx05_body_joint5: target angle 0.0 rad
    };

    std::cout << "\nTarget angles:" << std::endl;
    std::vector<std::string> waist_joint_names = {
        "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3",
        "idx04_body_joint4", "idx05_body_joint5"
    };
    for (size_t i = 0; i < waist_joint_names.size(); i++) {
        std::cout << "  " << waist_joint_names[i] << ": " << target_positions[i] << " rad" << std::endl;
    }

    // Execute waist joint position servo control
    WaistJointServoController controller;
    controller.executeWaistJointServoControl(target_positions);

    // Release GDK system resources
    if (GDKRelease() != GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

14. MoveArmJointServo()

  • Function: Arm joint position servo control interface. It must be controlled at a 100 Hz control frequency and supports normal mode and low-latency mode; normal mode is used by default.
  • Note: Low-latency mode has no collision protection; use it with caution for safety.
  • Parameters:
Parameter Type Description
positions const std::vector<double>& List of arm joint positions (radians), in the order "idx21_arm_l_joint1", "idx22_arm_l_joint2", "idx23_arm_l_joint3", "idx24_arm_l_joint4", "idx25_arm_l_joint5", "idx26_arm_l_joint6", "idx27_arm_l_joint7", "idx61_arm_r_joint1", "idx62_arm_r_joint2", "idx63_arm_r_joint3", "idx64_arm_r_joint4", "idx65_arm_r_joint5", "idx66_arm_r_joint6", "idx67_arm_r_joint7"
control_period const double Control period (seconds); recommended to be slightly larger than the control frequency
control_group const int Control group: 0 controls the left arm, 1 controls the right arm, 2 controls both arms
enable_low_latency const bool Whether to enable low-latency mode; defaults to false

Parameter description: - positions has a length of 7 (single arm) or 14 (both arms) - The values in positions must be within the corresponding joint's limit range (see the joint limit values below); otherwise, ErrorCode::kInvalidInput is returned - control_period indicates the control period, in seconds; recommended to be slightly larger than the control frequency - control_group indicates the control group: 0 controls the left arm, 1 controls the right arm, 2 controls both arms - enable_low_latency is used to select the control channel; low-latency mode is suitable for scenarios with higher real-time requirements

Joint Limit Values

The limit values for the arm joints (in radians) are as follows:

Joint Name Minimum Maximum
idx21_arm_l_joint1 -3.071796 3.071796
idx22_arm_l_joint2 -2.059505 2.059505
idx23_arm_l_joint3 -3.071796 3.071796
idx24_arm_l_joint4 -2.495838 1.012308
idx25_arm_l_joint5 -3.071796 3.071796
idx26_arm_l_joint6 -1.012308 1.012308
idx27_arm_l_joint7 -1.535907 1.535907
idx61_arm_r_joint1 -3.071796 3.071796
idx62_arm_r_joint2 -2.059505 2.059505
idx63_arm_r_joint3 -3.071796 3.071796
idx64_arm_r_joint4 -2.495838 1.012308
idx65_arm_r_joint5 -3.071796 3.071796
idx66_arm_r_joint6 -1.012308 1.012308
idx67_arm_r_joint7 -1.535907 1.535907

  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

  • Error handling:

  • If the length of positions is not 7 (single arm) or 14 (both arms), ErrorCode::kInvalidInput is returned
  • If the values in positions exceed the corresponding joint's limit range, ErrorCode::kInvalidInput is returned

  • For other error conditions, the corresponding error code is returned

  • Example:

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <vector>
#include <algorithm>

using namespace agibot::gdk;

// Control parameters
const double CONTROL_PERIOD = 0.01;  // Control period (seconds)
const double RATE_HZ = 100.0;        // Send frequency (Hz)
const double DURATION = 5.0;         // Control duration (seconds)

class ArmJointServoController {
private:
    Robot robot_;

    double getJointPositionByName(const JointStates& joint_states, const std::string& joint_name) {
        for (const auto& state : joint_states.states) {
            if (state.name == joint_name) {
                return state.motor_position;
            }
        }
        throw std::runtime_error("Joint name " + joint_name + " not found");
    }

    double interpolatePosition(double start_pos, double target_pos, double t) {
        return start_pos + t * (target_pos - start_pos);
    }

public:
    void executeArmJointServoControl(const std::vector<double>& target_positions, const int control_group) {
        std::this_thread::sleep_for(std::chrono::seconds(1));  // Wait 1 second

        // Get current joint states
        JointStates current_joint_states;
        if (robot_.GetJointStates(current_joint_states) != GDKRes::kSuccess) {
            std::cout << "Failed to get joint states" << std::endl;
            return;
        }

        // Get the starting position
        std::vector<std::string> arm_joint_names = {
            "idx21_arm_l_joint1", "idx22_arm_l_joint2", "idx23_arm_l_joint3",
            "idx24_arm_l_joint4", "idx25_arm_l_joint5", "idx26_arm_l_joint6",
            "idx27_arm_l_joint7",  // 7 joints of the left arm
            "idx61_arm_r_joint1", "idx62_arm_r_joint2", "idx63_arm_r_joint3",
            "idx64_arm_r_joint4", "idx65_arm_r_joint5", "idx66_arm_r_joint6",
            "idx67_arm_r_joint7"   // 7 joints of the right arm
        };
        std::vector<double> start_positions;
        for (const auto& joint_name : arm_joint_names) {
            try {
                double pos = getJointPositionByName(current_joint_states, joint_name);
                start_positions.push_back(pos);
                std::cout << "Joint " << joint_name << " current position: " << pos << " rad" << std::endl;
            } catch (const std::exception& e) {
                std::cout << "Error: " << e.what() << std::endl;
                return;
            }
        }

        // Calculate number of steps
        int n_steps = static_cast<int>(DURATION * RATE_HZ);
        std::cout << "Total steps: " << n_steps << ", duration: " << DURATION << " s" << std::endl;

        // Execute trajectory
        double dt = 1.0 / RATE_HZ;
        auto start_time = std::chrono::steady_clock::now();

        for (int i = 0; i < n_steps; i++) {
            double t = static_cast<double>(i) / (n_steps - 1);

            // Compute current target position (linear interpolation)
            std::vector<double> current_positions;
            for (size_t j = 0; j < 14; j++) {
                double interp_pos = interpolatePosition(
                    start_positions[j], target_positions[j], t
                );
                current_positions.push_back(interp_pos);
            }

            // Use normal mode (enable_low_latency=false, the default)
            auto res = robot_.MoveArmJointServo(
                current_positions, CONTROL_PERIOD, control_group
            );
            // To use low-latency mode, pass enable_low_latency=true:
            // auto res = robot_.MoveArmJointServo(
            //     current_positions, CONTROL_PERIOD, control_group, true
            // );
            if (res != GDKRes::kSuccess) {
                std::cout << "Failed to send control command, step: " << i << std::endl;
                return;
            }

            // Control the send rate
            auto elapsed = std::chrono::steady_clock::now() - start_time;
            auto expected_time = std::chrono::milliseconds(static_cast<int>((i + 1) * dt * 1000));
            auto sleep_time = expected_time - elapsed;
            if (sleep_time.count() > 0) {
                std::this_thread::sleep_for(sleep_time);
            }
        }

        std::cout << "Arm joint position servo control complete" << std::endl;

        // Hold the final position
        std::cout << "Entering final position hold (Ctrl+C to stop)..." << std::endl;
        try {
            while (true) {
                // Use normal mode (enable_low_latency=false, the default)
                auto res = robot_.MoveArmJointServo(
                    target_positions, CONTROL_PERIOD, control_group
                );
                // To use low-latency mode, pass enable_low_latency=true:
                // auto res = robot_.MoveArmJointServo(
                //     target_positions, CONTROL_PERIOD, control_group, true
                // );
                if (res != GDKRes::kSuccess) {
                    std::cout << "Failed to hold position" << std::endl;
                    break;
                }

                std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(dt * 1000)));
            }
        } catch (const std::exception& e) {
            std::cout << "Hold interrupted: " << e.what() << std::endl;
        }
    }
};

int main() {
    // Initialize the GDK system
    if (GDKInit() != GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // Get current joint states
    JointStates current_joint_states;
    if (robot.GetJointStates(current_joint_states) != GDKRes::kSuccess) {
        std::cout << "Failed to get joint states" << std::endl;
        GDKRelease();
        return -1;
    }

    // Define target positions (example: arm joints)
    std::vector<double> target_positions = {
        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  // 7 joints of the left arm
        0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0   // 7 joints of the right arm
    };

    std::cout << "\nTarget angles:" << std::endl;
    std::vector<std::string> arm_joint_names = {
        "idx21_arm_l_joint1", "idx22_arm_l_joint2", "idx23_arm_l_joint3",
        "idx24_arm_l_joint4", "idx25_arm_l_joint5", "idx26_arm_l_joint6",
        "idx27_arm_l_joint7",  // 7 joints of the left arm
        "idx61_arm_r_joint1", "idx62_arm_r_joint2", "idx63_arm_r_joint3",
        "idx64_arm_r_joint4", "idx65_arm_r_joint5", "idx66_arm_r_joint6",
        "idx67_arm_r_joint7"   // 7 joints of the right arm
    };
    for (size_t i = 0; i < arm_joint_names.size(); i++) {
        std::cout << "  " << arm_joint_names[i] << ": " << target_positions[i] << " rad" << std::endl;
    }

    // Execute arm joint position servo control
    ArmJointServoController controller;
    controller.executeArmJointServoControl(target_positions, 2);

    // Release GDK system resources
    if (GDKRelease() != GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

15. EndEffectorPoseControl()

  • Function: End-effector pose control interface
  • Note: This interface requires the sender to publish control commands at a 50 Hz frequency; step signals are not allowed. This interface has no collision detection, so be mindful of the environment when using it.
  • Parameters:
Parameter Type Description
end_pose const EndEffectorPose& End-effector pose control object
  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

EndEffectorPose Object Details

The EndEffectorPose struct contains the following members:

Member Type Description Unit
life_time double Control lifetime Seconds
group int32_t Control group: 0: unknown, 4: left arm, 8: right arm, 12: both arms None
left_end_effector_pose Pose Left end-effector pose (in the base_link frame) Pose
right_end_effector_pose Pose Right end-effector pose (in the base_link frame) Pose 
struct EndEffectorPose {
  double life_time{0.0};
  int32_t group{0};
  Pose left_end_effector_pose{};
  Pose right_end_effector_pose{};
};

Pose struct:

Member Type Description Unit
position Position Position information Meters
orientation Orientation Orientation information Quaternion

Position struct:

Member Type Description Unit
x double X coordinate Meters
y double Y coordinate Meters
z double Z coordinate Meters

Orientation struct:

Member Type Description Unit
x double Quaternion X component None
y double Quaternion Y component None
z double Quaternion Z component None
w double Quaternion W component None
  • Example:
#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <cmath>
#include <vector>
#include <algorithm>

using namespace agibot::gdk;

const std::string LEFT_NAME = "arm_l_end_link";
const std::string RIGHT_NAME = "arm_r_end_link";
const int32_t CONTROL_GROUP = 12;  // kBothArms

const Pose TARGET_LEFT = {
    {0.516, 0.433, 1.081},  // position
    {0.382, -0.146, 0.663, 0.626}  // orientation
};

const Pose TARGET_RIGHT = {
    {0.579, -0.306, 1.158},  // position
    {0.320, 0.655, 0.651, 0.206}  // orientation
};

const double MAX_STEP_CM = 0.1;  // Maximum step size (centimeters)
const double LIFETIME = 0.02;    // Lifetime (seconds)
const double RATE_HZ = 50.0;     // Send frequency (Hz)
const bool HOLD_FINAL = true;    // Whether to hold the final pose

class EndEffectorController {
private:
    Robot robot_;

    void slerp(const double q0[4], const double q1[4], double t, double result[4]) {
        double dot = q0[0]*q1[0] + q0[1]*q1[1] + q0[2]*q1[2] + q0[3]*q1[3];

        // If the dot product is negative, negate q1 to ensure the shortest path
        if (dot < 0.0) {
            dot = -dot;
            for (int i = 0; i < 4; i++) {
                result[i] = q0[i] + t * (-q1[i] - q0[i]);
            }
        } else {
            for (int i = 0; i < 4; i++) {
                result[i] = q0[i] + t * (q1[i] - q0[i]);
            }
        }

        // Clamp the dot product range
        dot = std::clamp(dot, -1.0, 1.0);

        if (dot > 0.9995) {
            // Linear interpolation
            double norm = 0.0;
            for (int i = 0; i < 4; i++) {
                norm += result[i] * result[i];
            }
            norm = std::sqrt(norm);
            if (norm > 0.0) {
                for (int i = 0; i < 4; i++) {
                    result[i] /= norm;
                }
            }
        } else {
            // Spherical linear interpolation
            double theta_0 = std::acos(dot);
            double sin_theta_0 = std::sin(theta_0);
            double theta = theta_0 * t;
            double sin_theta = std::sin(theta);
            double s0 = std::cos(theta) - dot * sin_theta / sin_theta_0;
            double s1 = sin_theta / sin_theta_0;

            for (int i = 0; i < 4; i++) {
                result[i] = s0 * q0[i] + s1 * q1[i];
            }
        }
    }

    double distanceBetweenPoints(const Vector3& p1, const Vector3& p2) {
        double dx = p2.x - p1.x;
        double dy = p2.y - p1.y;
        double dz = p2.z - p1.z;
        return std::sqrt(dx*dx + dy*dy + dz*dz);
    }

    int calculateNSteps(const Vector3& start, const Vector3& goal, double max_step_cm) {
        double dist_cm = distanceBetweenPoints(start, goal) * 100.0;
        return std::max(static_cast<int>(std::ceil(dist_cm / max_step_cm)), 1);
    }

    std::vector<Pose> planTrajectory(const Pose& start, const Pose& goal, int n_steps) {
        std::vector<Pose> trajectory;

        for (int i = 0; i < n_steps; i++) {
            double t = static_cast<double>(i) / (n_steps - 1);
            Pose pose;

            // Linear interpolation of position
            pose.position.x = start.position.x + t * (goal.position.x - start.position.x);
            pose.position.y = start.position.y + t * (goal.position.y - start.position.y);
            pose.position.z = start.position.z + t * (goal.position.z - start.position.z);

            // Quaternion SLERP interpolation
            double q0[4] = {start.orientation.x, start.orientation.y, start.orientation.z, start.orientation.w};
            double q1[4] = {goal.orientation.x, goal.orientation.y, goal.orientation.z, goal.orientation.w};
            double result[4];
            slerp(q0, q1, t, result);

            pose.orientation.x = result[0];
            pose.orientation.y = result[1];
            pose.orientation.z = result[2];
            pose.orientation.w = result[3];

            trajectory.push_back(pose);
        }

        return trajectory;
    }

    Pose findPoseByName(const std::vector<std::string>& frame_names,
                       const std::vector<Pose>& frame_poses,
                       const std::string& target_name) {
        for (size_t i = 0; i < frame_names.size(); i++) {
            if (frame_names[i] == target_name) {
                return frame_poses[i];
            }
        }
        throw std::runtime_error("Frame name " + target_name + " not found");
    }

public:
    void executeEndPoseControl() {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));  // Wait 1 second

        // Get the current state
        MotionControlStatus status;
        GDKRes result = robot_.GetMotionControlStatus(status);
        if (result != GDKRes::kSuccess) {
            std::cout << "Failed to get motion control status" << std::endl;
            return;
        }

        // Get the starting pose
        Pose start_left_pose = findPoseByName(status.frame_names, status.frame_poses, LEFT_NAME);
        Pose start_right_pose = findPoseByName(status.frame_names, status.frame_poses, RIGHT_NAME);

        // Calculate number of steps
        int n_left = calculateNSteps(start_left_pose.position, TARGET_LEFT.position, MAX_STEP_CM);
        int n_right = calculateNSteps(start_right_pose.position, TARGET_RIGHT.position, MAX_STEP_CM);
        int n_steps = std::max(n_left, n_right);

        std::cout << "Left arm steps: " << n_left << ", right arm steps: " << n_right << ", total steps: " << n_steps << std::endl;

        // Plan trajectory
        std::vector<Pose> traj_left = planTrajectory(start_left_pose, TARGET_LEFT, n_steps);
        std::vector<Pose> traj_right = planTrajectory(start_right_pose, TARGET_RIGHT, n_steps);

        // Execute trajectory
        double dt = 1.0 / RATE_HZ;
        for (int i = 0; i < n_steps; i++) {
            EndEffectorPose end_pose;
            end_pose.life_time = LIFETIME;
            end_pose.group = CONTROL_GROUP;
            end_pose.left_end_effector_pose = traj_left[i];
            end_pose.right_end_effector_pose = traj_right[i];

            result = robot_.EndEffectorPoseControl(end_pose);
            if (result != GDKRes::kSuccess) {
                std::cout << "Failed to send control command, step: " << i << std::endl;
                return;
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(dt * 1000)));
        }

        // Hold the final pose (corresponds to the Python version)
        if (HOLD_FINAL) {
            std::cout << "Entering end-effector pose hold (Ctrl+C to stop)..." << std::endl;
            try {
                while (true) {
                    EndEffectorPose end_pose;
                    end_pose.life_time = LIFETIME;
                    end_pose.group = CONTROL_GROUP;
                    end_pose.left_end_effector_pose = traj_left.back();
                    end_pose.right_end_effector_pose = traj_right.back();

                    result = robot_.EndEffectorPoseControl(end_pose);
                    if (result != GDKRes::kSuccess) {
                        std::cout << "Failed to hold pose" << std::endl;
                        break;
                    }

                    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(dt * 1000)));
                }
            } catch (const std::exception& e) {
                std::cout << "Hold interrupted: " << e.what() << std::endl;
            }
        }
    }
};

int main() {
    EndEffectorController controller;
    controller.executeEndPoseControl();
    return 0;
}

16. MoveEEPos()

  • Function: Controls the end-effector position
  • Parameters:
Parameter Type Description
joint_states const JointStates& Joint state control parameter

JointStates Object Details (for MoveEEPos)

The JointStates struct contains the following members:

Member Type Description Unit
nums size_t Number of joints; should equal states.size() None
group std::string Control group; must be "left_tool", "right_tool", or "dual_tool" String
target_type std::string Target type; see the table below for supported values String
states std::vector<JointState> List of joint states (required) List of joint states

Supported values of target_type and the corresponding joint-count requirements:

target_type Value Required Joint Count Description
"omnipicker" 1 Omnidirectional gripper, requires 1 joint
"dahuan" 1 Dahuan end-effector, requires 1 joint
"ctek90d" 1 CTEK90D end-effector, requires 1 joint
"o10_t2" 10 O10 dexterous-hand end-effector, requires 10 joints
"o12_t2" 12 O12 dexterous-hand end-effector, requires 12 joints

Joint position value ranges for each end-effector type:

  • Note: The joint value ranges are tied to the version; be mindful of the current version's value ranges when using them. For the current version, the ranges given in this document take precedence.
  • omnipicker: position ranges over [-0.785,0], where -0.785 means open and 0 means closed
  • dahuan: position ranges over [0,0.025], where 0 means open and 0.025 means closed
  • ctek90d: position ranges over [-0.91, 0], where -0.91 means open and 0 means closed
  • o10_t2: The joint position value ranges are as follows (in radians)

Left-hand joint limit values:

Joint Index Joint Name Minimum Maximum
0 idx31_hand_l_thumb_roll_joint -1.1213740444063567 0.029670597283903602
1 idx32_hand_l_thumb_abad_joint -0.04537856055185257 1.642354826126664
2 idx33_hand_l_thumb_mcp_joint -0.8415977653116657 0.0
3 idx36_hand_l_index_abad_joint 0.0 0.16406094968746698
4 idx37_hand_l_index_pip_joint 0.0 1.4835298641951802
5 idx39_hand_l_middle_pip_joint 0.0 1.4835298641951802
6 idx41_hand_l_ring_abad_joint -0.16929693744344995 0.0
7 idx42_hand_l_ring_pip_joint 0.0 1.4835298641951802
8 idx44_hand_l_pinky_abad_joint -0.1850049007113989 0.0
9 idx45_hand_l_pinky_pip_joint 0.0 1.4835298641951802

Right-hand joint limit values:

Joint Index Joint Name Minimum Maximum
0 idx71_hand_r_thumb_roll_joint -0.029670597283903602 1.1213740444063567
1 idx72_hand_r_thumb_abad_joint -1.642354826126664 0.04537856055185257
2 idx73_hand_r_thumb_mcp_joint 0.0 0.8415977653116657
3 idx76_hand_r_index_abad_joint -0.16406094968746698 0.0
4 idx77_hand_r_index_pip_joint 0.0 1.4835298641951802
5 idx79_hand_r_middle_pip_joint 0.0 1.4835298641951802
6 idx81_hand_r_ring_abad_joint 0.0 0.16929693744344995
7 idx82_hand_r_ring_pip_joint 0.0 1.4835298641951802
8 idx84_hand_r_pinky_abad_joint 0.0 0.1850049007113989
9 idx85_hand_r_pinky_pip_joint 0.0 1.4835298641951802

Typical state values: - Left hand open: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - Left hand grip: [-0.2, 1.45, -0.75, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0] - Right hand open: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - Right hand grip: [0.2, -1.45, 0.75, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0]

  • o12_t2: The joint position value ranges are as follows (in radians)

Left-hand joint limit values:

Joint Index Joint Name Minimum Maximum
0 idx31_hand_l_thumb_roll_joint -0.9425 0.0
1 idx32_hand_l_thumb_abad_joint 0.0 1.3875
2 idx33_hand_l_thumb_mcp_joint -0.8273 0.0
3 idx34_hand_l_thumb_pip_joint -1.2915 0.0
4 idx36_hand_l_index_abad_joint -0.2618 0.2618
5 idx37_hand_l_index_mcp_joint 0.0 1.3526
6 idx38_hand_l_index_pip_joint 0.0 1.5307
7 idx40_hand_l_middle_abad_joint -0.2618 0.2618
8 idx41_hand_l_middle_mcp_joint 0.0 1.3579
9 idx42_hand_l_middle_pip_joint 0.0 1.8151
10 idx44_hand_l_ring_mcp_joint 0.0 1.5359
11 idx47_hand_l_pinky_mcp_joint 0.0 1.5359

Right-hand joint limit values:

Joint Index Joint Name Minimum Maximum
0 idx71_hand_r_thumb_roll_joint 0.0 0.9425
1 idx72_hand_r_thumb_abad_joint -1.3875 0.0
2 idx73_hand_r_thumb_mcp_joint -0.8273 0.0
3 idx74_hand_r_thumb_pip_joint -1.2915 0.0
4 idx76_hand_r_index_abad_joint -0.2618 0.2618
5 idx77_hand_r_index_mcp_joint 0.0 1.3526
6 idx78_hand_r_index_pip_joint 0.0 1.5307
7 idx80_hand_r_middle_abad_joint -0.2618 0.2618
8 idx81_hand_r_middle_mcp_joint 0.0 1.3579
9 idx82_hand_r_middle_pip_joint 0.0 1.8151
10 idx84_hand_r_ring_mcp_joint 0.0 1.5359
11 idx87_hand_r_pinky_mcp_joint 0.0 1.5359

Typical state values: - Left hand open: [-0.53, 0.42, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - Left hand grip: [-0.77, 0.5, -0.4, -0.36, -0.12, 0.69, 0.46, 0.0, 0.72, 0.5, 0.63, 0.63] - Right hand open: [0.53, -0.42, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - Right hand grip: [0.77, -0.5, -0.4, -0.36, 0.12, 0.69, 0.46, 0.0, 0.72, 0.5, 0.63, 0.63]

Parameter description: - group must be "left_tool" (left end-effector), "right_tool" (right end-effector), or "dual_tool" (both end-effectors); any other value returns ErrorCode::kInvalidInput - When group is "left_tool" or "right_tool", states.size() must exactly match the joint-count requirement corresponding to target_type - When group is "dual_tool", states.size() must be 2x the joint-count requirement corresponding to target_type (the first half is used for the left end-effector, the second half for the right end-effector) - target_type must be one of the supported values listed above; any other value returns ErrorCode::kInvalidInput - nums must equal the size of the states vector - states cannot be empty; states.size() must be greater than 0, otherwise ErrorCode::kInvalidInput is returned

JointState struct:

Member Type Description Unit
position double Joint position (required) Opening degree

  • Return value: GDKRes, the operation result status code. Returns GDKRes::kSuccess on success

  • Error handling:

  • If joint_states.states.size() <= 0, ErrorCode::kInvalidInput is returned
  • If target_type is not one of the supported values ("omnipicker", "dahuan", "ctek90d", "o10_t2", "o12_t2"), ErrorCode::kInvalidInput is returned
  • If joint_states.states.size() does not match the joint-count requirement corresponding to target_type (for "dual_tool", it must be 2x), ErrorCode::kInvalidInput is returned
  • If group is not "left_tool", "right_tool", or "dual_tool", ErrorCode::kInvalidInput is returned
  • If a joint position value exceeds the value range for the corresponding end-effector type, ErrorCode::kInvalidInput is returned

  • For other error conditions, the corresponding error code is returned

  • Example:

Example 1: Controlling the left gripper (omnipicker type, requires 1 joint)

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Control the left gripper (omnipicker type, requires 1 joint)
    agibot::gdk::JointStates left_joint_states;
    left_joint_states.group = "left_tool";
    left_joint_states.target_type = "omnipicker";
    left_joint_states.states.resize(1);
    left_joint_states.states[0].position = 0;  // Value range [-0.785, 0]
    left_joint_states.nums = left_joint_states.states.size();

    if (robot.MoveEEPos(left_joint_states) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move left end effector position" << std::endl;
    } else {
        std::cout << "Left end-effector position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

Example 2: Controlling the right gripper (dahuan type, requires 1 joint)

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Control the right gripper (dahuan type, requires 1 joint)
    agibot::gdk::JointStates right_joint_states;
    right_joint_states.group = "right_tool";
    right_joint_states.target_type = "dahuan";
    right_joint_states.states.resize(1);
    right_joint_states.states[0].position = 0;  // Value range [0, 0.025]
    right_joint_states.nums = right_joint_states.states.size();

    if (robot.MoveEEPos(right_joint_states) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move right end effector position" << std::endl;
    } else {
        std::cout << "Right end-effector position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

Example 3: Controlling the left gripper (ctek90d type, requires 1 joint)

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Control the left gripper (ctek90d type, requires 1 joint)
    agibot::gdk::JointStates left_joint_states;
    left_joint_states.group = "left_tool";
    left_joint_states.target_type = "ctek90d";
    left_joint_states.states.resize(1);
    left_joint_states.states[0].position = 0;  // Value range [-0.91, 0]
    left_joint_states.nums = left_joint_states.states.size();

    if (robot.MoveEEPos(left_joint_states) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move left end effector position" << std::endl;
    } else {
        std::cout << "Left end-effector position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

Example 4: Controlling the left end-effector (o10_t2 type, requires 10 joints)

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Control the left end-effector (o10_t2 type, requires 10 joints)
    agibot::gdk::JointStates left_o10_states;
    left_o10_states.group = "left_tool";
    left_o10_states.target_type = "o10_t2";
    left_o10_states.states.resize(10);
    for (size_t i = 0; i < 10; ++i) {
        left_o10_states.states[i].position = 0.0;
    }
    left_o10_states.nums = left_o10_states.states.size();

    if (robot.MoveEEPos(left_o10_states) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move left end effector (o10_t2)" << std::endl;
    } else {
        std::cout << "Left end-effector (o10_t2) position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

Example 5: Controlling the right end-effector (o12_t2 type, requires 12 joints)

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Control the right end-effector (o12_t2 type, requires 12 joints)
    agibot::gdk::JointStates right_o12_states;
    right_o12_states.group = "right_tool";
    right_o12_states.target_type = "o12_t2";
    right_o12_states.states.resize(12);
    for (size_t i = 0; i < 12; ++i) {
        right_o12_states.states[i].position = 0.0;
    }
    right_o12_states.nums = right_o12_states.states.size();

    if (robot.MoveEEPos(right_o12_states) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move right end effector (o12_t2)" << std::endl;
    } else {
        std::cout << "Right end-effector (o12_t2) position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

Example 6: Controlling both end-effectors (dual_tool, requires 2x the joint count)

#include "gdk/gdk.h"
#include <iostream>
#include <chrono>
#include <thread>

int main()
{
    // Initialize the GDK system
    if (agibot::gdk::GDKInit() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK initialization failed" << std::endl;
        return -1;
    }
    std::cout << "GDK initialized successfully" << std::endl;

    agibot::gdk::Robot robot;
    std::cout << "Robot init" << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(1));

    // Control both end-effectors (dual_tool, requires 2x the joint count)
    // For example: using the omnipicker type requires 2 joints (1 left + 1 right)
    agibot::gdk::JointStates dual_states;
    dual_states.group = "dual_tool";
    dual_states.target_type = "omnipicker";
    dual_states.states.resize(2);
    // The first half is used for the left end-effector
    dual_states.states[0].position = 0.0;
    // The second half is used for the right end-effector
    dual_states.states[1].position = 0.0;
    dual_states.nums = dual_states.states.size();

    if (robot.MoveEEPos(dual_states) != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "Failed to move dual end effector" << std::endl;
    } else {
        std::cout << "Dual end-effector position control succeeded" << std::endl;
    }

    // Release GDK system resources
    if (agibot::gdk::GDKRelease() != agibot::gdk::GDKRes::kSuccess) {
        std::cout << "GDK release failed" << std::endl;
        return -1;
    }
    std::cout << "GDK released successfully" << std::endl;

    return 0;
}

Usage Notes

  1. GDK initialization: Before using Robot functionality, you must first call agibot::gdk::GDKInit() to initialize the GDK system
  2. GDK release: Before the program exits, you must call agibot::gdk::GDKRelease() to release GDK system resources
  3. Initialization wait: After creating the Robot object, it is recommended to wait 1 second to ensure the DDS connection is established
  4. Joint names: Confirm the correctness of joint names before use; they can be obtained via GetJointStates()
  5. Position range: Joint positions should be within a safe range to avoid exceeding mechanical limits
  6. Velocity limit: Set reasonable joint velocities to avoid hazards from excessively fast motion
  7. Lifetime: Set the request lifetime appropriately to avoid command expiration
  8. Error handling: Check return values promptly and handle possible error conditions
  9. Control group selection: Select the appropriate control group (left arm/right arm/both arms) when controlling the end-effector
  10. Pose setting: Pay attention to the correctness of the coordinate frame when setting the end-effector pose
  11. State monitoring: Periodically check error codes to ensure the robot operates safely
  12. Temperature monitoring: Monitor motor temperature to avoid damage from overheating
  13. Emergency stop state: Check the emergency stop state to ensure the robot can be controlled normally
  14. Data synchronization: Synchronize multi-sensor data based on timestamps
  15. Error handling: Always check the GDKRes return value to ensure the operation succeeded

Application Scenarios

  • Robot control: Implements comprehensive motion control of the robot
  • State monitoring: Monitors the state of the robot's components in real time
  • Action execution: Executes complex robot action sequences
  • Path planning: Performs path planning based on state information
  • Safety detection: Monitors abnormal robot states to ensure safe operation
  • End-effector control: Precisely controls the pose of the end-effector
  • Dual-arm coordination: Implements coordinated control of a dual-arm robot
  • Joint control: Implements high-precision joint position control
  • Fault diagnosis: Performs fault diagnosis and handling via error codes
  • Data fusion: Combines multi-sensor data for robot control