Skip to content

GDK Robot Control Interface Documentation (Python)

Overview

The robot control module provides the G02 robot with capabilities such as retrieving joint states, controlling robot motion, and retrieving whole-body status. Through the Python interface, developers can conveniently implement fine-grained control of robot motion, suitable for a variety of scenarios such as basic control, status detection, and motion recording/playback.

Interface Description

Robot Class

This class encapsulates the main functional interfaces for robot control.

1. get_joint_states()

  • Notes: Use motor_position and motor_velocity to obtain the motor position and velocity. position and velocity are fields reserved for low-speed motors and do not need to be used in the current version.
  • Function: Retrieves joint state information
  • Parameters: None
  • Returns: dict, containing the following attributes:
Attribute Type Description Unit
timestamp int Timestamp nanoseconds
nums int Number of joints integer
states list[dict] List of joint states list of dicts

Structure of each joint state in states:

Attribute Type Description Unit
name str Joint name string
mode int Joint mode integer
position float Joint position radians
velocity float Joint velocity radians/second
effort float Joint torque Newton-meters
motor_position float Motor position radians
motor_velocity float Motor velocity radians/second
motor_current float Motor current amperes
error_code int Error code integer
  • Example:
import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Get joint states
joint_states = robot.get_joint_states()
print(f"Joint count: {joint_states['nums']}")
print(f"Timestamp: {joint_states['timestamp']}")

for state in joint_states['states']:
    print(f"Joint: {state['name']}")
    print(f"  Position: {state['position']:.3f} rad")
    print(f"  Velocity: {state['velocity']:.3f} rad/s")
    print(f"  Torque: {state['effort']:.3f} N·m")
    print(f"  Motor position: {state['motor_position']:.3f} rad")
    print(f"  Motor current: {state['motor_current']:.3f} A")
    print(f"  Error code: {state['error_code']}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

2. get_whole_body_status()

  • Function: Retrieves whole-body status information
  • Parameters: None
  • Returns: dict, containing the following attributes:
Attribute Type Description Unit
timestamp int Timestamp nanoseconds
right_arm_error int Right arm error code integer
left_arm_error int Left arm error code integer
right_arm_control bool Right arm control state boolean
left_arm_control bool Left arm control state boolean
right_arm_estop bool Right arm e-stop state boolean
left_arm_estop bool Left arm e-stop state boolean
right_end_error int Right end effector error code integer
left_end_error int Left end effector error code integer
right_end_model str Right end effector model string
left_end_model str Left end effector model string
waist_error int Waist error code integer
lift_error int Lift error code integer
neck_error int Head error code integer
chassis_error int Chassis error code integer

Meaning of mode values:

Value Meaning
0 Stopped
1 G1 servo
2 Path planning
5 G2 servo
  • Example:
import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Get whole-body status
status = robot.get_whole_body_status()

# Print basic information
print(f"Timestamp: {status['timestamp']}")
print(f"Right end effector model: {status['right_end_model']}")
print(f"Left end effector model: {status['left_end_model']}")

# Check error status
print("\n=== Error status check ===")
if status['right_arm_error'] == 0:
    print("✅ Right arm OK")
else:
    print(f"❌ Right arm error code: {status['right_arm_error']}")

if status['left_arm_error'] == 0:
    print("✅ Left arm OK")
else:
    print(f"❌ Left arm error code: {status['left_arm_error']}")

if status['waist_error'] == 0:
    print("✅ Waist OK")
else:
    print(f"❌ Waist error code: {status['waist_error']}")

if status['neck_error'] == 0:
    print("✅ Head OK")
else:
    print(f"❌ Head error code: {status['neck_error']}")

if status['chassis_error'] == 0:
    print("✅ Chassis OK")
else:
    print(f"❌ Chassis error code: {status['chassis_error']}")

# Check control status
print("\n=== Control status ===")
print(f"Right arm control: {'Yes' if status['right_arm_control'] else 'No'}")
print(f"Left arm control: {'Yes' if status['left_arm_control'] else 'No'}")
print(f"Right arm e-stop: {'Yes' if status['right_arm_estop'] else 'No'}")
print(f"Left arm e-stop: {'Yes' if status['left_arm_estop'] else 'No'}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

3. get_motion_control_status()

  • Function: Retrieves end-effector motion control status
  • Parameters: None
  • Returns: A MotionControlStatus object, containing the following attributes:
Attribute Type Description Unit
frame_names list[str] List of end-effector joint names list of strings
frame_poses list[Pose] List of end-effector joint poses list of poses
collision_pairs_1 list[str] Collision pair list 1 list of strings
collision_pairs_2 list[str] Collision pair list 2 list of strings
mode int Motion mode integer
error_code int Error code integer
error_msg str Error message string
twists list[Twist] List of velocities list of velocities
wrenches list[Wrench] List of forces/torques list of forces/torques

Meaning of mode values:

Value Meaning
0 Stopped
1 G1 servo
2 Path planning
5 G2 servo
  • Example:
import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Get motion control status
status = robot.get_motion_control_status()
print(f"Motion mode: {status.mode}")
print(f"Error code: {status.error_code}")
print(f"Error message: {status.error_msg}")
print(f"Joint count: {len(status.frame_names)}")
print(f"Collision pair count: {len(status.collision_pairs_1)}")

# Print all joint names
for i, frame_name in enumerate(status.frame_names):
    print(f"Joint {i}: {frame_name}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

4. get_end_state()

  • Function: Retrieves end-effector state information
  • Parameters: None
  • Returns: dict, containing the following attributes:
Attribute Type Description Unit
left_end_state dict Left end effector state information dict
right_end_state dict Right end effector state information dict

Structure of left_end_state/right_end_state:

Attribute Type Description Unit
controlled bool Whether it is being controlled boolean
type int End effector type integer
names list[str] List of joint names list of strings
end_states list[dict] List of joint states list of dicts

Structure of each joint state in end_states:

Attribute Type Description Unit
id int Joint ID integer
enable bool Whether it is enabled boolean
position float Joint position radians
velocity float Joint velocity radians/second
effort float Joint torque Newton-meters
current float Motor current amperes
voltage float Motor voltage volts
temperature float Motor temperature Celsius
status int Status code integer
err_code int Error code integer
  • Example:
import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Get end effector state
end_state = robot.get_end_state()

# Print left end effector state
left_state = end_state['left_end_state']
print(f"Left end effector control status: {left_state['controlled']}")
print(f"Left end effector type: {left_state['type']}")
print(f"Left end effector joints: {left_state['names']}")

# Print left end effector joint details
for i, joint_state in enumerate(left_state['end_states']):
    print(f"\nLeft end effector joint {i+1}:")
    print(f"  Joint ID: {joint_state['id']}")
    print(f"  Enabled: {joint_state['enable']}")
    print(f"  Position: {joint_state['position']:.3f} stroke value")
    print(f"  Velocity: {joint_state['velocity']:.3f} stroke value/s")
    print(f"  Torque: {joint_state['effort']:.3f} N·m")
    print(f"  Current: {joint_state['current']:.3f} A")
    print(f"  Voltage: {joint_state['voltage']:.3f} V")
    print(f"  Temperature: {joint_state['temperature']:.1f} °C")
    print(f"  Status code: {joint_state['status']}")
    print(f"  Error code: {joint_state['err_code']}")

# Print right end effector state
right_state = end_state['right_end_state']
print(f"\nRight end effector control status: {right_state['controlled']}")
print(f"Right end effector type: {right_state['type']}")
print(f"Right end effector joints: {right_state['names']}")

# Check end effector status
print("\n=== End effector status check ===")
for side in ['left', 'right']:
    state = end_state[f'{side}_end_state']
    if state['controlled']:
        print(f"✅ {side} end effector is being controlled")
    else:
        print(f"❌ {side} end effector is not controlled")

    for joint_state in state['end_states']:
        if joint_state['err_code'] == 0:
            print(f"✅ {side} end effector joint {joint_state['id']} OK")
        else:
            print(f"❌ {side} end effector joint {joint_state['id']} error code: {joint_state['err_code']}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

5. get_chassis_power_state()

  • Function: Retrieves the chassis power state
  • Returns: A ChassisPowerState object, containing the following attributes:
Attribute Type Description Unit
battery_main_power_switch_state uint8_t Battery main power switch state unitless
emergency_stop_pedal_state uint8_t Emergency stop pedal state unitless
battery_main_power_switch_fault_state uint8_t Battery main power switch fault state unitless
emergency_stop_pedal_fault_state uint8_t Emergency stop pedal fault state unitless
chassis_power_board_state uint8_t Chassis power board state unitless
chassis_left_traction_motor_power_state uint8_t Chassis left traction motor power state unitless
chassis_right_traction_motor_power_state uint8_t Chassis right traction motor power state unitless
chassis_left_steering_motor_power_state uint8_t Chassis left steering motor power state unitless
chassis_right_steering_motor_power_state uint8_t Chassis right steering motor power state unitless
chassis_lidar1_power_state uint8_t Chassis LiDAR 1 power state unitless
chassis_lidar2_power_state uint8_t Chassis LiDAR 2 power state unitless
chassis_ultrasonic_radar_power_state uint8_t Chassis ultrasonic radar power state unitless
chassis_tof_camera_power_state uint8_t Chassis ToF camera power state unitless
chassis_ethernet_switch_power_state uint8_t Chassis Ethernet switch power state unitless
chassis_external_power_output_state uint8_t Chassis external power output state unitless
battery_main_power_output_switch_state uint8_t Battery main power output switch state unitless
battery_states list[BatteryState] List of battery states list of battery states
charge_plug_insert_state uint8_t Charging plug insertion state unitless
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 unitless
charge_plug_input_open_circuit_fault_state uint8_t Charging plug input open-circuit fault state unitless
chassis_led_strip_power_state uint8_t Chassis LED strip power state unitless
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 unitless
power_poe_bus_power_on_fault_state uint8_t PoE bus power-on fault state unitless
chassis_board_12v_output_fault_state uint8_t Chassis board 12V output fault state unitless
chassis_board_5v_output_fault_state uint8_t Chassis board 5V output fault state unitless
chassis_power_board_fault_state uint32_t Chassis power board fault state unitless
  • Example:
import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Get chassis power state
chassis_power_state = robot.get_chassis_power_state()
print(chassis_power_state)

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

6. get_chest_power_state()

  • Function: Retrieves the chest power state
  • Returns: A ChestPowerState object, containing the following attributes:
Attribute Type Description Unit
power_onoff_req uint8_t Power on/off request unitless
emergency_stop_button_req uint8_t Emergency stop button request unitless
power_switch_fault_state uint8_t Power switch fault state unitless
emergency_stop_button_fault_state uint8_t Emergency stop button fault state unitless
power_full_low_req uint8_t Power full/low request unitless
chest_power_board_power_state uint8_t Chest power board state unitless
domain_controller_power_state uint8_t Domain controller power state unitless
head_interactive_board_power_state uint8_t Head interactive board power state unitless
curved_screen_power_state uint8_t Curved screen power state unitless
head_yaw_motor_power_state uint8_t Head yaw motor power state unitless
head_pitch_motor_power_state uint8_t Head pitch motor power state unitless
head_roll_motor_power_state uint8_t Head roll motor power state unitless
fan_power_state uint8_t Fan power state unitless
chest_power_board_fan_fault_state uint8_t Chest power board fan fault state unitless
body_fan1_fault_state uint8_t Body fan 1 fault state unitless
body_fan2_fault_state uint8_t Body fan 2 fault state unitless
body_fan3_fault_state uint8_t Body fan 3 fault state unitless
body_fan4_fault_state uint8_t Body fan 4 fault state unitless
upper_body_led_strip_power_state uint8_t Upper body LED strip power state unitless
poe_power_state uint8_t PoE power state unitless
ipad_power_state uint8_t iPad power state unitless
chest_reserved_lidar_power_state uint8_t Chest reserved LiDAR power state unitless
chest_power_board_temperature float Chest power board temperature °C
chest_power_board_fault_state uint32_t Chest power board fault state unitless
  • Example:
import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Get chest power state
chest_power_state = robot.get_chest_power_state()
print(chest_power_state)

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

7. joint_control_request()

  • Function: Joint position planning control interface; the call returns after the target position has been reached.
  • Parameters: A JointControlReq object, containing the following attributes:
Attribute Type Description Unit
life_time float Lifetime seconds
joint_names list[str] List of joint names list of strings
joint_positions list[float] List of joint positions radians
joint_velocities list[float] List of joint velocities radians/second
detail str Details string
  • Returns: int, 0 indicates success; an exception is raised on failure

Joint limit values

The limit values (in radians) for each joint 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:
import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Create joint control request
joint_control_req = agibot_gdk.JointControlReq()
joint_control_req.joint_names = ["idx01_body_joint1", "idx02_body_joint2"]
joint_control_req.joint_positions = [0.0, 0.0]
joint_control_req.joint_velocities = [0.1, 0.1]
joint_control_req.life_time = 5.0
joint_control_req.detail = "Test joint control"

try:
    result = robot.joint_control_request(joint_control_req)
    print("Joint control request succeeded")
except Exception as e:
    print(f"Joint control request failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

8. move_head_joint()

  • Function: Head joint position planning control interface; the call returns after the target position has been reached.
  • Parameters:
  • positions: list[float], list of head joint positions, in the order "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"
  • velocities: list[float], list of head joint velocities (radians/second), in the order "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Example:

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control head position (in joint order: idx11_head_joint1, idx12_head_joint2, idx13_head_joint3)
head_positions = [0.0, 0.0, 0.0]  # List of head joint positions
head_velocities = [0.3, 0.3, 0.3]  # List of head joint velocities

try:
    result = robot.move_head_joint(head_positions, head_velocities)
    print("Head control succeeded")
except Exception as e:
    print(f"Head control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

9. move_waist_joint()

  • Function: Waist joint position planning control interface; the call returns after the target position has been reached.
  • Parameters:
  • positions: list[float], list of waist joint positions, in the order "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3", "idx04_body_joint4", "idx05_body_joint5"
  • velocities: list[float], list of waist joint velocities (radians/second), in the order "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3", "idx04_body_joint4", "idx05_body_joint5"

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Example:

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control waist position (in joint order: idx01_body_joint1, idx02_body_joint2, idx03_body_joint3, idx04_body_joint4, idx05_body_joint5)
waist_positions = [0.0, 0.0, 0.0, 0.0, 0.0]  # List of waist joint positions
waist_velocities = [0.3, 0.3, 0.3, 0.3, 0.3]  # List of waist joint velocities

try:
    result = robot.move_waist_joint(waist_positions, waist_velocities)
    print("Waist control succeeded")
except Exception as e:
    print(f"Waist control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

10. move_arm_joint()

  • Function: Arm joint position planning control interface; the call returns after the target position has been reached.
  • Parameters:
  • positions: list[float], list of arm joint positions, 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: list[float], list of arm joint velocities (radians/second), 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: int, control group; 0 controls the left arm, 1 controls the right arm, 2 controls both arms

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Example:

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control arm positions (in joint order: 7 left-arm joints + 7 right-arm joints)
arm_positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  # 7 left-arm joints
                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]  # 7 right-arm joints
arm_velocities = [0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3,  # 7 left-arm joint velocities
                  0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3]  # 7 right-arm joint velocities

try:
    result = robot.move_arm_joint(arm_positions, arm_velocities, 2)
    print("Arm control succeeded")
except Exception as e:
    print(f"Arm control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

11. joint_servo_control()

  • Function: Joint position servo control interface. Requires a control frequency of 100 Hz, and supports both normal mode and low-latency mode, with normal mode used by default.
  • Note: Low-latency mode has no collision protection — take care to ensure safety when using it. If you also need to control the end effector at the same time, use this interface to send the control command; see the example below for details.
  • Parameters:
  • joint_servo_control_req: a JointServoControlReq object, containing the following attributes:
Attribute Type Description Unit
control_period float Control period seconds
joint_names list[str] List of joint names list of strings
joint_positions list[float] List of joint positions radians
joint_velocities list[float] List of joint velocities radians/second
  • enable_low_latency: bool, optional parameter, defaults to False.

Parameter notes: - control_period denotes the control period, in seconds; it is recommended to make it slightly larger than the control interval - The joint_names and joint_positions lists must be the same length - Values in joint_positions must be within the limit range of the corresponding joint (see the joint limit values below), otherwise an exception is raised - joint_names and joint_positions must not be empty, otherwise an exception is raised - joint_velocities is a reserved parameter, currently unused, and may be left empty - enable_low_latency selects the control channel; low-latency mode is suited to scenarios with higher real-time requirements

Joint limit values

The limit values (in radians) for each joint 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 (dexterous hand o10_t2, left hand)
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 (dexterous hand o10_t2, right hand)
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 (dexterous hand o12_t2, left hand)
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 (dexterous hand o12_t2, right hand)
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

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Exceptions:

  • If joint_names or joint_positions is empty, the underlying layer returns ErrorCode::kInvalidInput, and Python raises std::runtime_error("JointServoControlRequest failed")
  • If a value in joint_positions exceeds the limit range of the corresponding joint, the underlying layer returns ErrorCode::kInvalidInput, and Python raises an exception

  • If retrieving the configuration parser fails, the underlying layer returns ErrorCode::kRuntimeError, and Python raises an exception

  • Example:

import agibot_gdk
import time
import math

# Control parameters
CONTROL_PERIOD = 0.01  # Control period (seconds)
RATE_HZ = 100.0        # Send frequency (Hz)
DURATION = 5.0         # Control duration (seconds)
MAX_POSITION_DELTA = 0.1  # Maximum position change (radians)


class JointServoControlController:
    def __init__(self, robot):
        self.robot = robot

    def get_joint_position_by_name(self, joint_states, joint_name):
        """Get a joint's position by joint name"""
        for state in joint_states['states']:
            if state['name'] == joint_name:
                return state['motor_position']
        raise RuntimeError(f"Joint name {joint_name} not found")

    def interpolate_position(self, start_pos, target_pos, t):
        """Compute an intermediate position via linear interpolation"""
        return start_pos + t * (target_pos - start_pos)

    def execute_joint_servo_control(self, target_joint_names, target_positions):
        """Execute joint position servo control"""
        time.sleep(1.0)  # Wait 1 second

        # Get current joint states
        current_joint_states = self.robot.get_joint_states()
        print(f"Current joint count: {current_joint_states['nums']}")

        # Get the starting positions
        start_positions = []
        for joint_name in target_joint_names:
            try:
                pos = self.get_joint_position_by_name(current_joint_states, joint_name)
                start_positions.append(pos)
                print(f"Joint {joint_name} current position: {pos:.3f} rad")
            except RuntimeError as e:
                print(f"Error: {e}")
                return

        # Compute the number of steps
        n_steps = int(DURATION * RATE_HZ)
        print(f"Total steps: {n_steps}, duration: {DURATION} s")

        # Execute the trajectory
        dt = 1.0 / RATE_HZ
        start_time = time.time()

        for i in range(n_steps):
            t = float(i) / (n_steps - 1) if n_steps > 1 else 0.0

            # Create a joint position servo request
            joint_servo_control_req = agibot_gdk.JointServoControlReq()
            joint_servo_control_req.control_period = CONTROL_PERIOD

            # Compute the current target position (linear interpolation)
            current_positions = []
            for j, joint_name in enumerate(target_joint_names):
                interp_pos = self.interpolate_position(
                    start_positions[j], target_positions[j], t
                )
                current_positions.append(interp_pos)

            joint_servo_control_req.joint_names = target_joint_names
            joint_servo_control_req.joint_positions = current_positions

            try:
                # Use normal mode (enable_low_latency=False, the default)
                result = self.robot.joint_servo_control(joint_servo_control_req)
                # To use low-latency mode, pass enable_low_latency=True:
                # result = self.robot.joint_servo_control(joint_servo_control_req, enable_low_latency=True)
                if result != 0:
                    print(f"Failed to send control command, step: {i}")
                    return
            except Exception as e:
                print(f"Exception sending control command, step: {i}, error: {e}")
                return

            # Control the send frequency
            elapsed = time.time() - start_time
            expected_time = (i + 1) * dt
            sleep_time = expected_time - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)

        print("Joint position servo control complete")

        # Hold the final position
        print("Entering final position hold (Ctrl+C to stop)...")
        try:
            while True:
                joint_servo_control_req = agibot_gdk.JointServoControlReq()
                joint_servo_control_req.control_period = CONTROL_PERIOD
                joint_servo_control_req.joint_names = target_joint_names
                joint_servo_control_req.joint_positions = target_positions

                try:
                    # Use normal mode (enable_low_latency=False, the default)
                    result = self.robot.joint_servo_control(joint_servo_control_req)
                    # To use low-latency mode, pass enable_low_latency=True:
                    # result = self.robot.joint_servo_control(joint_servo_control_req, enable_low_latency=True)
                    if result != 0:
                        print("Failed to hold position")
                        break
                except Exception as e:
                    print(f"Exception while holding position: {e}")
                    break

                time.sleep(dt)
        except KeyboardInterrupt:
            print("\nHold interrupted")


def main():
    # Initialize the GDK system
    if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
        print("GDK initialization failed")
        return

    print("GDK initialized successfully")

    try:
        robot = agibot_gdk.Robot()
        time.sleep(2)  # Wait for the robot to finish initializing

        # Get current joint states
        current_joint_states = robot.get_joint_states()
        print(f"Current joint count: {current_joint_states['nums']}")

        # Define the joints to control (example: the first 3 joints of the left arm)
        target_joint_names = [
            "idx21_arm_l_joint1",
            "idx22_arm_l_joint2",
            "idx23_arm_l_joint3"
        ]

        # Get the current positions to use as the starting positions (for interpolation)
        controller = JointServoControlController(robot)
        start_positions = []
        for joint_name in target_joint_names:
            pos = controller.get_joint_position_by_name(current_joint_states, joint_name)
            start_positions.append(pos)
            print(f"Joint {joint_name} current position: {pos:.3f} rad")

        # Set target angles (directly specify the target angle values, in radians)
        target_positions = [
            0.0,  # idx21_arm_l_joint1: target angle 0.0 radians
            0.0,  # idx22_arm_l_joint2: target angle 0.0 radians
            0.0   # idx23_arm_l_joint3: target angle 0.0 radians
        ]

        print(f"\nTarget angles:")
        for i, joint_name in enumerate(target_joint_names):
            print(f"  {joint_name}: {target_positions[i]:.3f} rad")

        # Execute joint position servo control
        controller.execute_joint_servo_control(
            target_joint_names, target_positions)

    except Exception as e:
        print(f"Error occurred during execution: {e}")
    finally:
        # Release GDK system resources
        if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
            print("GDK release failed")
        else:
            print("GDK released successfully")


if __name__ == "__main__":
    main()
  • Example — simultaneously controlling the arm and the end effector:
import agibot_gdk
import time

# Control parameters
CONTROL_PERIOD = 0.01  # Control period (seconds)
RATE_HZ = 100.0        # Send frequency (Hz)
DURATION = 3.0         # Duration of a single move (seconds)
HOLD_DURATION = 0.5    # Time to hold at 0 or 1 (seconds)
NUM_CYCLES = 3         # Number of back-and-forth cycles

# Names of the left arm's 7 joints (consistent with get_joint_states)
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",
]

def get_arm_positions_by_name(joint_states, joint_names):
    """Extract the list of positions by joint name from the return value of get_joint_states"""
    name_to_pos = {s["name"]: s["motor_position"] for s in joint_states["states"]}
    return [name_to_pos[name] for name in joint_names]

def get_ee_names_and_positions(end_state, side="left"):
    """Extract the joint name list and position list for the given side's end effector from the return value of get_end_state"""
    key = f"{side}_end_state"
    if key not in end_state:
        raise RuntimeError(f"{key} not found in get_end_state")
    state = end_state[key]
    names = state.get("names", [])
    positions = [s["position"] for s in state.get("end_states", [])]
    if len(names) != len(positions):
        raise RuntimeError(f"Length mismatch between names and end_states in {key}")
    return names, positions

def main():
    if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
        print("GDK initialization failed")
        return
    print("GDK initialized successfully")

    try:
        robot = agibot_gdk.Robot()
        time.sleep(2)

        # 1) Get the whole-body joints (including the arm) from get_joint_states
        joint_states = robot.get_joint_states()
        # 2) Get the end-effector joint names and current positions (e.g. left gripper/left hand) from get_end_state
        end_state = robot.get_end_state()
        ee_names, ee_positions = get_ee_names_and_positions(end_state, "left")

        # Merge: arm joints + end-effector joints
        all_names = ARM_L_JOINT_NAMES + ee_names
        arm_positions = get_arm_positions_by_name(joint_states, ARM_L_JOINT_NAMES)

        # The arm moves slightly, while the end-effector joint moves back and forth between -0.785 and 0 (omnipicker)
        arm_start = arm_positions[:]                           # Initial arm pose
        arm_target = [p + 0.05 for p in arm_positions]         # Target arm pose (small offset)
        ee_low = -0.785                                        # End-effector lower bound
        ee_high = 0.0                                          # End-effector upper bound

        print(f"Controlled joint count: arm {len(ARM_L_JOINT_NAMES)} + end effector {len(ee_names)} = {len(all_names)}")
        print(f"The arm will move in joint space between arm_start ↔ arm_target, and the end-effector joint will cycle between {ee_low} and {ee_high} {NUM_CYCLES} times")

        n_steps = int(DURATION * RATE_HZ)           # Number of steps for a single move
        hold_steps = int(HOLD_DURATION * RATE_HZ)   # Number of steps for the hold phase
        dt = 1.0 / RATE_HZ

        def send_control_command(arm_target_values, ee_target_values):
            """Helper function for sending the control command"""
            current = arm_target_values + ee_target_values
            req = agibot_gdk.JointServoControlReq()
            req.control_period = CONTROL_PERIOD
            req.joint_names = all_names
            req.joint_positions = current
            return robot.joint_servo_control(req)

        for cycle in range(NUM_CYCLES):
            print(f"\n=== Cycle {cycle+1}/{NUM_CYCLES}: arm arm_start -> arm_target, end effector {ee_low} -> {ee_high} ===")
            start_time = time.time()

            # Segment 1: arm arm_start -> arm_target, end effector ee_low -> ee_high
            for i in range(n_steps):
                t = float(i) / (n_steps - 1) if n_steps > 1 else 1.0
                current_arm = [
                    arm_start[j] + t * (arm_target[j] - arm_start[j])
                    for j in range(len(ARM_L_JOINT_NAMES))
                ]
                current_ee = [ee_low + t * (ee_high - ee_low) for _ in ee_names]

                result = send_control_command(current_arm, current_ee)
                if result != 0:
                    print(f"Send failed, cycle={cycle+1}, phase=0->1, step={i}")
                    raise RuntimeError("joint_servo_control failed")

                elapsed = time.time() - start_time
                sleep_time = (i + 1) * dt - elapsed
                if sleep_time > 0:
                    time.sleep(sleep_time)

            # Segment 2: hold at arm_target / ee_high
            print(f"=== Cycle {cycle+1}/{NUM_CYCLES}: holding at arm_target / {ee_high} for {HOLD_DURATION} s ===")
            start_time = time.time()
            current_arm = arm_target[:]
            current_ee = [ee_high for _ in ee_names]

            for i in range(hold_steps):
                result = send_control_command(current_arm, current_ee)
                if result != 0:
                    print(f"Send failed, cycle={cycle+1}, phase=hold@1, step={i}")
                    raise RuntimeError("joint_servo_control failed")

                elapsed = time.time() - start_time
                sleep_time = (i + 1) * dt - elapsed
                if sleep_time > 0:
                    time.sleep(sleep_time)

            # Segment 3: arm arm_target -> arm_start, end effector ee_high -> ee_low
            print(f"=== Cycle {cycle+1}/{NUM_CYCLES}: arm arm_target -> arm_start, end effector {ee_high} -> {ee_low} ===")
            start_time = time.time()

            for i in range(n_steps):
                t = float(i) / (n_steps - 1) if n_steps > 1 else 1.0
                current_arm = [
                    arm_target[j] + t * (arm_start[j] - arm_target[j])
                    for j in range(len(ARM_L_JOINT_NAMES))
                ]
                current_ee = [ee_high + t * (ee_low - ee_high) for _ in ee_names]

                result = send_control_command(current_arm, current_ee)
                if result != 0:
                    print(f"Send failed, cycle={cycle+1}, phase=1->0, step={i}")
                    raise RuntimeError("joint_servo_control failed")

                elapsed = time.time() - start_time
                sleep_time = (i + 1) * dt - elapsed
                if sleep_time > 0:
                    time.sleep(sleep_time)

            # Segment 4: hold at arm_start / ee_low
            print(f"=== Cycle {cycle+1}/{NUM_CYCLES}: holding at arm_start / {ee_low} for {HOLD_DURATION} s ===")
            start_time = time.time()
            current_arm = arm_start[:]
            current_ee = [ee_low for _ in ee_names]

            for i in range(hold_steps):
                result = send_control_command(current_arm, current_ee)
                if result != 0:
                    print(f"Send failed, cycle={cycle+1}, phase=hold@0, step={i}")
                    raise RuntimeError("joint_servo_control failed")

                elapsed = time.time() - start_time
                sleep_time = (i + 1) * dt - elapsed
                if sleep_time > 0:
                    time.sleep(sleep_time)

        print(f"\nArm arm_start↔arm_target and end-effector {ee_low}↔{ee_high} back-and-forth control finished")
    except Exception as e:
        print(f"Execution error: {e}")
    finally:
        if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
            print("GDK release failed")
        else:
            print("GDK released successfully")

if __name__ == "__main__":
    main()

12. move_head_joint_servo()

  • Function: Head joint position servo control interface. Requires a control frequency of 100 Hz, and supports both normal mode and low-latency mode, with normal mode used by default.
  • Note: Low-latency mode has no collision protection — take care to ensure safety when using it.
  • Parameters:
  • positions: list[float], list of head joint positions (radians), in the order "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"
  • control_period: float, control period (seconds); it is recommended to make it slightly larger than the control interval
  • enable_low_latency: bool, optional parameter, defaults to False, used to select the control channel

Parameter notes: - positions must have a length of 3 - Values in positions must be within the limit range of the corresponding joint (see the joint limit values below), otherwise an exception is raised - control_period denotes the control period, in seconds; it is recommended to make it slightly larger than the control interval - enable_low_latency selects the control channel; low-latency mode is suited to scenarios with higher real-time requirements

Joint limit values

The head joint limit values (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

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Exceptions:

  • If the length of positions is not 3, the underlying layer returns ErrorCode::kInvalidInput, and Python raises an exception

  • If a value in positions exceeds the limit range of the corresponding joint, the underlying layer returns ErrorCode::kInvalidInput, and Python raises an exception

  • Example:

import agibot_gdk
import time
import math

# Control parameters
CONTROL_PERIOD = 0.01  # Control period (seconds)
RATE_HZ = 100.0        # Send frequency (Hz)
DURATION = 5.0         # Control duration (seconds)

class HeadJointServoController:
    def __init__(self, robot):
        self.robot = robot

    def get_joint_position_by_name(self, joint_states, joint_name):
        """Get a joint's position by joint name"""
        for state in joint_states['states']:
            if state['name'] == joint_name:
                return state['motor_position']
        raise RuntimeError(f"Joint name {joint_name} not found")

    def interpolate_position(self, start_pos, target_pos, t):
        """Compute an intermediate position via linear interpolation"""
        return start_pos + t * (target_pos - start_pos)

    def execute_head_joint_servo_control(self, target_positions):
        """Execute head joint position servo control"""
        time.sleep(1.0)  # Wait 1 second

        # Get current joint states
        current_joint_states = self.robot.get_joint_states()

        # Get the starting positions
        head_joint_names = ["idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"]
        start_positions = []
        for joint_name in head_joint_names:
            try:
                pos = self.get_joint_position_by_name(current_joint_states, joint_name)
                start_positions.append(pos)
                print(f"Joint {joint_name} current position: {pos:.3f} rad")
            except RuntimeError as e:
                print(f"Error: {e}")
                return

        # Compute the number of steps
        n_steps = int(DURATION * RATE_HZ)
        print(f"Total steps: {n_steps}, duration: {DURATION} s")

        # Execute the trajectory
        dt = 1.0 / RATE_HZ
        start_time = time.time()

        for i in range(n_steps):
            t = float(i) / (n_steps - 1) if n_steps > 1 else 0.0

            # Compute the current target position (linear interpolation)
            current_positions = []
            for j in range(3):
                interp_pos = self.interpolate_position(
                    start_positions[j], target_positions[j], t
                )
                current_positions.append(interp_pos)

            try:
                # Use normal mode (enable_low_latency=False, the default)
                result = self.robot.move_head_joint_servo(
                    current_positions, CONTROL_PERIOD)
                # To use low-latency mode, pass enable_low_latency=True:
                # result = self.robot.move_head_joint_servo(
                #     current_positions, CONTROL_PERIOD, enable_low_latency=True
                # )
                if result != 0:
                    print(f"Failed to send control command, step: {i}")
                    return
            except Exception as e:
                print(f"Exception sending control command, step: {i}, error: {e}")
                return

            # Control the send frequency
            elapsed = time.time() - start_time
            expected_time = (i + 1) * dt
            sleep_time = expected_time - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)

        print("Head joint position servo control complete")

        # Hold the final position
        print("Entering final position hold (Ctrl+C to stop)...")
        try:
            while True:
                try:
                    # Use normal mode (enable_low_latency=False, the default)
                    result = self.robot.move_head_joint_servo(
                        target_positions, CONTROL_PERIOD
                    )
                    # To use low-latency mode, pass enable_low_latency=True:
                    # result = self.robot.move_head_joint_servo(
                    #     target_positions, CONTROL_PERIOD, enable_low_latency=True
                    # )
                    if result != 0:
                        print("Failed to hold position")
                        break
                except Exception as e:
                    print(f"Exception while holding position: {e}")
                    break

                time.sleep(dt)
        except KeyboardInterrupt:
            print("\nHold interrupted")


def main():
    # Initialize the GDK system
    if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
        print("GDK initialization failed")
        return

    print("GDK initialized successfully")

    try:
        robot = agibot_gdk.Robot()
        time.sleep(2)  # Wait for the robot to finish initializing

        # Get current joint states
        current_joint_states = robot.get_joint_states()

        # Define the target positions (example: head joints)
        target_positions = [
            0.0,  # idx11_head_joint1: target angle 0.0 radians
            0.0,  # idx12_head_joint2: target angle 0.0 radians
            0.0   # idx13_head_joint3: target angle 0.0 radians
        ]

        print(f"\nTarget angles:")
        head_joint_names = ["idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3"]
        for i, joint_name in enumerate(head_joint_names):
            print(f"  {joint_name}: {target_positions[i]:.3f} rad")

        # Execute head joint position servo control
        controller = HeadJointServoController(robot)
        controller.execute_head_joint_servo_control(target_positions)

    except Exception as e:
        print(f"Error occurred during execution: {e}")
    finally:
        # Release GDK system resources
        if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
            print("GDK release failed")
        else:
            print("GDK released successfully")


if __name__ == "__main__":
    main()

13. move_waist_joint_servo()

  • Function: Waist joint position servo control interface. Requires a control frequency of 100 Hz, and supports both normal mode and low-latency mode, with normal mode used by default.
  • Note: Low-latency mode has no collision protection — take care to ensure safety when using it.
  • Parameters:
  • positions: list[float], 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: float, control period (seconds); it is recommended to make it slightly larger than the control interval
  • enable_low_latency: bool, optional parameter, defaults to False, used to select the control channel

Parameter notes: - positions must have a length of 5 - Values in positions must be within the limit range of the corresponding joint (see the joint limit values below), otherwise an exception is raised - control_period denotes the control period, in seconds; it is recommended to make it slightly larger than the control interval - enable_low_latency selects the control channel; low-latency mode is suited to scenarios with higher real-time requirements

Joint limit values

The waist joint limit values (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

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Exceptions:

  • If the length of positions is not 5, the underlying layer returns ErrorCode::kInvalidInput, and Python raises an exception

  • If a value in positions exceeds the limit range of the corresponding joint, the underlying layer returns ErrorCode::kInvalidInput, and Python raises an exception

  • Example:

import agibot_gdk
import time
import math

# Control parameters
CONTROL_PERIOD = 0.01  # Control period (seconds)
RATE_HZ = 100.0        # Send frequency (Hz)
DURATION = 5.0         # Control duration (seconds)

class WaistJointServoController:
    def __init__(self, robot):
        self.robot = robot

    def get_joint_position_by_name(self, joint_states, joint_name):
        """Get a joint's position by joint name"""
        for state in joint_states['states']:
            if state['name'] == joint_name:
                return state['motor_position']
        raise RuntimeError(f"Joint name {joint_name} not found")

    def interpolate_position(self, start_pos, target_pos, t):
        """Compute an intermediate position via linear interpolation"""
        return start_pos + t * (target_pos - start_pos)

    def execute_waist_joint_servo_control(self, target_positions):
        """Execute waist joint position servo control"""
        time.sleep(1.0)  # Wait 1 second

        # Get current joint states
        current_joint_states = self.robot.get_joint_states()

        # Get the starting positions
        waist_joint_names = [
            "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3",
            "idx04_body_joint4", "idx05_body_joint5"
        ]
        start_positions = []
        for joint_name in waist_joint_names:
            try:
                pos = self.get_joint_position_by_name(current_joint_states, joint_name)
                start_positions.append(pos)
                print(f"Joint {joint_name} current position: {pos:.3f} rad")
            except RuntimeError as e:
                print(f"Error: {e}")
                return

        # Compute the number of steps
        n_steps = int(DURATION * RATE_HZ)
        print(f"Total steps: {n_steps}, duration: {DURATION} s")

        # Execute the trajectory
        dt = 1.0 / RATE_HZ
        start_time = time.time()

        for i in range(n_steps):
            t = float(i) / (n_steps - 1) if n_steps > 1 else 0.0

            # Compute the current target position (linear interpolation)
            current_positions = []
            for j in range(5):
                interp_pos = self.interpolate_position(
                    start_positions[j], target_positions[j], t
                )
                current_positions.append(interp_pos)

            try:
                # Use normal mode (enable_low_latency=False, the default)
                result = self.robot.move_waist_joint_servo(
                    current_positions, CONTROL_PERIOD
                )
                # To use low-latency mode, pass enable_low_latency=True:
                # result = self.robot.move_waist_joint_servo(
                #     current_positions, CONTROL_PERIOD, enable_low_latency=True
                # )
                if result != 0:
                    print(f"Failed to send control command, step: {i}")
                    return
            except Exception as e:
                print(f"Exception sending control command, step: {i}, error: {e}")
                return

            # Control the send frequency
            elapsed = time.time() - start_time
            expected_time = (i + 1) * dt
            sleep_time = expected_time - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)

        print("Waist joint position servo control complete")

        # Hold the final position
        print("Entering final position hold (Ctrl+C to stop)...")
        try:
            while True:
                try:
                    # Use normal mode (enable_low_latency=False, the default)
                    result = self.robot.move_waist_joint_servo(
                        target_positions, CONTROL_PERIOD
                    )
                    # To use low-latency mode, pass enable_low_latency=True:
                    # result = self.robot.move_waist_joint_servo(
                    #     target_positions, CONTROL_PERIOD, enable_low_latency=True
                    # )
                    if result != 0:
                        print("Failed to hold position")
                        break
                except Exception as e:
                    print(f"Exception while holding position: {e}")
                    break

                time.sleep(dt)
        except KeyboardInterrupt:
            print("\nHold interrupted")


def main():
    # Initialize the GDK system
    if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
        print("GDK initialization failed")
        return

    print("GDK initialized successfully")

    try:
        robot = agibot_gdk.Robot()
        time.sleep(2)  # Wait for the robot to finish initializing

        # Get current joint states
        current_joint_states = robot.get_joint_states()

        # Define the target positions (example: waist joints)
        target_positions = [
            0.0,  # idx01_body_joint1: target angle 0.0 radians
            0.0,  # idx02_body_joint2: target angle 0.0 radians
            0.0,  # idx03_body_joint3: target angle 0.0 radians
            0.0,  # idx04_body_joint4: target angle 0.0 radians
            0.0   # idx05_body_joint5: target angle 0.0 radians
        ]

        print(f"\nTarget angles:")
        waist_joint_names = [
            "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3",
            "idx04_body_joint4", "idx05_body_joint5"
        ]
        for i, joint_name in enumerate(waist_joint_names):
            print(f"  {joint_name}: {target_positions[i]:.3f} rad")

        # Execute waist joint position servo control
        controller = WaistJointServoController(robot)
        controller.execute_waist_joint_servo_control(target_positions)

    except Exception as e:
        print(f"Error occurred during execution: {e}")
    finally:
        # Release GDK system resources
        if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
            print("GDK release failed")
        else:
            print("GDK released successfully")


if __name__ == "__main__":
    main()

14. move_arm_joint_servo()

  • Function: Arm joint position servo control interface. Requires a control frequency of 100 Hz, and supports both normal mode and low-latency mode, with normal mode used by default.
  • Note: Low-latency mode has no collision protection — take care to ensure safety when using it.
  • Parameters:
  • positions: list[float], 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: float, control period (seconds); it is recommended to make it slightly larger than the control interval
  • control_group: int, control group; 0 controls the left arm, 1 controls the right arm, 2 controls both arms
  • enable_low_latency: bool, optional parameter, defaults to False, used to select the control channel

Parameter notes: - positions must have a length of 7 (left arm or right arm) or 14 (both arms) - Values in positions must be within the limit range of the corresponding joint (see the joint limit values below), otherwise an exception is raised - control_period denotes the control period, in seconds; it is recommended to make it slightly larger than the control interval - control_group denotes the control group: 0 controls the left arm, 1 controls the right arm, 2 controls both arms - enable_low_latency selects the control channel; low-latency mode is suited to scenarios with higher real-time requirements

Joint limit values

The arm joint limit values (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

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Exceptions:

  • If the length of positions is not 7 (left arm or right arm) or 14 (both arms), the underlying layer returns ErrorCode::kInvalidInput, and Python raises an exception

  • If a value in positions exceeds the limit range of the corresponding joint, the underlying layer returns ErrorCode::kInvalidInput, and Python raises an exception

  • Example:

import agibot_gdk
import time
import math

# Control parameters
CONTROL_PERIOD = 0.01  # Control period (seconds)
RATE_HZ = 100.0        # Send frequency (Hz)
DURATION = 5.0         # Control duration (seconds)

class ArmJointServoController:
    def __init__(self, robot):
        self.robot = robot

    def get_joint_position_by_name(self, joint_states, joint_name):
        """Get a joint's position by joint name"""
        for state in joint_states['states']:
            if state['name'] == joint_name:
                return state['motor_position']
        raise RuntimeError(f"Joint name {joint_name} not found")

    def interpolate_position(self, start_pos, target_pos, t):
        """Compute an intermediate position via linear interpolation"""
        return start_pos + t * (target_pos - start_pos)

    def execute_arm_joint_servo_control(self, target_positions, control_group):
        """Execute arm joint position servo control"""
        time.sleep(1.0)  # Wait 1 second

        # Get current joint states
        current_joint_states = self.robot.get_joint_states()

        # Get the starting positions
        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 left-arm joints
            "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 right-arm joints
        ]
        start_positions = []
        for joint_name in arm_joint_names:
            try:
                pos = self.get_joint_position_by_name(current_joint_states, joint_name)
                start_positions.append(pos)
                print(f"Joint {joint_name} current position: {pos:.3f} rad")
            except RuntimeError as e:
                print(f"Error: {e}")
                return

        # Compute the number of steps
        n_steps = int(DURATION * RATE_HZ)
        print(f"Total steps: {n_steps}, duration: {DURATION} s")

        # Execute the trajectory
        dt = 1.0 / RATE_HZ
        start_time = time.time()

        for i in range(n_steps):
            t = float(i) / (n_steps - 1) if n_steps > 1 else 0.0

            # Compute the current target position (linear interpolation)
            current_positions = []
            for j in range(14):
                interp_pos = self.interpolate_position(
                    start_positions[j], target_positions[j], t
                )
                current_positions.append(interp_pos)

            try:
                # Use normal mode (enable_low_latency=False, the default)
                result = self.robot.move_arm_joint_servo(
                    current_positions, CONTROL_PERIOD, control_group
                )
                # To use low-latency mode, pass enable_low_latency=True:
                # result = self.robot.move_arm_joint_servo(
                #     current_positions, CONTROL_PERIOD, control_group, enable_low_latency=True
                # )
                if result != 0:
                    print(f"Failed to send control command, step: {i}")
                    return
            except Exception as e:
                print(f"Exception sending control command, step: {i}, error: {e}")
                return

            # Control the send frequency
            elapsed = time.time() - start_time
            expected_time = (i + 1) * dt
            sleep_time = expected_time - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)

        print("Arm joint position servo control complete")

        # Hold the final position
        print("Entering final position hold (Ctrl+C to stop)...")
        try:
            while True:
                try:
                    # Use normal mode (enable_low_latency=False, the default)
                    result = self.robot.move_arm_joint_servo(
                        target_positions, CONTROL_PERIOD, control_group
                    )
                    # To use low-latency mode, pass enable_low_latency=True:
                    # result = self.robot.move_arm_joint_servo(
                    #     target_positions, CONTROL_PERIOD, control_group, enable_low_latency=True
                    # )
                    if result != 0:
                        print("Failed to hold position")
                        break
                except Exception as e:
                    print(f"Exception while holding position: {e}")
                    break

                time.sleep(dt)
        except KeyboardInterrupt:
            print("\nHold interrupted")


def main():
    # Initialize the GDK system
    if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
        print("GDK initialization failed")
        return

    print("GDK initialized successfully")

    try:
        robot = agibot_gdk.Robot()
        time.sleep(2)  # Wait for the robot to finish initializing

        # Get current joint states
        current_joint_states = robot.get_joint_states()

        # Define the target positions (example: arm joints)
        target_positions = [
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,  # 7 left-arm joints
            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0   # 7 right-arm joints
        ]

        print(f"\nTarget angles:")
        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 left-arm joints
            "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 right-arm joints
        ]
        for i, joint_name in enumerate(arm_joint_names):
            print(f"  {joint_name}: {target_positions[i]:.3f} rad")

        # Execute arm joint position servo control
        controller = ArmJointServoController(robot)
        controller.execute_arm_joint_servo_control(target_positions, 2)

    except Exception as e:
        print(f"Error occurred during execution: {e}")
    finally:
        # Release GDK system resources
        if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
            print("GDK release failed")
        else:
            print("GDK released successfully")


if __name__ == "__main__":
    main()

15. move_ee_pos()

  • Function: Controls the end effector position (gripper opening/closing)
  • Parameters: A JointStates object

Structure of the JointStates object:

Attribute Type Description Unit
group str Control group; must be "left_tool", "right_tool", or "dual_tool" string
target_type str Target type; see the table below for supported values string
states list[JointState] List of joint states (required) list of JointState
nums int Number of joints; must equal len(states) unitless

Supported target_type values and the corresponding required joint counts:

target_type value Required joint count Description
"omnipicker" 1 Omni-directional 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: Joint value ranges are tied to the version — pay attention to the current version's value ranges when using this; the documentation reflects the ranges for the current version
  • 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 value range for each joint position is as follows (unit: 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 closed (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 closed (grip): [0.2, -1.45, 0.75, 0.0, 1.0, 1.0, 0.0, 1.0, 0.0, 1.0]

  • o12_t2: the value range for each joint position is as follows (unit: 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 closed (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 closed (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]

Structure of the JointState object:

Attribute Type Description Unit
position float Joint position (required) degree of opening

Parameter notes: - group must be "left_tool" (left end effector), "right_tool" (right end effector), or "dual_tool" (both end effectors) - When group is "left_tool" or "right_tool", the length of the states list must exactly match the joint count required for the target_type - When group is "dual_tool", the length of the states list must be twice the joint count required for the target_type (the first half is for the left end effector, the second half for the right end effector) - target_type must be one of the supported values above; other values will return an error - states is a list of JointState objects; each JointState only needs the position field set - nums must equal len(states)

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Exceptions:

  • If target_type is not one of the supported values ("omnipicker", "dahuan", "ctek90d", "o10_t2", "o12_t2"), the underlying layer returns ErrorCode::kInvalidInput
  • If the length of states does not match the joint count required by target_type (for "dual_tool", it must be twice that count), the underlying layer returns ErrorCode::kInvalidInput
  • If group is not "left_tool", "right_tool", or "dual_tool", the underlying layer returns ErrorCode::kInvalidInput
  • If states is empty, the underlying layer returns ErrorCode::kInvalidInput
  • If a joint position value exceeds the value range for the corresponding end-effector type, the underlying layer returns ErrorCode::kInvalidInput

  • If the control command fails to execute, std::runtime_error("Failed to move end effector position") is raised

  • Examples:

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

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control the left gripper (omnipicker type, requires 1 joint)
joint_states_left = agibot_gdk.JointStates()
joint_states_left.group = "left_tool"
joint_states_left.target_type = "omnipicker"

joint_state = agibot_gdk.JointState()
joint_state.position = 0  # Value range [-0.785, 0]  
joint_states_left.states = [joint_state]
joint_states_left.nums = len(joint_states_left.states)

try:
    result = robot.move_ee_pos(joint_states_left)
    print("Left gripper control succeeded")
except Exception as e:
    print(f"Left gripper control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

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

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control the right gripper (dahuan type, requires 1 joint)
joint_states_right = agibot_gdk.JointStates()
joint_states_right.group = "right_tool"
joint_states_right.target_type = "dahuan"

joint_state = agibot_gdk.JointState()
joint_state.position = 0  # Right gripper position, value range [0, 0.025]
joint_states_right.states = [joint_state]
joint_states_right.nums = len(joint_states_right.states)

try:
    result = robot.move_ee_pos(joint_states_right)
    print("Right gripper control succeeded")
except Exception as e:
    print(f"Right gripper control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

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

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control the left gripper (ctek90d type, requires 1 joint)
joint_states_left = agibot_gdk.JointStates()
joint_states_left.group = "left_tool"
joint_states_left.target_type = "ctek90d"

joint_state = agibot_gdk.JointState()
joint_state.position = 0.5  # Value range [-0.91, 0]
joint_states_left.states = [joint_state]
joint_states_left.nums = len(joint_states_left.states)

try:
    result = robot.move_ee_pos(joint_states_left)
    print("Left gripper (ctek90d) control succeeded")
except Exception as e:
    print(f"Left gripper control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

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

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control the left end effector (o10_t2 type, requires 10 joints)
joint_states_left_o10 = agibot_gdk.JointStates()
joint_states_left_o10.group = "left_tool"
joint_states_left_o10.target_type = "o10_t2"

# Create the list of joint states
positions = [-0.2, 1.45, -0.75, 0, 1, 1, 0, 1, 0, 1]
states_list = []
for i in range(10):
    joint_state = agibot_gdk.JointState()
    joint_state.position = positions[i]
    states_list.append(joint_state)
# Assign the whole list directly
joint_states_left_o10.states = states_list
joint_states_left_o10.nums = len(joint_states_left_o10.states)

try:
    result = robot.move_ee_pos(joint_states_left_o10)
    print("Left end effector (o10_t2) control succeeded")
except Exception as e:
    print(f"Left end effector control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

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

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control the right end effector (o12_t2 type, requires 12 joints)
joint_states_right_o12 = agibot_gdk.JointStates()
joint_states_right_o12.group = "right_tool"
joint_states_right_o12.target_type = "o12_t2"

# Create the list of joint states
states_list = []
for i in range(12):
    joint_state = agibot_gdk.JointState()
    joint_state.position = 0
    states_list.append(joint_state)
# Assign the whole list directly
joint_states_right_o12.states = states_list
joint_states_right_o12.nums = len(joint_states_right_o12.states)

try:
    result = robot.move_ee_pos(joint_states_right_o12)
    print("Right end effector (o12_t2) control succeeded")
except Exception as e:
    print(f"Right end effector control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

Example 6: Controlling both end effectors (dual_tool, requires twice the joint count)

import agibot_gdk
import time

# Initialize the GDK system
if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
    print("GDK initialization failed")
    exit(1)
print("GDK initialized successfully")

robot = agibot_gdk.Robot()
time.sleep(2)  # Wait for the robot to finish initializing

# Control both end effectors (dual_tool, requires twice the joint count)
# For example: using the omnipicker type requires 2 joints (1 left + 1 right)
joint_states_dual = agibot_gdk.JointStates()
joint_states_dual.group = "dual_tool"
joint_states_dual.target_type = "omnipicker"

# The first half is for the left end effector
joint_state_left = agibot_gdk.JointState()
joint_state_left.position = 0
# The second half is for the right end effector
joint_state_right = agibot_gdk.JointState()
joint_state_right.position = 0
# Assign the whole list directly
joint_states_dual.states = [joint_state_left, joint_state_right]
joint_states_dual.nums = len(joint_states_dual.states)

try:
    result = robot.move_ee_pos(joint_states_dual)
    print("Dual end effector control succeeded")
except Exception as e:
    print(f"Dual end effector control failed: {e}")

# Release GDK system resources
if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
    print("GDK release failed")
else:
    print("GDK released successfully")

16. end_effector_pose_control()

  • Function: End-effector pose control
  • Note: This interface requires the sender to publish control commands at a frequency of 50 Hz, and step (discontinuous) signals are not allowed; this interface has no collision detection, so pay attention to the surrounding environment when using it.
  • Parameters: An EndEffectorPose object, containing the following attributes:
Attribute Type Description Unit
life_time float Lifetime seconds
group int Control group integer
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

  • Returns: int, 0 indicates success; an exception is raised on failure

  • Example: (Note: for safety, this example first moves the robot to its initial pose (standing), and then calls end-effector pose control — please ensure the space around the robot is safe before running it)

import agibot_gdk
import time
import math


# Constant definitions
LEFT_NAME = "arm_l_end_link"
RIGHT_NAME = "arm_r_end_link"

# Target poses (position and quaternion)
TARGET_LEFT_POSITION = [0.626755,0.301127306,1.08398]
TARGET_LEFT_ORIENTATION =  [0.5858436, -0.02614889,0.80973243,0.02089957]

TARGET_RIGHT_POSITION = [0.6099763,-0.3903393, 1.050190472]
TARGET_RIGHT_ORIENTATION =  [0.648815,-0.001096,0.76094223696, 0.0020416]

# Control parameters
MAX_STEP_CM = 0.1  # Maximum step size (centimeters)
LIFETIME = 0.02    # Lifetime (seconds)
RATE_HZ = 50.0     # Send frequency (Hz)
HOLD_FINAL = True  # Whether to hold the final pose


class EndEffectorController:
    def __init__(self, robot):
        self.robot = robot

    def slerp(self, q0, q1, t):
        """
        Spherical linear interpolation (SLERP) between quaternions
        q0, q1: quaternions [x, y, z, w]
        t: interpolation parameter [0, 1]
        Returns: the interpolated quaternion [x, y, z, w]
        """
        # Compute the dot product
        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
            q1_neg = [-q1[0], -q1[1], -q1[2], -q1[3]]
            result = [q0[i] + t * (q1_neg[i] - q0[i]) for i in range(4)]
        else:
            result = [q0[i] + t * (q1[i] - q0[i]) for i in range(4)]

        # Clamp the dot product range
        dot = max(-1.0, min(1.0, dot))

        if dot > 0.9995:
            # Linear interpolation
            norm = math.sqrt(sum(r*r for r in result))
            if norm > 0.0:
                result = [r / norm for r in result]
        else:
            # Spherical linear interpolation
            theta_0 = math.acos(dot)
            sin_theta_0 = math.sin(theta_0)
            theta = theta_0 * t
            sin_theta = math.sin(theta)
            s0 = math.cos(theta) - dot * sin_theta / sin_theta_0
            s1 = sin_theta / sin_theta_0

            result = [s0 * q0[i] + s1 * q1[i] for i in range(4)]

        return result

    def distance_between_points(self, p1, p2):
        """Compute the distance between two points"""
        dx = p2[0] - p1[0]
        dy = p2[1] - p1[1]
        dz = p2[2] - p1[2]
        return math.sqrt(dx*dx + dy*dy + dz*dz)

    def calculate_n_steps(self, start_pos, goal_pos, max_step_cm):
        """Compute the number of steps needed"""
        dist_cm = self.distance_between_points(start_pos, goal_pos) * 100.0
        return max(int(math.ceil(dist_cm / max_step_cm)), 1)

    def plan_trajectory(self, start_pose, goal_pose, n_steps):
        """
        Plan the trajectory
        start_pose, goal_pose: dicts containing position and orientation
        n_steps: number of steps
        Returns: the list of trajectory poses
        """
        trajectory = []

        for i in range(n_steps):
            t = float(i) / (n_steps - 1) if n_steps > 1 else 0.0

            # Linear interpolation of position
            pos = [
                start_pose['position'][0] + t * (goal_pose['position'][0] - start_pose['position'][0]),
                start_pose['position'][1] + t * (goal_pose['position'][1] - start_pose['position'][1]),
                start_pose['position'][2] + t * (goal_pose['position'][2] - start_pose['position'][2])
            ]

            # SLERP interpolation of the quaternion
            q0 = start_pose['orientation']
            q1 = goal_pose['orientation']
            quat = self.slerp(q0, q1, t)

            trajectory.append({
                'position': pos,
                'orientation': quat
            })

        return trajectory

    def find_pose_by_name(self, status, target_name):
        """Look up a pose by name"""
        for i, frame_name in enumerate(status.frame_names):
            if frame_name == target_name:
                pose = status.frame_poses[i]
                # Extract the position and quaternion
                position = [pose.position.x, pose.position.y, pose.position.z]
                orientation = [pose.orientation.x, pose.orientation.y,
                              pose.orientation.z, pose.orientation.w]
                return {'position': position, 'orientation': orientation}
        raise RuntimeError(f"Frame name {target_name} not found")

    def execute_end_pose_control(self):
        """Execute end-effector pose control"""
        time.sleep(1.0)  # Wait 1 second

        # Get the current status
        status = self.robot.get_motion_control_status()

        # Get the starting poses
        start_left_pose = self.find_pose_by_name(status, LEFT_NAME)
        start_right_pose = self.find_pose_by_name(status, RIGHT_NAME)

        # Target poses
        goal_left_pose = {
            'position': TARGET_LEFT_POSITION,
            'orientation': TARGET_LEFT_ORIENTATION
        }
        goal_right_pose = {
            'position': TARGET_RIGHT_POSITION,
            'orientation': TARGET_RIGHT_ORIENTATION
        }

        # Compute the number of steps
        n_left = self.calculate_n_steps(start_left_pose['position'],
                                        goal_left_pose['position'],
                                        MAX_STEP_CM)
        n_right = self.calculate_n_steps(start_right_pose['position'],
                                        goal_right_pose['position'],
                                        MAX_STEP_CM)
        n_steps = max(n_left, n_right)

        print(f"Left arm steps: {n_left}, right arm steps: {n_right}, total steps: {n_steps}")
        # Plan the trajectories
        traj_left = self.plan_trajectory(start_left_pose, goal_left_pose, n_steps)
        traj_right = self.plan_trajectory(start_right_pose, goal_right_pose, n_steps)

        # Execute the trajectory
        dt = 1.0 / RATE_HZ
        for i in range(n_steps):
            # Create the end-effector pose control request
            end_pose = agibot_gdk.EndEffectorPose()
            end_pose.life_time = LIFETIME
            end_pose.group = agibot_gdk.EndEffectorControlGroup.kBothArms

            # Set the left arm pose
            end_pose.left_end_effector_pose.position.x = traj_left[i]['position'][0]
            end_pose.left_end_effector_pose.position.y = traj_left[i]['position'][1]
            end_pose.left_end_effector_pose.position.z = traj_left[i]['position'][2]
            end_pose.left_end_effector_pose.orientation.x = traj_left[i]['orientation'][0]
            end_pose.left_end_effector_pose.orientation.y = traj_left[i]['orientation'][1]
            end_pose.left_end_effector_pose.orientation.z = traj_left[i]['orientation'][2]
            end_pose.left_end_effector_pose.orientation.w = traj_left[i]['orientation'][3]

            # Set the right arm pose
            end_pose.right_end_effector_pose.position.x = traj_right[i]['position'][0]
            end_pose.right_end_effector_pose.position.y = traj_right[i]['position'][1]
            end_pose.right_end_effector_pose.position.z = traj_right[i]['position'][2]
            end_pose.right_end_effector_pose.orientation.x = traj_right[i]['orientation'][0]
            end_pose.right_end_effector_pose.orientation.y = traj_right[i]['orientation'][1]
            end_pose.right_end_effector_pose.orientation.z = traj_right[i]['orientation'][2]
            end_pose.right_end_effector_pose.orientation.w = traj_right[i]['orientation'][3]

            try:
                result = self.robot.end_effector_pose_control(end_pose)
                if result != 0:
                    print(f"Failed to send control command, step: {i}")
                    return
                print(f"Control command sent successfully, step: {i}")
            except Exception as e:
                print(f"Exception sending control command, step: {i}, error: {e}")
                return

            time.sleep(dt)

        # Hold the final pose
        if HOLD_FINAL:
            print("Entering end-effector pose hold (Ctrl+C to stop)...")
            try:
                final_left = traj_left[-1]
                final_right = traj_right[-1]

                while True:
                    end_pose = agibot_gdk.EndEffectorPose()
                    end_pose.life_time = LIFETIME
                    end_pose.group = agibot_gdk.EndEffectorControlGroup.kBothArms

                    # Set the left arm pose
                    end_pose.left_end_effector_pose.position.x = final_left['position'][0]
                    end_pose.left_end_effector_pose.position.y = final_left['position'][1]
                    end_pose.left_end_effector_pose.position.z = final_left['position'][2]
                    end_pose.left_end_effector_pose.orientation.x = final_left['orientation'][0]
                    end_pose.left_end_effector_pose.orientation.y = final_left['orientation'][1]
                    end_pose.left_end_effector_pose.orientation.z = final_left['orientation'][2]
                    end_pose.left_end_effector_pose.orientation.w = final_left['orientation'][3]

                    # Set the right arm pose
                    end_pose.right_end_effector_pose.position.x = final_right['position'][0]
                    end_pose.right_end_effector_pose.position.y = final_right['position'][1]
                    end_pose.right_end_effector_pose.position.z = final_right['position'][2]
                    end_pose.right_end_effector_pose.orientation.x = final_right['orientation'][0]
                    end_pose.right_end_effector_pose.orientation.y = final_right['orientation'][1]
                    end_pose.right_end_effector_pose.orientation.z = final_right['orientation'][2]
                    end_pose.right_end_effector_pose.orientation.w = final_right['orientation'][3]

                    try:
                        result = self.robot.end_effector_pose_control(end_pose)
                        if result != 0:
                            print("Failed to hold pose")
                            break
                    except Exception as e:
                        print(f"Exception while holding pose: {e}")
                        break

                    time.sleep(dt)
            except KeyboardInterrupt:
                print("\nHold interrupted")


def main():
    # Initialize the GDK system
    if agibot_gdk.gdk_init() != agibot_gdk.GDKRes.kSuccess:
        print("GDK initialization failed")
        return

    print("GDK initialized successfully")

    try:
        robot = agibot_gdk.Robot()
        time.sleep(2)  # Wait for the robot to finish initializing

        print("Moving to initial pose...")
        joint_control_req = agibot_gdk.JointControlReq()
        joint_control_req.joint_names = [
            "idx01_body_joint1", "idx02_body_joint2", "idx03_body_joint3", "idx04_body_joint4", "idx05_body_joint5",
            "idx11_head_joint1", "idx12_head_joint2", "idx13_head_joint3",
            "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 left-arm joints
            "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 right-arm joints
        ]
        joint_control_req.joint_positions = [-0.12526109443163597,
                                            0.8407635483115572,
                                            -0.971927299903638,
                                            0, 
                                            0,
                                            0,
                                            0,
                                            0.0036426282540360732,
                                            1.7301117664332919,
                                            -1.1500181062743811,
                                            -1.5999388458662482,
                                            -1.79993398793182,
                                            -0.41992232715148303, 
                                            5.8962386534354331e-05, 
                                            3.8709046444301724e-05, 
                                            -1.7301030279359648, 
                                            -1.1500377304426637, 
                                            1.5999483134039236, 
                                            -1.6252835640650924, 
                                            0.42000178256260562, 
                                            1.3781858641160056e-05, 
                                            3.7151097206605374e-06]
        joint_control_req.joint_velocities = [0.3] * 22 # Set the velocities
        joint_control_req.life_time = 5.0

        result = robot.joint_control_request(joint_control_req)
        if result != 0:
            print("Failed to move to initial pose")
            return
        print("Successfully moved to initial pose, waiting to settle...")
        time.sleep(2)  

        controller = EndEffectorController(robot)
        controller.execute_end_pose_control()

    except Exception as e:
        print(f"Error occurred during execution: {e}")
    finally:
        # Release GDK system resources
        if agibot_gdk.gdk_release() != agibot_gdk.GDKRes.kSuccess:
            print("GDK release failed")
        else:
            print("GDK released successfully")


if __name__ == "__main__":
    main()

Usage Notes

  1. GDK initialization: agibot_gdk.gdk_init() must be called to initialize the GDK system before using any robot control functionality
  2. GDK release: agibot_gdk.gdk_release() must be called to release GDK system resources before the program exits
  3. Initialization wait: After creating a Robot object, it is recommended to wait 2 seconds to ensure the DDS connection is established
  4. Joint names: Confirm that joint names are correct before use; they can be retrieved via get_motion_control_status()
  5. Position range: Joint positions should stay within a safe range to avoid exceeding mechanical limits
  6. Velocity limits: Set reasonable joint velocities to avoid dangerous, overly fast motion
  7. Lifetime: Set a reasonable request lifetime to avoid commands expiring
  8. Exception handling: All interfaces raise a std::runtime_error exception on failure, which must be handled appropriately
  9. Data types: Position parameters use radians; velocity parameters use radians/second
  10. Timestamp precision: Timestamps are in nanoseconds, which can be used for precise time synchronization

Application Scenarios

  • Basic control: Implementing basic motion control of the robot
  • Status monitoring: Real-time monitoring of each robot joint's state
  • Motion recording: Recording and replaying robot motion sequences
  • Precise control: Implementing high-precision joint position control
  • Safety detection: Monitoring abnormal robot states to ensure safe operation
  • End-effector control: Controlling the opening/closing of grippers and other end effectors
  • Multi-joint coordination: Implementing coordinated motion across multiple joints
  • Pose control: Cartesian-coordinate-based end-effector pose control