Python Examples

This file provides brief examples for the publicly available functions and types in the API, demonstrating how to use these interfaces.

Robot Joint Names (G1 2.2)

Joint Group List

Robot joint group names include: ["head", "left_arm", "right_arm", "leg", "left_gripper", "right_gripper", "left_suction_cup", "right_suction_cup"]

Detailed Information for Each Joint Group

Joint Group	English Name	Number of Joints	Joint Name List
Head	head	2	head_joint1, head_joint2
Legs	leg	5	leg_joint1, leg_joint2, leg_joint3, leg_joint4, leg_joint5
Left Arm	left_arm	7	left_arm_joint1, left_arm_joint2, left_arm_joint3, left_arm_joint4, left_arm_joint5, left_arm_joint6, left_arm_joint7
Right Arm	right_arm	7	right_arm_joint1, right_arm_joint2, right_arm_joint3, right_arm_joint4, right_arm_joint5, right_arm_joint6, right_arm_joint7
Left Gripper	left_gripper	1	left_gripper_joint1
Right Gripper	right_gripper	1	right_gripper_joint1
Left Suction Cup	left_suction_cup	1	left_suction_cup_joint1
Right Suction Cup	right_suction_cup	1	right_suction_cup_joint1

How To Use joint_groups Correctly

A "joint group" is the SDK control unit instead of a single joint. This grouping is used to ensure:

• Kinematic-chain consistency: arm/head/leg joints are validated and controlled as one chain.
• Deterministic command ordering: joint_groups are expanded to concrete joints in group order.
• Group-level validation: active/passive group constraints are checked before execution.

Usage rules:

1. Prefer joint_groups when controlling full chains (head/arm/leg/gripper/suction cup).
2. If joint_names is provided, it takes precedence over joint_groups.
3. If both joint_groups and joint_names are empty, SDK defaults to all active body joint groups.
4. To avoid hard-coded mistakes, call get_joint_group_names() and get_joint_names(True, groups) first.

Typical Group Scenarios (G1)

Joint Group	Typical Scenario
head	Head orientation / camera aiming
left_arm / right_arm	Arm reaching and manipulation
leg	Lower-body posture adjustment
left_gripper / right_gripper	Grasp width control
left_suction_cup / right_suction_cup	Vacuum pick and place

Sensor Types and Frames

For sensor data access (get_rgb_data, get_depth_data, get_imu_data, get_lidar_data), use the SensorType enum to specify which sensor to query. Available SensorType enums are documented in: Python API Reference > SensorType.

For sensor extrinsic calibration, there are two methods:

• get_sensor_extrinsic(): SDK internally maps frame IDs, pass SensorType directly
• get_transform(): Requires explicit frame names. The second column below lists frame IDs for each sensor.

SensorType	Frame ID
HEAD_LEFT_CAMERA	head_left_camera_color_optical_frame
HEAD_RIGHT_CAMERA	head_right_camera_color_optical_frame
LEFT_ARM_CAMERA	left_arm_camera_color_optical_frame
LEFT_ARM_DEPTH_CAMERA	left_arm_camera_color_optical_frame
RIGHT_ARM_CAMERA	right_arm_camera_color_optical_frame
RIGHT_ARM_DEPTH_CAMERA	right_arm_camera_color_optical_frame
BASE_LIDAR	lidar_base_link
TORSO_IMU	imu_base_link
BASE_ULTRASONIC	— (no TF frame)
LEFT_FRONT_SURROUND_CAMERA	left_front_surround_color_optical_frame
RIGHT_FRONT_SURROUND_CAMERA	right_front_surround_color_optical_frame
LEFT_REAR_SURROUND_CAMERA	left_rear_surround_color_optical_frame
RIGHT_REAR_SURROUND_CAMERA	right_rear_surround_color_optical_frame

Note: Call get_frame_names() to get all available coordinate frames.

Class: GalbotRobot

Tips: If you get data immediately after program startup, the data may not be ready right away. You may sleep for a few seconds as appropriate.

Get Instance and Initialize (get_instance && init)

Applicable Scenarios: During program startup, get the robot singleton and complete SDK initialization. Must be called before using other APIs.

examples/python/galbot_robot/get_instance.py

from galbot_sdk.g1 import GalbotRobot
import time

# Get GalbotRobot
robot = GalbotRobot()

state = robot.init()
if not state:
    print("Initialization failed")
else:
    print("Initialization succeeded")

while robot.is_running():
    # business logic
    time.sleep(1)
    break

# Send exit signal to exit the program
robot.request_shutdown()
# Wait for exit state
robot.wait_for_shutdown()
# Release SDK-related resources
robot.destroy()

Log interface

Applicable Scenarios: Output logs using the SDK's built-in logging system, unifying log levels and formats.

examples/python/galbot_robot/logger_example.py

import galbot_sdk

cfg = {
    # Log storage directory; if empty, defaults to ~/galbot_sdk_log/user_log
    "path": "",

    # Log file name; if empty, defaults to <process_name>_<current_time>_<pid>_<thread_id>.log
    "file_name": "",

    # log bytes, log size,
    "file_max_size": 10 * 1024 * 1024,  # 10MB

    # Maximum rotating log files; oldest file is overwritten when limit exceeded
    "file_max_num": 5,

    # Whether to output logs to console; default is False
    "console_output": True,

    # Log output level, available values: debug, info, warning, error, critical
    "level": "info",
}

galbot_sdk.init_logger(cfg)

# Write log
galbot_sdk.debug("Debug example")
galbot_sdk.info("Info example")
galbot_sdk.warning("Warning example")
galbot_sdk.error("Error example")
galbot_sdk.critical("Critical example")

Set Joint Positions (set_joint_positions)

Applicable Scenarios: Single-point movement, low-frequency position control tasks. This interface performs velocity-limited trajectory interpolation internally, making it suitable for one-time movement to target joint angles.

WARNING: This interface is NOT suitable for high-frequency joint control scenarios with model inference output! Each call to this interface produces a new trajectory interpolation, continuous calls will result in discontinuous motion and delay.

If you are working on a model inference scenario, please use set_joint_commands or set_joint_commands_batch to issue joint commands directly.

examples/python/galbot_robot/set_joint_positions.py

import time
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import ControlStatus

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
print('Initialization succeeded')

# Program started, waiting for data
time.sleep(2)

# Set head joints to 0.2, 0.2, block and wait for motion to complete, max timeout 10s
joint_pos = [0.2, 0.2]
# Set head joint group; if empty, defaults to whole body joints ["leg", "head", "left_arm", "right_arm"]
joint_groups = ["head"]
# Whether to block until joints reach target
is_blocking = True
# Limit joint max speed to 0.1rad/s
max_speed = 0.1
# Maximum blocking wait time
timeout_s = 10

status = robot.set_joint_positions(
    joint_pos, joint_groups, [], is_blocking, max_speed, timeout_s
)

if status != ControlStatus.SUCCESS:
    print("Joint angle setting failed")
else:
    print('Joint angle setting succeeded')

time.sleep(1)

# Use specific joint names for control; this parameter overrides joint_groups
joint_names = ["head_joint1", "head_joint2"]
joint_pos = [0.0, 0.0]

status = robot.set_joint_positions(
    joint_pos, [], joint_names, is_blocking, max_speed, timeout_s
)

if status != ControlStatus.SUCCESS:
    print("Joint angle setting failed")
else:
    print('Joint angle setting succeeded')

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Set Gripper Command (set_gripper_command)

Applicable Scenarios: Control gripper opening and closing. Supports both position control and torque control modes.

examples/python/galbot_robot/set_gripper_command.py

import time
from galbot_sdk.g1 import GalbotRobot, G1JointGroup, ControlStatus

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
print('Initialization succeeded')
# Program started, waiting for data
time.sleep(2)

# Set left gripper width to 0.02m, speed 0.05m, torque 10N, block until gripper reaches position
status = robot.set_gripper_command(
    G1JointGroup.left_gripper, 0.02, 0.05, 10, True
)
if status != ControlStatus.SUCCESS:
    print("Set gripper failed")
else:
    print('Set gripper success')

# Set left gripper width to 0.1m, speed 0.05m, torque 10N, block until gripper reaches position
status = robot.set_gripper_command(
    G1JointGroup.left_gripper, 0.1, 0.05, 10, True
)

if status != ControlStatus.SUCCESS:
    print("Set gripper failed")
else:
    print('Set gripper success')

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Set Suction Cup Command (set_suction_cup_command)

Applicable Scenarios: Control suction cup to suck/release objects, get current suction cup status.

examples/python/galbot_robot/set_suction_cup_command.py

from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import G1JointGroup, ControlStatus
import time

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
time.sleep(1)
print('Initialization succeeded')

# Set suction cup joint group (right suction cup)
joint_group = G1JointGroup.right_suction_cup

# Whether to activate suction cup
activate = True  # True: activate suction cup, False: deactivate suction cup

# Send suction cup control command
status = robot.set_suction_cup_command(
    joint_group,
    activate
)

# Check execution results
if status != ControlStatus.SUCCESS:
    print("Set suction cup failed")
else:
    print("Set suction cup succeeded")

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Stop Trajectory Execution (stop_trajectory_execution)

Applicable Scenarios: Stop currently executing joint trajectory and interrupt ongoing motion.

examples/python/galbot_robot/stop_trajectory_execution.py

import signal
import threading
import time

from galbot_sdk.g1 import (
    ControlStatus,
    GalbotRobot,
    JointCommand,
    Trajectory,
    TrajectoryPoint,
)

# Set when the trajectory call has returned, so the worker stops waiting for a
# Ctrl-C that is never coming.
trajectory_done = threading.Event()
# Set once the worker has requested shutdown, so the main flow does not repeat it.
shutdown_requested = threading.Event()


def make_head_trajectory():
    trajectory = Trajectory()
    trajectory.joint_names = ["head_joint2"]
    point = TrajectoryPoint()
    point.time_from_start_second = 10.0
    command = JointCommand()
    command.position = 0.4
    point.joint_command_vec = [command]
    trajectory.points = [point]
    return trajectory


def stop_trajectory(robot):
    print("Worker thread: stopping trajectory execution...")
    status = robot.stop_trajectory_execution()
    if status == ControlStatus.SUCCESS:
        print("Worker thread: trajectory stop succeeded")
    else:
        print(f"Worker thread: trajectory stop FAILED: {status}")

    shutdown_requested.set()


def stop_trajectory_on_ctrl_c(robot):
    """Call stop from a worker while the main thread waits for the trajectory."""
    # sigwait() cannot be cancelled, so wait in slices: when the trajectory
    # finishes on its own no Ctrl-C ever arrives, and the worker still has to
    # exit or the join() in the main flow would block forever.
    while not trajectory_done.is_set():
        if signal.sigtimedwait({signal.SIGINT}, 0.2) is None:
            continue
        stop_trajectory(robot)
        return


def confirm_motion():
    print("⚠️  Ensure the emergency-stop button is released and the area is clear.")
    if input("Start trajectory execution? (y/n): ").strip().lower() != "y":
        raise SystemExit("Motion was not confirmed.")


confirm_motion()
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(2)
print("Initialization succeeded")

# execute_joint_trajectory blocks in the main thread. Ctrl-C is consumed by
# the worker, which stops the motion and then shuts the SDK down.
print("Trajectory is executing. Press Ctrl-C to stop it.")
stop_thread = threading.Thread(target=stop_trajectory_on_ctrl_c, args=(robot,))
stop_thread.start()
status = robot.execute_joint_trajectory(make_head_trajectory(), is_blocking=True)
trajectory_done.set()
print(f"Trajectory execution returned: {status}")
stop_thread.join()

signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")

Execute Joint Trajectory (execute_joint_trajectory)

Applicable Scenarios: Execute a predefined joint-space trajectory with multiple waypoints. The SDK executes the entire trajectory according to time nodes.

warning

When building Trajectory / TrajectoryPoint, collect items in a plain Python list and assign the whole list back, as the example below does. Calling .append() directly on attributes such as traj.points or traj_point.joint_command_vec has no effect, causing the interface to return INVALID_INPUT while the robot does not move. See Troubleshooting.

examples/python/galbot_robot/execute_joint_trajectory.py

from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import G1JointGroup, ControlStatus, Trajectory, TrajectoryPoint, JointCommand
import time
import numpy as np
from typing import List

# Generate trajectory point with position and time info
def generate_target_point(q: List[float], target_time: float = 10):
    """Generate target for joints"""
    joint_position = TrajectoryPoint()
    joint_position.time_from_start_second = target_time
    joint_command_vec = []
    for joint in q:
        joint_cmd = JointCommand()
        joint_cmd.position = joint
        joint_command_vec.append(joint_cmd)
    joint_position.joint_command_vec = joint_command_vec
    return joint_position

def generate_target_trajectory(trajectory, joint_groups=[], joint_names=[], dt=0.008):
    """Generate trajectory for joints"""
    if trajectory is None or np.ndim(trajectory) != 2 or len(trajectory) == 0:
        return None

    # Create Trajectory
    traj = Trajectory()
    traj.joint_groups = joint_groups
    traj.joint_names = joint_names

    time = 0.0
    points = []
    for state in trajectory:
        time += dt
        # Create single trajectory point
        traj_point = generate_target_point(state, time)
        points.append(traj_point)

    traj.points = points
    return traj

# Trajectory execution function
def traj_exec():
    # Get GalbotRobot singleton and initialize; only needs to be initialized once
    robot = GalbotRobot()
    robot.init()
    time.sleep(1)
    print("Initialization succeeded")

    head_traj = np.linspace(
        [0.0, 0.0],
        [0.5, 0.0],
        num=200,
    )
    # Whether to block and wait for trajectory execution to complete
    is_block = True
    # Specify which joint group trajectory to execute
    joint_groups = ["head"]
    # Execute specified joint trajectory; if provided, overrides joint_groups parameters
    joint_names = []
    status = robot.execute_joint_trajectory(generate_target_trajectory(head_traj.tolist(), joint_groups, joint_names), is_block)

    # Check execution results
    if status != ControlStatus.SUCCESS:
        print("Trajectory execution failed")
    else:
        print("Trajectory execution succeeded")

    # send SIGINT shutdown signal
    robot.request_shutdown()
    # Wait until entering shutdown state
    robot.wait_for_shutdown()
    # Perform SDK resource release
    robot.destroy()
    print('Resources released successfully')

traj_exec()

Set Joint Commands (set_joint_commands)

Applicable Scenarios: High-frequency joint control, such as model inference output (each inference step outputs one frame of joint commands), custom trajectory tracking. Issues joint commands directly to the low-level controller without extra interpolation.

examples/python/galbot_robot/set_joint_commands_example.py

import time
import math
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import JointCommand

def head_high_frequency_control():
    """
    Head high-frequency control example
    """

    control_frequency = 100.0  # Hz
    dt = 1.0 / control_frequency
    duration = 4.0  # Control for 4 seconds

    amplitude = 0.3  # Maximum oscillation amplitude (rad)
    frequency = 0.5  # Sine frequency (Hz)
    # Joint group name to control
    joint_groups = ["head"]
    # Fill this field to control specific joints, which will override joint_groups. If empty, controls all joints in joint_groups by default
    joint_names = []

    print("Start high-frequency head control")

    joint_commands = [JointCommand(), JointCommand()]

    start_time = time.time()

    while True:
        current_time = time.time() - start_time
        if current_time > duration:
            break

        # Generate sine trajectory
        target_position = amplitude * math.sin(
            2 * math.pi * frequency * current_time
        )

        # Set head joint angles
        joint_commands[0].position = target_position
        joint_commands[1].position = target_position
        print(f"current: {current_time:.2f}s, target: {target_position:.3f} rad")

        # Expected arrival time
        time_from_start_sec = 0.0

        execution_status = GalbotRobot().set_joint_commands(
            joint_commands,
            joint_groups,
            joint_names,
            time_from_start_sec
        )

        # Sleep at a fixed interval
        time.sleep(dt)

    print("end")

def main():
    # Get and initialize the GalbotRobot singleton
    robot = GalbotRobot()

    if robot.init():
        print("System initialized successfully!")
    else:
        print("System initialization failed!")
        return

    # Program started, waiting for data
    time.sleep(2)
    
    head_high_frequency_control()

    # Exit system and release SDK resources
    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    print("Program exited")


if __name__ == "__main__":
    main()

Set Joint Commands in Batch Mode (set_joint_commands_batch)

Applicable Scenarios: Batch issue multiple future frames of joint commands, suitable for scenarios where motion prediction models output multiple steps at once, improving control frequency and continuity.

warning

This interface also takes a Trajectory object and is built the same way as execute_joint_trajectory: collect items in a plain Python list and assign the whole list back. Calling .append() directly on attributes such as traj.points or traj_point.joint_command_vec has no effect, causing the interface to return INVALID_INPUT while the robot does not move. See Troubleshooting.

examples/python/galbot_robot/set_joint_commands_batch.py

from galbot_sdk.g1 import GalbotRobot, G1JointGroup, ControlStatus, Trajectory, TrajectoryPoint, JointCommand
import time
import numpy as np
from typing import List

# Generate trajectory point with position and time info
def generate_target_point(q: List[float], target_time: float = 10):
    """Generate target for joints"""
    joint_position = TrajectoryPoint()
    joint_position.time_from_start_second = target_time
    joint_command_vec = []
    for joint in q:
        joint_cmd = JointCommand()
        joint_cmd.position = joint
        joint_cmd.velocity = 0.0
        joint_cmd.acceleration = 0.0
        joint_cmd.effort = 0.0
        # joint_cmd.Kp = 0.0
        # joint_cmd.Kd = 0.0
        joint_command_vec.append(joint_cmd)
    joint_position.joint_command_vec = joint_command_vec
    return joint_position

def generate_batch_trajectory(trajectory, joint_groups=[], joint_names=[], dt=0.008):
    """Generate batch trajectory for joints"""
    if trajectory is None or np.ndim(trajectory) != 2 or len(trajectory) == 0:
        return None

    # Create Trajectory
    traj = Trajectory()
    traj.joint_groups = joint_groups
    traj.joint_names = joint_names

    time = 0.0
    points = []
    for state in trajectory:
        time += dt
        # Create single trajectory point
        traj_point = generate_target_point(state, time)
        points.append(traj_point)

    traj.points = points
    return traj

# Batch set-joint-command function
def batch_commands_exec():
    # Get GalbotRobot singleton and initialize; only needs to be initialized once
    robot = GalbotRobot()
    robot.init()
    time.sleep(1)
    print("Initialization succeeded")

    # Generate batched trajectory data (joint commands at multiple time points)
    head_traj = np.linspace(
        [0.0, 0.0],
        [0.5, 0.0],
        num=10,  # Number of batch trajectory points
    )
    # Specify which joint group trajectory to execute
    joint_groups = ["head"]
    # Execute specified joint trajectory; if provided, overrides joint_groups parameters
    joint_names = []
    
    # Batch set joint commands (non-blocking, returns immediately)
    status = robot.set_joint_commands_batch(generate_batch_trajectory(head_traj.tolist(), joint_groups, joint_names))

    # Check execution results
    if status != ControlStatus.SUCCESS:
        print("Batch command submission failed")
    else:
        print("Batch commands submitted, executing in background (non-blocking)")

    # Wait for a while to let the command execute
    time.sleep(1)

    # send SIGINT shutdown signal
    robot.request_shutdown()
    # Wait until entering shutdown state
    robot.wait_for_shutdown()
    # Perform SDK resource release
    robot.destroy()
    print('Resources released successfully')

batch_commands_exec()

Publish Raw Target Directly (publish_target)

Applicable Scenarios: Advanced users construct a SingoriXTarget directly and publish it through the WBCS publish channel. Suitable for high-frequency raw target streaming and one-shot dispatch of mixed joint/task targets.

examples/python/galbot_robot/publish_target_example.py

"""
G1 PublishTarget menu example for SDK mirror SingoriXTarget.

This example shows how to construct `SingoriXTarget` directly at the Python SDK
layer and send it to the low-level WBCS through `publish_target()`.

1. Joint-space commands are written into `target_group_trajectory_map`
2. Chassis pose / twist style task-space commands are written into `target_task_trajectory_map`
3. One `SingoriXTarget` can contain both joint trajectory and task trajectory
4. The current SDK does not automatically switch the chassis controller, so
   pose / twist / mixed scenes explicitly call `switch_controller(...)`
5. The base twist scene automatically sends a zero-twist target after the
   configured duration to stop the chassis
"""

import math
import time

from galbot_sdk.g1 import (
    ControlStatus,
    G1ControllerName,
    G1JointGroup,
    GalbotRobot,
    GroupCommand,
    JointCommand,
    Pose,
    SingoriXTarget,
    TargetConfig,
    TargetGroupTrajectory,
    TargetSampling,
    TargetTaskTrajectory,
    TaskCommand,
    Twist,
    Vector3,
    FrameTriad,
    TARGET_DATA_DEFAULT,
    TARGET_DATA_FRAME_POSE,
    TARGET_DATA_FRAME_TWIST,
    TARGET_TYPE_DEFAULT,
    TARGET_TYPE_OVERRIDE,
    TARGET_TYPE_PROVERRIDE,
)


CHASSIS_TASK_NAME = "chassis"
CHASSIS_SUBTASK_POSE = "chassis_pose"
CHASSIS_SUBTASK_TWIST = "chassis_twist"


def now_ns():
    return time.time_ns()


def status_to_string(status):
    return getattr(status, "name", str(status))


def yaw_to_quaternion(yaw):
    return [0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5)]


def make_empty_target():
    target = SingoriXTarget()
    target.header.timestamp_ns = now_ns()
    target.header.frame_id = "base_link"
    return target


def make_group_target_config():
    config = TargetConfig()
    config.target_data = TARGET_DATA_DEFAULT
    config.target_type = TARGET_TYPE_PROVERRIDE
    config.target_sampling = TargetSampling.TARGET_SAMPLING_DEFAULT
    config.target_priority = 1
    return config


def make_pose_target_config():
    config = TargetConfig()
    config.target_data = TARGET_DATA_FRAME_POSE
    # config.target_type = TARGET_TYPE_PROVERRIDE
    config.target_type = TARGET_TYPE_OVERRIDE
    config.target_sampling = TargetSampling.TARGET_SAMPLING_LINEAR_INTERPOLATE
    config.target_priority = 1
    return config


def make_twist_target_config():
    config = TargetConfig()
    config.target_data = TARGET_DATA_FRAME_TWIST
    config.target_type = TARGET_TYPE_OVERRIDE
    config.target_sampling = TargetSampling.TARGET_SAMPLING_DIRECT_PASS
    config.target_priority = 1
    return config


def make_joint_command(position):
    command = JointCommand()
    command.position = position
    command.velocity = 0.0
    command.acceleration = 0.0
    command.effort = 0.0
    return command


def make_vector3(x, y, z):
    vec = Vector3()
    vec.x = x
    vec.y = y
    vec.z = z
    return vec


def build_joint_target(group_name, joint_names, positions, time_from_start_s):
    target = make_empty_target()

    group_traj = TargetGroupTrajectory()
    group_traj.target_config = make_group_target_config()
    group_traj.joint_names = list(joint_names)

    command = GroupCommand()
    command.time_from_start_s = time_from_start_s
    command.joint_commands = [make_joint_command(position) for position in positions]
    group_traj.group_commands = [command]

    target.target_group_trajectory_map = {group_name: group_traj}
    return target


def build_chassis_pose_target(x, y, yaw, time_from_start_s, frame_id="rel(0)", reference_frame_id="odom"):
    target = make_empty_target()

    task_traj = TargetTaskTrajectory()
    task_traj.target_config = make_pose_target_config()
    task_traj.group_names = [G1JointGroup.chassis]
    task_traj.subtask_names = [f"{CHASSIS_SUBTASK_POSE}_{now_ns()}"]

    triad = FrameTriad()
    triad.header.timestamp_ns = now_ns()
    triad.header.frame_id = frame_id
    triad.body_frame_id = "base_link"
    triad.reference_frame_id = reference_frame_id
    triad.pose = Pose([x, y, 0.0], yaw_to_quaternion(yaw))

    command = TaskCommand()
    command.time_from_start_s = time_from_start_s
    command.subtask_commands = [triad]
    task_traj.task_commands = [command]

    target.target_task_trajectory_map = {CHASSIS_TASK_NAME: task_traj}
    return target


def build_chassis_twist_target(vx, vy, wz, time_from_start_s):
    target = make_empty_target()

    task_traj = TargetTaskTrajectory()
    task_traj.target_config = make_twist_target_config()
    task_traj.group_names = [G1JointGroup.chassis]
    task_traj.subtask_names = [f"{CHASSIS_SUBTASK_TWIST}_{now_ns()}"]

    twist = Twist()
    twist.linear = make_vector3(vx, vy, 0.0)
    twist.angular = make_vector3(0.0, 0.0, wz)

    triad = FrameTriad()
    triad.header.timestamp_ns = now_ns()
    triad.header.frame_id = "base_link"
    triad.body_frame_id = "base_link"
    triad.reference_frame_id = "base_link"
    triad.twist = twist

    command = TaskCommand()
    command.time_from_start_s = time_from_start_s
    command.subtask_commands = [triad]
    task_traj.task_commands = [command]

    target.target_task_trajectory_map = {CHASSIS_TASK_NAME: task_traj}
    return target


def build_stop_twist_target():
    return build_chassis_twist_target(0.0, 0.0, 0.0, 0.1)


def merge_targets(targets):
    merged = make_empty_target()
    group_map = {}
    task_map = {}
    for target in targets:
        group_map.update(target.target_group_trajectory_map)
        task_map.update(target.target_task_trajectory_map)
    merged.target_group_trajectory_map = group_map
    merged.target_task_trajectory_map = task_map
    return merged


def ensure_controller(robot, controller_name):
    status = robot.switch_controller(controller_name)
    print(f"switch_controller({controller_name}): {status_to_string(status)}")
    return status


def run_twist_scene(robot, scene_name, target, twist_duration_s):
    print(f"{scene_name}: start moving for {twist_duration_s} seconds")
    motion_status = robot.publish_target(target)
    print(f"{scene_name} publish_target status: {status_to_string(motion_status)}")
    if motion_status != ControlStatus.SUCCESS:
        return motion_status

    time.sleep(twist_duration_s)
    print(f"{scene_name}: send stop twist target")
    stop_status = robot.publish_target(build_stop_twist_target())
    print(f"{scene_name} stop status: {status_to_string(stop_status)}")
    return stop_status


def print_menu():
    print(
        "\nAvailable commands:\n"
        "  joint        - publish a joint-only head target\n"
        "  base_pose    - publish a chassis pose target\n"
        "  base_twist   - publish a chassis twist target with auto stop\n"
        "  mixed_pose   - publish head + left_arm + chassis pose in one target\n"
        "  mixed_twist  - publish head + left_arm + chassis twist in one target\n"
        "  quit         - exit example\n"
    )


def main():
    robot = GalbotRobot()
    if not robot.init():
        print("robot init failed")
        return

    time.sleep(2)

    head_joint_names = robot.get_joint_names(True, [G1JointGroup.head])
    left_arm_joint_names = robot.get_joint_names(True, [G1JointGroup.left_arm])
    if not head_joint_names or not left_arm_joint_names:
        print("failed to fetch active head/left_arm joints")
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return

    head_single_joint = [head_joint_names[0]]
    arm_single_joint = [left_arm_joint_names[0]]
    joint_time_s = 3.0
    pose_time_s = 4.0
    twist_command_time_s = 0.2
    twist_duration_s = 2.0

    print_menu()

    while True:
        command = input("Enter command: ").strip()
        if command == "quit":
            break

        if command == "joint":
            target = build_joint_target(G1JointGroup.head, head_single_joint, [0.2], joint_time_s)
            status = robot.publish_target(target)
            print(f"joint publish_target status: {status_to_string(status)}")
            continue

        if command == "base_pose":
            if ensure_controller(robot, G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
                continue
            target = build_chassis_pose_target(0.2, 0.0, 0.0, pose_time_s)
            status = robot.publish_target(target)
            print(f"base_pose publish_target status: {status_to_string(status)}")
            continue

        if command == "base_twist":
            if ensure_controller(robot, G1ControllerName.CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
                continue
            target = build_chassis_twist_target(0.05, 0.0, 0.0, twist_command_time_s)
            run_twist_scene(robot, "base_twist", target, twist_duration_s)
            continue

        if command == "mixed_pose":
            if ensure_controller(robot, G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
                continue
            target = merge_targets(
                [
                    build_joint_target(G1JointGroup.head, head_single_joint, [0.2], joint_time_s),
                    build_joint_target(G1JointGroup.left_arm, arm_single_joint, [0.1], joint_time_s),
                    build_chassis_pose_target(0.1, 0.0, 0.0, pose_time_s),
                ]
            )
            status = robot.publish_target(target)
            print(f"mixed_pose publish_target status: {status_to_string(status)}")
            continue

        if command == "mixed_twist":
            if ensure_controller(robot, G1ControllerName.CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
                continue
            target = merge_targets(
                [
                    build_joint_target(G1JointGroup.head, head_single_joint, [-0.2], joint_time_s),
                    build_joint_target(G1JointGroup.left_arm, arm_single_joint, [-0.1], joint_time_s),
                    build_chassis_twist_target(0.05, 0.0, 0.0, twist_command_time_s),
                ]
            )
            run_twist_scene(robot, "mixed_twist", target, twist_duration_s)
            continue

        print(f"Unknown command: {command}")
        print_menu()

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()


if __name__ == "__main__":
    main()

Request Raw Target Directly (request_target)

Applicable Scenarios: Advanced users construct a SingoriXTarget directly and send it through the WBCS request channel. Suitable when the caller needs the service-side error response for a raw target dispatch.

examples/python/galbot_robot/request_target_example.py

"""
G1 RequestTarget menu example for SDK mirror SingoriXTarget.

This example shows how to construct `SingoriXTarget` directly at the Python SDK
layer and send it to the low-level WBCS through `request_target()`.

1. Joint-space commands are written into `target_group_trajectory_map`
2. Chassis pose / twist style task-space commands are written into `target_task_trajectory_map`
3. One `SingoriXTarget` can contain both joint trajectory and task trajectory
4. The current SDK does not automatically switch the chassis controller, so
   pose / twist / mixed scenes explicitly call `switch_controller(...)`
5. The base twist scene automatically sends a zero-twist target after the
   configured duration to stop the chassis
"""

import math
import time

from galbot_sdk.g1 import (
    ControlStatus,
    ErrorInfo,
    G1ControllerName,
    G1JointGroup,
    GalbotRobot,
    GroupCommand,
    JointCommand,
    Pose,
    SingoriXTarget,
    TargetConfig,
    TargetGroupTrajectory,
    TargetSampling,
    TargetTaskTrajectory,
    TaskCommand,
    Twist,
    Vector3,
    FrameTriad,
    TARGET_DATA_DEFAULT,
    TARGET_DATA_FRAME_POSE,
    TARGET_DATA_FRAME_TWIST,
    TARGET_TYPE_DEFAULT,
    TARGET_TYPE_OVERRIDE,
    TARGET_TYPE_PROVERRIDE,
)


CHASSIS_TASK_NAME = "chassis"
CHASSIS_SUBTASK_POSE = "chassis_pose"
CHASSIS_SUBTASK_TWIST = "chassis_twist"


def now_ns():
    return time.time_ns()


def yaw_to_quaternion(yaw):
    return [0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5)]


def make_empty_target():
    target = SingoriXTarget()
    target.header.timestamp_ns = now_ns()
    target.header.frame_id = "base_link"
    return target


def make_group_target_config():
    config = TargetConfig()
    config.target_data = TARGET_DATA_DEFAULT
    config.target_type = TARGET_TYPE_PROVERRIDE
    config.target_sampling = TargetSampling.TARGET_SAMPLING_DEFAULT
    config.target_priority = 1
    return config


def make_pose_target_config():
    config = TargetConfig()
    config.target_data = TARGET_DATA_FRAME_POSE
    config.target_type = TARGET_TYPE_OVERRIDE
    config.target_sampling = TargetSampling.TARGET_SAMPLING_LINEAR_INTERPOLATE
    config.target_priority = 1
    return config


def make_twist_target_config():
    config = TargetConfig()
    config.target_data = TARGET_DATA_FRAME_TWIST
    config.target_type = TARGET_TYPE_OVERRIDE
    config.target_sampling = TargetSampling.TARGET_SAMPLING_DIRECT_PASS
    config.target_priority = 1
    return config


def make_joint_command(position):
    command = JointCommand()
    command.position = position
    command.velocity = 0.0
    command.acceleration = 0.0
    command.effort = 0.0
    return command


def make_vector3(x, y, z):
    vec = Vector3()
    vec.x = x
    vec.y = y
    vec.z = z
    return vec


def build_joint_target(group_name, joint_names, positions, time_from_start_s):
    target = make_empty_target()

    group_traj = TargetGroupTrajectory()
    group_traj.target_config = make_group_target_config()
    group_traj.joint_names = list(joint_names)

    command = GroupCommand()
    command.time_from_start_s = time_from_start_s
    command.joint_commands = [make_joint_command(position) for position in positions]
    group_traj.group_commands = [command]

    target.target_group_trajectory_map = {group_name: group_traj}
    return target


def build_chassis_pose_target(x, y, yaw, time_from_start_s, frame_id="rel(0)", reference_frame_id="odom"):
    target = make_empty_target()

    task_traj = TargetTaskTrajectory()
    task_traj.target_config = make_pose_target_config()
    task_traj.group_names = [G1JointGroup.chassis]
    task_traj.subtask_names = [f"{CHASSIS_SUBTASK_POSE}_{now_ns()}"]

    triad = FrameTriad()
    triad.header.timestamp_ns = now_ns()
    triad.header.frame_id = frame_id
    triad.body_frame_id = "base_link"
    triad.reference_frame_id = reference_frame_id
    triad.pose = Pose([x, y, 0.0], yaw_to_quaternion(yaw))

    command = TaskCommand()
    command.time_from_start_s = time_from_start_s
    command.subtask_commands = [triad]
    task_traj.task_commands = [command]

    target.target_task_trajectory_map = {CHASSIS_TASK_NAME: task_traj}
    return target


def build_chassis_twist_target(vx, vy, wz, time_from_start_s):
    target = make_empty_target()

    task_traj = TargetTaskTrajectory()
    task_traj.target_config = make_twist_target_config()
    task_traj.group_names = [G1JointGroup.chassis]
    task_traj.subtask_names = [f"{CHASSIS_SUBTASK_TWIST}_{now_ns()}"]

    twist = Twist()
    twist.linear = make_vector3(vx, vy, 0.0)
    twist.angular = make_vector3(0.0, 0.0, wz)

    triad = FrameTriad()
    triad.header.timestamp_ns = now_ns()
    triad.header.frame_id = "base_link"
    triad.body_frame_id = "base_link"
    triad.reference_frame_id = "base_link"
    triad.twist = twist

    command = TaskCommand()
    command.time_from_start_s = time_from_start_s
    command.subtask_commands = [triad]
    task_traj.task_commands = [command]

    target.target_task_trajectory_map = {CHASSIS_TASK_NAME: task_traj}
    return target


def build_stop_twist_target():
    return build_chassis_twist_target(0.0, 0.0, 0.0, 0.1)


def merge_targets(targets):
    merged = make_empty_target()
    group_map = {}
    task_map = {}
    for target in targets:
        group_map.update(target.target_group_trajectory_map)
        task_map.update(target.target_task_trajectory_map)
    merged.target_group_trajectory_map = group_map
    merged.target_task_trajectory_map = task_map
    return merged


def print_error_info(scene_name, error_info):
    if error_info is None:
        print(f"{scene_name}: request_target returned None")
        return
    if not isinstance(error_info, ErrorInfo):
        print(f"{scene_name}: unexpected response type {type(error_info)}")
        return
    if not error_info.error_vec:
        print(f"{scene_name}: request_target success, service returned no errors")
        return

    print(f"{scene_name}: request_target returned {len(error_info.error_vec)} error entries:")
    for error in error_info.error_vec:
        print(
            f"  component={error.commpent}, code={error.error_code}, description={error.description}"
        )


def ensure_controller(robot, controller_name):
    status = robot.switch_controller(controller_name)
    print(f"switch_controller({controller_name}): {getattr(status, 'name', status)}")
    return status


def run_twist_scene(robot, scene_name, target, twist_duration_s):
    print(f"{scene_name}: start moving for {twist_duration_s} seconds")
    print_error_info(scene_name, robot.request_target(target))
    time.sleep(twist_duration_s)
    print(f"{scene_name}: send stop twist target")
    print_error_info(f"{scene_name}_stop", robot.request_target(build_stop_twist_target()))


def print_menu():
    print(
        "\nAvailable commands:\n"
        "  joint        - request a joint-only head target\n"
        "  base_pose    - request a chassis pose target\n"
        "  base_twist   - request a chassis twist target with auto stop\n"
        "  mixed_pose   - request head + left_arm + chassis pose in one target\n"
        "  mixed_twist  - request head + left_arm + chassis twist in one target\n"
        "  quit         - exit example\n"
    )


def main():
    robot = GalbotRobot()
    if not robot.init():
        print("robot init failed")
        return

    time.sleep(2)

    head_joint_names = robot.get_joint_names(True, [G1JointGroup.head])
    if not head_joint_names:
        print("failed to fetch active head joints")
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return

    head_single_joint = [head_joint_names[0]]
    joint_time_s = 3.0
    pose_time_s = 4.0
    twist_command_time_s = 0.2
    twist_duration_s = 2.0

    print_menu()

    while True:
        command = input("Enter command: ").strip()
        if command == "quit":
            break

        if command == "joint":
            target = build_joint_target(G1JointGroup.head, head_single_joint, [0.2], joint_time_s)
            print_error_info("joint", robot.request_target(target))
            continue

        if command == "base_pose":
            if ensure_controller(robot, G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
                continue
            target = build_chassis_pose_target(0.2, 0.0, 0.0, pose_time_s)
            print_error_info("base_pose", robot.request_target(target))
            continue

        if command == "base_twist":
            if ensure_controller(robot, G1ControllerName.CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
                continue
            target = build_chassis_twist_target(0.05, 0.0, 0.0, twist_command_time_s)
            run_twist_scene(robot, "base_twist", target, twist_duration_s)
            continue

        if command == "mixed_pose":
            if ensure_controller(robot, G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
                continue
            target = merge_targets(
                [
                    build_joint_target(G1JointGroup.head, head_single_joint, [0.2], joint_time_s),
                    build_chassis_pose_target(0.1, 0.0, 0.0, pose_time_s),
                ]
            )
            print_error_info("mixed_pose", robot.request_target(target))
            continue

        if command == "mixed_twist":
            if ensure_controller(robot, G1ControllerName.CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
                continue
            target = merge_targets(
                [
                    build_joint_target(G1JointGroup.head, head_single_joint, [-0.2], joint_time_s),
                    build_chassis_twist_target(0.05, 0.0, 0.0, twist_command_time_s),
                ]
            )
            run_twist_scene(robot, "mixed_twist", target, twist_duration_s)
            continue

        print(f"Unknown command: {command}")
        print_menu()

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()


if __name__ == "__main__":
    main()

Get And Set Base Velocity (get_base_velocity && set_base_velocity)

duration_s=0 publishes once; a positive value blocks the calling thread and publishes at 10 Hz for the window measured after the first successful send. Expiry only ends publishing; it does not call stop_base(). Negative, non-finite, or unrepresentable durations return INVALID_INPUT. Actual stopping depends on the watchdog; SUCCESS does not confirm a stopped base. Do not issue concurrent base commands during publishing: they do not cancel the loop. Use single-publish mode for caller-controlled stopping.

Applicable Scenarios: Control and get linear and angular velocity of the mobile base, used for navigation or remote control.

examples/python/galbot_robot/get_set_base_velocity.py

from galbot_sdk.g1 import GalbotRobot, ControlStatus
import time


def print_base_velocity(base_velocity_info: dict):
    """
    base_velocity_info: dict, includes:
        - 'linear_velocity': [vx, vy, vz] Linear velocity (m/s)
        - 'angular_velocity': [wx, wy, wz] Angular velocity (rad/s)
    """
    if not base_velocity_info:
        print("Base velocity data is empty")
        return

    # Print linear and angular velocity in the same format as the standalone example.
    linear_velocity = base_velocity_info.get("linear_velocity", [])
    if linear_velocity:
        print(
            f"Linear velocity (m/s): vx={linear_velocity[0]}, vy={linear_velocity[1]}, "
            f"vz={linear_velocity[2]}"
        )

    angular_velocity = base_velocity_info.get("angular_velocity", [])
    if angular_velocity:
        print(
            f"Angular velocity (rad/s): wx={angular_velocity[0]}, wy={angular_velocity[1]}, "
            f"wz={angular_velocity[2]}"
        )


# Get GalbotRobot
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")

# Read base velocity before issuing the motion command.
base_velocity_before = robot.get_base_velocity()
if base_velocity_before:
    print("Base velocity before command:")
    print_base_velocity(base_velocity_before)
else:
    print("Failed to get base velocity before command.")

# Set chassis speed
linear_velocity = [0.2, 0.0, 0.0]  # 0.2 m/s
angular_velocity = [0.0, 0.0, 0.0]  # 0.0 rad/s

duration_s = 3.0  # Block while publishing at 10 Hz for 2 seconds; no stop is sent.
status = robot.set_base_velocity(linear_velocity, angular_velocity, duration_s)

if status == ControlStatus.SUCCESS:
    print(f"Velocity publishing completed after {duration_s} seconds; stopping depends on the watchdog.")
else:
    print("Set chassis speed failed.")

# Observe immediately; publishing completion does not imply a stopped base.

# Read base velocity again after the command has finished.
base_velocity_after = robot.get_base_velocity()
if base_velocity_after:
    print("Base velocity after command:")
    print_base_velocity(base_velocity_after)
else:
    print("Failed to get base velocity after command.")

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Joint States (get_joint_states)

Applicable Scenarios: Get complete state information of all joints, including position, velocity, current, etc. Suitable for algorithms requiring full joint information (such as dynamics calculation, state estimation). The following example uses the left arm.

examples/python/galbot_robot/get_joint_states.py

import time
from galbot_sdk.g1 import GalbotRobot

def print_joint_states(joint_states):
    """
    joint_state_vec: List of JointState; each object has
    joint_name, position, velocity, acceleration, effort, current
    """
    for js in joint_states:
        print(f" : joint_name = {js.joint_name} , position = {js.position} , velocity = {js.velocity} "
            f", acceleration = {js.acceleration} , effort = {js.effort} , current = {js.current}"
            f", timestamp_ns = {js.timestamp_ns}")

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get joint states by joint group names; returns all joints if empty
joint_group_names = ["left_arm"]
ret = robot.get_joint_states(joint_group_names, [])
if not ret:
    print(f"No joint states returned for joint groups {joint_group_names}")
else:
    print_joint_states(ret)

# Get specified joint states; if provided, overrides joint group input
joint_names = ["left_arm_joint1", "left_arm_joint2"]
state_ret = robot.get_joint_states([], joint_names)
if not state_ret:
    print(f"No joint states returned for joint names {joint_names}")
else:
    print_joint_states(state_ret)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Joint Positions (get_joint_positions)

Applicable Scenarios: Only get current joint positions. Use this interface when you only need joint position information, it's more lightweight than get_joint_states.

examples/python/galbot_robot/get_joint_positions.py

import time
from galbot_sdk.g1 import GalbotRobot

def print_joint_positions(joint_positions):
    print(f"pos count is {len(joint_positions)}")
    for pos in joint_positions:
        print(pos)

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

# Get joint positions by joint group names; returns all joints if empty
joint_group_names = ["left_arm"]
ret = robot.get_joint_positions(joint_group_names, [])
print("Left arm joint positions:")
print_joint_positions(ret)
# Get specified joint positions; if provided, overrides joint group input
joint_names = ["left_arm_joint1", "left_arm_joint2"]
state_ret = robot.get_joint_positions([], joint_names)
print("Left arm joints 1 and 2 positions:")
print_joint_positions(state_ret)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Joint Names (get_joint_names)

Applicable Scenarios: Get a list of all joint names of the robot, used for iterating through joints or generating configuration files.

examples/python/galbot_robot/get_joint_names.py

import time
from galbot_sdk.g1 import GalbotRobot

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

# Get specified joint names; joint groups include ["leg", "head", "left_arm", "right_arm"]
joint_group_names = ["head"]
only_active_joint = True  # Get active joints
head_joint_names = robot.get_joint_names(only_active_joint, joint_group_names)
print("Head joint names:")
for i, name in enumerate(head_joint_names):
    print(f"{i}: {name}")

# Passing an empty list returns all joint group information by default
all_joint_names = robot.get_joint_names(only_active_joint, [])
print("All joint names:")
for i, name in enumerate(all_joint_names):
    print(f"{i}: {name}")

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Gripper State (get_gripper_state)

Applicable Scenarios: Get current gripper position and status, determine if gripper is open or closed.

examples/python/galbot_robot/get_gripper_state.py

import time
from galbot_sdk.g1 import GalbotRobot, G1JointGroup

def print_gripper_state(joint_group, gripper_state):
    """
    joint_group: G1JointGroup enum value
    gripper_state: object including timestamp_ns, width, velocity, effort, is_moving
    """
    print(f"Timestamp (ns): {gripper_state.timestamp_ns}")
    print(
        f"width {gripper_state.width} "
        f"velocity {gripper_state.velocity} "
        f"effort {gripper_state.effort} "
        f"is moving {gripper_state.is_moving}"
    )

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()

# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

# Set gripper joint group (left gripper)
joint_group = G1JointGroup.left_gripper

# Get gripper state
gripper_state = robot.get_gripper_state(joint_group)

if gripper_state is None:
    print("get gripper state error")
else:
    print("Left gripper state is as follows:")
    print_gripper_state(joint_group, gripper_state)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Suction Cup State (get_suction_cup_state)

Applicable Scenarios: Get whether the suction cup is currently suctioned and whether it successfully detected an object.

examples/python/galbot_robot/get_suction_cup_state.py

import time
from galbot_sdk.g1 import GalbotRobot, G1JointGroup

def print_suction_cup_state(suction_cup_state):
    """
    suction_cup_state: object including timestamp_ns, pressure, activation, action_state
    """
    group_name = joint_group.name
    print(f"Timestamp (ns): {suction_cup_state.timestamp_ns}")
    print(
        f"pressure {suction_cup_state.pressure} "
        f"activation {suction_cup_state.activation} "
        f"action state {int(suction_cup_state.action_state)}"
    )

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()

# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

# Set suction cup joint group (right suction cup)
joint_group = G1JointGroup.right_suction_cup

# Get suction cup state
suction_cup_state = robot.get_suction_cup_state(joint_group)

if suction_cup_state is None:
    print("get suction cup error")
else:
    print("Right suction cup status:")
    print_suction_cup_state(suction_cup_state)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Transform (get_transform)

Applicable Scenarios: Get the transformation (pose) between two coordinate frames, used in robotic arm grasping, visual localization and other scenarios.

examples/python/galbot_robot/get_transform.py

import time
from galbot_sdk.g1 import GalbotRobot

def print_pose(pose_vec):
    """
    pose_vec: list of floats
    """
    print("pose_vec = [" + ", ".join(str(p) for p in pose_vec) + "]")

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()

# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

# Set target frame and source frame
target_frame = "base_link"
source_frame = "left_arm_link1"
timestamp_ns = 0    # 0 means fetch the latest TF transform value

# Get coordinate transform
ret_val = robot.get_transform(target_frame, source_frame)

if not ret_val[0]:
    print("get_transform error")
else:
    print("tf_timestamp_ns:", ret_val[1])
    print_pose(ret_val[0])

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get IMU Data (get_imu_data)

Applicable Scenarios: Get acceleration, angular velocity and pose information from the base IMU, used for state estimation and motion analysis.

examples/python/galbot_robot/get_imu_data.py

import time
from galbot_sdk.g1 import GalbotRobot, SensorType
from typing import Dict

def print_imu_data(imu_data: dict):
    """
    imu_data: dict, includes:
        - 'timestamp_ns'
        - 'accel'   : {'x', 'y', 'z'}
        - 'gyro'    : {'x', 'y', 'z'}
        - 'magnet'  : {'x', 'y', 'z'}
    """
    if not imu_data:
        print("IMU data is empty")
        return

    print(f"Timestamp (ns): {imu_data.get('timestamp_ns')}")

    accel = imu_data.get("accel", {})
    gyro = imu_data.get("gyro", {})
    magnet = imu_data.get("magnet", {})

    print(
        f"Accelerometer: x={accel.get('x')}, "
        f"y={accel.get('y')}, "
        f"z={accel.get('z')}"
    )
    print(
        f"Gyroscope:     x={gyro.get('x')}, "
        f"y={gyro.get('y')}, "
        f"z={gyro.get('z')}"
    )
    print(
        f"Magnetometer:  x={magnet.get('x')}, "
        f"y={magnet.get('y')}, "
        f"z={magnet.get('z')}"
    )

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
# - SensorType::TORSO_IMU: torso IMU
# - SensorType::LIDAR_IMU: lidar IMU
# - SensorType::CHASSIS_IMU: Chassis lidar IMU
robot.init({SensorType.CHASSIS_IMU})

# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

imu_data = robot.get_imu_data(SensorType.CHASSIS_IMU)
if not imu_data:
    print("No imu data!")
else:
    print("IMU data:")
    print_imu_data(imu_data)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Battery Information (get_bms_information)

Applicable Scenarios: Get battery voltage, charge level and other information, used for low battery detection and status monitoring.

examples/python/galbot_robot/get_bms_information.py

import time
from galbot_sdk.g1 import GalbotRobot


def print_bms_information(bms_info: dict):
    """
    bms_info: dict, includes:
        - 'voltage'            : float (V)
        - 'current'            : float (A)
        - 'battery_level'      : float (0-100)
        - 'temperature'        : float (°C)
        - 'charging_status'    : str or int
        - 'health_status'      : str or int
        - 'capacity'           : float (Ah)
    """
    if not bms_info:
        print("BMS information is empty")
        return

    print(f"Voltage (V): {bms_info.get('voltage')}")
    print(f"Current (A): {bms_info.get('current')}")
    print(f"Battery level (%): {bms_info.get('battery_level')}")
    print(f"Temperature (C): {bms_info.get('temperature')}")
    print(f"Charging status: {bms_info.get('charging_status')}")
    print(f"Health status: {bms_info.get('health_status')}")
    print(f"Capacity (Ah): {bms_info.get('capacity')}")


# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()

# Program started, waiting for data
time.sleep(3)
print("Initialization succeeded")

bms_info = robot.get_bms_information()
print_bms_information(bms_info)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")

Get Device Information (get_device_information)

Applicable Scenarios: Get device information such as hardware version and firmware version, used for debugging and compatibility checking.

examples/python/galbot_robot/get_device_information.py

import time
from galbot_sdk.g1 import GalbotRobot
from typing import Dict

def print_device_info(device_info: dict):
    """
    device_info: dict, including the following fields:
        - 'model': Device model (str)
        - 'serial_number': Serial number (str)
        - 'firmware_version': Firmware version (str)
        - 'hardware_version': Hardware version (str)
        - 'manufacturer': Manufacturer (str)
    """
    if not device_info:
        print("Device information is empty")
        return

    print("Device information:")
    print(f"  Model: {device_info.get('model', 'N/A')}")
    print(f"  Serial number: {device_info.get('serial_number', 'N/A')}")
    print(f"  Firmware version: {device_info.get('firmware_version', 'N/A')}")
    print(f"  Hardware version: {device_info.get('hardware_version', 'N/A')}")
    print(f"  : {device_info.get('manufacturer', 'N/A')}")

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()

# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

device_info = robot.get_device_information()
if not device_info:
    print("Failed to get device information!")
else:
    print("Device information retrieved successfully!")
    print_device_info(device_info)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Camera Image Data (get_rgb_data && get_depth_data)

Applicable Scenarios: Get RGB image and depth image data, used for visual perception, object detection, SLAM and other tasks.

examples/python/galbot_robot/get_camera_data.py

try:
    from galbot_sdk.g1 import GalbotRobot, RgbOutputFormat, SensorType
except ImportError:
    print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
    exit(1)

import os

try:
    import cv2
except ImportError:
    os.system("pip install opencv-python")
    import cv2

try:
    import numpy as np
except ImportError:
    os.system("pip install numpy")
    import numpy as np

import time
from typing import Dict

def decode_compressed_image(compressed_image):
    """
    decode CompressedImage image

    Parameters:
        compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]

    Returns:
        numpy.ndarray: decoded image
    """
    image_data = compressed_image["data"]
    if compressed_image["format"].lower() in ("jpeg", "jpg", "rgb8"):
        return decode_rgb_image(image_data)
    elif compressed_image["format"] == "16UC1":
        return decode_depth_image(compressed_image)
    else:
        raise ValueError(f"Unsupport data format: {compressed_image['format']}")

def decode_ir_image(ir_image):
    """decode IR (infrared) grayscale image.
    Supports JPEG-compressed mono8 and raw mono8 byte streams.
    """
    data = ir_image["data"]
    fmt = ir_image.get("format", "")

    # Raw mono8: plain pixel bytes, use height/width directly
    if fmt == "mono8":
        h = ir_image.get("height", 0)
        w = ir_image.get("width", 0)
        if h == 0 or w == 0:
            raise ValueError(f"decode_ir_image: height/width unavailable for mono8, got {h}x{w}")
        return np.frombuffer(data, dtype=np.uint8).reshape(h, w).copy()

    # Compressed (JPEG / PNG)
    nparr = np.frombuffer(data, np.uint8)
    img = cv2.imdecode(nparr, cv2.IMREAD_GRAYSCALE)
    if img is None:
        raise ValueError(f"decode_ir_image: imdecode failed, format='{fmt}', size={len(data)}")
    return img

def decode_rgb_image(image_data):
    """decode rgb image"""
    nparr = np.frombuffer(image_data, np.uint8)
    img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
    if img is None:
        raise ValueError("Fail to Decode RGB Image")
    return img

def decode_depth_image(image_data):
    """decode depth image"""
    depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()

    # Check whether height and width exist
    if "height" not in image_data or "width" not in image_data:
        raise ValueError("Missing 'height' or 'width' in depth image metadata.")
    if image_data["height"] == 0 or image_data["width"] == 0:
        raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")

    # Parse depth image
    depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
    depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]

    return depth_img

def main():
    SHOW_IMAGE = False
    robot = GalbotRobot()

    # Get left arm RGB, depth and IR images
    enable_sensor_set = {SensorType.LEFT_ARM_CAMERA,
                        SensorType.LEFT_ARM_DEPTH_CAMERA,
                        SensorType.LEFT_ARM_INFRA_CAMERA_1,
                        SensorType.LEFT_ARM_INFRA_CAMERA_2,}
    robot.init(enable_sensor_set)
    print("Initialization succeeded")
    # Program started, waiting for data
    time.sleep(5)
    # Get left arm RGB image
    rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
    if not rgb_image_data:
        print("No rgb image data!")
    else:
        print("get rgb image suceess")
        print(rgb_image_data['header'])
        img = decode_compressed_image(rgb_image_data)
        
        # Save RGB image
        cv2.imwrite("rgb_image_data.jpg", img)
        # 可视化RGB图像
        if SHOW_IMAGE:
            cv2.namedWindow("rgb image", cv2.WINDOW_NORMAL)
            cv2.imshow("rgb image", img)
            cv2.waitKey(0)
            cv2.destroyAllWindows()

    # Get left arm depth image
    depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
    if not depth_data or "data" not in depth_data:
        print("Depth camera not ready")
    else:
        print("get depth data suceess")
        print(depth_data['header'])
        depth_img_raw = decode_compressed_image(depth_data)
        depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize depth values to 0-1 range
        depth_img = depth_img.astype(np.uint8)

        # Save depth image
        cv2.imwrite("depth_data.jpg", depth_img)
        # 可视化深度图
        if SHOW_IMAGE:
            cv2.namedWindow("depth image", cv2.WINDOW_NORMAL)
            cv2.imshow("depth image", depth_img)
            cv2.waitKey(0)
            cv2.destroyAllWindows()

    # Get left arm IR images
    for sensor_type, filename in [
        (SensorType.LEFT_ARM_INFRA_CAMERA_1, "ir_infra1.jpg"),
        (SensorType.LEFT_ARM_INFRA_CAMERA_2, "ir_infra2.jpg"),
    ]:
        ir_data = robot.get_ir_data(sensor_type)
        if not ir_data or "data" not in ir_data:
            print(f"IR camera {sensor_type} not ready (ir_enabled may be false)")
        else:
            print(f"get ir data {sensor_type} success")
            print(ir_data['header'])
            print(ir_data['format'])
            ir_img = decode_ir_image(ir_data)

            cv2.imwrite(filename, ir_img)
            if SHOW_IMAGE:
                cv2.namedWindow(f"ir image {sensor_type}", cv2.WINDOW_NORMAL)
                cv2.imshow(f"ir image {sensor_type}", ir_img)
                cv2.waitKey(0)
                cv2.destroyAllWindows()

    # send SIGINT shutdown signal
    robot.request_shutdown()
    # Wait until entering shutdown state
    robot.wait_for_shutdown()
    # Perform SDK resource release
    robot.destroy()
    print('Resources released successfully')
    
if __name__=="__main__":
    main()

Subscribe Video Data (subscribe_video_data)

Applicable Scenarios: Subscribe to a sensor's H.264 video stream and receive encoded frames via a callback, used when you need push-based delivery of compressed video for recording, real-time processing, or network streaming.

examples/python/galbot_robot/subscribe_video_data.py

import threading
import time

try:
    from galbot_sdk.g1 import GalbotRobot, SensorStatus, SensorType
except ImportError:
    print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
    exit(1)


OUTPUT_PATH = "head_left.h264"
DURATION = 10

output_file = None
frame_count = 0
frame_count_lock = threading.Lock()


# Important:
# The video callback is executed by the SDK internal dispatch thread. 
# Do not perform time-consuming operations in this callback; keep it 
# short and non-blocking. Long-running work may delay video frame delivery 
# for this subscription.
def video_data_callback(video_data: dict):
    global frame_count

    header = video_data.get("header", {})
    data = video_data.get("data", b"")
    if output_file is not None and data:
        # This sample callback only writes the raw H.264 stream to file without expensive transcoding here.
        output_file.write(data)

    with frame_count_lock:
        frame_count += 1
        current_count = frame_count

    if current_count == 1 or current_count % 30 == 0:
        print(
            "frame_count={}, timestamp_ns={}, format={}, bytes={}".format(
                current_count,
                header.get("timestamp_ns"),
                video_data.get("format"),
                len(data),
            )
        )


def main():
    global output_file

    robot = GalbotRobot()

    # Enable only the camera used by this example.
    if not robot.init({SensorType.HEAD_LEFT_CAMERA}):
        print("GalbotRobot initialization failed")
        return
    print("Initialization succeeded")

    output_file = open(OUTPUT_PATH, "wb")
    print(f"writing H.264 stream to {OUTPUT_PATH}")

    status = robot.subscribe_video_data(SensorType.HEAD_LEFT_CAMERA, video_data_callback)
    if status != SensorStatus.SUCCESS:
        print(f"subscribe_video_data failed, status={status}")
        output_file.close()
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return
    print("subscribe_video_data success")

    try:
        time.sleep(DURATION)
    except KeyboardInterrupt:
        print("Interrupted by user")
    finally:
        # Unsubscribe clears the user callback without recreating SDK reader resources.
        status = robot.unsubscribe_video_data(SensorType.HEAD_LEFT_CAMERA)
        print(f"unsubscribe_video_data status={status}")

        if output_file is not None:
            output_file.close()

        with frame_count_lock:
            total_frames = frame_count
        print(f"received {total_frames} frames")
        print(f"saved H.264 stream to {OUTPUT_PATH}")

        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        print("Resources released successfully")

if __name__ == "__main__":
    main()

Get Sensor Parameters (get_camera_intrinsic && get_sensor_extrinsic)

Applicable Scenarios: Get camera intrinsics and sensor extrinsics relative to the robot base, used for computer vision calculations and 3D reconstruction.

examples/python/galbot_robot/get_camera_params.py

try:
    from galbot_sdk.g1 import GalbotRobot, RgbOutputFormat, SensorType
except ImportError:
    print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
    exit(1)

import os

try:
    import cv2
except ImportError:
    os.system("pip install opencv-python")
    import cv2

try:
    import numpy as np
except ImportError:
    os.system("pip install numpy")
    import numpy as np

import time
from typing import Dict

def decode_compressed_image(compressed_image):
    """
    decode CompressedImage image

    Parameters:
        compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]

    Returns:
        numpy.ndarray: decoded image
    """
    image_data = compressed_image["data"]
    if compressed_image["format"].lower() in ("jpeg", "jpg", "rgb8"):
        return decode_rgb_image(image_data)
    elif compressed_image["format"] == "16UC1":
        return decode_depth_image(compressed_image)
    else:
        raise ValueError(f"Unsupport data format: {compressed_image['format']}")

def decode_rgb_image(image_data):
    """decode rgb image"""
    nparr = np.frombuffer(image_data, np.uint8)
    img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
    if img is None:
        raise ValueError("Fail to Decode RGB Image")
    return img

def decode_depth_image(image_data):
    """decode depth image"""
    depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()

    # Check whether height and width exist
    if "height" not in image_data or "width" not in image_data:
        raise ValueError("Missing 'height' or 'width' in depth image metadata.")
    if image_data["height"] == 0 or image_data["width"] == 0:
        raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")

    # Parse depth image
    depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
    depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]

    return depth_img

def main():
    robot = GalbotRobot()

    # Get left arm RGB and depth images, right arm depth image, chassis lidar data, torso IMU data
    enable_sensor_set = {SensorType.LEFT_ARM_CAMERA, # Left arm depth camera
                        SensorType.LEFT_ARM_DEPTH_CAMERA,} # Left arm RGB camera
    robot.init(enable_sensor_set)
    print("Initialization succeeded")
    
    # Program started, waiting for data
    time.sleep(5)
    
    # Get left arm RGB image
    rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
    if not rgb_image_data:
        print("No rgb image data!")
    else:
        print("get rgb image suceess")
        print(rgb_image_data['header'])
        img = decode_compressed_image(rgb_image_data)
        
        # Save RGB image
        cv2.imwrite("rgb_image_data.jpg", img)

    # Get left arm camera intrinsics
    camera_intrinsics = robot.get_camera_intrinsic(SensorType.LEFT_ARM_CAMERA)
    if not camera_intrinsics:
        print("No camera intrinsics data!")
    else:
        print("get camera intrinsics suceess")
        print(camera_intrinsics)

    # Get left arm depth image
    depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
    if not depth_data or "data" not in depth_data:
        print("Depth camera not ready")
    else:
        print("get depth data suceess")
        print(depth_data['header'])
        depth_img_raw = decode_compressed_image(depth_data)
        depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize depth values to 0-1 range
        depth_img = depth_img.astype(np.uint8)

        # Save depth image
        cv2.imwrite("depth_data.jpg", depth_img)

    # Get left-arm depth camera intrinsics
    camera_intrinsics = robot.get_camera_intrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
    if not camera_intrinsics:
        print("No camera intrinsics data!")
    else:
        print("get camera intrinsics suceess")
        print(camera_intrinsics)

    # Extrinsics
    time.sleep(2)
    camera_extrinsics, timestamp_ns = robot.get_sensor_extrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
    if not camera_extrinsics:
        print("No camera extrinsics data!")
    else:
        print("get camera extrinsics suceess")
        print(camera_extrinsics)

    # send SIGINT shutdown signal
    robot.request_shutdown()
    # Wait until entering shutdown state
    robot.wait_for_shutdown()
    # Perform SDK resource release
    robot.destroy()
    print('Resources released successfully')
    
if __name__=="__main__":
    main()

Get Lidar Data (get_lidar_data)

Applicable Scenarios: Get LiDAR point cloud data, used for navigation, obstacle avoidance, and environment modeling.

examples/python/galbot_robot/get_lidar_data.py

from galbot_sdk.g1 import GalbotRobot, SensorType
from typing import Dict
import time
import numpy as np

def convert_pointcloud(cloud):
    """
    Convert cloud dict to NumPy array dictionary

    Args:
        pointcloud_msg: PointCloud2 protobuf message object

    Returns:
        Dictionary: {field_name: NumPy array}
        - Single-element fields: shape (N,)
        - Multi-element fields: shape (N, count) or (N,)
        - N = width * height (total number of points)
    """

    if not cloud:
        return {}

    num_points = cloud["height"] * cloud["width"]
    if num_points == 0:
        return {}

    DTYPE_MAP = {
        1: np.int8,
        2: np.uint8,
        3: np.int16,
        4: np.uint16,
        5: np.int32,
        6: np.uint32,
        7: np.float32,
        8: np.float64
    }
    dtype_list = []
    for field in cloud["fields"]:
        # Get base data type
        np_dtype_class = DTYPE_MAP.get(field["datatype"])
        if np_dtype_class is None:
            raise ValueError(f"Unsupported data type: {field['datatype']}")

        dtype_inst = np.dtype(np_dtype_class)

        # Handle byte order (endianness)
        if dtype_inst.itemsize > 1:
            byteorder = '>' if cloud["is_bigendian"] else '<'
            dtype_inst = dtype_inst.newbyteorder(byteorder)

        # Add to dtype list
        if field["count"] == 1:
            dtype_list.append((field["name"], dtype_inst))
        else:
            # Multi-element fields (e.g., rgb)
            dtype_list.append((field["name"], dtype_inst, field["count"]))

    # Create structured dtype
    dtype = np.dtype(dtype_list)

    # Data integrity check
    expected_size = num_points * cloud["point_step"]
    if len(cloud["data"]) < expected_size:
        raise ValueError(
            f"Insufficient data length: expected {expected_size} bytes, "
            f"actual {len(cloud['data'])} bytes"
        )

    # Create NumPy structured array from binary data
    # count parameter ensures only expected number of points are read
    arr = np.frombuffer(cloud["data"], dtype=dtype, count=num_points)

    # Convert to regular dictionary (copy data to avoid modifying original)
    result = {}
    for field in cloud["fields"]:
        field_data = arr[field["name"]]

        # Handle shape of multi-element fields
        if field["count"] == 1:
            result[field["name"]] = field_data.copy()
        else:
            # Keep original shape or flatten, choose according to needs
            result[field["name"]] = field_data.copy()

    return result


def get_xyz_array(pointcloud_dict: Dict[str, np.ndarray], 
                remove_nan: bool = False) -> np.ndarray:
    """
    Extract XYZ coordinate array from converted point cloud dictionary

    Args:
        pointcloud_dict: Dictionary returned by pointcloud2_to_numpy()
        remove_nan: Whether to remove points containing NaN (for FLOAT32/FLOAT64 types)

    Returns:
        Nx3 point coordinate array
    """
    required = ['x', 'y', 'z']
    if not all(k in pointcloud_dict for k in required):
        raise ValueError("Point cloud data missing required xyz fields")

    points = np.stack([pointcloud_dict['x'], 
                    pointcloud_dict['y'], 
                    pointcloud_dict['z']], axis=1)

    if remove_nan:
        mask = ~np.isnan(points).any(axis=1)
        points = points[mask]

    return points

def save_xyz_to_pcd(xyz_array: np.ndarray, filename: str, binary: bool = False) -> None:
    """
    Save XYZ coordinates to PCD file format (simplest option for coordinate-only data)

    Args:
        xyz_array: Nx3 array of XYZ coordinates
        filename: Output PCD file path
        binary: If True, saves in binary format; otherwise ASCII
    """
    if xyz_array.ndim != 2 or xyz_array.shape[1] != 3:
        raise ValueError(f"xyz_array must have shape (N, 3), got {xyz_array.shape}")

    num_points = xyz_array.shape[0]
    header = [
        "# .PCD v0.7 - Point Cloud Data file format",
        "VERSION 0.7",
        "FIELDS x y z",
        "SIZE 4 4 4",
        "TYPE F F F",  # F = float32
        "COUNT 1 1 1",
        f"WIDTH {num_points}",
        "HEIGHT 1",
        "VIEWPOINT 0 0 0 1 0 0 0",
        f"POINTS {num_points}",
        f"DATA {'binary' if binary else 'ascii'}"
    ]

    if binary:
        with open(filename, 'wb') as f:
            f.write(('\n'.join(header) + '\n').encode('ascii'))
            f.write(xyz_array.astype(np.float32).tobytes())
    else:
        with open(filename, 'w') as f:
            f.write('\n'.join(header) + '\n')
            np.savetxt(f, xyz_array, fmt='%f')

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
enable_sensor_set = {SensorType.BASE_LIDAR}
# To save resource overhead, only cameras and LiDAR sensors passed during initialization can retrieve data
robot.init(enable_sensor_set)
print("Initialization succeeded")
# Program started, waiting for data
time.sleep(4)

cloud = robot.get_lidar_data(SensorType.BASE_LIDAR)
if not cloud:
    print("No lidar data!")
else:
    pointcloud_dict = convert_pointcloud(cloud)
    xyz_points = get_xyz_array(pointcloud_dict)
    save_xyz_to_pcd(xyz_points, "output_xyz.pcd")
    print(pointcloud_dict)
    print("get lidar data success")

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Synchronized Observation Data (get_synced_observation)

Applicable Scenarios: Get synchronized observation data, mainly for obtaining time-aligned input in model inference scenarios, supporting RGB camera, depth camera, and joint data.

examples/python/galbot_robot/get_synced_observation.py

import time

from galbot_sdk.g1 import (
    GalbotRobot,
    JointState,
    JointStateMessage,
    RgbData,
    SensorType,
    SyncedObservation,
)

NS_TO_MS = 1e-6


def main():
    # 1) Initialize robot with sync mode enabled.
    #    The 2nd init arg must be True, otherwise get_synced_observation() is unavailable.
    robot = GalbotRobot()
    ok = robot.init(
        {
            SensorType.HEAD_LEFT_CAMERA,
            SensorType.LEFT_ARM_CAMERA,
            SensorType.RIGHT_ARM_CAMERA,
        },
        True,  # enable_sync_mode
    )
    if not ok:
        print("init failed")
        robot.destroy()
        return

    time.sleep(2)

    # 2) Build camera list for synchronization.
    #    cameras[0] is the anchor camera; other cameras are matched by nearest timestamp.
    cameras = [
        SensorType.LEFT_ARM_CAMERA,  # anchor
        SensorType.RIGHT_ARM_CAMERA,  # nearest-neighbor
        SensorType.HEAD_LEFT_CAMERA,  # nearest-neighbor
    ]

    # 3) Call API (typed return).
    obs: SyncedObservation | None = robot.get_synced_observation(cameras, True)
    if not obs:
        print("get_synced_observation failed")
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return

    # 4) Read synced RGB frames from typed field: obs.rgb_data_map.
    #    Sync-mode RGB payloads are tightly packed CPU-owned NV12, not JPEG.
    print("[Synced RGB]")
    rgb_map: dict[SensorType, RgbData] = obs.rgb_data_map
    anchor_camera = cameras[0]
    anchor_frame: RgbData | None = rgb_map.get(anchor_camera, None)
    anchor_ts_ns = (
        anchor_frame.header.timestamp_ns if anchor_frame is not None else None
    )
    print(f"anchor_camera={anchor_camera}, anchor_timestamp_ns={anchor_ts_ns}")

    for cam in cameras:
        frame: RgbData | None = rgb_map.get(cam, None)
        cam_ts_ns = frame.header.timestamp_ns if frame is not None else None
        frame_format = frame.format if frame is not None else "N/A"
        dimensions = f"{frame.width}x{frame.height}" if frame is not None else "N/A"
        payload_bytes = len(frame.data) if frame is not None else 0
        delta_ms_str = "N/A"
        if anchor_ts_ns is not None and cam_ts_ns is not None:
            delta_ms_str = f"{(cam_ts_ns - anchor_ts_ns) * NS_TO_MS:.3f}"
        print(
            f"camera={cam}, timestamp_ns={cam_ts_ns if cam_ts_ns is not None else 'N/A'}, "
            f"delta_to_anchor_ms={delta_ms_str}, format={frame_format}, "
            f"dimensions={dimensions}, bytes={payload_bytes}"
        )

    # 5) Read optional joint snapshot from typed field: obs.joint_state
    #    joint_state is JointStateMessage | None.
    joint_state: JointStateMessage | None = obs.joint_state
    print("[Joint]")
    joint_ts_ns = joint_state.timestamp_ns if joint_state is not None else None
    joint_delta_ms_str = "N/A"
    if anchor_ts_ns is not None and joint_ts_ns is not None:
        joint_delta_ms_str = f"{(joint_ts_ns - anchor_ts_ns) * NS_TO_MS:.3f}"
    print(
        "joint_timestamp_ns:",
        joint_ts_ns if joint_ts_ns is not None else "N/A",
        "delta_to_anchor_ms:",
        joint_delta_ms_str,
    )
    # 6) Each element in joint_state_vec is typed JointState and contains joint_name.
    joint_state_vec: list[JointState] = (
        joint_state.joint_state_vec if joint_state is not None else []
    )
    print("joint_count:", len(joint_state_vec))
    for i, js in enumerate(joint_state_vec[:5]):
        print(
            f"  [{i}] joint_name={js.joint_name}, "
            f"position={js.position}, "
            f"velocity={js.velocity}, "
            f"effort={js.effort}"
        )

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()


if __name__ == "__main__":
    main()

One-Click Reset to Zero – Odometry Frame (whole_body_reset_zero_odom)

Applicable Scenarios: Quickly move all joints to zero position (initial pose) based on odometry frame. Typically used for initialization after program startup.

examples/python/galbot_robot/whole_body_reset_zero_odom_example.py

from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import GalbotMotion
import time


def main():
    robot = GalbotRobot()
    motion = GalbotMotion()

    if not robot.init():
        print("GalbotRobot init failed.")
        return
    if not motion.init():
        print("GalbotMotion init failed.")
        return

    time.sleep(2)

    # Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
    whole_body_joint_1 = [
        0.25, 1.1, 0.85, 0.0, 0.0,       # leg
        0.5, 0.5,                        # head
        2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.2,   # left_arm
        -2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.2        # right_arm
    ]

    # Base pose command odom(x, y, yaw)
    base_x_1 = 0.2
    base_y_1 = 0.0
    base_yaw_1 = 0.0

    # Optional frames (frame_id: base_link/odom/map, reference_frame_id: odom/map)
    frame_id = "base_link"
    reference_frame_id = "odom"

    # Chassis pose interpolation time (seconds), used to generate a smooth chassis trajectory
    base_time_s = 15.0

    time.sleep(1)

    # reset to zero
    result = robot.zero_whole_body_and_base(
        frame_id,
        reference_frame_id,
        True,
        0.2,
        15.0,
    )
    print("Zero joint status:", result[0])
    print("Zero base status:", result[1])

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()


if __name__ == "__main__":
    main()

One-Click Reset to Zero – Map Frame (whole_body_reset_zero_map)

Applicable Scenarios: Quickly move all joints to zero position (initial pose) based on map frame.

examples/python/galbot_robot/whole_body_reset_zero_map_example.py

from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import GalbotMotion
import time


def main():
    robot = GalbotRobot()
    motion = GalbotMotion()

    if not robot.init():
        print("GalbotRobot init failed.")
        return
    if not motion.init():
        print("GalbotMotion init failed.")
        return

    time.sleep(2)

    # Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
    whole_body_joint_1 = [
        0.25, 1.1, 0.85, 0.0, 0.0,       # leg
        0.5, 0.5,                        # head
        2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.2,   # left_arm
        -2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.2        # right_arm
    ]

    # Base pose command map(x, y, yaw) Note: adjust based on the robot's actual localization in the map frame
    base_x_1 = -0.4
    base_y_1 = 0.226593
    base_yaw_1 = 0.0

    # Optional frames (frame_id: base_link/odom/map, reference_frame_id: odom/map)
    frame_id = "base_link"
    reference_frame_id = "map"

    time.sleep(1)

    # reset to zero
    result = robot.zero_whole_body_and_base(
        frame_id,
        reference_frame_id,
        True,
        0.2,
        15.0,
    )
    print("Zero joint status:", result[0])
    print("Zero base status:", result[1])

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()


if __name__ == "__main__":
    main()

Emergency Stop and Resume (emergency_stop / resume_from_emergency_stop)

Applicable Scenarios: Immediately halt all robot motor power for safety-critical stops, then re-enable all robot motors and restore normal operation after the emergency stop has been triggered.

examples/python/galbot_robot/emergency_stop_and_resume_example.py

import signal
import threading
import time

from galbot_sdk.g1 import ControlStatus, GalbotRobot

motion_done = threading.Event()
emergency_triggered = threading.Event()


def move_head_joints(robot, target_position):
    status = robot.set_joint_positions(
        [target_position], [], ["head_joint2"], True, 0.05, 15.0
    )
    print(f"Head motion returned: {status}")


def emergency_stop_on_ctrl_c(robot):
    while not motion_done.is_set():
        if signal.sigtimedwait({signal.SIGINT}, 0.2) is not None:
            break
    else:
        return
    print("Worker thread: triggering software emergency stop...")
    emergency_triggered.set()
    status = robot.emergency_stop()
    print(f"Worker thread: emergency_stop returned: {status}")


def confirm_motion():
    print("⚠️  Ensure the emergency-stop button is released and the area is clear.")
    if input("Start head emergency-stop test? (y/n): ").strip().lower() != "y":
        raise SystemExit("Motion was not confirmed.")


confirm_motion()
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")

# Move an arm in the main thread, then emergency-stop it after Ctrl-C.
print("Head is moving. Press Ctrl-C to trigger software emergency stop.")
stop_thread = threading.Thread(target=emergency_stop_on_ctrl_c, args=(robot,))
stop_thread.start()
target_position = 0.4
move_head_joints(robot, target_position)
motion_done.set()
stop_thread.join()

if emergency_triggered.is_set():
    print("Waiting 3 seconds before resuming from emergency stop...")
    time.sleep(3)
    print("Resuming from software emergency stop...")
    status = robot.resume_from_emergency_stop()
    if status == ControlStatus.SUCCESS:
        print("Resume from emergency stop successfully.")
        print("Waiting 10 seconds for the robot to stabilize after recovery...")
        time.sleep(10)
        print("Resuming head motion to the original target position...")
        move_head_joints(robot, target_position)
    else:
        print("Resume from emergency stop failed.")

signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")

Whole-body End-effector Commands (set_end_effector_command / clear_end_effector_command)

Applicable Scenarios: High-frequency model-inference scenarios that require real-time Cartesian end-effector control. Use clear_end_effector_command to stop and clear the active real-time end-effector command.

examples/python/galbot_robot/set_end_effector_commands.py

"""
WBC end-effector command example

Demonstrates the whole-body control end-effector trajectory API:
- set_end_effector_command: publish pose targets (each pose is [x,y,z,qx,qy,qz,qw])
- clear_end_effector_command: clear end-effector task commands on the WBC channel

reference_frames: omit or [] to use ``world`` for every pose; otherwise one string per pose,
typically ``world`.
"""

import sys
import os
import time

# Add pybind library path to PYTHONPATH
script_path = os.path.abspath(__file__)
sdk_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(script_path))))

# Add package path (for galbot_sdk module)
package_path = os.path.join(sdk_root, "pybind", "package")
# Add lib path (for .so files)
lib_path = os.path.join(sdk_root, "pybind", "output", "linux-x86_64-gcc940", "lib")

for p in [package_path, lib_path]:
    if p not in sys.path:
        sys.path.insert(0, p)

print(f"Added paths: {package_path}, {lib_path}")

from galbot_sdk.g1 import GalbotRobot


def main():
    # Get GalbotRobot singleton and initialize
    robot = GalbotRobot()
    robot.init()
    print("Initialization successful")

    time.sleep(2)

    ee_info = robot.get_wbc_end_effector_poses()# Before using this function, ensure that `using_wbc` in the configuration file is set to `true`.
    print("\nCurrent WBC end-effector poses:")
    for frame, pose in ee_info.items():
        print(f"  Frame: {frame}, Pose: {pose}")

    ree_pose = ee_info.get("ree_pose")  # keys: "ree_pose", "lee_pose", "head_pose"
    if ree_pose and len(ree_pose) >= 7:
        target_pose = list(ree_pose[:7])
        target_pose[0] -= 0.1

        # Example: absolute pose — omit reference_frames to default every pose to "world"
        robot.set_end_effector_command(
            poses=[target_pose],
            end_effector_frames=["right_arm_end_effector_mount_link"],
        )
        print('\nPublished end-effector command (default reference_frames → world)')

    time.sleep(3)

    robot.clear_end_effector_command()  # clear and return to default reference joint pose
    # robot.clear_end_effector_command(hold=True)   # clear and hold current joint pose
    
    # Send shutdown signal
    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    print("Resource cleanup successful")


if __name__ == "__main__":
    main()

Audio Playback (play_audio / stop_audio)

Applicable Scenarios: Play an audio file through the robot speaker and stop the current playback task.

examples/python/galbot_robot/audio_example.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import sys
import time
import struct
import math
import os
from galbot_sdk.g1 import GalbotRobot

# global variable
audio_callback_count = 0
last_audio_type = None

# Command help table
COMMAND_TABLE = [
    {
        "cmd": "start_microphone_stream_input",
        "description": "Start microphone streaming audio input feature",
        "parameters": "No parameters",
        "example": "start_microphone_stream_input"
    },
    {
        "cmd": "stop_microphone_stream_input",
        "description": "Stop the specified microphone streaming audio input",
        "parameters": "No parameters",
        "example": "stop_microphone_stream_input"
    },
    {
        "cmd": "write_audio_stream_output",
        "description": "PCM audiodata audio output",
        "parameters": "Enter audio file name to play (16K 16bit Mono PCM audio file path):",
        "example": "write_audio_stream_output"
    },
    {
        "cmd": "stop_audio_stream_output",
        "description": "Stop the specified audio output stream or all active audio output streams",
        "parameters": "No parameters",
        "example": "stop_audio_stream_output"
    },
    {
        "cmd": "set_volume",
        "description": "Set system global volume",
        "parameters": "Please enter volume setting parameter (0-100)",
        "example": "set_volume"
    },
    {
        "cmd": "get_volume",
        "description": "Get current system global volume",
        "parameters": "No parameters",
        "example": "get_volume"
    },
    {
        "cmd": "help",
        "description": "Show help",
        "parameters": "No parameters",
        "example": "help"
    },
    {
        "cmd": "q",
        "description": "Exit program",
        "parameters": "No parameters",
        "example": "q"
    }
]

# command
def print_command_help():
    print("\n================ Command Help Table =================")
    print(f"{'Command':<40} | {'Description':<30} | {'Parameters':<20} | {'example':<30}")
    print("-" * 130)
    for cmd in COMMAND_TABLE:
        print(
            f"{cmd['cmd']:<40} | {cmd['description']:<30} | {cmd['parameters']:<20} | {cmd['example']:<30}")
    print("=============================================\n")


# Safely read float input
def safe_read_float(prompt: str = "") -> float:
    """
    Safely get float input

    Args:
        prompt: prompt text

    Returns:
        Valid float input
    """
    if prompt:
        print(prompt, end='', flush=True)
    
    try:
        value = float(input())
        return value
    except ValueError:
        print("Input error, please enter a valid float.")
        return None
    except EOFError:
        print("\nInput ended")
        return None


# Safely read integer input
def safe_read_int(prompt: str = "") -> int:
    """
    Safely get integer input

    Args:
        prompt: prompt text

    Returns:
        Valid integer input
    """
    if prompt:
        print(prompt, end='', flush=True)
    
    try:
        value = int(input())
        return value
    except ValueError:
        print("Input error, please enter a valid integer.")
        return None
    except EOFError:
        print("\nInput ended")
        return None

# Safely read filename input and handle input errors
def safe_read_filename(prompt: str = "") -> str:
    """
    Safely get filename input

    Args:
        prompt: prompt text

    Returns:
        Valid filename input
    """
    if prompt:
        print(prompt, end='', flush=True)
    
    try:
        filename = input()
        return filename
    except ValueError:
        print("Input error, please enter a valid file name.")

# Save audio data
def save_audio_to_file(filename: str, audio_data: bytes):
    """
    Save audio data

    Args:
        filename: file name
        audio_data: audio data (bytes)
    """
    try:
        with open(filename, 'ab') as f:
            f.write(audio_data)
        # print(f"✓ Audio appended and saved to {filename}")
    except Exception as e:
        print(f"✗ Failed to save file: {e}")


# Read audio data from file
def read_audio_from_file(filename: str, chunk_size: int = 2560) -> list:
    """
    Read audio data from file

    Args:
        filename: file name
        chunk_size: chunk size (bytes)

    Returns:
        Audio data block list
    """
    try:
        audio_chunks = []
        with open(filename, 'rb') as f:
            while True:
                chunk = f.read(chunk_size)
                if not chunk:
                    break
                audio_chunks.append(chunk)
        print(f"✓ Read {len(audio_chunks)} chunks from {filename}")
        return audio_chunks
    except FileNotFoundError:
        print(f"✗ File not found: {filename}")
        return []
    except Exception as e:
        print(f"✗ Failed to read file: {e}")
        return []


# Audio callback function
def audio_callback(audio_data: dict):
    """
    Audio data callback function

    Args:
        audio_data: audio data dictionary including header, type, format, data
    """
    global audio_callback_count, last_audio_type
    
    if not audio_data:
        return

    header = audio_data.get("header", {})
    timestamp_ns = header.get("timestamp_ns", 0)

    audio_type = audio_data.get("type", "unknown")
    audio_format = audio_data.get("format", "unknown")
    data_size = len(audio_data.get("data", b""))

    audio_callback_count += 1
    last_audio_type = audio_type

    # Print audio data info
    if audio_type == "vad_begin":
        print(f"\n[VAD_BEGIN] Voice activity detection started")
        pass
    elif audio_type == "vad_end":
        print(f"[VAD_END] Voice activity detection ended")
        pass
    elif audio_type == "vad_chunk":
        # print(f"[AUDIO_CHUNK] Received {data_size} bytes of {audio_format} audio data (timestamp_ns: {timestamp_ns})")
        save_audio_to_file("mic_vad.pcm", audio_data.get("data", b""))
        pass
    elif audio_type == "denoise_chunk":
        # print(f"[DENOISE_AUDIO] Received {data_size} bytes of denoised audio (timestamp_ns: {timestamp_ns})")
        save_audio_to_file("mic_denoise.pcm", audio_data.get("data", b""))
        pass
    elif audio_type == "waken_up":
        print(f"\n[WAKEN_UP] Wake-up event triggered: ", audio_data.get("data", ""))
        pass
    else:
        print(f"[{audio_type}] Received audio data, size: {data_size} bytes")
        pass


def main():
    # Get GalbotRobot singleton
    base_instance = GalbotRobot()

    ok = base_instance.init()
    if not ok:
        print("Initialization failed")
        return -1

    # Waiting for init done
    time.sleep(5)

    print("\n========================================")
    print("     Galbot audio test tool started")
    print("========================================\n")

    print_command_help()

    microphone_stream_id = ""
    speaker_stream_id = "speaker_audio_output_test"
    while base_instance.is_running():
        print("Enter operation to execute:", end='', flush=True)
        cmd_line = input().strip()

        if not cmd_line:
            continue

        parts = cmd_line.split()
        cmd = parts[0]

        try:
            if cmd == "help":
                print_command_help()

            elif cmd == "start_microphone_stream_input":
                print("Starting microphone streaming audio input...")
                global audio_callback_count, last_audio_type
                
                microphone_stream_id = base_instance.start_microphone_stream_input(
                    callback=audio_callback,
                    chunk_size=2560,
                    use_raw_audio=False
                )
                audio_callback_count = 0
                last_audio_type = None
                print(f"✓ Microphone started successfully, stream ID: {microphone_stream_id}")

            elif cmd == "stop_microphone_stream_input":
                if microphone_stream_id is None:
                    print("No active microphone stream")
                else:
                    print(f"Stopping microphone stream (ID: {microphone_stream_id})...")
                    base_instance.stop_microphone_stream_input(microphone_stream_id)
                    print(f"✓ Microphone stopped")
                    print(f"  - Number of callbacks: {audio_callback_count}")
                    print(f"  - Last audio type: {last_audio_type}")
                    microphone_stream_id = None

            elif cmd == "write_audio_stream_output":
                audio_output_file = safe_read_filename("Enter audio file name to play (16K 16bit Mono PCM audio file path): ")
                print("Reading audio file...")
                # Read and play audio
                audio_chunks = read_audio_from_file(audio_output_file, chunk_size=2560)
                if audio_chunks:
                    print(f"Playing audio stream ID: {speaker_stream_id}")
                    
                    for i, chunk in enumerate(audio_chunks):
                        success = base_instance.write_audio_stream_output(chunk, speaker_stream_id)
                        if not success:
                            print(f"✗ Playback failed for chunk {i+1}")
                            break

                        time.sleep(0.05)

                    print(f"✓ Audio playback completed")

            elif cmd == "stop_audio_stream_output":
                print("Stopping audio output stream...")
                base_instance.stop_audio_stream_output(speaker_stream_id)
                print("✓ Audio output stream stopped")

            elif cmd == "set_volume":
                volume = safe_read_float("Please enter volume setting parameter (0-100): ")
                if volume is None or volume < 0 or volume > 100:
                    print(f"✗ Volume must be between 0-100; input value: {volume}")
                    continue

                if (not base_instance.set_volume(volume)):
                    print(f"✗ Failed to set volume({volume})")
                print(f"✓ Volume set to: {volume}%")

            elif cmd == "get_volume":
                volume = base_instance.get_volume()
                print(f"Current system volume: {volume}%")

            elif cmd == "q":
                print("Program exiting...")
                break

            else:
                print(f"Unknown command: {cmd}, enter 'help' to see available commands")

        except (ValueError, TypeError) as e:
            print(f"Parameters error: {e}")
        except Exception as e:
            print(f"Command execution exception: {e}")

    base_instance.request_shutdown()
    base_instance.wait_for_shutdown()
    base_instance.destroy()
    print("Program exited")


if __name__ == "__main__":
    main()

Controller Management

Applicable Scenarios: Query controller states and switch or manage the controllers used by robot components.

examples/python/galbot_robot/controller_management_example.py

import time

from galbot_sdk.g1 import GalbotRobot, ControlStatus, G1ControllerName, G1JointGroup


def print_active_controller(group_name, controller_name):
    print(f"Active controller for {group_name}: {controller_name}")


def print_status(operation, status):
    if status == ControlStatus.SUCCESS:
        print(f"{operation}: SUCCESS")
    else:
        # Assuming status can be cast to int or printed directly
        print(f"{operation}: FAILED (Status: {status})")


def main():
    # Get robot instance and init
    robot = GalbotRobot()
    print("Initializing robot...")
    if robot.init():
        print("System initialized successfully!")
    else:
        print("System initialization failed!")
        return

    # Wait for data readiness
    time.sleep(1)

    # Define the controller we want to test
    # Using LEFT_ARM_PVT_CTRL as the test subject
    test_controller = G1ControllerName.LEFT_ARM_PVT_CTRL
    controller_str = "LEFT_ARM_PVT_CTRL"
    # Using string for display and G1JointGroup enum for querying
    group_str = "left_arm"
    test_group = G1JointGroup.left_arm

    print("\n--- Starting Controller Management Example ---")

    # 1. Get active controller before switching
    active_controller = robot.get_active_controller(test_group)
    print_active_controller(group_str, active_controller)

    # 2. Switch controller
    print(f"\n[Test 1] Switching to {controller_str}...")
    status = robot.switch_controller(test_controller)
    print_status("switch_controller", status)

    time.sleep(0.5)
    active_controller = robot.get_active_controller(test_group)
    print_active_controller(group_str, active_controller)

    # 3. Stop controller
    print(f"\n[Test 2] Stopping controller for group {group_str}...")
    # Using string overload/version for group
    status = robot.stop_controller(group_str)
    print_status("stop_controller", status)

    time.sleep(0.5)

    # 4. Release controller
    print(f"\n[Test 3] Releasing controller for group {group_str}...")
    status = robot.release_controller(group_str)
    print_status("release_controller", status)

    time.sleep(0.5)

    # 5. Acquire controller
    print(f"\n[Test 4] Acquiring {controller_str}...")
    status = robot.acquire_controller(test_controller)
    print_status("acquire_controller", status)

    time.sleep(0.5)

    # 6. Start controller
    print(f"\n[Test 5] Starting controller for group {group_str}...")
    status = robot.start_controller(group_str)
    print_status("start_controller", status)

    time.sleep(0.5)

    print("\n--- Controller Management Example Finished ---")

    # Shutdown
    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    print("Resources released successfully")


if __name__ == "__main__":
    main()

Stop Controller (stop_controller)

Applicable Scenarios: Stop an active controller while it is executing a motion.

examples/python/galbot_robot/stop_controller.py

"""Stop an active head controller from another Python thread."""

import signal
import threading
import time

from galbot_sdk.g1 import GalbotRobot

motion_done = threading.Event()
shutdown_requested = threading.Event()


def move_head_joints(robot, target_position):
    status = robot.set_joint_positions(
        [target_position], [], ["head_joint2"], False, 0.05, 15.0
    )
    print(f"Head motion command sent: {status}")


def stop_controller_on_ctrl_c(robot, group_name):
    while not motion_done.is_set():
        if signal.sigtimedwait({signal.SIGINT}, 0.2) is not None:
            break
    else:
        return
    print(f"Worker thread: stopping {group_name} controller...")
    status = robot.stop_controller(group_name)
    print(f"Worker thread: stop_controller returned: {status}")
    shutdown_requested.set()


def confirm_motion():
    print("⚠️  Ensure the emergency-stop button is released and the area is clear.")
    if input("Start head motion? (y/n): ").strip().lower() != "y":
        raise SystemExit("Motion was not confirmed.")


confirm_motion()
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(1)

group_name = "head"

# Send a non-blocking motion command, then stop its controller after Ctrl-C.
print("Head is moving. Press Ctrl-C to stop its controller.")
stop_thread = threading.Thread(
    target=stop_controller_on_ctrl_c, args=(robot, group_name)
)
stop_thread.start()
move_head_joints(robot, 0.4)
# The command is non-blocking, so retain the Ctrl-C listener for its full
# command timeout unless it has already requested shutdown.
shutdown_requested.wait(timeout=15.0)
motion_done.set()
stop_thread.join()

signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")

Get And Set Dexterous Hand State (get_dexhand_state && set_dexhand_command)

Applicable Scenarios: Read joint and operating states from a configured dexterous hand and send target joint commands to a configured dexterous hand.

examples/python/galbot_robot/get_set_dexhand_state.py

import time
from galbot_sdk.g1 import GalbotRobot, G1JointGroup, ControlStatus, JointCommand, DexHandType

CYCLE_COUNT = 3
COMMAND_INTERVAL_SECONDS = 1.0
INIT_WAIT_SECONDS = 2.0

# The following demo positions are copied from the standalone set_dexhand_command example.
# Different dexterous hand models expose different joint counts and units, so each model
# needs its own open/close target list.
INSPIRE_RH56DFX_POSITION_OPEN = [
    1000.0,     # finger1
    1000.0,     # finger2
    1000.0,     # finger3
    1000.0,     # finger4
    1000.0,     # finger5
    1000.0,     # finger6
]

INSPIRE_RH56DFX_POSITION_CLOSE = [
    850.0,     # finger1
    350.0,     # finger2
    60.0,     # finger3
    60.0,     # finger4
    60.0,     # finger5
    60.0,     # finger6
]

INSPIRE_RH56F2_POSITION_OPEN = [
    1750.0,     # thumb_bend
    1550.0,     # thumb_rotate
    1740.0,     # index
    1740.0,     # middle
    1740.0,     # ring
    1740.0,     # little
]

INSPIRE_RH56F2_POSITION_CLOSE = [
    1600.0,     # thumb_bend
    1250.0,     # thumb_rotate
    950.0,     # index
    950.0,     # middle
    950.0,     # ring
    950.0,     # little
]

BRAINCO_POSITION_OPEN = [
    100.0,      # finger1
    100.0,      # finger2
    100.0,      # finger3
    100.0,      # finger4
    100.0,      # finger5
    100.0,      # finger6
]

BRAINCO_POSITION_CLOSE = [
    33.3,      # finger1
    100.0,      # finger2
    86.7,      # finger3
    86.7,      # finger4
    86.7,      # finger5
    86.7,      # finger6
]

LINKER_L20_POSITION_OPEN = [
    1.3543754995475996,     # thumb_roll
    1.562069680534925,      # thumb_yaw
    0.23561944901923448,    # index_yaw
    0.23561944901923448,    # middle_yaw
    0.23561944901923448,    # ring_yaw
    0.23561944901923448,    # little_yaw
    0.82030474843733492,    # thumb_root
    1.2217304763960306,     # index_root
    1.2217304763960306,     # middle_root
    1.2217304763960306,     # ring_root
    1.2217304763960306,     # little_root
    1.2042771838760873,     # thumb_tip
    1.7453292519943295,     # index_tip
    1.7453292519943295,     # middle_tip
    1.7453292519943295,     # ring_tip
    1.7453292519943295,     # little_tip
]

LINKER_L20_POSITION_CLOSE = [
    0.85,     # thumb_roll
    1.55,     # thumb_yaw
    0.00,    # index_yaw
    0.00,    # middle_yaw
    0.00,    # ring_yaw
    0.00,    # little_yaw
    0.40,    # thumb_root
    0.00,     # index_root
    0.00,     # middle_root
    0.00,     # ring_root
    0.00,     # little_root
    0.35,     # thumb_tip
    0.00,     # index_tip
    0.00,     # middle_tip
    0.00,     # ring_tip
    0.00,     # little_tip
]

SHARPA_POSITION_OPEN = [
    0.0,     # thumb CMC_FE
    0.0,     # thumb CMC_AA
    0.0,     # thumb MCP_FE
    0.0,     # thumb MCP_AA
    0.0,     # thumb IP
    0.0,     # index MCP_FE
    0.0,     # index MCP_AA
    0.0,     # index PIP
    0.0,     # index DIP
    0.0,     # middle MCP_FE
    0.0,     # middle MCP_AA
    0.0,     # middle PIP
    0.0,     # middle DIP
    0.0,     # ring MCP_FE
    0.0,     # ring MCP_AA
    0.0,     # ring PIP
    0.0,     # ring DIP
    0.0,     # pinky CMC
    0.0,     # pinky MCP_FE
    0.0,     # pinky MCP_AA
    0.0,     # pinky PIP
    0.0,     # pinky DIP
]

SHARPA_POSITION_CLOSE = [
    0.87,    # thumb CMC_FE
    0.0,     # thumb CMC_AA
    0.44,    # thumb MCP_FE
    0.0,     # thumb MCP_AA
    0.87,    # thumb IP
    0.78,    # index MCP_FE
    0.0,     # index MCP_AA
    0.87,    # index PIP
    0.70,    # index DIP
    0.78,    # middle MCP_FE
    0.0,     # middle MCP_AA
    0.87,    # middle PIP
    0.70,    # middle DIP
    0.78,    # ring MCP_FE
    0.0,     # ring MCP_AA
    0.87,    # ring PIP
    0.70,    # ring DIP
    0.13,    # pinky CMC
    0.78,    # pinky MCP_FE
    0.0,     # pinky MCP_AA
    0.87,    # pinky PIP
    0.70,    # pinky DIP
]

DEXHAND_TYPE_MAP = {
    "inspire": DexHandType.INSPIRE,
    "inspire_rh56dfx": DexHandType.INSPIRE_RH56DFX,
    "inspire_rh56f2": DexHandType.INSPIRE_RH56F2,
    "brainco": DexHandType.BRAINCO,
    "revo": DexHandType.BRAINCO,
    "sharpa": DexHandType.SHARPA,
    "linker_l20": DexHandType.LINKER_L20,
}

EXIT_INPUTS = {"q", "quit", "exit"}
SUPPORTED_TYPE_TEXT = ", ".join(DEXHAND_TYPE_MAP.keys())


def print_supported_types() -> None:
    """Print all dexhand model names accepted by this combined get/set example."""
    print("Supported dexterous hand types:")
    print("  inspire          Compatibility alias for inspire_rh56f2")
    print("  inspire_rh56dfx  Inspire RH56DFX dexterous hand")
    print("  inspire_rh56f2   Inspire RH56F2 dexterous hand")
    print("  brainco          BrainCo dexterous hand")
    print("  revo             Alias for brainco")
    print("  sharpa           Sharpa dexterous hand")
    print("  linker_l20       Linker Hand L20 dexterous hand")


def read_dexhand_type(side_name: str):
    """Read one hand type before robot initialization so invalid input fails early."""
    while True:
        raw_value = input(f"Enter {side_name} dexterous hand type (or q to quit): ").strip()
        key = raw_value.lower()
        if key in EXIT_INPUTS:
            return None, None
        if key in DEXHAND_TYPE_MAP:
            return DEXHAND_TYPE_MAP[key], key

        print(f"Unsupported dexterous hand type: {raw_value}")
        print(f"Please choose from: {SUPPORTED_TYPE_TEXT}")


def make_dexhand_command(positions) -> list:
    """Convert a list of target positions into SDK JointCommand objects."""
    commands = []
    for position in positions:
        cmd = JointCommand()
        cmd.position = position
        commands.append(cmd)
    return commands


def dexhand_motion_positions(dexhand_type: DexHandType, action: str) -> list:
    """Return the demo open or close position list for the selected hand model."""
    is_open = action == "open"
    if dexhand_type == DexHandType.INSPIRE_RH56DFX:
        return INSPIRE_RH56DFX_POSITION_OPEN if is_open else INSPIRE_RH56DFX_POSITION_CLOSE
    if dexhand_type in (DexHandType.INSPIRE, DexHandType.INSPIRE_RH56F2):
        return INSPIRE_RH56F2_POSITION_OPEN if is_open else INSPIRE_RH56F2_POSITION_CLOSE
    if dexhand_type == DexHandType.BRAINCO:
        return BRAINCO_POSITION_OPEN if is_open else BRAINCO_POSITION_CLOSE
    if dexhand_type == DexHandType.SHARPA:
        return SHARPA_POSITION_OPEN if is_open else SHARPA_POSITION_CLOSE
    if dexhand_type == DexHandType.LINKER_L20:
        return LINKER_L20_POSITION_OPEN if is_open else LINKER_L20_POSITION_CLOSE
    raise ValueError(f"Unsupported dexhand type: {dexhand_type}")


def print_dexhand_state(hand_name: str, dexhand_state, dexhand_type: DexHandType) -> None:
    """Print joint states, and print force sensors for Sharpa hands when available."""
    type_label = "sharpa" if dexhand_type == DexHandType.SHARPA else "dexterous"
    print(f"{hand_name} {type_label} hand state:")
    print(f"Timestamp (ns): {dexhand_state.timestamp_ns}")

    joint_state_vec = dexhand_state.joint_state.joint_state_vec
    print(f"  Joint states ({len(joint_state_vec)} joints):")
    for i, js in enumerate(joint_state_vec):
        if dexhand_type == DexHandType.SHARPA:
            # Sharpa state does not use the generic acceleration field in the same way.
            print(
                f"    joint{i + 1}: position={js.position:.4f}, velocity={js.velocity:.4f}, "
                f"effort={js.effort:.4f}, current={js.current:.4f}"
            )
        else:
            # Some SDK backends fill joint_name; otherwise keep a stable fallback label.
            joint_label = js.joint_name if js.joint_name else f"{hand_name.lower()}_dexhand_joint{i + 1}"
            print(
                f"    {joint_label}: "
                f"position={js.position:.4f}, velocity={js.velocity:.4f}, "
                f"acceleration={js.acceleration:.4f}, "
                f"effort={js.effort:.4f}, current={js.current:.4f}"
            )

    if dexhand_type != DexHandType.SHARPA:
        return

    force_map = dexhand_state.force_sensor_map
    if not force_map:
        print("  (no force sensor data)")
    else:
        print(f"  Force sensors ({len(force_map)} sensors):")
        for sensor_name, effort in force_map.items():
            print(
                f"    {sensor_name} @ {effort.timestamp_ns}: "
                f"Fx={effort.force.x:.4f}, Fy={effort.force.y:.4f}, Fz={effort.force.z:.4f}, "
                f"Mx={effort.torque.x:.4f}, My={effort.torque.y:.4f}, Mz={effort.torque.z:.4f}"
            )


def main():
    print("Starting get_set_dexhand_state example")
    print_supported_types()

    # Ask the user to select the actual dexterous hand model installed on each side.
    left_type, left_label = read_dexhand_type("left")
    if left_type is None:
        print("Example canceled before robot initialization")
        return

    right_type, right_label = read_dexhand_type("right")
    if right_type is None:
        print("Example canceled before robot initialization")
        return

    robot_initialized = False
    robot = GalbotRobot()
    try:
        # Initialize the robot SDK before sending commands or reading states.
        robot.init()
        robot_initialized = True
        print("Initialization succeeded")
        time.sleep(INIT_WAIT_SECONDS)

        # Run one initial open command, then run three close/open demo cycles.
        completed = True
        for action_index, action in enumerate(["open"] + ["close", "open"] * CYCLE_COUNT):
            if action_index > 0 and action == "close":
                cycle_index = (action_index + 1) // 2
                print(f"Running dexterous hand cycle {cycle_index}/{CYCLE_COUNT}")

            # Send the target position command to the left dexterous hand.
            left_positions = dexhand_motion_positions(left_type, action)
            left_command = make_dexhand_command(left_positions)
            left_status = robot.set_dexhand_command(
                G1JointGroup.left_dexhand,
                left_command,
                left_type,
                False,
            )
            if left_status != ControlStatus.SUCCESS:
                print(
                    f"Failed to {action} left {left_label} dexterous hand "
                    f"({len(left_command)} joints), status={left_status}"
                )
                completed = False
            else:
                print(f"Left {left_label} dexterous hand {action} command sent ({len(left_command)} joints)")

            # Send the same action to the right dexterous hand with its own model type.
            right_positions = dexhand_motion_positions(right_type, action)
            right_command = make_dexhand_command(right_positions)
            right_status = robot.set_dexhand_command(
                G1JointGroup.right_dexhand,
                right_command,
                right_type,
                False,
            )
            if right_status != ControlStatus.SUCCESS:
                print(
                    f"Failed to {action} right {right_label} dexterous hand "
                    f"({len(right_command)} joints), status={right_status}"
                )
                completed = False
            else:
                print(f"Right {right_label} dexterous hand {action} command sent ({len(right_command)} joints)")

            # Give the hardware or simulator time to execute the command before reading state.
            time.sleep(COMMAND_INTERVAL_SECONDS)

            # Read back the left hand state after the command has been applied.
            left_state = robot.get_dexhand_state(G1JointGroup.left_dexhand, left_type)
            if left_state is None:
                print("get left dexterous hand state error")
                completed = False
            else:
                print_dexhand_state("Left", left_state, left_type)

            # Read back the right hand state after the command has been applied.
            right_state = robot.get_dexhand_state(G1JointGroup.right_dexhand, right_type)
            if right_state is None:
                print("get right dexterous hand state error")
                completed = False
            else:
                print_dexhand_state("Right", right_state, right_type)

        if completed:
            print("get_set_dexhand_state example completed")
        else:
            print("get_set_dexhand_state example completed with one or more errors")
    finally:
        # Release SDK resources only after the robot has been initialized.
        if robot_initialized:
            robot.request_shutdown()
            robot.wait_for_shutdown()
            robot.destroy()
            print("Resources released successfully")


if __name__ == "__main__":
    main()

Get Force Sensor Data (get_force_sensor_data)

Applicable Scenarios: Obtain force and torque measurements for contact detection and force-aware manipulation.

examples/python/galbot_robot/get_force_sensor_data.py

import time
from galbot_sdk.g1 import GalbotRobot, GalbotOneFoxtrotSensor


def force_sensor_type_to_string(sensor_type: GalbotOneFoxtrotSensor) -> str:
    """Convert force sensor type to string"""
    type_map = {
        GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE: "LEFT_WRIST_FORCE",
        GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE: "RIGHT_WRIST_FORCE",
    }
    return type_map.get(sensor_type, "UNKNOWN_FORCE_SENSOR")


def print_force_data(sensor_type: GalbotOneFoxtrotSensor, force_data: dict):
    """
    Print force sensor data
    
    force_data: dict, including the following fields:
        - 'timestamp_ns': Timestamp (ns)
        - 'force': {'x', 'y', 'z'} Force (N)
        - 'torque': {'x', 'y', 'z'} Torque (N·m)
    """
    print(f"--- {force_sensor_type_to_string(sensor_type)} ---")
    if not force_data:
        print("  Force data is empty")
        return

    print(f"  Timestamp (ns): {force_data.get('timestamp_ns')}")
    
    force = force_data.get("force", {})
    print(f"  Force (N):  fx={force.get('x')}, fy={force.get('y')}, fz={force.get('z')}")
    
    torque = force_data.get("torque", {})
    print(f"  Torque (Nm): tx={torque.get('x')}, ty={torque.get('y')}, tz={torque.get('z')}")


# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()

# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

# Get left wrist force sensor data
print("\n===== Get left wrist force sensor data =====")
left_force_data = robot.get_force_sensor_data(GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE)
print_force_data(GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE, left_force_data)

# Get calibrated left-arm wrench expressed in base_link
print("\n===== Get calibrated left wrist force data in base_link =====")
calibrated_left_force_data = robot.get_force_sensor_data(
    GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE, calibrated=True, ref_frame="base_link"
)
print_force_data(GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE, calibrated_left_force_data)

# Get right wrist force sensor data
print("\n===== Get right wrist force sensor data =====")
right_force_data = robot.get_force_sensor_data(GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE)
print_force_data(GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE, right_force_data)

# Keep calibrated right-arm wrench in its native end-effector mount frame
print("\n===== Get calibrated right wrist force data in mount frame =====")
calibrated_right_force_data = robot.get_force_sensor_data(
    GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE,
    calibrated=True,
    ref_frame="right_arm_end_effector_mount_link",
)
print_force_data(GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE, calibrated_right_force_data)

# Iterate and get all force sensor data
print("\n===== Get all force sensor data =====")
force_sensor_types = [
    GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE,
    GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE,
]

for sensor_type in force_sensor_types:
    force_data = robot.get_force_sensor_data(sensor_type)
    print_force_data(sensor_type, force_data)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Odometry Data (get_odom)

Applicable Scenarios: Read the robot base pose and motion state reported by odometry.

examples/python/galbot_robot/get_odom.py

import time
from galbot_sdk.g1 import GalbotRobot


def print_odom_data(odom_data: dict):
    """
    Print odometry data
    
    odom_data: dict, includes:
        - 'timestamp_ns': Timestamp (ns)
        - 'position': [x, y, z] Position (m)
        - 'orientation': [qx, qy, qz, qw] Quaternion orientation
        - 'linear_velocity': [vx, vy, vz] Linear velocity (m/s)
        - 'angular_velocity': [wx, wy, wz] Angular velocity (rad/s)
    """
    if not odom_data:
        print("Odom data is empty")
        return

    print(f"Timestamp (ns): {odom_data.get('timestamp_ns')}")
    
    position = odom_data.get("position", [])
    if position:
        print(f"Position (m): x={position[0]}, y={position[1]}, z={position[2]}")
    
    orientation = odom_data.get("orientation", [])
    if orientation:
        print(f"Orientation (quaternion): qx={orientation[0]}, qy={orientation[1]}, "
              f"qz={orientation[2]}, qw={orientation[3]}")

    linear_velocity = odom_data.get("linear_velocity", [])
    if linear_velocity:
        print(f"Linear velocity (m/s): vx={linear_velocity[0]}, vy={linear_velocity[1]}, "
              f"vz={linear_velocity[2]}")

    angular_velocity = odom_data.get("angular_velocity", [])
    if angular_velocity:
        print(f"Angular velocity (rad/s): wx={angular_velocity[0]}, wy={angular_velocity[1]}, "
              f"wz={angular_velocity[2]}")


# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()

# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

# Get odometry data
odom_data = robot.get_odom()
if not odom_data:
    print("No odom data!")
else:
    print("Odometry data:")
    print_odom_data(odom_data)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Get Ultrasonic Data (get_ultrasonic_data)

Applicable Scenarios: Read ultrasonic distance measurements for short-range obstacle awareness.

examples/python/galbot_robot/get_ultrasonic_data.py

import time
from galbot_sdk.g1 import GalbotRobot, SensorType, UltrasonicType


def ultrasonic_type_to_string(ultrasonic_type: UltrasonicType) -> str:
    """Convert ultrasonic sensor type to string"""
    type_map = {
        UltrasonicType.FRONT_LEFT: "FRONT_LEFT",
        UltrasonicType.FRONT_RIGHT: "FRONT_RIGHT",
        UltrasonicType.RIGHT_LEFT: "RIGHT_LEFT",
        UltrasonicType.RIGHT_RIGHT: "RIGHT_RIGHT",
        UltrasonicType.BACK_LEFT: "BACK_LEFT",
        UltrasonicType.BACK_RIGHT: "BACK_RIGHT",
        UltrasonicType.LEFT_LEFT: "LEFT_LEFT",
        UltrasonicType.LEFT_RIGHT: "LEFT_RIGHT",
    }
    return type_map.get(ultrasonic_type, "UNKNOWN_ULTRASONIC")


def print_ultrasonic_data(ultrasonic_type: UltrasonicType, ultrasonic_data: dict):
    """
    Print ultrasonic sensor data
    
    ultrasonic_data: dict, including the following fields:
        - 'timestamp_ns': Timestamp (ns)
        - 'distance': Distance (m)
    """
    print(f"--- {ultrasonic_type_to_string(ultrasonic_type)} ---")
    if not ultrasonic_data:
        print("  Ultrasonic data is empty")
        return

    print(f"  Timestamp (ns): {ultrasonic_data.get('timestamp_ns')}")
    print(f"  Distance (m): {ultrasonic_data.get('distance')}")


# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()

# Initialize sensors; only sensors passed during initialization can retrieve data to save resources
enable_sensor_set = {SensorType.BASE_ULTRASONIC}
robot.init(enable_sensor_set)

# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")

# Get single ultrasonic sensor data
print("\n===== Get single ultrasonic sensor data =====")
ultrasonic_data = robot.get_ultrasonic_data(UltrasonicType.FRONT_LEFT)
print_ultrasonic_data(UltrasonicType.FRONT_LEFT, ultrasonic_data)

# Iterate to get all 8 ultrasonic sensor data
print("\n===== Get all ultrasonic sensor data =====")
ultrasonic_types = [
    UltrasonicType.FRONT_LEFT,
    UltrasonicType.FRONT_RIGHT,
    UltrasonicType.RIGHT_LEFT,
    UltrasonicType.RIGHT_RIGHT,
    UltrasonicType.BACK_LEFT,
    UltrasonicType.BACK_RIGHT,
    UltrasonicType.LEFT_LEFT,
    UltrasonicType.LEFT_RIGHT,
]

for ultrasonic_type in ultrasonic_types:
    data = robot.get_ultrasonic_data(ultrasonic_type)
    print_ultrasonic_data(ultrasonic_type, data)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Installation Verification

Applicable Scenarios: Verify SDK installation, robot connection, initialization, and basic interface availability.

examples/python/galbot_robot/installation_welcome_verify.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

from __future__ import annotations

import os
import sys
import time
from typing import List, Sequence

import cv2
import numpy as np

from galbot_sdk.g1 import (
    ControlStatus,
    GalbotRobot,
    JointCommand,
    SensorType, RgbOutputFormat,
    Trajectory,
    TrajectoryPoint,
)

_K_SPEED_RAD_S = 0.12
_K_TIMEOUT_S = 20.0
_K_WELCOME_PCM_REL: str = ""

_K_PRESET_LEG: List[float] = [0.5, 1.5, 1.0, 0.0, 0.0]
_K_PRESET_HEAD: List[float] = [0.0, 0.0]
_K_PRESET_LEFT_ARM: List[float] = [2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0]
_K_PRESET_RIGHT_ARM: List[float] = [-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0]

_K_HEAD_ARMS_DEMO_GROUPS: List[str] = ["head", "left_arm", "right_arm"]
_K_HEAD_ARMS_DEMO_DT_S = 0.06
_K_HEAD_ARMS_DEMO_SEG1 = 28
_K_HEAD_ARMS_DEMO_SEG2 = 32
_K_HEAD_ARMS_DEMO_SEG3 = 28
_K_HEAD_SWAY_YAW_POS_RAD = 0.22
_K_HEAD_SWAY_YAW_NEG_RAD = -0.22


def _getenv_string(key: str) -> str:
    v = os.environ.get(key)
    return v if v else ""


def resolve_welcome_pcm_path() -> str:
    env_pcm = _getenv_string("GALBOT_WELCOME_PCM")
    if env_pcm:
        return env_pcm
    return _K_WELCOME_PCM_REL


def _print_summary(step_description: str, step_passed: bool) -> None:
    print(("[PASS] " if step_passed else "[FAIL] ") + step_description)


def make_point(joint_positions_rad: Sequence[float], time_from_start_sec: float) -> TrajectoryPoint:
    point = TrajectoryPoint()
    point.time_from_start_second = time_from_start_sec
    cmds: List[JointCommand] = []
    for q in joint_positions_rad:
        jc = JointCommand()
        jc.position = float(q)
        cmds.append(jc)
    point.joint_command_vec = cmds
    return point


def build_trajectory(
    joint_waypoints_rad: List[List[float]], joint_groups: List[str], time_step_sec: float
) -> Trajectory:
    trajectory = Trajectory()
    trajectory.joint_groups = joint_groups
    trajectory.joint_names = []
    time_from_start_sec = 0.0
    points: List[TrajectoryPoint] = []
    for waypoint in joint_waypoints_rad:
        time_from_start_sec += time_step_sec
        points.append(make_point(waypoint, time_from_start_sec))
    trajectory.points = points
    return trajectory


def linspace_rows(
    start_positions_rad: Sequence[float], end_positions_rad: Sequence[float], num_samples: int
) -> List[List[float]]:
    if num_samples < 2:
        return [list(start_positions_rad)]
    out: List[List[float]] = []
    n = num_samples - 1
    for sample_idx in range(num_samples):
        blend = sample_idx / n
        row = [
            float(start_positions_rad[j]) + blend * (float(end_positions_rad[j]) - float(start_positions_rad[j]))
            for j in range(len(start_positions_rad))
        ]
        out.append(row)
    return out


def append_rows(dest: List[List[float]], src: List[List[float]]) -> None:
    dest.extend(src)


def upper_body_preset_pose_rad() -> List[float]:
    return list(_K_PRESET_HEAD) + list(_K_PRESET_LEFT_ARM) + list(_K_PRESET_RIGHT_ARM)


def build_slow_head_arms_demo_waypoints_rad() -> List[List[float]]:
    """Slow head yaw left-right (joint2 fixed); arms shift slightly with the sway. Legs not in trajectory."""
    home = upper_body_preset_pose_rad()
    pose_yaw_pos = (
        [_K_HEAD_SWAY_YAW_POS_RAD, 0.0]
        + [2.0 + 0.07, -1.5 + 0.04, -0.6, -1.7, 0.0, -0.8 + 0.04, 0.0]
        + [-2.0 - 0.07, 1.5 - 0.04, 0.6, 1.7, 0.0, 0.8 - 0.04, 0.0]
    )
    pose_yaw_neg = (
        [_K_HEAD_SWAY_YAW_NEG_RAD, 0.0]
        + [2.0 - 0.05, -1.5 - 0.03, -0.6, -1.7, 0.0, -0.8 - 0.03, 0.0]
        + [-2.0 + 0.05, 1.5 + 0.03, 0.6, 1.7, 0.0, 0.8 + 0.03, 0.0]
    )
    rows: List[List[float]] = []
    append_rows(rows, linspace_rows(home, pose_yaw_pos, _K_HEAD_ARMS_DEMO_SEG1))
    append_rows(rows, linspace_rows(pose_yaw_pos, pose_yaw_neg, _K_HEAD_ARMS_DEMO_SEG2))
    append_rows(rows, linspace_rows(pose_yaw_neg, home, _K_HEAD_ARMS_DEMO_SEG3))
    return rows


def play_pcm(robot: GalbotRobot, pcm_file_path: str) -> bool:
    k_pcm_read_chunk_bytes = 2560
    audio_stream_id = "install_welcome_pcm"
    try:
        with open(pcm_file_path, "rb") as pcm_input:
            while True:
                chunk = pcm_input.read(k_pcm_read_chunk_bytes)
                if not chunk:
                    break
                if not robot.write_audio_stream_output(chunk, audio_stream_id):
                    print("[audio] write_audio_stream_output failed", file=sys.stderr)
                    # robot.stop_audio_stream_output(audio_stream_id)
                    return False
                time.sleep(0.05)
    except OSError as e:
        print(f"[audio] Failed to open/read PCM: {pcm_file_path} ({e})", file=sys.stderr)
        return False
    # robot.stop_audio_stream_output(audio_stream_id)
    return True


def _decode_rgb_image_bytes(image_data: bytes) -> np.ndarray | None:
    nparr = np.frombuffer(image_data, np.uint8)
    img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
    return img


def decode_head_rgb(rgb_dict: dict) -> np.ndarray | None:
    if not rgb_dict or "data" not in rgb_dict:
        return None
    data = rgb_dict["data"]
    if not data:
        return None
    return _decode_rgb_image_bytes(bytes(data))


def main() -> int:
    body_preset_step_passed = False
    welcome_pcm_playback_ok = False
    head_camera_capture_ok = False

    robot = GalbotRobot()
    required_sensors = {SensorType.HEAD_LEFT_CAMERA}
    if not robot.init(required_sensors):
        print("GalbotRobot::init failed", file=sys.stderr)
        return 1
    print("Robot initialized (HEAD_LEFT_CAMERA enabled)")
    time.sleep(5.0)

    if not robot.set_volume(100.0):
        print(
            "[WARN] set_volume(100) failed; welcome PCM may play quietly",
            file=sys.stderr,
        )

    leg_step_ok = False
    upper_step_ok = False
    demo_step_ok = False

    leg_set_status = robot.set_joint_positions(
        list(_K_PRESET_LEG), ["leg"], [], True, _K_SPEED_RAD_S, _K_TIMEOUT_S
    )
    leg_step_ok = leg_set_status == ControlStatus.SUCCESS
    if not leg_step_ok:
        print("[FAIL] Leg set_joint_positions not SUCCESS (blocking)", file=sys.stderr)
    _print_summary("Leg set_joint_positions (blocking)", leg_step_ok)

    if leg_step_ok:
        upper_body_joint_targets_rad: List[float] = []
        upper_body_joint_targets_rad.extend(_K_PRESET_HEAD)
        upper_body_joint_targets_rad.extend(_K_PRESET_LEFT_ARM)
        upper_body_joint_targets_rad.extend(_K_PRESET_RIGHT_ARM)

        upper_body_set_status = robot.set_joint_positions(
            upper_body_joint_targets_rad,
            _K_HEAD_ARMS_DEMO_GROUPS,
            [],
            True,
            _K_SPEED_RAD_S,
            _K_TIMEOUT_S,
        )
        upper_step_ok = upper_body_set_status == ControlStatus.SUCCESS
        if not upper_step_ok:
            print(
                "[FAIL] head/left_arm/right_arm set_joint_positions not SUCCESS (blocking)",
                file=sys.stderr,
            )

        _print_summary("Upper set_joint_positions head+arms (blocking)", upper_step_ok)

        if upper_step_ok:
            demo_waypoints_rad = build_slow_head_arms_demo_waypoints_rad()
            demo_trajectory = build_trajectory(
                demo_waypoints_rad, _K_HEAD_ARMS_DEMO_GROUPS, _K_HEAD_ARMS_DEMO_DT_S
            )
            demo_exec_status = robot.execute_joint_trajectory(demo_trajectory, True)
            demo_step_ok = demo_exec_status == ControlStatus.SUCCESS
            if not demo_step_ok:
                print("[FAIL] Head/arms execute_joint_trajectory not SUCCESS (blocking)", file=sys.stderr)
            _print_summary("Head/arms demo trajectory (blocking)", demo_step_ok)
        else:
            print(
                "[INFO] Skipping head/arms demo: upper set_joint_positions did not return SUCCESS.",
                file=sys.stderr,
            )
            _print_summary("Head/arms demo trajectory (blocking)", False)
    else:
        print(
            "[INFO] Skipping upper-body preset and head/arms demo: leg set_joint_positions not SUCCESS.",
            file=sys.stderr,
        )
        _print_summary("Upper set_joint_positions head+arms (blocking)", False)
        _print_summary("Head/arms demo trajectory (blocking)", False)

    body_preset_step_passed = leg_step_ok and upper_step_ok and demo_step_ok

    resolved_welcome_pcm_path = resolve_welcome_pcm_path()
    print("PCM path: " + resolved_welcome_pcm_path)
    if not resolved_welcome_pcm_path:
        print(
            "[FAIL] Welcome PCM path is empty: set GALBOT_WELCOME_PCM (no built-in default path).",
            file=sys.stderr,
        )
        welcome_pcm_playback_ok = False
    else:
        try:
            with open(resolved_welcome_pcm_path, "rb"):
                pass
        except OSError as e:
            print(
                f"[FAIL] PCM file not found or not readable: {resolved_welcome_pcm_path} ({e})",
                file=sys.stderr,
            )
            welcome_pcm_playback_ok = False
        else:
            welcome_pcm_playback_ok = play_pcm(robot, resolved_welcome_pcm_path)
    _print_summary("Welcome PCM playback", welcome_pcm_playback_ok)

    head_rgb_data = robot.get_rgb_data(SensorType.HEAD_LEFT_CAMERA, RgbOutputFormat.JPEG, True)
    if not head_rgb_data:
        print("[FAIL] get_rgb_data returned null", file=sys.stderr)
        head_camera_capture_ok = False
    elif isinstance(head_rgb_data, dict):
        raw = head_rgb_data.get("data")
        raw_len = len(raw) if raw is not None and hasattr(raw, "__len__") else 0
        if raw_len == 0:
            print("[FAIL] head camera data missing or empty", file=sys.stderr)
            head_camera_capture_ok = False
        else:
            head_camera_capture_ok = True
            fmt = head_rgb_data.get("format", "")
            print(f"Head camera: {raw_len} bytes format={fmt}")
            head_image_mat = decode_head_rgb(head_rgb_data)
            if head_image_mat is not None and head_image_mat.size > 0:
                print(f"  decoded size {head_image_mat.shape[1]}x{head_image_mat.shape[0]}")
    else:
        head_camera_capture_ok = True
        print("Head camera: get_rgb_data returned non-empty object")
    _print_summary("Head camera data", head_camera_capture_ok)

    print("\n======== Summary ========")
    _print_summary("Motion verify (blocking)", body_preset_step_passed)
    _print_summary("Audio", welcome_pcm_playback_ok)
    _print_summary("Head camera", head_camera_capture_ok)

    installation_all_checks_passed = (
        body_preset_step_passed and welcome_pcm_playback_ok and head_camera_capture_ok
    )
    print("\nOverall: PASS\n" if installation_all_checks_passed else "\nOverall: FAIL\n")

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()

    return 0 if installation_all_checks_passed else 1


if __name__ == "__main__":
    sys.exit(main())

Set Base Pose (set_base_pose)

Applicable Scenarios: Command the mobile base to move to a target pose in the selected reference frame.

examples/python/galbot_robot/set_base_pose.py

import time

from galbot_sdk.g1 import ControlStatus, GalbotRobot, Pose


def set_yaw_orientation(pose, yaw):
    import math

    half_yaw = 0.5 * yaw
    pose.orientation.x = 0.0
    pose.orientation.y = 0.0
    pose.orientation.z = math.sin(half_yaw)
    pose.orientation.w = math.cos(half_yaw)




def test1(robot):
    # Test 1: set base pose using Pose.
    pose = Pose()
    pose.position.x = 0.3  # Move to x = 0.3 m in the odom frame.
    pose.position.y = 0.0
    pose.position.z = 0.0
    set_yaw_orientation(pose, 0.0)

    status = robot.set_base_pose(pose, True, 15.0)
    if status == ControlStatus.SUCCESS:
        print("[Test 1] Pose-struct set succeeded.")
    else:
        print("[Test 1] Pose-struct set failed or timed out.")

    time.sleep(2)

def test2(robot):
    # Test 2: set base pose using frame ids. "rel(0)" is relative to the current base pose.
    x = -0.3  # Move backward by 0.3 m from the current pose.
    y = 0.0
    yaw = 0.0
    frame_id = "rel(0)"
    reference_frame_id = "odom"
    is_blocking = True
    timeout_s = 15.0

    status = robot.set_base_pose(x, y, yaw, frame_id, reference_frame_id, is_blocking, timeout_s)
    if status == ControlStatus.SUCCESS:
        print("[Test 2] Relative set succeeded.")
    else:
        print("[Test 2] Relative set failed or timed out.")

    time.sleep(2)

def test3(robot):
    # Test 3: set base pose using explicit interpolation time.
    x = 0.0  # Keep the third target at zero relative displacement.
    y = 0.0
    yaw = 0.0
    # frame_id = "base_link"
    frame_id = "rel(0)"
    # frame_id = "odom"
    reference_frame_id = "odom"
    time_from_start_s = 5.0
    is_blocking = True
    timeout_s = 15.0

    status = robot.set_base_pose(
        x,
        y,
        yaw,
        frame_id,
        reference_frame_id,
        time_from_start_s,
        is_blocking,
        timeout_s,
    )
    if status == ControlStatus.SUCCESS:
        print("[Test 3] Full-control set succeeded.")
    else:
        print("[Test 3] Full-control set failed or timed out.")

    time.sleep(2)


if __name__ == "__main__":

    robot = GalbotRobot()
    robot.init()
    time.sleep(2)
    print("Initialization succeeded")

    # Run all three API forms in sequence so every target is executed.
    test1(robot)
    test2(robot)
    test3(robot)

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    print("Resources released successfully")

Set Robot Configuration (set_config)

Applicable Scenarios: Configure supported robot services or runtime parameters through the unified configuration interface.

examples/python/galbot_robot/set_config.py

import time

from galbot_sdk.g1 import ConfigService, ConfigItem, ControlStatus, GalbotRobot

# Full list of settable fields (navigation service, camera services, motion
# planning service and control service), their meaning, type and valid range:
# docs/g1/en/set_config_reference.md


def example_navigation(robot):
    # Navigation service: adjust replanning thresholds.
    status = robot.set_config(
        ConfigService.NAVIGATION,
        [
            ConfigItem("replan_threshold", 30.0),
            ConfigItem("no_replan_threshold", 1.0),
        ],
    )
    if status == ControlStatus.SUCCESS:
        print("[Navigation] set_config succeeded. Restart the device for the new config to take effect.")
    else:
        print(f"[Navigation] set_config failed: {status}. Check the SDK log for details.")


def example_camera(robot):
    # Front head camera: set resolution.
    # color_width/color_height must be set together in the same call, and the
    # combination must be one of the documented valid resolutions.
    status = robot.set_config(
        ConfigService.FRONT_HEAD_CAMERA,
        [
            ConfigItem("color_width", 1280),
            ConfigItem("color_height", 992),
        ],
    )
    if status == ControlStatus.SUCCESS:
        print("[Camera] set_config succeeded. Restart the device for the new config to take effect.")
    else:
        print(f"[Camera] set_config failed: {status}. Check the SDK log for details.")


def example_motion_plan(robot):
    # Motion planning service: set a non-per-chain field (plan_timeout).
    status = robot.set_config(
        ConfigService.MOTION_PLAN,
        [
            ConfigItem("plan_timeout", 5.0),
        ],
    )
    if status == ControlStatus.SUCCESS:
        print("[MotionPlan] set_config succeeded. Restart the device for the new config to take effect.")
    else:
        print(f"[MotionPlan] set_config failed: {status}. Check the SDK log for details.")


def example_control(robot):
    # Control service: report_error_skip is a general parameter shared by
    # both G-series and S1 (robot_config.toml).
    status = robot.set_config(
        ConfigService.CONTROL,
        [
            ConfigItem("report_error_skip", 1),
        ],
    )
    if status == ControlStatus.SUCCESS:
        print("[Control] set_config succeeded. Restart the device for the new config to take effect.")
    else:
        print(f"[Control] set_config failed: {status}. Check the SDK log for details.")


if __name__ == "__main__":
    robot = GalbotRobot()
    robot.init()
    time.sleep(2)
    print("Initialization succeeded")

    example_navigation(robot)
    example_camera(robot)
    example_motion_plan(robot)
    example_control(robot)

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    print("Resources released successfully")

Read Robot Configuration (get_config)

Use case: Read the active scene's persisted configuration, or read built-in robot defaults with use_default=True to compare the two.

examples/python/galbot_robot/get_config.py

import time
from galbot_sdk.g1 import ConfigService, ControlStatus, GalbotRobot

def print_fields(title, status, fields):
    print(f"[{title}] status: {status}")
    if status != ControlStatus.SUCCESS:
        print("Some fields could not be read. Successful fields are shown below; check the SDK log for details.")
    for field in fields:
        print(f"  {field.key} = {field.value}")

if __name__ == "__main__":
    robot = GalbotRobot()
    robot.init()
    time.sleep(2)
    status, fields = robot.get_config(ConfigService.CONTROL, ["report_error_skip", "report_sensor_skip"])
    print_fields("Control selected fields", status, fields)
    status, fields = robot.get_config(ConfigService.CONTROL, [])
    print_fields("Control all fields", status, fields)
    status, fields = robot.get_config(ConfigService.CONTROL, ["report_error_skip", "report_sensor_skip"], use_default=True)
    print_fields("Control built-in defaults", status, fields)
    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()

Set Leg Height (set_leg_height)

Applicable Scenarios: Adjust the robot body height through the leg mechanism.

examples/python/galbot_robot/set_leg_height_example.py

import time

from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotRobot


def print_status(operation, status):
    if status == ControlStatus.SUCCESS:
        print(f"{operation} succeeded.")
    else:
        print(f"{operation} failed, status: {status}")


def get_leg_height(robot, label):
    pose, timestamp_ns = robot.get_transform(
        "base_link", "head_base_link", timestamp_ns=0, timeout_ms=500
    )
    height_m = pose[2]
    print(
        f"{label}: pose=[{', '.join(f'{value:.3f}' for value in pose)}], "
        f"tf_timestamp_ns={timestamp_ns}"
    )
    return height_m >= 0.0, height_m


def confirm_leg_height_motion(current_height_m, target_height_m):
    direction = "lower" if target_height_m < current_height_m else "raise"
    motion_distance_m = abs(target_height_m - current_height_m)
    response = input(
        f"The leg mechanism is about to {direction} the body by "
        f"{motion_distance_m:.3f} m, from {current_height_m:.3f} m "
        f"to {target_height_m:.3f} m. "
        "Confirm the surrounding environment is clear and safe to avoid collisions. "
        "Enter y to continue; any other input cancels: "
    )
    return response.strip().lower() == "y"


def main():
    # Get and initialize the G1 robot instance.
    robot = GalbotRobot()
    print("Initializing robot...")
    if not robot.init():
        print("System initialization failed!")
        return
    print("System initialized successfully!")

    # Wait for WBC communication and state data to become ready.
    time.sleep(2)

    height_offset_m = 0.20
    duration_s = 4.0

    height_valid, initial_height_m = get_leg_height(robot, "Initial leg height")
    if not height_valid:
        print("Failed to get a valid initial leg height.")
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return

    lowered_height_m = initial_height_m - height_offset_m
    if not confirm_leg_height_motion(initial_height_m, lowered_height_m):
        print("Leg height motion cancelled. Exiting program.")
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return

    print(
        f"Lowering leg height by {height_offset_m:.3f} m "
        f"to {lowered_height_m:.3f} m."
    )
    status = robot.set_leg_height(lowered_height_m, duration_s, True)
    print_status("set_leg_height(lowered)", status)

    lowered_height_valid, actual_lowered_height_m = get_leg_height(
        robot, "Height after lowering"
    )
    if not lowered_height_valid:
        print("Failed to get the current leg height; cancelling the restore motion.")
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return
    print(
        "Lowered target error: "
        f"{abs(actual_lowered_height_m - lowered_height_m):.3f} m."
    )

    if not confirm_leg_height_motion(actual_lowered_height_m, initial_height_m):
        print("Leg height restore motion cancelled. Exiting program.")
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return

    print(f"Restoring initial leg height: {initial_height_m:.3f} m.")
    status = robot.set_leg_height(initial_height_m, duration_s, True)
    print_status("set_leg_height(initial)", status)

    _, final_height_m = get_leg_height(robot, "Final leg height")
    print(
        "Restored height error: "
        f"{abs(final_height_m - initial_height_m):.3f} m."
    )

    time.sleep(0.5)
    print("Switching controller back to leg_pvt_ctrl...")
    status = robot.switch_controller(G1ControllerName.LEG_PVT_CTRL)
    print_status("switch_controller(leg_pvt_ctrl)", status)

    print("Waiting 1 second before shutdown...")
    time.sleep(1)

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    print("Resources released successfully.")


if __name__ == "__main__":
    main()

Stop Base Motion (stop_base)

Applicable Scenarios: Stop the current base motion command immediately when navigation is not managing the base.

examples/python/galbot_robot/stop_base.py

import signal
import threading
import time

from galbot_sdk.g1 import ControlStatus, GalbotRobot

motion_done = threading.Event()
shutdown_requested = threading.Event()


def stop_base_on_ctrl_c():
    """Notify the main thread to stop publishing velocity after Ctrl-C."""
    while not motion_done.is_set():
        if signal.sigtimedwait({signal.SIGINT}, 0.2) is not None:
            shutdown_requested.set()
            return


def confirm_motion():
    print("⚠️  Ensure the emergency-stop button is released and the area is clear.")
    if input("Start chassis motion? (y/n): ").strip().lower() != "y":
        raise SystemExit("Motion was not confirmed.")


confirm_motion()
# Block SIGINT before SDK initialization so all SDK threads inherit the mask.
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")

print("Chassis is moving. Press Ctrl-C to stop it.")
stop_thread = threading.Thread(target=stop_base_on_ctrl_c)
stop_thread.start()
start_time = time.monotonic()
while not shutdown_requested.is_set() and time.monotonic() - start_time < 5.0:
    next_publish_time = time.monotonic() + 0.1
    status = robot.set_base_velocity(
        [0.1, 0.0, 0.0], [0.0, 0.0, 0.0], duration_s=0.0
    )
    if status != ControlStatus.SUCCESS:
        print(f"Failed to send chassis velocity: {status}")
        break
    shutdown_requested.wait(
        timeout=max(0.0, min(next_publish_time, start_time + 5.0) - time.monotonic())
    )
motion_done.set()
stop_thread.join()

print("Sending chassis stop command...")
for _ in range(10):
    status = robot.stop_base()
    if status == ControlStatus.SUCCESS:
        print("Chassis motion stopped successfully")
        break
    print("Chassis stop failed, retrying...")
    time.sleep(0.2)
else:
    print("Chassis stop timed out")

# AsyncShutdown raises SIGINT on its calling thread, so unblock it here first.
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")

Class: GalbotMotion

Get Instance and Initialize

Applicable Scenarios: Get the motion planning module singleton and initialize. Must be called before using motion planning functions.

examples/python/galbot_motion/get_instance.py

from galbot_sdk.g1 import GalbotMotion
from galbot_sdk.g1 import GalbotRobot

# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()

if motion.init():
    print("GalbotMotion initialized successfully")
else:
    print("GalbotMotion initialization failed")

if robot.init():
    print("GalbotRobot initialized successfully")
else:
    print("GalbotRobot initialization failed")

# You can still manage the robot lifecycle through GalbotRobot
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Forward Kinematics (Using Current State or Specified RobotStates)

Applicable Scenarios: Calculate end effector pose based on given joint angles. Used for robotic arm task verification and state analysis.

examples/python/galbot_motion/fk.py

import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()

def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Execution result: SUCCESS, execution successful")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Execution result: TIMEOUT, execution timed out")
        elif(status == gm.MotionStatus.FAULT):
            print("Execution result: FAULT, a fault occurred and execution cannot continue")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Execution result: INIT_FAILED, failed to create internal communication components")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Execution result: IN_PROGRESS, in motion but not yet in position")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Execution result: PUBLISH_FAIL, data transmission failed")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Execution result: COMM_DISCONNECTED, connection failed")

if motion.init():
    print("GalbotMotion initialized successfully")
else:
    print("GalbotMotion initialization failed")
if robot.init():
    print("GalbotRobot initialized successfully")
else:
    print("GalbotRobot initialization failed")

# Program started, waiting for data
time.sleep(1)

chain_joints = {
    "leg": [0.4992, 1.4991, 1.0005, 0.0000, -0.0004],
    "head": [0.0000, 0.0],
    "left_arm": [1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000],
    "right_arm": [-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000],
}
end_link = "left_arm_end_effector_mount_link"
reference_frame = "base_link"

# Scenario 1: Forward kinematics using the current robot state
try:
    status, pose = motion.forward_kinematics(end_link, reference_frame)
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Forward kinematics calculation failed"
    print(f"✅ Basic forward kinematics successful: pose={pose}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ Basic forward kinematics exception: {e}")

# Scenario 2: Forward kinematics with custom arm joints
try:
    status, pose = motion.forward_kinematics(
        end_link, reference_frame, {"left_arm": chain_joints["left_arm"]}, gm.Parameter()
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Forward kinematics calculation failed"
    print(f"✅ Custom-joint forward kinematics successful: pose={pose}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ Custom-joint forward kinematics exception: {e}")

# Scenario 3: Forward kinematics based on RobotStates
try:
    current_state = motion.get_robot_states()
    if not current_state.whole_body_joint:
        print("❌ RobotStates-based FK: robot state is empty; ensure sensors/WBC are ready")
    else:
        status, pose = motion.forward_kinematics_by_state(
            end_link, current_state, reference_frame, gm.Parameter()
        )
        printStatus(status)
        assert status == gm.MotionStatus.SUCCESS, "Forward kinematics calculation failed"
        print(f"✅ RobotStates-based forward kinematics successful: pose={pose}")
except Exception as e:
    print(f"❌ RobotStates-based forward kinematics exception: {e}")

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Inverse Kinematics (Basic and RobotStates-based)

Applicable Scenarios: Solve for the required joint angles based on the desired end effector pose. This is a core step for operations like robotic arm grasping.

examples/python/galbot_motion/ik.py

import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()

def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Execution result: SUCCESS, execution successful")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Execution result: TIMEOUT, execution timed out")
        elif(status == gm.MotionStatus.FAULT):
            print("Execution result: FAULT, a fault occurred and execution cannot continue")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Execution result: INIT_FAILED, failed to create internal communication components")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Execution result: IN_PROGRESS, in motion but not yet in position")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Execution result: PUBLISH_FAIL, data transmission failed")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Execution result: COMM_DISCONNECTED, connection failed")

if motion.init():
    print("GalbotMotion initialized successfully")
else:
    print("GalbotMotion initialization failed")
if robot.init():
    print("GalbotRobot initialized successfully")
else:
    print("GalbotRobot initialization failed")

# Program started, waiting for data
time.sleep(1)

chain_joints = {
    "leg": [0.4992,1.4991,1.0005,0.0000,-0.0004],
    "head": [0.0000,0.0],
    "left_arm": [1.9999,-1.6000,-0.5999,-1.6999,0.0000,-0.7999,0.0000],
    "right_arm": [-2.0000,1.6001,0.6001,1.7000,0.0000,0.8000,0.0000]
}
chain_pose_baselink = {
    "leg": [0.0596,-0.0000,1.0327,0.5000,0.5003,0.4997,0.5000],
    "head": [0.0599,0.0002,1.4098,-0.7072,0.0037,0.0037,0.7069],
    "left_arm": [0.1267,0.2342,0.7356,0.0220,0.0127,0.0343,0.9991],
    "right_arm": [0.1267,-0.2345,0.7358,-0.0225,0.0126,-0.0343,0.9991]
}
whole_body_joint = [
    num for key in ["leg", "head", "left_arm", "right_arm"] 
    for num in chain_joints[key]
]
base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
custom_param = gm.Parameter()
reference_frame = "base_link"
target_frame = "EndEffector"
target_chain = "left_arm"
one_chain = [target_chain]
chain_with_torso = [target_chain, "torso"]
error_chains = [target_chain, "torso", "head"]
# Scenario 1: Single-chain inverse kinematics
try:
    status, joint_map = motion.inverse_kinematics(
        target_pose=chain_pose_baselink[target_chain],
        chain_names=one_chain,
        target_frame=target_frame,
        reference_frame=reference_frame,
        enable_collision_check=False  # Disable collision checking for accelerated testing
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Inverse kinematics calculation failed"
    print(f"✅ Basic IK succeeded: joint angles={joint_map}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ Basic IK exception: {e}")

# Scenario 2: Arm chain + torso inverse kinematics
try:
    status, joint_map = motion.inverse_kinematics(
        target_pose=chain_pose_baselink[target_chain],
        chain_names=chain_with_torso,
        target_frame=target_frame,
        reference_frame=reference_frame,
        enable_collision_check=False  # Disable collision checking for accelerated testing
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Inverse kinematics calculation failed"
    print(f"✅ IK with custom initial joints succeeded: joint angles={joint_map}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ IK with custom initial joints exception: {e}")

# Scenario 3: invalid chain combination
try:
    status, joint_map = motion.inverse_kinematics(
        target_pose=chain_pose_baselink[target_chain],
        chain_names=error_chains,
        target_frame=target_frame,
        reference_frame=reference_frame,
        enable_collision_check=False  # Disable collision checking for accelerated testing
    )
    printStatus(status)
    assert status == gm.MotionStatus.INVALID_INPUT, "Inverse kinematics calculation failed"
    print(f"✅ Invalid chain-combination input check passed")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ IK with custom initial joints exception: {e}")

# Scenario 4: Use reference joints
try:
    # initial_joint_positions can specify chain joints as IK reference; unspecified chain joints are filled from whole-body joints
    status, joint_map = motion.inverse_kinematics(
        target_pose=chain_pose_baselink[target_chain],
        chain_names=one_chain,
        target_frame=target_frame,
        reference_frame=reference_frame,
        initial_joint_positions=chain_joints,
        enable_collision_check=False  # Disable collision checking for accelerated testing
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Inverse kinematics calculation failed"
    print(f"✅ IK with custom initial joints succeeded: joint angles={joint_map}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ IK with custom initial joints exception: {e}")

# Scenario 5: Use RobotStates
try:
    ref_robot_state = gm.RobotStates()
    ref_robot_state.chain_name = target_chain
    ref_robot_state.whole_body_joint = whole_body_joint
    ref_robot_state.base_state = base_state
    target_frame = "EndEffector"
    reference_frame = "base_link"
    status, joint_map = motion.inverse_kinematics_by_state(
        target_pose=chain_pose_baselink[target_chain],
        chain_names=one_chain,
        target_frame=target_frame,
        reference_frame=reference_frame,
        reference_robot_states=ref_robot_state
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Inverse kinematics calculation failed"
    print(f"✅ RobotStates-based IK succeeded: joint angles={joint_map}")
except Exception as e:
    print(f"❌ RobotStates-based IK exception: {e}")

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Get and Set End Effector Pose

Applicable Scenarios: Directly get current end effector pose, or move the end effector to a specified pose. Integrates inverse kinematics calculation and execution.

examples/python/galbot_motion/get_set_end_effector_pose.py

import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()

def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Execution result: SUCCESS, execution successful")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Execution result: TIMEOUT, execution timed out")
        elif(status == gm.MotionStatus.FAULT):
            print("Execution result: FAULT, a fault occurred and execution cannot continue")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Execution result: INIT_FAILED, failed to create internal communication components")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Execution result: IN_PROGRESS, in motion but not yet in position")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Execution result: PUBLISH_FAIL, data transmission failed")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Execution result: COMM_DISCONNECTED, connection failed")

if motion.init():
    print("GalbotMotion initialized successfully")
else:
    print("GalbotMotion initialization failed")
if robot.init():
    print("GalbotRobot initialized successfully")
else:
    print("GalbotRobot initialization failed")

# Program started, waiting for data
time.sleep(1)

chain_pose_baselink = {
    "leg": [0.0596,-0.0000,1.0327,0.5000,0.5003,0.4997,0.5000],
    "head": [0.0599,0.0002,1.4098,-0.7072,0.0037,0.0037,0.7069],
    "left_arm": [0.1267,0.2342,0.7356,0.0220,0.0127,0.0343,0.9991],
    "right_arm": [0.1267,-0.2345,0.7358,-0.0225,0.0126,-0.0343,0.9991]
}
custom_param = gm.Parameter()
target_frame = "EndEffector"
reference_frame = "base_link"
target_chain = "left_arm"
# Scenario 1: Basic version
try:
    end_ee_link = "left_arm_end_effector_mount_link"
    status, pose = motion.get_end_effector_pose(
        end_effector_frame=end_ee_link,
        reference_frame=reference_frame
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
    print(f"✅ Basic end-effector pose retrieval succeeded: {pose}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ Basic end-effector pose retrieval exception: {e}")

# Scenario 2: Specify chain name + custom frame
try:
    status, pose = motion.get_end_effector_pose_on_chain(
        chain_name=target_chain,
        frame_id=target_frame,
        reference_frame=reference_frame
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
    print(f"✅ End-effector pose retrieval by specified chain succeeded: {pose}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ End-effector pose retrieval by specified chain exception: {e}")

end_effector_frame="left_arm"
reference_frame = "base_link"
motion_timeout = 10.0
try:
    status = motion.set_end_effector_pose(
        target_pose=chain_pose_baselink[end_effector_frame],
        end_effector_frame=end_effector_frame,
        reference_frame=reference_frame,
        enable_collision_check=False,
        is_blocking=False,
        timeout=motion_timeout,
        params=custom_param
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Set end-effector pose failed"
    print(f"✅ End-effector pose set succeeded: status={status}")
    time.sleep(motion_timeout + 1.0)
except Exception as e:
    print(f"❌ End-effector pose setting exception: {e}")

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Single Point Motion Planning (Joint Space and Cartesian Space)

Applicable Scenarios: Plan a collision-free motion trajectory from the current pose to a single target pose. Suitable for single-point target arrival tasks.

examples/python/galbot_motion/motion_plan.py

"""G1 motion_plan example aligned with motion_plan_example.cpp."""

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

def initialize_interfaces():
    motion = GalbotMotion()
    print("GalbotMotion::get_instance() status: no return status; call completed.")
    robot = GalbotRobot()
    print("GalbotRobot::get_instance() status: no return status; call completed.")
    if not motion.init():
        print("GalbotMotion::init() status: FAILED")
        return None, None
    print("GalbotMotion::init() status: SUCCESS")
    if not robot.init():
        print("GalbotRobot::init() status: FAILED")
        return None, None
    print("GalbotRobot::init() status: SUCCESS")
    time.sleep(1.0)
    return motion, robot


def shutdown_robot(robot):
    robot.request_shutdown()
    print("request_shutdown() status: no return status; call completed.")
    robot.wait_for_shutdown()
    print("wait_for_shutdown() status: no return status; call completed.")
    robot.destroy()
    print("destroy() status: no return status; call completed.")


def print_control_status(api_name, status):
    result = "SUCCESS" if status == gm.ControlStatus.SUCCESS else "FAILED"
    print(f"{api_name} status: {str(status).rsplit('.', 1)[-1]} ({result})")


def move_robot_to_home_position(robot):
    print("Move robot to the G1 home position.")
    status = robot.set_joint_positions(
        [
            0.5, 1.5, 1.0, 0.0, 0.0,
            0.0, 0.0,
            2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
            -2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0,
        ],
        ["leg", "head", "left_arm", "right_arm"],
        [],
        True,
        0.1,
        30.0,
    )
    print_control_status("GalbotRobot::set_joint_positions(home)", status)
    return status == gm.ControlStatus.SUCCESS


def capture_current_pose(motion, robot, chain, current):
    joint_names = list(motion.get_chain_joint_names(chain))
    print(f"get_chain_joint_names({chain}) status: {'SUCCESS' if joint_names else 'FAILED'}")
    if not joint_names:
        return False
    joints = list(robot.get_joint_positions([], joint_names))
    success = len(joints) == len(joint_names)
    print(f"GalbotRobot::get_joint_positions({chain}) status: {'SUCCESS' if success else 'FAILED'}")
    if not success:
        return False
    current["joint_names"][chain] = joint_names
    current["joints"][chain] = joints
    print(f"Current {chain} joints: {joints}")

    status, pose = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link")
    result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
    print(f"get_end_effector_pose_on_chain({chain}) status: {motion.status_to_string(status)} ({result})")
    pose = list(pose)
    if status != gm.MotionStatus.SUCCESS or len(pose) != 7:
        return False
    current["poses"][chain] = pose
    print(f"Current {chain} pose [x, y, z, qx, qy, qz, qw]: {pose}")
    return True


def make_joint_target(chain, joint_positions, joint_names=None):
    target = gm.MotionPlanChainTarget()
    target.chain_name = chain
    target.mode = gm.MotionPlanTargetMode.kJoint
    target.joint.chain_name = chain
    target.joint.joint_positions = list(joint_positions)
    target.joint.joint_names = list(joint_names or [])
    return target


def make_cartesian_target(chain, pose, assist_chains=None):
    target = gm.MotionPlanChainTarget()
    target.chain_name = chain
    target.mode = gm.MotionPlanTargetMode.kCartesian
    target.cart.chain_name = chain
    target.cart.frame_id = "EndEffector"
    target.cart.reference_frame = "base_link"
    target.cart.pose = gm.Pose(list(pose))
    target.cart.assist_chains = set(assist_chains or [])
    return target


def make_planner_config(check_collision):
    params = gm.Parameter()
    params.set_direct_execute(True)
    params.set_blocking(True)
    params.set_timeout(120.0)
    params.set_check_collision(check_collision)
    params.set_reference_frame("base_link")
    params.set_actuate("with_torso")
    return params


def print_waypoints(label, waypoints):
    print(f"{label} waypoints: {len(waypoints)}")
    for index, waypoint in enumerate(waypoints):
        print(f"  waypoint[{index}] targets: {len(waypoint)}")
        for target in waypoint:
            if target.mode == gm.MotionPlanTargetMode.kJoint:
                print(f"    {target.chain_name} JOINT q={list(target.joint.joint_positions)}")
            else:
                print(f"    {target.chain_name} CART frame_id={target.cart.frame_id} reference_frame={target.cart.reference_frame}")


def print_traj_result(label, status, traj, motion):
    result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
    print(f"{label} status: {motion.status_to_string(status)} ({result})")
    if status == gm.MotionStatus.SUCCESS:
        if not traj:
            print("Trajectory map is empty.")
        for chain, points in traj.items():
            print(f"  {chain} trajectory points: {len(points)}")


def make_example_inputs(current):
    left_names = current["joint_names"]["left_arm"]
    right_names = current["joint_names"]["right_arm"]
    torso = lambda: make_joint_target("torso", [0.0], ["leg_joint4"])
    cart = lambda chain, pose: make_cartesian_target(chain, pose, ["torso"])

    waypoints = [
        [
            make_joint_target(
                "left_arm",
                [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
                left_names,
            ),
            make_joint_target(
                "right_arm",
                [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0],
                right_names,
            ),
            torso(),
        ],
        [
            make_joint_target(
                "left_arm",
                [1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
                left_names,
            ),
            torso(),
        ],
        [
            cart("left_arm", [0.32666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
            cart("right_arm", [0.32666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
        ],
        [
            cart("left_arm", [0.32666, 0.43435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
            cart("right_arm", [0.32666, -0.43435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
        ],
        [
            make_joint_target(
                "left_arm",
                [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
                left_names,
            ),
            make_joint_target(
                "right_arm",
                [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0],
                right_names,
            ),
        ],
    ]
    return waypoints, make_planner_config(check_collision=False)


def run_example():
    motion, robot = initialize_interfaces()
    if motion is None or robot is None:
        return 1

    try:
        if not move_robot_to_home_position(robot):
            return 1
        time.sleep(0.5)

        current = {"joint_names": {}, "joints": {}, "poses": {}}
        if not capture_current_pose(motion, robot, "left_arm", current):
            return 1
        if not capture_current_pose(motion, robot, "right_arm", current):
            return 1

        waypoints, params = make_example_inputs(current)
        print_waypoints("motion_plan", waypoints)
        print(
            "Immediate execution is enabled; waypoints are loaded from "
            "motion_plan_targets.json reference values."
        )
        status, traj = motion.motion_plan(waypoints, params)
        print_traj_result("motion_plan", status, traj, motion)
        return 0 if status == gm.MotionStatus.SUCCESS else 1
    finally:
        shutdown_robot(robot)


if __name__ == "__main__":
    raise SystemExit(run_example())

Multi-Waypoint Trajectory Planning

Applicable Scenarios: Plan a continuous motion trajectory through multiple waypoints, suitable for complex motions that need to pass through multiple intermediate points.

examples/python/galbot_motion/motion_plan_multi_waypoints.py

import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot


def confirm_robot_safety():
    print("WARNING: The robot will move to the initial joint state. Release the emergency stop and clear nearby obstacles.")
    return input("Continue? (y/n): ").strip().lower() == "y"


def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Execution result: SUCCESS, execution successful")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Execution result: TIMEOUT, execution timed out")
        elif(status == gm.MotionStatus.FAULT):
            print("Execution result: FAULT, a fault occurred and execution cannot continue")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Execution result: INIT_FAILED, failed to create internal communication components")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Execution result: IN_PROGRESS, in motion but not yet in position")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Execution result: PUBLISH_FAIL, data transmission failed")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Execution result: COMM_DISCONNECTED, connection failed")


def main():
    if not confirm_robot_safety():
        print("Example cancelled.")
        return

    # Get and initialize the GalbotMotion singleton
    motion = GalbotMotion()
    robot = GalbotRobot()

    if motion.init():
        print("GalbotMotion initialized successfully")
    else:
        print("GalbotMotion initialization failed")
    if robot.init():
        print("GalbotRobot initialized successfully")
    else:
        print("GalbotRobot initialization failed")

    # Program started, waiting for data
    time.sleep(2)

    chain_joints = {
        "leg": [0.5, 1.5, 1.0, 0.0, 0.0],
        "head": [0.0, 0.0],
        "left_arm": [2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0],
        "right_arm": [-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0]
    }

    # FK results of chain_joints in base_link, showing their physical correspondence.
    chain_pose_baselink = {
        "leg": [0.0545, -0.0013, 1.0377, 0.4956, 0.4950, 0.5060, 0.5033],
        "head": [0.0517, -0.0016, 1.4155, -0.7046, -0.0042, -0.0056, 0.7096],
        "left_arm": [0.1298, 0.2608, 0.7418, 0.0734, 0.0209, -0.0233, 0.9968],
        "right_arm": [0.1261, -0.2575, 0.7424, -0.0466, 0.0223, 0.0058, 0.9986]
    }
    whole_body_joint = [
        num for key in ["leg", "head", "left_arm", "right_arm"]
        for num in chain_joints[key]
    ]
    move_status = robot.set_joint_positions(
        whole_body_joint,
        ["leg", "head", "left_arm", "right_arm"],
        [],
        True,
        0.1,
        30.0,
    )
    if move_status != gm.ControlStatus.SUCCESS:
        print(f"❌ Failed to move to the initial joint state: {move_status}")
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return
    print("✅ Moved to the initial whole-body joint state")
    time.sleep(3.0)

    base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
    custom_param = gm.Parameter()

    # NOTE: The two scenarios express the same physical waypoints as Cartesian
    # poses and joint positions, so their planning results are consistent. Setting
    # custom_param.set_move_line(True) applies Cartesian-space line planning to both.
    # 注意：以下两个场景分别使用笛卡尔位姿和关节位置表达相同的物理路点，
    # 因此规划结果一致。设置 custom_param.set_move_line(True) 后，二者均按
    # 笛卡尔空间直线方式规划。

    # Scenario 1: Multi-waypoint planning in Cartesian space (PoseState target)
    try:
        # Construct target pose
        target_pose_state = gm.PoseState()
        target_pose_state.chain_name = "left_arm"

        # Construct waypoints (3 intermediate poses)
        waypoint_poses = [
            [0.1297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968],
            [0.2297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968],
            [0.3297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968],
            [0.3297, 0.2608, 0.8417, 0.0734, 0.0209, -0.0233, 0.9968],
        ]

        status, traj = motion.motion_plan_multi_waypoints(
            target=target_pose_state,
            waypoint_poses=waypoint_poses,
            enable_collision_check=False,
            params=custom_param
        )
        printStatus(status)
        assert status == gm.MotionStatus.SUCCESS, "Cartesian multi-waypoint single-chain planning failed"
        if traj != {}:
            print(f"✅ Cartesian waypoint single-chain planning succeeded: trajectory points={len(traj[target_pose_state.chain_name])}")
            time.sleep(0.8)
        else:
            print(f"⚠️ Return status is SUCCESS, but trajectory is empty; possibly already reached, check whether the target matches current state or is within tolerance")
    except Exception as e:
        print(f"❌ Cartesian multi-point motion planning exception: {e}")

    # Scenario 2: Multi-waypoint planning in joint space (JointStates target)
    try:
        # Construct target pose
        target_joint = gm.JointStates()
        target_joint.chain_name = "left_arm"

        # Construct waypoints (3 intermediate poses)
        waypoints = [
            [2.0000, -1.5001, -0.6001, -1.7000, 0.0001, -0.7999, 0.0000],
            [1.7805, -1.4841, -0.5856, -1.6857, -0.0083, -0.5952, 0.0090],
            [1.5313, -1.4746, -0.5672, -1.5923, -0.0109, -0.4406, 0.0185],
            [1.6264, -1.4551, -0.5976, -1.9286, -0.0427, -0.1972, 0.0289]
        ]

        status, traj = motion.motion_plan_multi_waypoints(
            target=target_joint,
            waypoint_poses=waypoints,
            enable_collision_check=False,
            params=custom_param
        )
        printStatus(status)
        assert status == gm.MotionStatus.SUCCESS, "Cartesian multi-waypoint single-chain planning failed"
        if traj != {}:
            print(f"✅ Joint-waypoint single-chain planning succeeded: trajectory points={len(traj[target_joint.chain_name])}")
        else:
            print(f"⚠️ Return status is SUCCESS, but trajectory is empty; possibly already reached, check whether the target matches current state or is within tolerance")
    except Exception as e:
        print(f"❌ Joint-space multi-point motion planning exception: {e}")

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()


if __name__ == "__main__":
    main()

Collision Detection

Applicable Scenarios: Check whether a given joint configuration will cause self-collision or collision with environmental obstacles. Used for feasibility checking before motion planning.

examples/python/galbot_motion/check_collision.py

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot, GalbotNavigation   
import time

# NOTE:
# - GalbotMotion currently does NOT provide real-time obstacle perception / automatic environment updates.
# - Collision checking uses self-collision + the collision world built from objects you load manually via
#   add_obstacle()/attach_target_object() (including point clouds if you load them explicitly).

motion = GalbotMotion()
robot = GalbotRobot()
nav = GalbotNavigation()

def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Result: SUCCESS")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Result: TIMEOUT")
        elif(status == gm.MotionStatus.FAULT):
            print("Result: FAULT")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Result: INVALID_INPUT")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Result: INIT_FAILED")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Result: IN_PROGRESS")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Result: STOPPED_UNREACHED")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Result: DATA_FETCH_FAILED")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Result: PUBLISH_FAIL")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Result: COMM_DISCONNECTED")

if motion.init():
    print("GalbotMotion init OK")
else:
    print("GalbotMotion init FAILED")
if robot.init():
    print("GalbotRobot init OK")
else:
    print("GalbotRobot init FAILED")
if nav.init():
    print("GalbotNavigation init OK")
else:
    print("GalbotNavigation init FAILED")

# Wait for data to be ready.
time.sleep(3)

chain_joints = {
    "leg": [0.4992,1.4991,1.0005,0.0000,-0.0004],
    "head": [0.0000,0.0],
    "left_arm": [1.9999,-1.6000,-0.5999,-1.6999,0.0000,-0.7999,0.0000],
    "right_arm": [-2.0000,1.6001,0.6001,1.7000,0.0000,0.8000,0.0000]
}

whole_body_joint = [
    num for key in ["leg", "head", "left_arm", "right_arm"] 
    for num in chain_joints[key]
]
base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
custom_param = gm.Parameter()

try:
    # Build RobotStates list to check.
    check_states = [gm.RobotStates() for _ in range(2)]
    check_states[0].whole_body_joint = whole_body_joint
    check_states[0].base_state = base_state

    bad_left_arm_joint = [1.99995,-1.60004,0.599905,-1.69994,0,-0.799924,0]

    check_left_arm = gm.JointStates()
    check_left_arm.chain_name = "left_arm"
    check_left_arm.joint_positions = bad_left_arm_joint
    check_states[1] = check_left_arm

    status, collision_res = motion.check_collision(
        start=check_states,
        enable_collision_check=True
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Collision check failed"
    assert len(collision_res) == len(check_states), "Result size mismatch"
    print(f"OK: collision check finished: {collision_res} (False=no collision)")
except Exception as e:
    print(f"ERROR: collision check exception: {e}")

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Attach Tool

Applicable Scenarios: Attach a tool (such as a gripper, suction cup, etc. to the robot end effector, updating the motion planning model to account for tool mass and collision volume.

examples/python/galbot_motion/attach_tool.py

import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()

def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Execution result: SUCCESS, execution successful")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Execution result: TIMEOUT, execution timed out")
        elif(status == gm.MotionStatus.FAULT):
            print("Execution result: FAULT, a fault occurred and execution cannot continue")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Execution result: INIT_FAILED, failed to create internal communication components")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Execution result: IN_PROGRESS, in motion but not yet in position")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Execution result: PUBLISH_FAIL, data transmission failed")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Execution result: COMM_DISCONNECTED, connection failed")

if motion.init():
    print("GalbotMotion initialized successfully")
else:
    print("GalbotMotion initialization failed")
if robot.init():
    print("GalbotRobot initialized successfully")
else:
    print("GalbotRobot initialization failed")

# Program started, waiting for data
time.sleep(2)

try:
    chain_name = "left_arm"
    tool_name = "suction_cup"
    status = motion.attach_tool(
        chain=chain_name,
        tool=tool_name
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "load failed"
    print(f"✅ Tool attached successfully")

    # Clean up the attached tool model to avoid affecting subsequent examples.
    status = motion.detach_tool(chain=chain_name)
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "detach failed"
    print(f"✅ Tool detached successfully")
except Exception as e:
    print(f"❌ Tool operation exception: {e}")

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Detach Tool

Applicable Scenarios: Remove an attached tool from the robot end effector, restoring the original robot model.

examples/python/galbot_motion/detach_tool.py

import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()

def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Execution result: SUCCESS, execution successful")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Execution result: TIMEOUT, execution timed out")
        elif(status == gm.MotionStatus.FAULT):
            print("Execution result: FAULT, a fault occurred and execution cannot continue")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Execution result: INIT_FAILED, failed to create internal communication components")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Execution result: IN_PROGRESS, in motion but not yet in position")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Execution result: PUBLISH_FAIL, data transmission failed")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Execution result: COMM_DISCONNECTED, connection failed")

if motion.init():
    print("GalbotMotion initialized successfully")
else:
    print("GalbotMotion initialization failed")
if robot.init():
    print("GalbotRobot initialized successfully")
else:
    print("GalbotRobot initialization failed")

# Program started, waiting for data
time.sleep(2)

# 1. Detach tool
try:
    chain_name = "left_arm"
    status = motion.detach_tool(
        chain=chain_name
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "detach failed"
    print(f"✅ Tool detached successfully")
except Exception as e:
    print(f"❌ Tool detachment exception: {e}") 

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Get Link Names List

Applicable Scenarios: Get a list of names of all links in the robot model, used for collision detection and motion planning debugging.

examples/python/galbot_motion/get_link_names.py

import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()

if motion.init():
    print("GalbotMotion initialized successfully")
else:
    print("GalbotMotion initialization failed")
if robot.init():
    print("GalbotRobot initialized successfully")
else:
    print("GalbotRobot initialization failed")

# Program started, waiting for data
time.sleep(2)

try:
    # Get all link names
    all_link_names = motion.get_link_names(only_end_effector=False)
    print(f"\nAll link names (total {len(all_link_names)}):")
    for i, link_name in enumerate(all_link_names, 1):
        print(f"  {i}. {link_name}")

    # getend effectorexecute link
    ee_link_names = motion.get_link_names(only_end_effector=True)
    print(f"\nEnd-effector link names (total {len(ee_link_names)}):")
    for i, link_name in enumerate(ee_link_names, 1):
        print(f"  {i}. {link_name}")

except Exception as e:
    print(f"❌ Link-name retrieval exception: {e}")

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Add/Remove Environment Collision Objects

Applicable Scenarios: Add obstacles to the motion planning environment or remove added obstacles, enabling motion planning to account for environmental obstacles.

examples/python/galbot_motion/add_obstacle.py

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

# NOTE:
# - GalbotMotion currently does NOT provide real-time obstacle perception / automatic environment updates.
# - If you want Motion collision checking to consider obstacles (including point clouds), you must load them
#   manually via add_obstacle()/attach_target_object().

motion = GalbotMotion()
robot = GalbotRobot()

def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Result: SUCCESS")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Result: TIMEOUT")
        elif(status == gm.MotionStatus.FAULT):
            print("Result: FAULT")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Result: INVALID_INPUT")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Result: INIT_FAILED")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Result: IN_PROGRESS")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Result: STOPPED_UNREACHED")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Result: DATA_FETCH_FAILED")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Result: PUBLISH_FAIL")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Result: COMM_DISCONNECTED")

if motion.init():
    print("GalbotMotion init OK")
else:
    print("GalbotMotion init FAILED")
if robot.init():
    print("GalbotRobot init OK")
else:
    print("GalbotRobot init FAILED")

# Wait for data to be ready.
time.sleep(2)

# 1) Add a box collision object into Motion environment.
#    This affects Motion-side collision checking (e.g., motion_plan/check_collision).
try:
    obstacle_id = "box_test_1"
    obj_type = "box"
    obj_pose = [1.0, 0.0, 1.0, 0,0,0,1]
    obj_size = [1.0, 1.0, 1.0]
    target_frame = "world"
    status = motion.add_obstacle(
        obstacle_id=obstacle_id,
        obstacle_type=obj_type,
        pose=obj_pose,
        scale=obj_size,
        target_frame=target_frame
    )
    printStatus(status)
    motion.clear_obstacle()
    assert status == gm.MotionStatus.SUCCESS, "Failed to add obstacle"
    print(f"OK: added obstacle: {obstacle_id}")
except Exception as e:
    print(f"ERROR: add obstacle exception: {e}")

# 2) Add a duplicate ID (expected to fail).
try:
    obstacle_id = "box_test_1"
    obj_type = "box"
    obj_pose = [1.0, 0.0, 1.0, 0,0,0,1]
    obj_size = [1.0, 1.0, 1.0]
    target_frame = "world"
    status = motion.add_obstacle(
        obstacle_id=obstacle_id,
        obstacle_type=obj_type,
        pose=obj_pose,
        scale=obj_size,
        target_frame=target_frame
    )
    status = motion.add_obstacle(
        obstacle_id=obstacle_id,
        obstacle_type=obj_type,
        pose=obj_pose,
        scale=obj_size,
        target_frame=target_frame
    )
    printStatus(status)
    motion.clear_obstacle()
    assert status == gm.MotionStatus.FAULT, "Expected duplicate obstacle ID to fail"
    print("OK: duplicate obstacle ID is rejected")
except Exception as e:
    print(f"ERROR: duplicate obstacle exception: {e}")

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Add Multiple Obstacles (add_obstacles)

Applicable Scenarios: Add several environment collision objects in one operation before motion planning.

examples/python/galbot_motion/add_multi_obstacle.py

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

# NOTE:
# - GalbotMotion currently does NOT provide real-time obstacle perception / automatic environment updates.
# - If you want Motion collision checking to consider obstacles (including point clouds), you must load them
#   manually via add_obstacle()/attach_target_object().

motion = GalbotMotion()
robot = GalbotRobot()

def printStatus(status):
        if(status == gm.MotionStatus.SUCCESS):
            print("Result: SUCCESS")
        elif(status == gm.MotionStatus.TIMEOUT):
            print("Result: TIMEOUT")
        elif(status == gm.MotionStatus.FAULT):
            print("Result: FAULT")
        elif(status == gm.MotionStatus.INVALID_INPUT):
            print("Result: INVALID_INPUT")
        elif(status == gm.MotionStatus.INIT_FAILED):
            print("Result: INIT_FAILED")
        elif(status == gm.MotionStatus.IN_PROGRESS):
            print("Result: IN_PROGRESS")
        elif(status == gm.MotionStatus.STOPPED_UNREACHED):
            print("Result: STOPPED_UNREACHED")
        elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
            print("Result: DATA_FETCH_FAILED")
        elif(status == gm.MotionStatus.PUBLISH_FAIL):
            print("Result: PUBLISH_FAIL")
        elif(status == gm.MotionStatus.COMM_DISCONNECTED):
            print("Result: COMM_DISCONNECTED")

if motion.init():
    print("GalbotMotion init OK")
else:
    print("GalbotMotion init FAILED")
if robot.init():
    print("GalbotRobot init OK")
else:
    print("GalbotRobot init FAILED")

# Wait for data to be ready.
time.sleep(2)

# 1) Add a box collision object into Motion environment.
#    This affects Motion-side collision checking (e.g., motion_plan/check_collision).
try:
    obstacle_id = "box_test_1"
    obj_type = "box"
    obj_pose = [1.0, 0.0, 1.0, 0,0,0,1]
    obj_size = [1.0, 1.0, 1.0]
    target_frame = "world"
    status = motion.add_obstacle(
        obstacle_id=obstacle_id,
        obstacle_type=obj_type,
        pose=obj_pose,
        scale=obj_size,
        target_frame=target_frame
    )
    printStatus(status)
    motion.clear_obstacle()
    assert status == gm.MotionStatus.SUCCESS, "Failed to add obstacle"
    print(f"OK: added obstacle: {obstacle_id}")
except Exception as e:
    print(f"ERROR: add obstacle exception: {e}")

# 2) Add a sphere collision object into Motion environment.
#    This affects Motion-side collision checking (e.g., motion_plan/check_collision).
try:
    obstacle_id = "sphere_1"
    obj_type = "sphere"
    obj_pose = [2.0, 0.0, 1.0, 0,0,0,1]
    obj_size = [1.0,0,0]
    target_frame = "world"
    status = motion.add_obstacle(
        obstacle_id=obstacle_id,
        obstacle_type=obj_type,
        pose=obj_pose,
        scale=obj_size,
        target_frame=target_frame
    )
    printStatus(status)
    motion.clear_obstacle()
    assert status == gm.MotionStatus.SUCCESS, "Failed to add obstacle"
    print(f"OK: added obstacle: {obstacle_id}")
except Exception as e:
    print(f"ERROR: add obstacle exception: {e}")


# 3) Add a cylinder collision object into Motion environment.
#    This affects Motion-side collision checking (e.g., motion_plan/check_collision).
try:
    obstacle_id = "cylinder_1"
    obj_type = "cylinder"
    obj_pose = [3.0, 0.0, 1.0, 0,0,0,1]
    obj_size = [1.0,1.0,0]
    target_frame = "world"
    status = motion.add_obstacle(
        obstacle_id=obstacle_id,
        obstacle_type=obj_type,
        pose=obj_pose,
        scale=obj_size,
        target_frame=target_frame
    )
    printStatus(status)
    motion.clear_obstacle()
    assert status == gm.MotionStatus.SUCCESS, "Failed to add obstacle"
    print(f"OK: added obstacle: {obstacle_id}")
except Exception as e:
    print(f"ERROR: add obstacle exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Detailed Collision Check (check_collision_detail)

Applicable Scenarios: Inspect detailed collision results and identify the robot links or environment objects involved.

examples/python/galbot_motion/check_collision_detail.py

import sys

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotNavigation, GalbotRobot


def print_collision_details(motion, title, status, collision_infos):
    print(f"{title} status: {motion.status_to_string(status)}")
    if status != gm.MotionStatus.SUCCESS:
        return
    if not collision_infos:
        print("  no collision pair returned")
        return
    for i, info in enumerate(collision_infos):
        label = "COLLISION" if info.is_collision else "NO COLLISION"
        print(
            f"  - pair [{i}]: {label}, {info.link1} <-> {info.link2}, "
            f"distance: {info.distance}, type: {info.collision_type}"
        )


motion = GalbotMotion()
robot = GalbotRobot()
navigation = GalbotNavigation()

if not motion.init():
    print("GalbotMotion init FAILED", file=sys.stderr)
    sys.exit(1)
if not robot.init():
    print("GalbotRobot init FAILED", file=sys.stderr)
    sys.exit(1)
if not navigation.init():
    print("GalbotNavigation init FAILED", file=sys.stderr)
    sys.exit(1)

params = gm.Parameter()
params.set_timeout(5.0)

try:
    print(">> Detailed collision check: current robot state")
    status, collision_infos = motion.check_collision_detail(
        robot_states=[],
        is_check_once=False,
        is_log=False,
        params=params,
    )
    print_collision_details(motion, "current robot state", status, collision_infos)

    chain_joints = {
        "leg": [0.4992, 1.4991, 1.0005, 0.0000, -0.0004],
        "head": [0.0000, 0.0],
        "left_arm": [2.0, -1.6, -0.6, -1.7, 0.0, -0.8, 0.0],
        "right_arm": [-2.0, 1.6, 0.6, 1.7, 0.0, 0.8, 0.0],
    }
    whole_body_joint = [num for key in ["leg", "head", "left_arm", "right_arm"] for num in chain_joints[key]]

    whole_body_state = gm.RobotStates()
    whole_body_state.whole_body_joint = whole_body_joint
    whole_body_state.base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]

    left_arm_state = gm.JointStates()
    left_arm_state.chain_name = "left_arm"
    left_arm_state.joint_positions = [2.0, -1.6, 0.6, -1.7, 0.0, -0.8, 0.0]

    print(">> Detailed collision check: explicit robot states")
    status, collision_infos = motion.check_collision_detail(
        robot_states=[whole_body_state, left_arm_state],
        is_check_once=False,
        is_log=True,
        params=params,
    )
    print_collision_details(motion, "explicit robot states", status, collision_infos)
except Exception as e:
    print(f"ERROR: check_collision_detail exception: {e}", file=sys.stderr)

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Combined Motion Planning (combine_plan)

Applicable Scenarios: Combine multiple chain targets into one coordinated planning request.

examples/python/galbot_motion/combine_plan.py

"""G1 combine_plan example aligned with combine_plan_example.cpp."""

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

START_JOINT_STATE = {
    "leg": [0.499191, 1.49907, 1.00046, 0.0, 0.0],
    "head": [0.0, 0.00989756],
    "left_arm": [1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
    "right_arm": [-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0],
}
START_MOVE_SPEED_RAD_S = 0.12
START_MOVE_TIMEOUT_S = 30.0
UPPER_BODY_GROUPS = ["head", "left_arm", "right_arm"]


def initialize_interfaces():
    motion = GalbotMotion()
    print("GalbotMotion::get_instance() status: no return status; call completed.")
    robot = GalbotRobot()
    print("GalbotRobot::get_instance() status: no return status; call completed.")
    if not motion.init():
        print("GalbotMotion::init() status: FAILED")
        return None, None
    print("GalbotMotion::init() status: SUCCESS")
    if not robot.init():
        print("GalbotRobot::init() status: FAILED")
        return None, None
    print("GalbotRobot::init() status: SUCCESS")
    time.sleep(1.0)
    return motion, robot


def shutdown_robot(robot):
    robot.request_shutdown()
    print("request_shutdown() status: no return status; call completed.")
    robot.wait_for_shutdown()
    print("wait_for_shutdown() status: no return status; call completed.")
    robot.destroy()
    print("destroy() status: no return status; call completed.")


def print_control_status(api_name, status):
    result = "SUCCESS" if status == gm.ControlStatus.SUCCESS else "FAILED"
    print(f"{api_name} status: {str(status).rsplit('.', 1)[-1]} ({result})")


def move_robot_to_start_state(robot):
    print("Move robot to combine_plan start state.")
    print(f"  leg start joints: {START_JOINT_STATE['leg']}")
    # controller_status = robot.switch_controller(gm.G1ControllerName.LEG_PVT_CTRL)
    # print_control_status("GalbotRobot::switch_controller(leg_pvt_ctrl)", controller_status)
    # if controller_status != gm.ControlStatus.SUCCESS:
    #     return False
    # time.sleep(0.5)
    leg_status = robot.set_joint_positions(
        START_JOINT_STATE["leg"], ["leg"], [], True,
        START_MOVE_SPEED_RAD_S, START_MOVE_TIMEOUT_S
    )
    print_control_status("GalbotRobot::set_joint_positions(leg start)", leg_status)
    if leg_status != gm.ControlStatus.SUCCESS:
        return False
    upper_body = (
        START_JOINT_STATE["head"]
        + START_JOINT_STATE["left_arm"]
        + START_JOINT_STATE["right_arm"]
    )
    print(f"  upper-body start joints [head, left_arm, right_arm]: {upper_body}")
    status = robot.set_joint_positions(
        upper_body, UPPER_BODY_GROUPS, [], True,
        START_MOVE_SPEED_RAD_S, START_MOVE_TIMEOUT_S
    )
    print_control_status("GalbotRobot::set_joint_positions(head+arms start)", status)
    return status == gm.ControlStatus.SUCCESS


def capture_current_pose(motion, robot, chain, current):
    joint_names = list(motion.get_chain_joint_names(chain))
    print(f"get_chain_joint_names({chain}) status: {'SUCCESS' if joint_names else 'FAILED'}")
    if not joint_names:
        return False
    joints = list(robot.get_joint_positions([], joint_names))
    success = len(joints) == len(joint_names)
    print(f"GalbotRobot::get_joint_positions({chain}) status: {'SUCCESS' if success else 'FAILED'}")
    if not success:
        return False
    current["joint_names"][chain] = joint_names
    current["joints"][chain] = joints
    print(f"Current {chain} joints: {joints}")
    status, pose = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link")
    result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
    print(f"get_end_effector_pose_on_chain({chain}) status: {motion.status_to_string(status)} ({result})")
    pose = list(pose)
    if status != gm.MotionStatus.SUCCESS or len(pose) != 7:
        return False
    current["poses"][chain] = pose
    print(f"Current {chain} pose [x, y, z, qx, qy, qz, qw]: {pose}")
    return True


def make_joint_target(chain, joint_positions, joint_names=None):
    target = gm.MotionPlanChainTarget()
    target.chain_name = chain
    target.mode = gm.MotionPlanTargetMode.kJoint
    target.joint.chain_name = chain
    target.joint.joint_positions = list(joint_positions)
    target.joint.joint_names = list(joint_names or [])
    return target


def make_cartesian_target(chain, pose, assist_chains=None):
    target = gm.MotionPlanChainTarget()
    target.chain_name = chain
    target.mode = gm.MotionPlanTargetMode.kCartesian
    target.cart.chain_name = chain
    target.cart.frame_id = "EndEffector"
    target.cart.reference_frame = "base_link"
    target.cart.pose = gm.Pose(list(pose))
    target.cart.assist_chains = set(assist_chains or [])
    return target


def make_planner_config(check_collision):
    params = gm.Parameter()
    params.set_direct_execute(True)
    params.set_blocking(True)
    params.set_timeout(120.0)
    params.set_check_collision(check_collision)
    params.set_reference_frame("base_link")
    params.set_actuate("with_torso")
    return params


def print_plan_requests(plan_requests):
    plan_type_names = {
        gm.MotionPlanType.MOTION_PLAN: "MOTION_PLAN",
        gm.MotionPlanType.TRAJ_PLAN: "TRAJ_PLAN",
        gm.MotionPlanType.MOVE_LINE: "MOVE_LINE",
    }
    print(f"combine_plan plan requests: {len(plan_requests)}")
    for request_index, request in enumerate(plan_requests):
        print(
            f"  request[{request_index}] type={plan_type_names.get(request.plan_type, 'UNKNOWN')} "
            f"waypoints={len(request.target)}"
        )
        for waypoint_index, waypoint in enumerate(request.target):
            print(f"    waypoint[{waypoint_index}] targets: {len(waypoint)}")
            for target in waypoint:
                mode = "JOINT" if target.mode == gm.MotionPlanTargetMode.kJoint else "CART"
                print(f"      {target.chain_name} {mode}")


def print_traj_result(label, status, traj, motion):
    result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
    print(f"{label} status: {motion.status_to_string(status)} ({result})")
    if status == gm.MotionStatus.SUCCESS:
        if not traj:
            print("Trajectory map is empty.")
        for chain, points in traj.items():
            print(f"  {chain} trajectory points: {len(points)}")


def make_plan_request(plan_type, target):
    request = gm.PlanRequest()
    request.plan_type = plan_type
    request.enforce_pass = True
    request.target = target
    return request


def make_example_inputs(current):
    left_names = current["joint_names"]["left_arm"]
    right_names = current["joint_names"]["right_arm"]
    cart = lambda chain, pose: make_cartesian_target(chain, pose, ["torso"])

    request0 = make_plan_request(
        gm.MotionPlanType.MOTION_PLAN,
        [
            [
                make_joint_target(
                    "left_arm",
                    [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
                    left_names,
                ),
                cart("right_arm", [0.22666, -0.23435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
            ],
            [
                make_joint_target(
                    "left_arm",
                    [1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
                    left_names,
                )
            ],
            [
                make_joint_target(
                    "left_arm",
                    [1.99995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
                    left_names,
                ),
                cart("right_arm", [0.12666, -0.23435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
            ],
        ],
    )

    request1 = make_plan_request(
        gm.MotionPlanType.MOVE_LINE,
        [
            [
                cart("left_arm", [0.42666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
                cart("right_arm", [0.422666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
            ],
            [cart("left_arm", [0.12666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0])],
            [cart("right_arm", [0.12666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0])],
        ],
    )

    request2 = make_plan_request(
        gm.MotionPlanType.MOTION_PLAN,
        [
            [
                make_joint_target(
                    "left_arm",
                    [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
                    left_names,
                )
            ],
            [
                make_joint_target(
                    "right_arm",
                    [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0],
                    right_names,
                )
            ],
            [make_joint_target("torso", [0.0, 0.0], ["leg_joint4", "leg_joint5"])],
        ],
    )

    plan_requests = [request0, request1, request2]
    return plan_requests, make_planner_config(check_collision=True)


def run_example():
    motion, robot = initialize_interfaces()
    if motion is None or robot is None:
        return 1

    try:
        if not move_robot_to_start_state(robot):
            return 1
        time.sleep(0.5)

        current = {"joint_names": {}, "joints": {}, "poses": {}}
        if not capture_current_pose(motion, robot, "left_arm", current):
            return 1
        if not capture_current_pose(motion, robot, "right_arm", current):
            return 1

        plan_requests, params = make_example_inputs(current)
        print_plan_requests(plan_requests)
        print(
            "Immediate execution is enabled; plan requests are loaded from "
            "combine_file_example_plan_reqs.json reference values."
        )
        status, traj = motion.combine_plan(plan_requests, params)
        print_traj_result("combine_plan", status, traj, motion)
        return 0 if status == gm.MotionStatus.SUCCESS else 1
    finally:
        shutdown_robot(robot)


if __name__ == "__main__":
    raise SystemExit(run_example())

Get Chain Joint Names (get_chain_joint_names)

Applicable Scenarios: Query the ordered joint names belonging to a kinematic chain before constructing joint states or targets.

examples/python/galbot_motion/get_chain_joint_names.py

import time

from galbot_sdk.g1 import GalbotMotion, GalbotRobot


def run_example():
    motion = GalbotMotion()
    robot = GalbotRobot()
    # Initialize SDK interfaces directly in the example flow.
    if not motion.init():
        print("GalbotMotion init FAILED")
        return 1
    if not robot.init():
        print("GalbotRobot init FAILED")
        return 1
    print("GalbotMotion/GalbotRobot init OK")
    time.sleep(1)

    # Include common G1 chains and one invalid name to show the empty-result behavior.
    for chain in ["left_arm", "right_arm", "leg", "head", "invalid_chain"]:
        # This is the API being demonstrated: query joint names by chain name.
        joint_names = list(motion.get_chain_joint_names(chain))
        print(f"get_chain_joint_names({chain}): {joint_names}")

    # Clean shutdown releases SDK resources before the process exits.
    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    return 0


if __name__ == "__main__":
    raise SystemExit(run_example())

Get Motion Configuration (get_config_by_type)

Applicable Scenarios: Read the current motion configuration for a specified configuration type.

examples/python/galbot_motion/get_config_by_type.py

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot


def run_example():
    motion = GalbotMotion()
    robot = GalbotRobot()
    # Initialize SDK interfaces directly in the example flow.
    if not motion.init():
        print("GalbotMotion init FAILED")
        return 1
    if not robot.init():
        print("GalbotRobot init FAILED")
        return 1
    print("GalbotMotion/GalbotRobot init OK")
    time.sleep(1)

    # These are common motion-planning service config categories.
    config_types = [
        "sampler",
        "ik_solver",
        "trajectory_planner",
        "traj_validity_checker",
    ]

    all_success = True
    for config_type in config_types:
        # This is the API being demonstrated: query one config payload by type.
        status, config = motion.get_config_by_type(config_type)
        print(
            f"get_config_by_type({config_type}) status: {motion.status_to_string(status)}, "
            f"payload bytes: {len(config.common_str)}"
        )
        all_success = all_success and status == gm.MotionStatus.SUCCESS

    # Clean shutdown releases SDK resources before the process exits.
    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    return 0 if all_success else 1


if __name__ == "__main__":
    raise SystemExit(run_example())

Get Jacobian Matrix (get_jacobian)

Applicable Scenarios: Calculate an end-effector Jacobian for differential kinematics and velocity analysis.

examples/python/galbot_motion/get_jacobian.py

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot


def print_status(status):
    labels = {
        gm.MotionStatus.SUCCESS: "SUCCESS",
        gm.MotionStatus.TIMEOUT: "TIMEOUT",
        gm.MotionStatus.FAULT: "FAULT",
        gm.MotionStatus.INVALID_INPUT: "INVALID_INPUT",
        gm.MotionStatus.INIT_FAILED: "INIT_FAILED",
        gm.MotionStatus.IN_PROGRESS: "IN_PROGRESS",
        gm.MotionStatus.STOPPED_UNREACHED: "STOPPED_UNREACHED",
        gm.MotionStatus.DATA_FETCH_FAILED: "DATA_FETCH_FAILED",
        gm.MotionStatus.PUBLISH_FAIL: "PUBLISH_FAIL",
        gm.MotionStatus.COMM_DISCONNECTED: "COMM_DISCONNECTED",
    }
    return labels.get(status, "UNKNOWN")


def print_vector(values):
    print("[", end="")
    for index, value in enumerate(values):
        if index > 0:
            print(", ", end="")
        print(value, end="")
    print("]", end="")


def print_joint_state(joint_state):
    print("{")
    for chain_name, joints in joint_state.items():
        print(f"    {chain_name}: ", end="")
        print_vector(joints)
        print()
    print("  }")


def print_get_jacobian_params(
    chain_name,
    target_frame,
    reference_frame,
    joint_state,
):
    print("get_jacobian parameters:")
    print(f"  chain_name: {chain_name}")
    print(f"  target_frame: {target_frame}")
    print(f"  reference_frame: {reference_frame}")
    print("  joint_state: ", end="")
    print_joint_state(joint_state)


def print_jacobian(label, result):
    status, matrix = result
    print(f"[{label}] Status: {print_status(status)}")
    if status != gm.MotionStatus.SUCCESS or not matrix:
        print("Jacobian computation failed or returned an empty matrix.")
        return

    rows = len(matrix)
    cols = len(matrix[0])
    print(f"Jacobian matrix ({rows}x{cols}):")
    for row_index, row in enumerate(matrix):
        row_str = f"  row {row_index}:"
        for value in row:
            row_str += f" {value:10.5f}"
        print(row_str)


def make_fixed_joint_values(dof):
    fixed_values = [-1.2, -0.8, -0.4, 0.0, 0.4, 0.8, 1.2]
    return [fixed_values[index % len(fixed_values)] for index in range(dof)]


def run_example():
    motion = GalbotMotion()
    robot = GalbotRobot()
    robot_initialized = False

    if not motion.init():
        print("GalbotMotion initialization failed")
        return 1
    print("GalbotMotion initialized successfully")

    if not robot.init():
        print("GalbotRobot initialization failed")
        return 1
    robot_initialized = True
    print("GalbotRobot initialized successfully")

    try:
        time.sleep(3)

        support_chains = set(motion.get_supported_chains())
        print(f"support_chains ({len(support_chains)}): {sorted(support_chains)}")
        # support_chains.discard("torso")
        if not support_chains:
            support_chains = {"head", "left_arm", "right_arm", "torso", "leg"}

        try:
            current_chain_joint_state = motion.get_chain_joint_state()
        except Exception as e:
            print(f"get_chain_joint_state exception: {e}")
            current_chain_joint_state = {}

        target_frame = "EndEffector"
        reference_frame = "base_link"

        # Known chain DOF used only when the runtime joint state is unavailable.
        fallback_dof = {"head": 2, "left_arm": 7, "right_arm": 7, "leg": 5}

        for chain_name in sorted(support_chains):
            # Prefer the runtime joint count; fall back to a known DOF when it is unavailable.
            current_joints = current_chain_joint_state.get(chain_name, [])
            if current_joints:
                dof = len(current_joints)
            else:
                dof = fallback_dof.get(chain_name, 0)
            if dof == 0:
                print(f"Skip chain '{chain_name}': unable to determine chain DOF.")
                continue

            joint_state = {chain_name: make_fixed_joint_values(dof)}

            try:
                print(f"=== get_jacobian: {chain_name} ===")
                print_get_jacobian_params(
                    chain_name,
                    target_frame,
                    reference_frame,
                    joint_state,
                )
                result = motion.get_jacobian(
                    chain_name,
                    target_frame,
                    reference_frame,
                    joint_state,
                )
                print_jacobian(chain_name, result)
            except Exception as e:
                print(f"{chain_name} exception: {e}")

        return 0
    finally:
        if robot_initialized:
            robot.request_shutdown()
            robot.wait_for_shutdown()
            robot.destroy()


if __name__ == "__main__":
    raise SystemExit(run_example())

Get Jacobian from Robot State (get_jacobian_by_state)

Applicable Scenarios: Calculate a Jacobian at an explicitly supplied robot state instead of the current state.

examples/python/galbot_motion/get_jacobian_by_state.py

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot


def print_status(status):
    labels = {
        gm.MotionStatus.SUCCESS: "SUCCESS",
        gm.MotionStatus.TIMEOUT: "TIMEOUT",
        gm.MotionStatus.FAULT: "FAULT",
        gm.MotionStatus.INVALID_INPUT: "INVALID_INPUT",
        gm.MotionStatus.INIT_FAILED: "INIT_FAILED",
        gm.MotionStatus.IN_PROGRESS: "IN_PROGRESS",
        gm.MotionStatus.STOPPED_UNREACHED: "STOPPED_UNREACHED",
        gm.MotionStatus.DATA_FETCH_FAILED: "DATA_FETCH_FAILED",
        gm.MotionStatus.PUBLISH_FAIL: "PUBLISH_FAIL",
        gm.MotionStatus.COMM_DISCONNECTED: "COMM_DISCONNECTED",
    }
    return labels.get(status, "UNKNOWN")


def print_vector(values):
    print("[", end="")
    for index, value in enumerate(values):
        if index > 0:
            print(", ", end="")
        print(value, end="")
    print("]", end="")


def print_robot_states(robot_states):
    if robot_states is None:
        print("None (use current complete robot state)")
        return

    whole_body_joint = list(getattr(robot_states, "whole_body_joint", []))
    base_state = list(getattr(robot_states, "base_state", []))
    print("RobotStates")
    print(f"    whole_body_joint[{len(whole_body_joint)}]: ", end="")
    print_vector(whole_body_joint)
    print()
    print(f"    base_state[{len(base_state)}]: ", end="")
    print_vector(base_state)
    print()


def print_get_jacobian_by_state_params(
    chain_name,
    target_frame,
    reference_frame,
    reference_robot_states,
):
    print("get_jacobian_by_state parameters:")
    print(f"  chain_name: {chain_name}")
    print(f"  target_frame: {target_frame}")
    print(f"  reference_frame: {reference_frame}")
    print("  reference_robot_states: ", end="")
    print_robot_states(reference_robot_states)


def print_jacobian(label, result):
    status, matrix = result
    print(f"[{label}] Status: {print_status(status)}")
    if status != gm.MotionStatus.SUCCESS or not matrix:
        print("Jacobian computation failed or returned an empty matrix.")
        return

    rows = len(matrix)
    cols = len(matrix[0])
    print(f"Jacobian matrix ({rows}x{cols}):")
    for row_index, row in enumerate(matrix):
        row_str = f"  row {row_index}:"
        for value in row:
            row_str += f" {value:10.5f}"
        print(row_str)


def make_fixed_whole_body_joint(dof):
    fixed_values = [
        0.3,
        0.8,
        0.5,
        0.0,
        0.0,
        0.1,
        -0.2,
        1.2,
        -0.8,
        0.4,
        -1.0,
        0.2,
        -0.6,
        0.0,
        -1.2,
        0.8,
        -0.4,
        1.0,
        -0.2,
        0.6,
        0.0,
    ]
    return [fixed_values[index % len(fixed_values)] for index in range(dof)]


def run_example():
    motion = GalbotMotion()
    robot = GalbotRobot()
    robot_initialized = False

    if not motion.init():
        print("GalbotMotion initialization failed")
        return 1
    print("GalbotMotion initialized successfully")

    if not robot.init():
        print("GalbotRobot initialization failed")
        return 1
    robot_initialized = True
    print("GalbotRobot initialized successfully")

    try:
        time.sleep(3)

        support_chains = set(motion.get_supported_chains())
        print(f"support_chains ({len(support_chains)}): {sorted(support_chains)}")
        if not support_chains:
            support_chains = {"head", "left_arm", "right_arm", "torso", "leg"}

        target_frame = "EndEffector"
        reference_frame = "base_link"

        whole_body_dof = 21
        try:
            current_robot_state = motion.get_robot_states()
            if getattr(current_robot_state, "whole_body_joint", []):
                whole_body_dof = len(current_robot_state.whole_body_joint)
        except Exception as e:
            print(f"get_robot_states exception: {e}")

        reference_robot_states = gm.RobotStates()
        reference_robot_states.whole_body_joint = make_fixed_whole_body_joint(
            whole_body_dof
        )
        reference_robot_states.base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]

        # Compute the Jacobian for every chain using an explicit RobotStates
        for chain_name in sorted(support_chains):
            try:
                print(
                    f"=== get_jacobian_by_state (explicit RobotStates): {chain_name} ==="
                )
                print_get_jacobian_by_state_params(
                    chain_name,
                    target_frame,
                    reference_frame,
                    reference_robot_states,
                )
                result = motion.get_jacobian_by_state(
                    chain_name,
                    target_frame,
                    reference_frame,
                    reference_robot_states,
                )
                print_jacobian(f"{chain_name} explicit RobotStates", result)
            except Exception as e:
                print(f"{chain_name} explicit RobotStates exception: {e}")

        # Compute the Jacobian for every chain using None (the current robot state)
        for chain_name in sorted(support_chains):
            try:
                print(f"=== get_jacobian_by_state (None RobotStates): {chain_name} ===")
                print_get_jacobian_by_state_params(
                    chain_name,
                    target_frame,
                    reference_frame,
                    None,
                )
                result = motion.get_jacobian_by_state(
                    chain_name,
                    target_frame,
                    reference_frame,
                    None,
                )
                print_jacobian(f"{chain_name} None RobotStates", result)
            except Exception as e:
                print(f"{chain_name} None RobotStates exception: {e}")

        return 0
    finally:
        if robot_initialized:
            robot.request_shutdown()
            robot.wait_for_shutdown()
            robot.destroy()


if __name__ == "__main__":
    raise SystemExit(run_example())

General Inverse Kinematics

Applicable Scenarios: Solve inverse kinematics using general target and constraint configuration for more complex planning tasks.

examples/python/galbot_motion/inverse_kinematics_general.py

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot


def make_pose_target_from_current(current_pose, axis=0):
    # Pose is [x, y, z, qx, qy, qz, qw]; move one translation axis only.
    target = list(current_pose)
    if len(target) == 7 and axis < 3:
        target[axis] += max(abs(target[axis]) * 0.1, 0.02)
    return target


def make_cartesian_target(chain, pose, assist_chains=None):
    target = gm.MotionPlanChainTarget()
    target.chain_name = chain
    target.mode = gm.MotionPlanTargetMode.kCartesian
    # frame_id is the end-effector frame; reference_frame is the IK reference frame.
    target.cart.chain_name = chain
    target.cart.frame_id = "EndEffector"
    target.cart.reference_frame = "base_link"
    target.cart.pose = gm.Pose(list(pose))
    target.cart.assist_chains = set(assist_chains or [])
    return target


def run_example():
    motion = GalbotMotion()
    robot = GalbotRobot()
    # Initialize SDK interfaces directly in the example flow.
    if not motion.init():
        print("GalbotMotion init FAILED")
        return 1
    if not robot.init():
        print("GalbotRobot init FAILED")
        return 1
    print("GalbotMotion/GalbotRobot init OK")
    time.sleep(1)

    # Read the current end-effector pose first; the IK target is a small relative move.
    pose_status, current_pose = motion.get_end_effector_pose_on_chain(
        chain_name="left_arm",
        frame_id="EndEffector",
        reference_frame="base_link",
    )
    print(f"get_end_effector_pose_on_chain(left_arm) status: {motion.status_to_string(pose_status)}")
    if pose_status != gm.MotionStatus.SUCCESS:
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return 1

    current_pose = list(current_pose)
    target_pose = make_pose_target_from_current(current_pose)
    print(f"Current left_arm pose: {current_pose}")
    print(f"Target left_arm pose (+10% x translation): {target_pose}")

    # inverse_kinematics_general accepts the same waypoint target format as planning APIs.
    target_waypoint = [
        make_cartesian_target("left_arm", target_pose, ["leg"]),
    ]

    # Use the current whole-body state as the IK reference seed when it is available.
    reference_state = motion.get_robot_states()
    if not getattr(reference_state, "whole_body_joint", []):
        reference_state = None

    # IK uses Parameter for common options such as blocking, timeout, and reference frame.
    params = gm.Parameter()
    params.set_direct_execute(False)
    params.set_blocking(True)
    params.set_timeout(20.0)
    params.set_check_collision(False)
    params.set_reference_frame("base_link")

    # This is the API being demonstrated: solve joint values for the target waypoint.
    status, joint_map = motion.inverse_kinematics_general(
        target_waypoint=target_waypoint,
        reference_robot_states=reference_state,
        params=params,
    )
    print(f"inverse_kinematics_general status: {motion.status_to_string(status)}")
    for chain, joints in joint_map.items():
        print(f"  {chain} joint_names: {list(joints.joint_names)}")
        print(f"  {chain} joint_positions: {list(joints.joint_positions)}")

    # Clean shutdown releases SDK resources before the process exits.
    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    return 0 if status == gm.MotionStatus.SUCCESS else 1


if __name__ == "__main__":
    raise SystemExit(run_example())

Linear Cartesian Motion (move_line)

Applicable Scenarios: Plan and execute a straight-line end-effector movement in Cartesian space.

examples/python/galbot_motion/move_line.py

"""G1 move_line example aligned with move_line_example.cpp."""

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

def initialize_interfaces():
    motion = GalbotMotion()
    print("GalbotMotion::get_instance() status: no return status; call completed.")
    robot = GalbotRobot()
    print("GalbotRobot::get_instance() status: no return status; call completed.")
    if not motion.init():
        print("GalbotMotion::init() status: FAILED")
        return None, None
    print("GalbotMotion::init() status: SUCCESS")
    if not robot.init():
        print("GalbotRobot::init() status: FAILED")
        return None, None
    print("GalbotRobot::init() status: SUCCESS")
    time.sleep(1.0)
    return motion, robot


def shutdown_robot(robot):
    robot.request_shutdown()
    print("request_shutdown() status: no return status; call completed.")
    robot.wait_for_shutdown()
    print("wait_for_shutdown() status: no return status; call completed.")
    robot.destroy()
    print("destroy() status: no return status; call completed.")


def print_control_status(api_name, status):
    result = "SUCCESS" if status == gm.ControlStatus.SUCCESS else "FAILED"
    print(f"{api_name} status: {str(status).rsplit('.', 1)[-1]} ({result})")


def move_robot_to_home_position(robot):
    print("Move robot to the G1 home position.")
    status = robot.set_joint_positions(
        [
            0.5, 1.5, 1.0, 0.0, 0.0,
            0.0, 0.0,
            2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
            -2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0,
        ],
        ["leg", "head", "left_arm", "right_arm"],
        [],
        True,
        0.1,
        30.0,
    )
    print_control_status("GalbotRobot::set_joint_positions(home)", status)
    return status == gm.ControlStatus.SUCCESS


def capture_current_pose(motion, robot, chain, current):
    joint_names = list(motion.get_chain_joint_names(chain))
    print(f"get_chain_joint_names({chain}) status: {'SUCCESS' if joint_names else 'FAILED'}")
    if not joint_names:
        return False
    joints = list(robot.get_joint_positions([], joint_names))
    success = len(joints) == len(joint_names)
    print(f"GalbotRobot::get_joint_positions({chain}) status: {'SUCCESS' if success else 'FAILED'}")
    if not success:
        return False
    current["joint_names"][chain] = joint_names
    current["joints"][chain] = joints
    print(f"Current {chain} joints: {joints}")
    status, pose = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link")
    result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
    print(f"get_end_effector_pose_on_chain({chain}) status: {motion.status_to_string(status)} ({result})")
    pose = list(pose)
    if status != gm.MotionStatus.SUCCESS or len(pose) != 7:
        return False
    current["poses"][chain] = pose
    print(f"Current {chain} pose [x, y, z, qx, qy, qz, qw]: {pose}")
    return True


def make_cartesian_target(chain, pose, assist_chains=None):
    target = gm.MotionPlanChainTarget()
    target.chain_name = chain
    target.mode = gm.MotionPlanTargetMode.kCartesian
    target.cart.chain_name = chain
    target.cart.frame_id = "EndEffector"
    target.cart.reference_frame = "base_link"
    target.cart.pose = gm.Pose(list(pose))
    target.cart.assist_chains = set(assist_chains or [])
    return target


def make_planner_config(check_collision):
    params = gm.Parameter()
    params.set_direct_execute(True)
    params.set_blocking(True)
    params.set_timeout(120.0)
    params.set_check_collision(check_collision)
    params.set_reference_frame("base_link")
    params.set_actuate("with_torso")
    return params


def print_waypoints(label, waypoints):
    print(f"{label} waypoints: {len(waypoints)}")
    for index, waypoint in enumerate(waypoints):
        print(f"  waypoint[{index}] targets: {len(waypoint)}")
        for target in waypoint:
            print(f"    {target.chain_name} CART frame_id={target.cart.frame_id} reference_frame={target.cart.reference_frame}")


def print_traj_result(label, status, traj, motion):
    result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
    print(f"{label} status: {motion.status_to_string(status)} ({result})")
    if status == gm.MotionStatus.SUCCESS:
        if not traj:
            print("Trajectory map is empty.")
        for chain, points in traj.items():
            print(f"  {chain} trajectory points: {len(points)}")


def make_example_inputs():
    cart = lambda chain, pose: make_cartesian_target(chain, pose, ["torso"])
    waypoints = [
        [
            cart("left_arm", [0.42666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
            cart("right_arm", [0.422666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
        ],
        [
            cart("left_arm", [0.42666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
            cart("right_arm", [0.422666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
        ],
        [
            cart("left_arm", [0.12666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
            cart("right_arm", [0.12666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
        ],
        [
            cart("left_arm", [0.12666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
            cart("right_arm", [0.12666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
        ],
    ]
    return waypoints, make_planner_config(check_collision=False)


def run_example():
    motion, robot = initialize_interfaces()
    if motion is None or robot is None:
        return 1

    try:
        if not move_robot_to_home_position(robot):
            return 1
        time.sleep(0.5)

        current = {"joint_names": {}, "joints": {}, "poses": {}}
        if not capture_current_pose(motion, robot, "left_arm", current):
            return 1
        if not capture_current_pose(motion, robot, "right_arm", current):
            return 1

        waypoints, params = make_example_inputs()
        print_waypoints("move_line", waypoints)
        print(
            "Immediate execution is enabled; waypoints are loaded from "
            "move_line_targets.json reference values."
        )
        status, traj = motion.move_line(waypoints, params)
        print_traj_result("move_line", status, traj, motion)
        return 0 if status == gm.MotionStatus.SUCCESS else 1
    finally:
        shutdown_robot(robot)


if __name__ == "__main__":
    raise SystemExit(run_example())

Set Motion Configuration (set_config_by_type)

Applicable Scenarios: Update motion-planning parameters selected by configuration type.

examples/python/galbot_motion/set_config_by_type.py

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot


def run_example():
    motion = GalbotMotion()
    robot = GalbotRobot()
    # Initialize SDK interfaces directly in the example flow.
    if not motion.init():
        print("GalbotMotion init FAILED")
        return 1
    if not robot.init():
        print("GalbotRobot init FAILED")
        return 1
    print("GalbotMotion/GalbotRobot init OK")
    time.sleep(1)

    # Read the current config first. This keeps the example non-destructive.
    get_status, config = motion.get_config_by_type("sampler")
    print(f"get_config_by_type(sampler) status: {motion.status_to_string(get_status)}")
    if get_status != gm.MotionStatus.SUCCESS:
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return 1

    print(f"Current sampler config payload bytes: {len(config.common_str)}")
    # Send the unchanged payload back to cover the API call without changing behavior.
    set_status = motion.set_config_by_type(config)
    print(f"set_config_by_type(sampler, unchanged payload) status: {motion.status_to_string(set_status)}")

    # Clean shutdown releases SDK resources before the process exits.
    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    return 0 if set_status == gm.MotionStatus.SUCCESS else 1


if __name__ == "__main__":
    raise SystemExit(run_example())

Trajectory Planning (traj_plan)

Applicable Scenarios: Generate a time-parameterized trajectory for a prepared motion-planning request.

examples/python/galbot_motion/traj_plan.py

"""G1 traj_plan example aligned with traj_plan_example.cpp."""

import time

import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot

def initialize_interfaces():
    motion = GalbotMotion()
    print("GalbotMotion::get_instance() status: no return status; call completed.")
    robot = GalbotRobot()
    print("GalbotRobot::get_instance() status: no return status; call completed.")
    if not motion.init():
        print("GalbotMotion::init() status: FAILED")
        return None, None
    print("GalbotMotion::init() status: SUCCESS")
    if not robot.init():
        print("GalbotRobot::init() status: FAILED")
        return None, None
    print("GalbotRobot::init() status: SUCCESS")
    time.sleep(1.0)
    return motion, robot


def shutdown_robot(robot):
    robot.request_shutdown()
    print("request_shutdown() status: no return status; call completed.")
    robot.wait_for_shutdown()
    print("wait_for_shutdown() status: no return status; call completed.")
    robot.destroy()
    print("destroy() status: no return status; call completed.")


def print_control_status(api_name, status):
    result = "SUCCESS" if status == gm.ControlStatus.SUCCESS else "FAILED"
    print(f"{api_name} status: {str(status).rsplit('.', 1)[-1]} ({result})")


def move_robot_to_home_position(robot):
    print("Move robot to the G1 home position.")
    status = robot.set_joint_positions(
        [
            0.5, 1.5, 1.0, 0.0, 0.0,
            0.0, 0.0,
            2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
            -2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0,
        ],
        ["leg", "head", "left_arm", "right_arm"],
        [],
        True,
        0.1,
        30.0,
    )
    print_control_status("GalbotRobot::set_joint_positions(home)", status)
    return status == gm.ControlStatus.SUCCESS


def capture_current_joints(motion, robot, chains, current):
    for chain in chains:
        joint_names = list(motion.get_chain_joint_names(chain))
        print(f"get_chain_joint_names({chain}) status: {'SUCCESS' if joint_names else 'FAILED'}")
        if not joint_names:
            return False
        joints = list(robot.get_joint_positions([], joint_names))
        success = len(joints) == len(joint_names)
        print(f"GalbotRobot::get_joint_positions({chain}) status: {'SUCCESS' if success else 'FAILED'}")
        if not success:
            return False
        current["joint_names"][chain] = joint_names
        current["joints"][chain] = joints
        print(f"Current {chain} joints: {joints}")
    return True


def make_joint_target(chain, joint_positions, joint_names=None):
    target = gm.MotionPlanChainTarget()
    target.chain_name = chain
    target.mode = gm.MotionPlanTargetMode.kJoint
    target.joint.chain_name = chain
    target.joint.joint_positions = list(joint_positions)
    target.joint.joint_names = list(joint_names or [])
    return target


def print_waypoints(label, waypoints):
    print(f"{label} waypoints: {len(waypoints)}")
    for index, waypoint in enumerate(waypoints):
        print(f"  waypoint[{index}] targets: {len(waypoint)}")
        for target in waypoint:
            print(f"    {target.chain_name} JOINT q={list(target.joint.joint_positions)}")


def print_traj_result(label, status, traj, motion):
    result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
    print(f"{label} status: {motion.status_to_string(status)} ({result})")
    if status == gm.MotionStatus.SUCCESS:
        if not traj:
            print("Trajectory map is empty.")
        for chain, points in traj.items():
            print(f"  {chain} trajectory points: {len(points)}")


def make_example_inputs(current):
    left_names = current["joint_names"]["left_arm"]
    right_names = current["joint_names"]["right_arm"]

    waypoints = [
        [
            make_joint_target(
                "left_arm", [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
            ),
        ],
        [
            make_joint_target(
                "left_arm", [1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
            ),
        ],
        [
            make_joint_target(
                "left_arm", [1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0], right_names
            ),
        ],
    ]

    params = gm.Parameter()
    params.set_direct_execute(True)
    params.set_blocking(True)
    params.set_timeout(120.0)
    params.set_check_collision(False)
    params.set_reference_frame("base_link")
    params.set_actuate("with_chain_only")
    return waypoints, params


def make_example_inputs_with_leg(current):
    left_names = current["joint_names"]["left_arm"]
    right_names = current["joint_names"]["right_arm"]
    leg_names = current["joint_names"]["leg"]
    leg_target = list(current["joints"]["leg"])
    leg_target[3] = 0.0
    leg_target[4] = 0.0

    waypoints = [
        [
            make_joint_target(
                "left_arm", [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
            ),
            make_joint_target("leg", leg_target, leg_names),
        ],
        [
            make_joint_target(
                "left_arm", [1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
            ),
            make_joint_target("leg", leg_target, leg_names),
        ],
        [
            make_joint_target(
                "left_arm", [1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0], right_names
            ),
            make_joint_target("leg", leg_target, leg_names),
        ],
    ]

    params = gm.Parameter()
    params.set_direct_execute(True)
    params.set_blocking(True)
    params.set_timeout(120.0)
    params.set_check_collision(False)
    params.set_reference_frame("base_link")
    params.set_actuate("with_leg")
    return waypoints, params


def make_example_inputs_with_torso(current):
    left_names = current["joint_names"]["left_arm"]
    right_names = current["joint_names"]["right_arm"]

    waypoints = [
        [
            make_joint_target(
                "left_arm", [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
            ),
            make_joint_target("torso", [0.0], ["leg_joint4"]),
        ],
        [
            make_joint_target(
                "left_arm", [1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
            ),
            make_joint_target("torso", [0.0], ["leg_joint4"]),
        ],
        [
            make_joint_target(
                "left_arm", [1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
            ),
            make_joint_target(
                "right_arm", [-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0], right_names
            ),
            make_joint_target("torso", [0.0], ["leg_joint4"]),
        ],
    ]

    params = gm.Parameter()
    params.set_direct_execute(True)
    params.set_blocking(True)
    params.set_timeout(120.0)
    params.set_check_collision(False)
    params.set_reference_frame("base_link")
    params.set_actuate("with_torso")
    return waypoints, params


def run_example():
    motion, robot = initialize_interfaces()
    if motion is None or robot is None:
        return 1

    try:
        if not move_robot_to_home_position(robot):
            return 1
        time.sleep(0.5)

        current = {"joint_names": {}, "joints": {}}
        if not capture_current_joints(
            motion, robot, ["leg", "left_arm", "right_arm"], current
        ):
            return 1

        waypoints, params = make_example_inputs_with_torso(current)
        print_waypoints("traj_plan", waypoints)
        print(
            "Immediate execution is enabled; waypoints are loaded from "
            "motion_plan_targets.json reference values."
        )
        status, traj = motion.traj_plan(waypoints, params)
        print_traj_result("traj_plan", status, traj, motion)
        return 0 if status == gm.MotionStatus.SUCCESS else 1
    finally:
        shutdown_robot(robot)


if __name__ == "__main__":
    raise SystemExit(run_example())

Class: GalbotNavigation

Get Instance / Initialize

Applicable Scenarios: Get the navigation module singleton and initialize. Must be called before using navigation functions.

examples/python/galbot_navigation/get_instance.py

from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
import numpy as np

# Initialize system and navigation module
robot = GalbotRobot()
robot.init()

nav = GalbotNavigation()
nav.init()

print("GalbotNavigation has been initialized:", nav is not None)

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Relocalize / Is Localized / Get Current Pose

Applicable Scenarios: Trigger relocalization, check if the robot is successfully localized, and get the robot's current pose in the map.

examples/python/galbot_navigation/relocalized.py

from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
import numpy as np
import time

nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()

init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])

# success
while not nav.is_localized():
    nav.relocalize(init_pose)
    time.sleep(0.5)

print("Current pose:", nav.get_current_pose())

nav.stop_navigation()
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')

Non-blocking Navigation + Polling for Arrival

Applicable Scenarios: Start navigation without blocking the current thread, allowing other processing during navigation, and need to poll to determine if the target has been reached. Suitable for scenarios requiring asynchronous processing.

examples/python/galbot_navigation/navigate_to_goal.py

from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import ControlStatus, G1ControllerName
import numpy as np
import time
import sys

nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()

goal = np.array([0.5, 0.0, 0.0, 0, 0, 0.0, 1.0])

res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
    print("Failed to switch controller!")
    sys.exit(1)
else:
    print("Controller switched successfully!")

nav.navigate_to_goal(goal, enable_collision_check=True, is_blocking=False, timeout=20)

start_time = time.time()
reached = False

while True:
    if nav.check_goal_arrival():
        reached = True
        break
    if time.time() - start_time > 20:
        print("Navigation timed out; target not reached within 20s")
        break
    print("Navigating...")
    time.sleep(0.5)

if reached:
    print("Target reached")

nav.stop_navigation()
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")

Move Straight to Goal

Applicable Scenarios: Move the robot in a straight line to the target point.

examples/python/galbot_navigation/move_straight_to.py

from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import ControlStatus, G1ControllerName
import numpy as np
import time
import sys

nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()

target = np.array([0.2, 0.0, 0.0, 0, 0, 0.0, 1.0])

res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
    print("Failed to switch controller!")
    sys.exit(1)
else:
    print("Controller switched successfully!")

nav.move_straight_to(target, is_blocking=False, timeout=10)
time.sleep(1.0)
nav.stop_navigation()

# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")

Stop Navigation (stop_navigation)

Applicable Scenarios: Stop an ongoing navigation task.

examples/python/galbot_navigation/stop_navigation.py

"""Stop a blocking navigation request from another Python thread."""

import signal
import threading
import time

import numpy as np
from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot

# Set when the navigation call has returned, so the worker stops waiting for a
# Ctrl-C that is never coming.
navigation_done = threading.Event()
def stop_navigation_on_ctrl_c(navigation):
    # sigwait() cannot be cancelled, so wait in slices: when the navigation
    # finishes on its own no Ctrl-C ever arrives, and the worker still has to
    # exit or the join() in the main flow would block forever.
    while not navigation_done.is_set():
        if signal.sigtimedwait({signal.SIGINT}, 0.2) is None:
            continue
        print("Worker thread: stopping navigation...")
        succeeded, detail = navigation.stop_navigation()
        if succeeded:
            print("Worker thread: navigation stop succeeded")
        else:
            print(f"Worker thread: navigation stop FAILED: {detail}")
        return


def confirm_motion():
    print("⚠️  Ensure the emergency-stop button is released and the path is clear.")
    if input("Start straight navigation? (y/n): ").strip().lower() != "y":
        raise SystemExit("Motion was not confirmed.")


confirm_motion()
# Block SIGINT before SDK initialization so all SDK threads inherit the mask.
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
navigation = GalbotNavigation()
robot.init()
navigation.init()
time.sleep(3)  # Wait for the PNS service to enter its working state.

if robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
    print("Failed to switch controller!")
else:
    # move_straight_to is sent through PNS and is covered by stop_navigation.
    # It does not require map localization, so it is a reliable stop example.
    print("Robot is moving straight. Press Ctrl-C to stop navigation.")
    stop_thread = threading.Thread(
        target=stop_navigation_on_ctrl_c, args=(navigation,)
    )
    stop_thread.start()
    result = navigation.move_straight_to(
        np.array([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]),
        is_blocking=True,
        timeout=10,
    )
    navigation_done.set()
    print(f"move_straight_to returned: {result}")
    stop_thread.join()

signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")

Get Navigation Status + Polling for SUCCESS/FAILED or Timeout

Applicable Scenarios: Get the execution status of the current navigation task, used for polling in non-blocking navigation.

examples/python/galbot_navigation/get_navigation_status.py

"""
example: navigation get_navigation_status, SUCCESS/FAILED timeout exit,
Avoid deadlock and execute error logic.
"""

from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus, ControlStatus, G1ControllerName
import numpy as np
import time
import sys

nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()

goal = np.array([0.5, 0.0, 0.0, 0, 0, 0.0, 1.0])
timeout_s = 20.0
poll_interval_s = 0.5

res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
    print("Failed to switch controller!")
    sys.exit(1)
else:
    print("Controller switched successfully!")
# Non-blocking navigation
nav.navigate_to_goal(
    goal, enable_collision_check=True, is_blocking=False, timeout=timeout_s
)
start = time.time()

while True:
    status = nav.get_navigation_status()
    elapsed = time.time() - start

    if status == NavigationTaskStatus.SUCCESS:
        print("Target reached")
        break
    if status == NavigationTaskStatus.FAILED:
        print("Navigation failed; exit error-handling logic promptly")
        break
    if elapsed >= timeout_s:
        print("navigationtimeout, exit")
        break

    if status == NavigationTaskStatus.RUNNING:
        print(f"Navigating... Status: {status.name}, elapsed: {elapsed:.1f}s")
    else:
        print(f"Status: {status.name}, elapsed: {elapsed:.1f}s")

    time.sleep(poll_interval_s)

nav.stop_navigation()
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")

Complete Running Example (Simple Workflow)

Applicable Scenarios: Demonstrates the complete usage workflow of navigation functionality, including initialization, relocalization, navigation to target and other steps for reference.

examples/python/galbot_navigation/complete_example.py

from galbot_sdk.g1 import GalbotRobot, GalbotNavigation
from galbot_sdk.g1 import ControlStatus, G1ControllerName
import numpy as np
import time
import sys

robot = GalbotRobot()
robot.init()
nav = GalbotNavigation()
nav.init()

init_pose = np.array([0.0, 0.0, 0.0, 0, 0, 0.0, 1.0])
goal_pose = np.array([1.0, 0.0, 0.0, 0, 0, 0.0, 1.0])

res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
    print("Failed to switch controller!")
    sys.exit(1)
else:
    print("Controller switched successfully!")

while not nav.is_localized():
    nav.relocalize(init_pose)
    time.sleep(0.5)

if nav.check_path_reachability(goal_pose, nav.get_current_pose()):
    nav.navigate_to_goal(
        goal_pose, enable_collision_check=True, is_blocking=True, timeout=30
    )
    print("Whether reached:", nav.check_goal_arrival())

nav.stop_navigation()
# Shutdown system
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()

Navigation with Attached Bounding-box Filtering

Applicable Scenarios: Filter navigation collision geometry around an attached object carried by the robot.

examples/python/galbot_navigation/bounding_box_filter.py

from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot


def print_boxes(boxes):
    print(f"Current bounding boxes: {len(boxes)}")
    for index, box in enumerate(boxes):
        print(f"--- Box [{index}] ---")
        print("Tag:", box["box_tag"])
        print("Parent:", box["parent_link_name"])
        print("Size:", box["box_size"])
        print("Pose:", box["box_pose"])


robot = GalbotRobot()
robot.init()

nav = GalbotNavigation()
nav.init()

# Add a box so navigation does not treat the corresponding fused points as obstacles.
box_info = {
    "box_size": [0.4, 0.3, 0.28],
    "box_pose": [0.45, 0.0, 0.25, 0.0, 0.0, 0.0, 1.0],
    "box_tag": 1001,
    "parent_link_name": "base_link",
}
attached_box_info = {
    "box_size": [0.4, 0.3, 0.28],
    "box_pose": [0.2, 0.0, -0.2, 0.0, 0.0, 0.0, 1.0],
    "box_tag": 2001,
    "parent_link_name": "left_arm_end_effector_mount_link",
}

success, status = nav.add_bounding_box(box_info)
print("add_bounding_box:", success, status)

print_boxes(nav.get_bounding_box())

success, status = nav.attach_box_to_link(attached_box_info, [])
print("attach_box_to_link:", success, status)

success, status = nav.detach_box_from_link(2001)
print("detach_box_from_link:", success, status)

success, status = nav.remove_bounding_box(1001)
print("remove_bounding_box:", success, status)

print_boxes(nav.get_bounding_box())

robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")

Navigate Along a Trajectory (navigate_along_trajectory)

Applicable Scenarios: Execute a navigation route represented by an ordered trajectory of base poses.

examples/python/galbot_navigation/navigate_along_trajectory.py

from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus, Pose

import numpy as np
import sys
import time


kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kTaskTimeoutSec = 100.0


def navigation_target_status_to_string(status):
    if status == NavigationTaskStatus.UNKNOWN:
        return "UNKNOWN"
    if status == NavigationTaskStatus.RUNNING:
        return "RUNNING"
    if status == NavigationTaskStatus.SUCCESS:
        return "SUCCESS"
    if status == NavigationTaskStatus.FAILED:
        return "FAILED"
    if status == NavigationTaskStatus.INTERRUPTED:
        return "INTERRUPTED"
    if status == NavigationTaskStatus.OCCUPIED:
        return "OCCUPIED"
    if status == NavigationTaskStatus.COLLISION:
        return "COLLISION"
    if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
        return "CLOSE_TO_OBSTACLE"
    return "UNKNOWN"


def format_pose(pose):
    values = [
        pose.position.x,
        pose.position.y,
        pose.position.z,
        pose.orientation.x,
        pose.orientation.y,
        pose.orientation.z,
        pose.orientation.w,
    ]
    return "[" + ", ".join(f"{value:g}" for value in values) + "]"


def print_current_pose(navigation, indent="  "):
    current_pose = navigation.get_current_pose()
    print(
        f"{indent}current_pose: ["
        f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
        f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
    )


def wait_for_localization(navigation, init_pose):
    retry = 0
    while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
        navigation.relocalize(init_pose)
        time.sleep(kLocalizationRetrySleepMs / 1000.0)
        retry += 1
    return navigation.is_localized()


def print_task_snapshot(navigation, handle, snapshot, include_pose=True):
    print(
        f"  task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
        f"status: {navigation_target_status_to_string(snapshot.status)}"
    )
    if include_pose:
        print_current_pose(navigation, indent="    ")


def monitor_task_until_terminal(navigation, handle, timeout_s):
    start_time = time.time()
    while True:
        snapshot = navigation.get_navigation_target_status(handle.task_id)
        print_task_snapshot(navigation, handle, snapshot)

        if snapshot.status in {
            NavigationTaskStatus.SUCCESS,
            NavigationTaskStatus.FAILED,
            NavigationTaskStatus.INTERRUPTED,
            NavigationTaskStatus.OCCUPIED,
            NavigationTaskStatus.COLLISION,
            NavigationTaskStatus.CLOSE_TO_OBSTACLE,
        }:
            return

        if (time.time() - start_time) >= timeout_s:
            print("  timeout reached while waiting for task completion.")
            return

        time.sleep(0.5)


def main():
    navigation = GalbotNavigation()
    robot = GalbotRobot()

    if robot.init():
        print("Base instance initialized successfully!")
    else:
        print("Base instance initialization failed!")
        return -1

    if navigation.init():
        print("Navigation instance initialized successfully!")
    else:
        print("Navigation instance initialization failed!")
        return -1

    res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
    if res != ControlStatus.SUCCESS:
        print("Failed to switch controller!")
        return -1

    init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
    waypoints = [
        Pose([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]),
        Pose([0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]),
        Pose([1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]),
    ]

    time.sleep(3)

    if not wait_for_localization(navigation, init_pose):
        print("localization failed", file=sys.stderr)
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return -1

    time.sleep(2)

    print("set target: waypoints")
    for i, waypoint in enumerate(waypoints):
        print(f"  waypoint_{i}: {format_pose(waypoint)}")

    frame_id = "map"
    speed_ratio = 1.0
    enable_collision_check = False

    handle = navigation.navigate_along_trajectory(waypoints, frame_id, speed_ratio, enable_collision_check)

    if handle.request_sent:
        monitor_task_until_terminal(navigation, handle, kTaskTimeoutSec)
    else:
        print("trajectory task submission failed")

    navigation.stop_navigation()

    print("\nfinal task summary")
    snapshot = navigation.get_navigation_target_status(handle.task_id)
    print(
        f"  task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
        f"status: {navigation_target_status_to_string(snapshot.status)}"
    )

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()

    return 0


if __name__ == "__main__":
    sys.exit(main())

Navigate to Goal V2 (navigate_to_goal_v2)

Applicable Scenarios: Navigate to a target pose using the extended goal interface and its additional options.

examples/python/galbot_navigation/navigate_to_goal_v2.py

from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot

import numpy as np
import sys
import time


kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500


def print_current_pose(navigation, indent="  "):
    current_pose = navigation.get_current_pose()
    print(
        f"{indent}current_pose: ["
        f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
        f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
    )


def wait_for_localization(navigation, init_pose):
    retry = 0
    while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
        navigation.relocalize(init_pose)
        time.sleep(kLocalizationRetrySleepMs / 1000.0)
        retry += 1
    return navigation.is_localized()


def main():
    navigation = GalbotNavigation()
    robot = GalbotRobot()

    if robot.init():
        print("Base instance initialized successfully!")
    else:
        print("Base instance initialization failed!", file=sys.stderr)
        return -1

    if navigation.init():
        print("Navigation instance initialized successfully!")
    else:
        print("Navigation instance initialization failed!", file=sys.stderr)
        return -1

    res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
    if res != ControlStatus.SUCCESS:
        print("Failed to switch controller!", file=sys.stderr)
        return -1

    init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)

    time.sleep(3)

    if not wait_for_localization(navigation, init_pose):
        print("localization failed", file=sys.stderr)
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return -1

    print_current_pose(navigation)

    relative_goal = np.array([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
    max_vel = np.array([0.5, 0.5, 0.5], dtype=np.float64)
    pose_frame = "base_link"
    enable_collision_check = True
    is_blocking = True
    timeout_s = 20.0
    omni_plan = False

    print("navigate_to_goal_v2 relative target in base_link frame")
    success, status = navigation.navigate_to_goal_v2(
        relative_goal,
        max_vel,
        pose_frame=pose_frame,
        enable_collision_check=enable_collision_check,
        is_blocking=is_blocking,
        timeout=timeout_s,
        omni_plan=omni_plan,
    )
    print(f"navigate_to_goal_v2 result: success={success}, status={status}")

    print_current_pose(navigation)

    navigation.stop_navigation()
    print("navigation stopped")

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()

    print("example completed ...")
    return 0


if __name__ == "__main__":
    sys.exit(main())

Velocity Navigation (navigate_with_velocity)

Applicable Scenarios: Control base translation and rotation continuously with velocity commands.

examples/python/galbot_navigation/navigate_with_velocity.py

from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus

import sys
import time


def print_current_pose(navigation, indent="  "):
    current_pose = navigation.get_current_pose()
    print(
        f"{indent}current_pose: ["
        f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
        f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
    )


def init_sdk():
    navigation = GalbotNavigation()
    robot = GalbotRobot()

    if robot.init():
        print("Base instance initialized successfully!")
    else:
        print("Base instance initialization failed!", file=sys.stderr)
        return None, None

    if navigation.init():
        print("Navigation instance initialized successfully!")
    else:
        print("Navigation instance initialization failed!", file=sys.stderr)
        return None, None

    res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
    if res != ControlStatus.SUCCESS:
        print("Failed to switch controller!", file=sys.stderr)
        return None, None

    time.sleep(3)
    print_current_pose(navigation)
    return navigation, robot


def shutdown_sdk(navigation, robot):
    navigation.stop_navigation()
    print_current_pose(navigation)
    print("navigation stopped")

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    print("example completed ...")


def monitor_navigation(navigation, timeout_s, poll_interval_s=0.5):
    start = time.time()
    while True:
        status = navigation.get_navigation_status()
        elapsed = time.time() - start
        print(f"  status: {status.name}, elapsed: {elapsed:.1f}s")
        print_current_pose(navigation, indent="    ")

        if status in (
            NavigationTaskStatus.SUCCESS,
            NavigationTaskStatus.FAILED,
            NavigationTaskStatus.INTERRUPTED,
            NavigationTaskStatus.OCCUPIED,
            NavigationTaskStatus.COLLISION,
            NavigationTaskStatus.CLOSE_TO_OBSTACLE,
        ):
            break

        if elapsed >= timeout_s:
            print("  monitor timeout reached")
            break

        time.sleep(poll_interval_s)


def test_base_vel_cmd():
    navigation, robot = init_sdk()
    if navigation is None or robot is None:
        return -1

    vx = 0.3
    vy = 0.0
    vyaw = 0.0
    duration_s = 6.0
    enable_collision_check = True

    print(
        f"navigate_with_velocity: vx={vx}, vy={vy}, "
        f"vyaw={vyaw}, duration_s={duration_s}"
    )
    success, status = navigation.navigate_with_velocity(
        vx, vy, vyaw, duration_s, enable_collision_check
    )
    print(f"navigate_with_velocity result: success={success}, status={status}")

    monitor_navigation(navigation, timeout_s=duration_s + 2.0)

    shutdown_sdk(navigation, robot)
    return 0


def test_change_vel_cmd():
    navigation, robot = init_sdk()
    if navigation is None or robot is None:
        return -1

    print("send first velocity command")
    success, status = navigation.navigate_with_velocity(0.3, 0.0, 0.0, 6.0, True)
    print(f"navigate_with_velocity result 1: success={success}, status={status}")
    monitor_navigation(navigation, timeout_s=4.0)

    print("send second velocity command with y direction")
    success, status = navigation.navigate_with_velocity(0.1, 0.2, 0.0, 6.0, True)
    print(f"navigate_with_velocity result 2: success={success}, status={status}")
    monitor_navigation(navigation, timeout_s=8.0)

    shutdown_sdk(navigation, robot)
    return 0


def main():
    return test_base_vel_cmd()

    # return test_change_vel_cmd()


if __name__ == "__main__":
    sys.exit(main())

Navigation Configuration

Applicable Scenarios: Read and update runtime configuration used by the navigation module.

examples/python/galbot_navigation/navigation_set_get_config.py

from galbot_sdk.g1 import GalbotNavigation, GalbotRobot

import numpy as np
import sys


def print_result(name, result):
    success, status = result
    print(f"{name}: success={success}, status={status}")


def main():
    navigation = GalbotNavigation()
    robot = GalbotRobot()

    if robot.init():
        print("Base instance initialized successfully!")
    else:
        print("Base instance initialization failed!", file=sys.stderr)
        return -1

    if navigation.init():
        print("Navigation instance initialized successfully!")
    else:
        print("Navigation instance initialization failed!", file=sys.stderr)
        return -1

    print("dump navigation configs before setting parameters")
    print_result("dump_navigation_configs", navigation.dump_navigation_configs())

    # valid range: [0.05, 1.5]
    vel_limit = np.array([0.5, 0.5, 0.5], dtype=np.float64)
    # valid range: [0.05, 6.0]
    acc_limit = np.array([0.5, 0.5, 0.5], dtype=np.float64)
    # valid range: [0.05, 12.0]
    jerk_limit = np.array([5.0, 5.0, 5.0], dtype=np.float64)
    # valid range: [0.03, 2.0]
    arrival_threshold = np.array([0.05, 0.05, 0.05], dtype=np.float64)
    timeout_s = 30.0

    print_result(
        "set_navigation_velocity_limit",
        navigation.set_navigation_velocity_limit(vel_limit),
    )
    print_result(
        "set_navigation_kinematics_limits",
        navigation.set_navigation_kinematics_limits(vel_limit, acc_limit, jerk_limit),
    )
    print_result("set_navigation_timeout", navigation.set_navigation_timeout(timeout_s))
    print_result(
        "set_navigation_arrival_threshold",
        navigation.set_navigation_arrival_threshold(arrival_threshold),
    )

    print("dump navigation configs after setting parameters")
    print_result("dump_navigation_configs", navigation.dump_navigation_configs())

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()

    return 0


if __name__ == "__main__":
    sys.exit(main())

Navigate Through Waypoints (navigation_through_waypoints)

Applicable Scenarios: Make the robot visit multiple navigation waypoints in a specified order.

examples/python/galbot_navigation/navigation_through_waypoints.py

from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus, Pose, Waypoint, WaypointParams

import numpy as np
import sys
import time


kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kTaskTimeoutSec = 100.0


def navigation_target_status_to_string(status):
    if status == NavigationTaskStatus.UNKNOWN:
        return "UNKNOWN"
    if status == NavigationTaskStatus.RUNNING:
        return "RUNNING"
    if status == NavigationTaskStatus.SUCCESS:
        return "SUCCESS"
    if status == NavigationTaskStatus.FAILED:
        return "FAILED"
    if status == NavigationTaskStatus.INTERRUPTED:
        return "INTERRUPTED"
    if status == NavigationTaskStatus.OCCUPIED:
        return "OCCUPIED"
    if status == NavigationTaskStatus.COLLISION:
        return "COLLISION"
    if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
        return "CLOSE_TO_OBSTACLE"
    return "UNKNOWN"


def format_pose(pose):
    values = [
        pose.position.x,
        pose.position.y,
        pose.position.z,
        pose.orientation.x,
        pose.orientation.y,
        pose.orientation.z,
        pose.orientation.w,
    ]
    return "[" + ", ".join(f"{value:g}" for value in values) + "]"


def print_current_pose(navigation, indent="  "):
    current_pose = navigation.get_current_pose()
    print(
        f"{indent}current_pose: ["
        f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
        f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
    )


def wait_for_localization(navigation, init_pose):
    retry = 0
    while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
        navigation.relocalize(init_pose)
        time.sleep(kLocalizationRetrySleepMs / 1000.0)
        retry += 1
    return navigation.is_localized()


def print_task_snapshot(navigation, handle, snapshot):
    print(
        f"  task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
        f"status: {navigation_target_status_to_string(snapshot.status)}"
    )
    print_current_pose(navigation, indent="    ")


def monitor_task_until_terminal(navigation, handle, timeout_s):
    start_time = time.time()
    while True:
        snapshot = navigation.get_navigation_target_status(handle.task_id)
        print_task_snapshot(navigation, handle, snapshot)

        now = time.time()
        if snapshot.status in {
            NavigationTaskStatus.SUCCESS,
            NavigationTaskStatus.FAILED,
            NavigationTaskStatus.INTERRUPTED,
            NavigationTaskStatus.OCCUPIED,
            NavigationTaskStatus.COLLISION,
            NavigationTaskStatus.CLOSE_TO_OBSTACLE,
        }:
            return

        if (now - start_time) >= timeout_s:
            print("  timeout reached while waiting for task completion.")
            return

        time.sleep(1.0)


def make_waypoint(pose, params=None):
    if params is None:
        params = WaypointParams()
    return Waypoint(pose, params)


def main():
    navigation = GalbotNavigation()
    robot = GalbotRobot()

    if robot.init():
        print("Base instance initialized successfully!")
    else:
        print("Base instance initialization failed!")
        return -1

    if navigation.init():
        print("Navigation instance initialized successfully!")
    else:
        print("Navigation instance initialization failed!")
        return -1

    res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
    if res != ControlStatus.SUCCESS:
        print("Failed to switch controller!")
        return -1

    init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)

    waypoint_2_params = WaypointParams()
    waypoint_2_params.arrival_position_threshold_x = 0.02
    waypoint_2_params.arrival_position_threshold_y = 0.02
    waypoint_2_params.arrival_orientation_threshold = 0.08
    waypoint_2_params.velocity_scale = 0.2

    waypoint_3_params = WaypointParams()
    waypoint_3_params.arrival_position_threshold_x = 0.04
    waypoint_3_params.arrival_position_threshold_y = 0.04
    waypoint_3_params.arrival_orientation_threshold = 0.05
    waypoint_3_params.velocity_scale = 0.8
    waypoint_3_params.acceleration_scale = 0.8
    waypoint_3_params.jerk_scale = 0.8

    waypoints = [
        make_waypoint(Pose([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])),
        make_waypoint(Pose([0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]), waypoint_2_params),
        make_waypoint(Pose([1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]), waypoint_3_params),
    ]

    time.sleep(3)

    if not wait_for_localization(navigation, init_pose):
        print("localization failed", file=sys.stderr)
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return -1

    time.sleep(2)

    print("set target: waypoints")
    for i, waypoint in enumerate(waypoints):
        print(f"  waypoint_{i}: {format_pose(waypoint.pose)}")

    frame_id = "map"
    enable_collision_check = True

    handle = navigation.navigate_through_waypoints(waypoints, frame_id, enable_collision_check)

    if handle.request_sent:
        monitor_task_until_terminal(navigation, handle, kTaskTimeoutSec)
    else:
        print("navigation failed to submit task")

    navigation.stop_navigation()

    print("\nfinal task summary")
    snapshot = navigation.get_navigation_target_status(handle.task_id)
    print(
        f"  task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
        f"status: {navigation_target_status_to_string(snapshot.status)}"
    )

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()

    return 0


if __name__ == "__main__":
    sys.exit(main())

Set Navigation Target (set_navigation_target)

Applicable Scenarios: Set a navigation target separately before starting or managing the navigation task.

examples/python/galbot_navigation/set_navigation_target.py

from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus, Pose

import numpy as np
import sys
import time


kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kPollIntervalMs = 500
kMonitorDurationMs = 2000


def navigation_target_status_to_string(status):
    if status == NavigationTaskStatus.UNKNOWN:
        return "UNKNOWN"
    if status == NavigationTaskStatus.RUNNING:
        return "RUNNING"
    if status == NavigationTaskStatus.SUCCESS:
        return "SUCCESS"
    if status == NavigationTaskStatus.FAILED:
        return "FAILED"
    if status == NavigationTaskStatus.INTERRUPTED:
        return "INTERRUPTED"
    if status == NavigationTaskStatus.OCCUPIED:
        return "OCCUPIED"
    if status == NavigationTaskStatus.COLLISION:
        return "COLLISION"
    if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
        return "CLOSE_TO_OBSTACLE"
    return "UNKNOWN"


def format_pose(pose):
    values = [
        pose.position.x,
        pose.position.y,
        pose.position.z,
        pose.orientation.x,
        pose.orientation.y,
        pose.orientation.z,
        pose.orientation.w,
    ]
    return "[" + ", ".join(f"{value:g}" for value in values) + "]"


def print_current_pose(navigation, indent="  "):
    current_pose = navigation.get_current_pose()
    print(
        f"{indent}current_pose: ["
        f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
        f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
    )


def print_task_snapshot(navigation, handle, snapshot, include_pose=True):
    print(
        f"  task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
        f"status: {navigation_target_status_to_string(snapshot.status)}"
    )
    if include_pose:
        print_current_pose(navigation, indent="    ")


def wait_for_localization(navigation, init_pose):
    retry = 0
    while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
        navigation.relocalize(init_pose)
        time.sleep(kLocalizationRetrySleepMs / 1000.0)
        retry += 1
    return navigation.is_localized()


def print_task_summary(navigation, handles):
    print("\nfinal task summary")
    for handle in handles:
        snapshot = navigation.get_navigation_target_status(handle.task_id)
        print_task_snapshot(navigation, handle, snapshot, include_pose=False)


def main():
    navigation = GalbotNavigation()
    robot = GalbotRobot()

    if robot.init():
        print("Base instance initialized successfully!")
    else:
        print("Base instance initialization failed!")
        return -1

    if navigation.init():
        print("Navigation instance initialized successfully!")
    else:
        print("Navigation instance initialization failed!")
        return -1

    res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
    if res != ControlStatus.SUCCESS:
        print("Failed to switch controller!")
        return -1

    init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)

    time.sleep(3)

    if not wait_for_localization(navigation, init_pose):
        print("localization failed", file=sys.stderr)
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()
        return -1

    frame_id = "map"
    speed_ratio = 0.9
    enable_collision_check = True
    target_points = [
        Pose([1.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
        Pose([0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
        Pose([0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
    ]

    handles = []

    for i, target in enumerate(target_points):
        print(f"set target: dynamic_goal_{i}, target_pose: {format_pose(target)}")

        handle = navigation.set_navigation_target(target, frame_id, speed_ratio, enable_collision_check)
        handles.append(handle)

        start_time = time.time()
        while True:
            snapshot = navigation.get_navigation_target_status(handle.task_id)
            print_task_snapshot(navigation, handle, snapshot)

            now = time.time()
            elapsed_ms = (now - start_time) * 1000.0
            if elapsed_ms >= kMonitorDurationMs:
                break

            time.sleep(kPollIntervalMs / 1000.0)

    print_task_summary(navigation, handles)

    print("\nstop navigation and shutdown")
    navigation.stop_navigation()

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()

    return 0


if __name__ == "__main__":
    sys.exit(main())

Class: GalbotPerception

Foundation Stereo: single run_once and save a colorized depth image

Applicable scenarios: Trigger a single stereo depth estimation inference and retrieve the result.

examples/python/galbot_perception/foundation_stereo_example.py

"""Foundation stereo depth example (G1): single run_once, save a pseudo-color depth image."""

import time

import cv2
import numpy as np

try:
    from galbot_sdk.g1 import (
        GalbotPerception,
        GalbotRobot,
        PerceptionModule,
    )
except ImportError:
    print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
    raise

OUTPUT_IMAGE_PATH = "foundation_stereo_depth.png"


def main():
    robot = GalbotRobot()
    try:
        if not robot.init():
            print("Robot init failed")
            return
        print("Robot init OK")

        perception = GalbotPerception()
        if not perception.init({PerceptionModule.FOUNDATION_STEREO, PerceptionModule.LIGHT_STEREO}):
            print("Perception init failed")
            return
        print("Perception init OK")

        time.sleep(12)  # Wait for perception models to load
        print("Triggering single inference...")

        if not perception.run_once(PerceptionModule.FOUNDATION_STEREO):
            print("run_once failed to send command")
            return

        print("Waiting for inference result...")
        if not perception.wait_for_new_result(
            PerceptionModule.FOUNDATION_STEREO, timeout_s=6.0
        ):
            print("Timed out waiting for inference result")
            return

        ok, result = perception.get_latest_result(PerceptionModule.FOUNDATION_STEREO)
        if not ok:
            print("get_latest_result failed")
            return

        print(result.get_result_info())

        depth_map = result.instance_mask
        if depth_map is None:
            print("No depth map (instance_mask is empty)")
            return

        print(f"Depth map shape: {depth_map.shape}, dtype: {depth_map.dtype}")
        print(f"Depth value range: [{np.nanmin(depth_map)}, {np.nanmax(depth_map)}]")

        depth_f = depth_map.astype(np.float32)
        valid = depth_f[depth_f > 0]
        if valid.size > 0:
            vmin, vmax = np.percentile(valid, [1, 99])
            normalized = np.clip((depth_f - vmin) / (vmax - vmin + 1e-6), 0, 1)
        else:
            normalized = np.zeros_like(depth_f)
        colored = cv2.applyColorMap(
            (normalized * 255).astype(np.uint8), cv2.COLORMAP_TURBO
        )

        if cv2.imwrite(OUTPUT_IMAGE_PATH, colored):
            print(f"Saved: {OUTPUT_IMAGE_PATH}")
        else:
            print(f"Failed to save: {OUTPUT_IMAGE_PATH}")
    finally:
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()


if __name__ == "__main__":
    main()

Class: Parameter

Applicable Scenarios: Create a motion planning parameter object used to configure various parameters for motion planning such as velocity limits, acceleration limits, planning time, etc.

examples/python/galbot_motion/create_parameter.py

from galbot_sdk.g1 import Parameter, create_parameter, G1JointGroup

# Create Parameter via constructor and set options
p = Parameter()
p.set_blocking(True)
p.set_check_collision(False)
p.set_timeout(5.0)
p.set_actuate('with_chain_only')
p.set_tool_pose(False)
p.set_reference_frame('base_link')

p.joint_state = {
    G1JointGroup.left_arm: [0.0] * 7,
    # Can add others if needed:
    # G1JointGroup.right_arm: [0.0] * 7,
    # G1JointGroup.leg: [0.0] * 4,
}

print('blocking:', p.get_blocking())
print('collision check:', p.get_check_collision())
print('timeout:', p.get_timeout())

# Or use factory function to quickly create Parameter
p2 = create_parameter(direct_execute=False, blocking=True, timeout=3.0, actuate='with_chain_only', tool_pose=False, check_collision=True)
print('Factory-created timeout:', p2.get_timeout())

Utility Functions

check_motion_status

Applicable Scenarios: Check motion planning execution result status, determine if planning succeeded, and print error messages.

examples/python/galbot_motion/check_motion_status.py

from galbot_sdk.g1 import MotionStatus, check_motion_status

status_str = check_motion_status(MotionStatus.SUCCESS)
print('MotionStatus string:', status_str)

create_joint_state / create_pose_state

Applicable Scenarios: Quickly create joint state and pose state objects, facilitating motion planning interface calls.

examples/python/galbot_motion/create_pose_state.py

from galbot_sdk.g1 import create_joint_state, create_pose_state, JointStates, PoseState, Pose

# Create helper objects using the factory function
js = create_joint_state()
ps = create_pose_state()

# Fill example fields
js.chain_name = 'left_arm'
js.joint_positions = [0.0] * 7

ps.chain_name = 'left_arm'
# 7D vector: x, y, z, qx, qy, qz, qw
ps.pose = Pose([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])

print(type(js), js.chain_name)
print(type(ps), ps.chain_name)

Tutorial Source Examples

The following runnable tutorials are maintained in examples/g1/python/tutorials. For walkthroughs and safety notes, see Tutorials.

Example 1. Galbot Control Started

Applicable Scenarios: Basic G1 joint control: execute the dual-arm heart gesture and restore the original pose. Before running, confirm working mode and that the emergency-stop button is released. For gripper control and more control cases, see the galbot_robot examples on this page.

examples/python/tutorials/example1_galbot_control_started.py

"""
Note: When running this example, please ensure the robot's `emergency stop button` is released;
"""
import time

try:
    from galbot_sdk.g1 import GalbotRobot
    from galbot_sdk.g1 import ControlStatus
except ImportError:
    print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
    exit(1)


def demo_heart_pose(robot: GalbotRobot,
                    joint_group_names: list,
                    position_seq: list,
                    is_blocking: bool,
                    max_speed: float,
                    timeout_s: float,
                    retry_count: int = 3
                    ):
    """
    Robot heart gesture demonstration function

    Parameters:
        robot (GalbotRobot): GalbotRobot instance
        joint_group_names (list): List of joint group names to control
        position_seq (list): List of joint group angle sequences to set
        is_blocking (bool): Whether to set angles in blocking mode
        max_speed (float): Maximum speed
        timeout_s (float): Timeout time (seconds)
        retry_count (int, optional): Number of retries, default is 3

    Returns:
        None
    """

    # Get current joint group angles for subsequent restoration
    original_pos = robot.get_joint_positions(joint_group_names, [])
    print(f"Current angles of joint group {joint_group_names}: {original_pos}")

    # Start heart gesture
    pos_idx = 0
    print("Starting heart gesture...")
    while True:
        time.sleep(1)
        pos = position_seq[pos_idx]
        control_status = robot.set_joint_positions(
            pos, joint_group_names, [], is_blocking, max_speed, timeout_s
        )

        # If setting fails, retry 3 times
        retry_cnt = retry_count
        while control_status != ControlStatus.SUCCESS and retry_cnt > 0:
            print(f"Setting angles for joint group {joint_group_names} failed, retrying {retry_cnt}...")
            retry_cnt = retry_cnt - 1
            time.sleep(1)
            control_status = robot.set_joint_positions(
                pos, joint_group_names, [], is_blocking, max_speed, timeout_s
            )
        # If successful, switch to next pose sequence
        if control_status == ControlStatus.SUCCESS:
            print(f"Setting angles for joint group {joint_group_names} successful")
            pos_idx = pos_idx + 1
        # If failed, break the loop
        else:
            print(f"Setting angles for joint group {joint_group_names} failed")
            break

        # If all pose sequences are completed, break the loop
        if pos_idx > len(position_seq) - 1:
            break

    # Get current joint group angles
    print("Showing heart gesture for 15 seconds, then restoring original pose...")
    time.sleep(5)

    # Restore original pose joint group angles
    control_status = robot.set_joint_positions(
        original_pos, joint_group_names, [], is_blocking, max_speed, timeout_s
    )
    # If setting fails, retry 5 times
    retry_cnt = retry_count
    while control_status != ControlStatus.SUCCESS and retry_cnt > 0:
        print(f"Setting angles for joint group {joint_group_names} failed, retrying {retry_cnt}...")
        retry_cnt = retry_cnt - 1
        time.sleep(2)
        control_status = robot.set_joint_positions(
            original_pos, joint_group_names, [], is_blocking, max_speed, timeout_s
        )
    # If successful, restore original pose
    if control_status == ControlStatus.SUCCESS:
        print(f"Restoring angles for joint group {joint_group_names} successful")
    else:
        print(f"Restoring angles for joint group {joint_group_names} failed")

def check_robot_safety():
    """Check if the robot is safe"""
    # Prompt for precautions
    print("⚠️  Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations.")
    while True:
        key = input("Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...")
        if key == 'y':
            print("User confirmed, continuing execution...")
            break
        elif key == 'n':
            print("User not confirmed, program exiting...")
            exit(1)
        else:
            print("Input error, please enter 'y' or 'n'")

def main():
    check_robot_safety()

    try:
        # Get robot instance
        robot = GalbotRobot()
        
        # Initialize robot
        state = robot.init()
        if not state:
            print(f"Initialization failed")
            exit(1)
        else:
            print(f"Initialization successful")
            print(f"Is robot running: {robot.is_running()}")

        # Wait for data preparation
        time.sleep(3)

        # Get list of joint names
        joint_names = robot.get_joint_names()
        if len(joint_names) > 0:
            print(f"List of joint names: {joint_names}")
        else:
            print(f"Failed to get list of joint names")

        # Get joint positions using joint group names, empty returns all joints by default
        joint_group_names = ["left_arm", "right_arm"]
        # Left and right arm heart gesture sequence
        position_seq = [
            [1.53, 0.36, -2.54, -1.80, 0.12, -0.82, 0.09, # left_arm
             -1.53, -0.36, 2.54, 1.80, -0.12, 0.82, -0.09] # right_arm
        ]
        # Whether to block and wait for joints to reach position
        is_blocking = True
        # Limit maximum joint speed to 0.1rad/s
        max_speed = 0.1
        # Maximum blocking wait time
        timeout_s = 20

        # Perform heartbeat gesture
        demo_heart_pose(robot, joint_group_names, position_seq,
                        is_blocking, max_speed, timeout_s)

    except Exception as e:
        print(f"An exception occurred: {e}")
    finally:
        # Actively send SIGINT shutdown signal
        robot.request_shutdown()
        # Wait to enter shutdown state
        robot.wait_for_shutdown()
        # Release SDK resources
        robot.destroy()
        print('Resource release successful')


if __name__ == "__main__":
    main()

Example 2. Arm Manipulation

Applicable Scenarios: Learn forward/inverse kinematics and use the planned result to control an arm.

examples/python/tutorials/example2_arm_manipulation.py

"""
Note: When running this example, please ensure the robot's motion control service `/data/galbot/bin/service_motion_plan`,
    robot state publishing service `/data/galbot/bin/robot_state_publish`,
    and hand-eye calibration publishing service `/data/galbot/bin/eyehand_calib_publish` are loaded;
"""
try:
    import galbot_sdk.g1 as gm
    from galbot_sdk.g1 import GalbotMotion
    from galbot_sdk.g1 import GalbotRobot
    from galbot_sdk.g1 import ControlStatus
except ImportError:
    print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
    exit(1)

import os

try:
    import numpy as np
except ImportError:
    os.system("pip install numpy")
    import numpy as np

import time
from typing import Sequence, Dict

def printStatus(status):
    if(status == gm.MotionStatus.SUCCESS):
        print("Execution result: SUCCESS, Execution successful")
    elif(status == gm.MotionStatus.TIMEOUT):
        print("Execution result: TIMEOUT, Execution timeout")
    elif(status == gm.MotionStatus.FAULT):
        print("Execution result: FAULT, Fault occurred, unable to continue execution")
    elif(status == gm.MotionStatus.INVALID_INPUT):
        print("Execution result: INVALID_INPUT, Input parameters do not meet requirements")
    elif(status == gm.MotionStatus.INIT_FAILED):
        print("Execution result: INIT_FAILED, Internal communication component creation failed")
    elif(status == gm.MotionStatus.IN_PROGRESS):
        print("Execution result: IN_PROGRESS, Moving but not in position")
    elif(status == gm.MotionStatus.STOPPED_UNREACHED):
        print("Execution result: STOPPED_UNREACHED, Stopped but did not reach target")
    elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
        print("Execution result: DATA_FETCH_FAILED, Data acquisition failed")
    elif(status == gm.MotionStatus.PUBLISH_FAIL):
        print("Execution result: PUBLISH_FAIL, Data sending failed")
    elif(status == gm.MotionStatus.COMM_DISCONNECTED):
        print("Execution result: COMM_DISCONNECTED, Connection failed")

def quat_normalize(q: np.ndarray) -> np.ndarray:
    q = np.array(q, dtype=np.float64)
    return q / np.linalg.norm(q)

def quat_conjugate(q: np.ndarray) -> np.ndarray:
    """
    Calculate the conjugate of a quaternion

    Parameters:
        q (np.ndarray): Input quaternion [x, y, z, w]

    Returns:
        np.ndarray: Conjugate of the quaternion [x, y, z, w]
    """
    qx, qy, qz, qw = q
    return np.array([-qx, -qy, -qz, qw])

def quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
    """
    Calculate the product of two quaternions

    Parameters:
        q1 (np.ndarray): First quaternion [x, y, z, w]
        q2 (np.ndarray): Second quaternion [x, y, z, w]

    Returns:
        np.ndarray: Product of the two quaternions [x, y, z, w]
    """
    x1, y1, z1, w1 = q1
    x2, y2, z2, w2 = q2
    return np.array([
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2,
        w1*w2 - x1*x2 - y1*y2 - z1*z2
    ])

def orientation_error_angle(A: np.ndarray, B: np.ndarray) -> float:
    """
    Calculate the rotation angle error between two quaternions (radians)

    Parameters:
        A (np.ndarray): First quaternion [x, y, z, w]
        B (np.ndarray): Second quaternion [x, y, z, w]

    Returns:
        float: Rotation angle error (radians)
    """
    qA = quat_normalize(A[3:7])
    qB = quat_normalize(B[3:7])

    q_err = quat_multiply(qB, quat_conjugate(qA))
    q_err = quat_normalize(q_err)

    # Numerical stability
    qw = np.clip(q_err[3], -1.0, 1.0)

    angle = 2 * np.arccos(qw)
    return angle  # Unit: radians


def calculate_error(pose1: Sequence[float], pose2: Sequence[float]) -> Dict[str, float]:
    """
    Calculate position error and rotation error between two poses (radians)

    Parameters:
        pose1 (Sequence[float]): First pose [x, y, z, qx, qy, qz, qw]
        pose2 (Sequence[float]): Second pose [x, y, z, qx, qy, qz, qw]

    Returns:
        dict: Dictionary containing position error (meters) and rotation error (radians)
    """
    A, B = np.array(pose1), np.array(pose2)
    pos_err = np.linalg.norm(A[:3] - B[:3])
    rot_err = orientation_error_angle(A, B)

    return {
        "position_error_norm": pos_err,
        "orientation_error_rad": rot_err,
        "orientation_error_deg": np.degrees(rot_err)
    }

def check_robot_safety():
    """Check if the robot is safe"""
    # Prompt for precautions
    print("⚠️  Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations.")
    while True:
        key = input("Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...")
        if key == 'y':
            print("User confirmed, continuing execution...")
            break
        elif key == 'n':
            print("User not confirmed, program exiting...")
            exit(1)
        else:
            print("Input error, please enter 'y' or 'n'")

def main():
    check_robot_safety()
    try:
        # Get GalbotMotion singleton and initialize
        robot = GalbotRobot()
        motion = GalbotMotion()

        if motion.init():
            print("GalbotMotion initialization successful")
        else:
            print("GalbotMotion initialization failed")
        if robot.init():
            print("GalbotRobot initialization successful")
        else:
            print("GalbotRobot initialization failed")

        # Program starts immediately, wait for data readiness time
        time.sleep(3)

        # Define target pose
        chain_pose_baselink = {
            "leg": [0.0596,-0.0000,1.0327,0.5000,0.5003,0.4997,0.5000],
            "head": [0.0599,0.0002,1.4098,-0.7072,0.0037,0.0037,0.7069],
            "left_arm": [0.1267,0.2342,0.7356,0.0220,0.0127,0.0343,0.9991],
            "right_arm": [0.1267,-0.2345,0.7358,-0.0225,0.0126,-0.0343,0.9991]
        }

        # set initial joint positions
        joint_pos = [0.5, 1.5, 1.0, 0.0, 0.0,
                     0.0, 0.0,
                     2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
                     -2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0]
        joint_groups_names = ["leg", "head", "left_arm", "right_arm"]
        joint_names = []
        is_blocking = True
        max_speed_rad_s = 0.1
        timeout_s = 30.0

        status = robot.set_joint_positions(
            joint_pos, joint_groups_names, joint_names, is_blocking, max_speed_rad_s, timeout_s
        )

        if status != ControlStatus.SUCCESS:
            print("set join position failed")
        else:
            print("set join position successful")
                
        # Define target chain name, target pose, reference pose, end link
        target_frame = "EndEffector"
        reference_frame = "base_link"
        target_chain = "left_arm"
        end_link = "left_arm_end_effector_mount_link"

        # 1. Get current left_arm end-effector pose
        try:
            status, original_pose = motion.get_end_effector_pose_on_chain(
                chain_name=target_chain,
                frame_id=target_frame,
                reference_frame=reference_frame
            )
            assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
            print(f"✅ Current {target_chain} end-effector pose: {original_pose}")
            time.sleep(0.8)
        except Exception as e:
            print(f"❌ Exception getting end-effector pose by chain name: {e}")

        # 2. Solve joint angles based on target pose IK and verify the solution
        # 2.1 Solve joint angles joint_angles_ik for target pose through IK
        try:
            status, joint_angles_ik = motion.inverse_kinematics(
                target_pose=chain_pose_baselink[target_chain],
                chain_names=[target_chain],
                target_frame=target_frame,
                reference_frame=reference_frame,
                enable_collision_check=False # Disable collision detection
            )
            assert status == gm.MotionStatus.SUCCESS, "IK solving failed"
            print(f"✅ Target {target_chain} IK solving successful joint_angles_ik: {joint_angles_ik}")
            time.sleep(1)
        except Exception as e:
            print(f"❌ IK solving exception: {e}")

        # 2.2 Set end-effector pose to target pose tgt_pose_ik by setting joint group angles joint_angles_ik
        try:
            status = robot.set_joint_positions(
                joint_angles_ik[target_chain], 
                [target_chain], 
                [], 
                True,
                0.1,
                20.0,
            )
            assert status == ControlStatus.SUCCESS, "Setting joint group angles failed"
            print(f"✅ Setting {target_chain} joint group angles successful.")
            time.sleep(1)
        except Exception as e:
            print(f"❌ Setting {target_chain} joint group angles exception: {e}")

        # 2.3 Verify whether the set joint group angles are consistent with the solved angles
        try:
            status, tgt_pose_ik = motion.get_end_effector_pose_on_chain(
                chain_name=target_chain,
                frame_id=target_frame,
                reference_frame=reference_frame
            )
            assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
            print(f"✅ Getting {target_chain} end-effector pose successful: {tgt_pose_ik}")
            time.sleep(1)

            error = calculate_error(tgt_pose_ik, chain_pose_baselink[target_chain])
            print(f"End-effector pose error: {error}")
        except Exception as e:
            print(f"❌ Getting {target_chain} end-effector pose exception: {e}")

        # 2.4 Verify whether the end-effector pose tgt_pose_fk corresponding to joint group angles joint_angles_ik solved by FK is consistent with target pose tgt_pose_ik
        try:
            status, tgt_pose_fk = motion.forward_kinematics(
                target_frame=end_link,
                reference_frame=reference_frame,
                joint_state=joint_angles_ik,
                params=gm.Parameter()
            )
            assert status == gm.MotionStatus.SUCCESS, "FK solving failed"
            print(f"✅ Target {target_chain} FK solving successful: {tgt_pose_fk}")
            time.sleep(1)

            error = calculate_error(tgt_pose_fk, chain_pose_baselink[target_chain])
            print(f"FK solving error: {error}")
        except Exception as e:
            print(f"❌ FK solving exception: {e}")

        time.sleep(3)
        print()

        # 3. Restore to original pose by setting end-effector pose
        # 3.1 Set end-effector pose to restore to original pose
        try:
            status = motion.set_end_effector_pose(
                target_pose=original_pose,
                end_effector_frame=target_chain,
                reference_frame=reference_frame,
                enable_collision_check=False,
                is_blocking=True,
                timeout=5.0,
                params=gm.Parameter()
            )
            assert status == gm.MotionStatus.SUCCESS, "Setting end-effector pose failed"
            print(f"✅ Setting end-effector pose successful: status={status}")
            time.sleep(1)
        except Exception as e:
            print(f"❌ Setting {target_chain} end-effector pose exception: {e}")

        # 3.2 Get end-effector pose and verify whether it has been restored to original pose
        try:
            status, original_pose_rec = motion.get_end_effector_pose_on_chain(
                chain_name=target_chain,
                frame_id=target_frame,
                reference_frame=reference_frame
            )
            assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
            print(f"✅ Getting {target_chain} end-effector pose successful: {original_pose_rec}")
            time.sleep(0.8)
            
            error = calculate_error(original_pose_rec, original_pose)
            print(f"Restore end-effector pose error: {error}")
        except Exception as e:
            print(f"❌ Setting end-effector pose exception: {e}")
    
    except Exception as e:
        print(f"❌ Main program exception: {e}")
    finally:
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()

if __name__=="__main__":
    main()

Example 3. Robot Navigation

Applicable Scenarios: Initialize navigation, localize the robot, and navigate to a target pose.

examples/python/tutorials/example3_robot_navigation.py

"""
Note: When running this example, please confirm that the robot's navigation function `/data/galbot/bin/service_navigation_plan` has been loaded;
"""
try:
    from galbot_sdk.g1 import GalbotNavigation
    from galbot_sdk.g1 import GalbotRobot
except ImportError:
    print("Failed to import galbot_sdk, please install it first or check if it's in PYTHONPATH")
    exit(1)

import os

try:
    import numpy as np
except ImportError:
    os.system("pip install numpy")
    import numpy as np

import time
from typing import Sequence, Dict

def quat_normalize(q: np.ndarray) -> np.ndarray:
    q = np.array(q, dtype=np.float64)
    return q / np.linalg.norm(q)

def quat_conjugate(q: np.ndarray) -> np.ndarray:
    """
    Calculate the conjugate of a quaternion

    Parameters:
        q (np.ndarray): Input quaternion [x, y, z, w]

    Returns:
        np.ndarray: Conjugate of the quaternion [x, y, z, w]
    """
    qx, qy, qz, qw = q
    return np.array([-qx, -qy, -qz, qw])

def quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
    """
    Calculate the product of two quaternions

    Parameters:
        q1 (np.ndarray): First quaternion [x, y, z, w]
        q2 (np.ndarray): Second quaternion [x, y, z, w]

    Returns:
        np.ndarray: Product of the two quaternions [x, y, z, w]
    """
    x1, y1, z1, w1 = q1
    x2, y2, z2, w2 = q2
    return np.array([
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2,
        w1*w2 - x1*x2 - y1*y2 - z1*z2
    ])

def quat_to_matrix(q: Sequence[float]) -> np.ndarray:
    """
    Convert quaternion [x, y, z, w] to a 3x3 rotation matrix.
    """
    x, y, z, w = quat_normalize(q)
    xx, yy, zz = x * x, y * y, z * z
    xy, xz, yz = x * y, x * z, y * z
    wx, wy, wz = w * x, w * y, w * z

    return np.array([
        [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz),         2.0 * (xz + wy)],
        [2.0 * (xy + wz),         1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
        [2.0 * (xz - wy),         2.0 * (yz + wx),       1.0 - 2.0 * (xx + yy)],
    ], dtype=np.float64)

def matrix_to_quat(m: np.ndarray) -> np.ndarray:
    """
    Convert a 3x3 rotation matrix to quaternion [x, y, z, w].
    """
    m = np.asarray(m, dtype=np.float64)
    trace = float(np.trace(m))

    if trace > 0.0:
        s = np.sqrt(trace + 1.0) * 2.0
        w = 0.25 * s
        x = (m[2, 1] - m[1, 2]) / s
        y = (m[0, 2] - m[2, 0]) / s
        z = (m[1, 0] - m[0, 1]) / s
    elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
        s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0
        w = (m[2, 1] - m[1, 2]) / s
        x = 0.25 * s
        y = (m[0, 1] + m[1, 0]) / s
        z = (m[0, 2] + m[2, 0]) / s
    elif m[1, 1] > m[2, 2]:
        s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0
        w = (m[0, 2] - m[2, 0]) / s
        x = (m[0, 1] + m[1, 0]) / s
        y = 0.25 * s
        z = (m[1, 2] + m[2, 1]) / s
    else:
        s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0
        w = (m[1, 0] - m[0, 1]) / s
        x = (m[0, 2] + m[2, 0]) / s
        y = (m[1, 2] + m[2, 1]) / s
        z = 0.25 * s

    return quat_normalize([x, y, z, w])

def orientation_error_angle(A: np.ndarray, B: np.ndarray) -> float:
    """
    Calculate the rotation angle error between two quaternions (in radians)

    Parameters:
        A (np.ndarray): First quaternion [x, y, z, w]
        B (np.ndarray): Second quaternion [x, y, z, w]

    Returns:
        float: Rotation angle error (in radians)
    """
    qA = quat_normalize(A[3:7])
    qB = quat_normalize(B[3:7])

    q_err = quat_multiply(qB, quat_conjugate(qA))
    q_err = quat_normalize(q_err)

    # Numerically stable
    qw = np.clip(q_err[3], -1.0, 1.0)

    angle = 2 * np.arccos(qw)
    return angle  # Unit: radians


def calculate_error(pose1: Sequence[float], pose2: Sequence[float]) -> Dict[str, float]:
    """
    Calculate the position error and rotation error between two poses (in radians)

    Parameters:
        pose1 (Sequence[float]): First pose, [x, y, z, qx, qy, qz, qw]
        pose2 (Sequence[float]): Second pose, [x, y, z, qx, qy, qz, qw]

    Returns:
        dict: Dictionary containing position error (in meters) and rotation error (in radians)
    """
    A, B = np.array(pose1), np.array(pose2)
    pos_err = np.linalg.norm(A[:3] - B[:3])
    rot_err = orientation_error_angle(A, B)

    return {
        "position_error_norm": pos_err,
        "orientation_error_rad": rot_err,
        "orientation_error_deg": np.degrees(rot_err)
    }

def local_pose_to_global(start_pose: Sequence[float], local_pose: Sequence[float]):
    """
    Convert local pose to global pose

    Parameters:
        start_pose (Sequence[float]): Start pose, [x, y, z, qx, qy, qz, qw]
        local_pose (Sequence[float]): Local pose, [x, y, z, qx, qy, qz, qw]
    
    Returns:
        Sequence[float]: Global pose, [x, y, z, qx, qy, qz, qw]
    """
    start_mat = np.eye(4)
    start_mat[:3, :3] = quat_to_matrix(start_pose[3:7])
    start_mat[:3, 3] = [start_pose[0], start_pose[1], start_pose[2]]

    local_mat = np.eye(4)
    local_mat[:3, :3] = quat_to_matrix(local_pose[3:7])
    local_mat[:3, 3] = [local_pose[0], local_pose[1], local_pose[2]]

    global_mat = start_mat @ local_mat

    return global_mat[:3, 3].tolist() + matrix_to_quat(global_mat[:3, :3]).tolist()

def demo_square_move(robot: GalbotRobot, nav: GalbotNavigation):
    """
    Demonstrate robot moving in a square pattern in navigation environment

    Parameters:
        robot (GalbotRobot): Robot instance
        nav (GalbotNavigation): Navigation instance
    """
    try:
        start_pose = nav.get_current_pose()
    except Exception as e:
        print(f"Failed to get current pose: {e}")
        return
    
    # Move forward 0.5m, turn left 90 degrees
    local_pose = [0.5, 0.0, 0.0, 0.0, 0.0, 0.707, 0.707] 
    
    try:
        # Move forward 0.5m each time, turn left 90 degrees, repeat 4 times to form a square
        for _ in range(4):
            # Calculate target pose
            cur_pose = nav.get_current_pose()
            goal_pose = local_pose_to_global(cur_pose, local_pose)
            
            # Check if path is reachable
            if nav.check_path_reachability(goal_pose, cur_pose):
                # Navigate to target pose
                retry_cnt = 3
                while True:
                    status = nav.navigate_to_goal(goal_pose, enable_collision_check=True, is_blocking=True, timeout=30)
                    time.sleep(0.5)
                    retry_cnt -= 1
                    if nav.check_goal_arrival() or retry_cnt < 0:
                        break
                    else:
                        print(f"Navigation failed, retrying...{retry_cnt}")
                print("navigate_to_goal return status:", status)
                print("Has arrived:", nav.check_goal_arrival())
            else:
                print("Path unreachable or unsafe")

        cur_pose = nav.get_current_pose()
        print(f"Current pose: {cur_pose}, Error from start pose: {calculate_error(cur_pose, start_pose)}")
    except Exception as e:
        print(f"Exception occurred during navigation: {e}")

def move_to_original(robot: GalbotRobot, nav: GalbotNavigation):
    """
    Demonstrate robot returning to start pose in navigation environment

    Parameters:
        robot (GalbotRobot): Robot instance
        nav (GalbotNavigation): Navigation instance
    """
    cur_pose = nav.get_current_pose()
    goal_pose = [0, 0, 0, 0, 0, 0, 1]
    
    try:
        if nav.check_path_reachability(goal_pose, cur_pose):
            retry_cnt = 3
            while True:
                status = nav.navigate_to_goal(goal_pose, enable_collision_check=True, is_blocking=True, timeout=30)
                time.sleep(0.5)
                retry_cnt -= 1
                if nav.check_goal_arrival() or retry_cnt < 0:
                    break
                else:
                    print(f"Navigation failed, retrying...{retry_cnt}")
            print("navigate_to_goal return status:", status)
            print("Has arrived:", nav.check_goal_arrival())
        else:
            print("Path unreachable or unsafe")
    except Exception as e:
        print(f"Exception occurred during navigation: {e}")

def check_robot_safety():
    """Check if robot is safe"""
    # Prompt important notes
    print("⚠️  Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations; 3. Please ensure the area around the robot is clear of obstacles.")
    while True:
        key = input("Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)...")
        if key == 'y':
            print("User confirmed, continuing...")
            break
        elif key == 'n':
            print("User did not confirm, exiting program...")
            exit(1)
        else:
            print("Invalid input, please enter 'y' or 'n'")

def main():
    check_robot_safety()
    try:
        # Get robot instance
        robot = GalbotRobot()
        # Get navigation instance
        nav = GalbotNavigation()
        
        # Initialize robot
        if robot.init():
            print("Robot initialization successful")
        else:
            print("Robot initialization failed")
        # Initialize navigation
        if nav.init():  
            print("Navigation initialization successful")
        else:
            print("Navigation initialization failed")
        
        # Wait for data preparation
        time.sleep(1)
        
        # Check initial localization status
        is_localized = nav.is_localized()
        if not is_localized:
            print("Localization failed, attempting to re-localize: Please move the robot to the origin of the map!")
        time.sleep(3)
        while not is_localized:
            nav.relocalize([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
            time.sleep(0.5)
            is_localized = nav.is_localized()

        # square_move
        demo_square_move(robot, nav)

    except Exception as e:
        print(f"Exception occurred: {e}")
    finally:
        robot.request_shutdown()
        robot.wait_for_shutdown()
        robot.destroy()

if __name__ == "__main__":
    main()

Example 4. Sensor Data Collection

Applicable Scenarios: Acquire camera, depth, lidar, IMU, and other robot sensor data.

examples/python/tutorials/example4_sensor_data_collect.py

"""
Note: When running this example, please confirm that the robot's left arm camera driver `/data/galbot/bin/left_arm_camera_capture`
    and radar driver `/data/galbot/bin/service_lidar_capture` have been loaded;
"""
try:
    from galbot_sdk.g1 import GalbotRobot
    from galbot_sdk.g1 import RgbOutputFormat, SensorType
except ImportError:
    print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
    exit(1)

import os

try:
    import open3d as o3d
except ImportError:
    os.system("pip install open3d")
    import open3d as o3d

try:
    import cv2
except ImportError:
    os.system("pip install opencv-python")
    import cv2

try:
    import numpy as np
except ImportError:
    os.system("pip install numpy")
    import numpy as np

import time
from typing import Dict


def convert_pointcloud(cloud):
    """
    Convert cloud dict to NumPy array dictionary

    Parameters:
        cloud (dict): PointCloud2 protobuf message object

    Returns:
        Dictionary: {field_name: NumPy array}
        - Single-element fields: shape (N,)
        - Multi-element fields: shape (N, count) or (N,)
        - N = width * height (total number of points)
    """

    if not cloud:
        return {}

    num_points = cloud["height"] * cloud["width"]
    if num_points == 0:
        return {}

    DTYPE_MAP = {
        1: np.int8,
        2: np.uint8,
        3: np.int16,
        4: np.uint16,
        5: np.int32,
        6: np.uint32,
        7: np.float32,
        8: np.float64
    }
    dtype_list = []
    for field in cloud["fields"]:
        # Get base data type
        np_dtype_class = DTYPE_MAP.get(field["datatype"])
        if np_dtype_class is None:
            raise ValueError(f"Unsupported data type: {field['datatype']}")

        dtype_inst = np.dtype(np_dtype_class)

        # Handle byte order (endianness)
        if dtype_inst.itemsize > 1:
            byteorder = '>' if cloud["is_bigendian"] else '<'
            dtype_inst = dtype_inst.newbyteorder(byteorder)

        # Add to dtype list
        if field["count"] == 1:
            dtype_list.append((field["name"], dtype_inst))
        else:
            # Multi-element fields (e.g., rgb)
            dtype_list.append((field["name"], dtype_inst, field["count"]))

    # Create structured dtype
    dtype = np.dtype(dtype_list)

    # Data integrity check
    expected_size = num_points * cloud["point_step"]
    if len(cloud["data"]) < expected_size:
        raise ValueError(
            f"Insufficient data length: expected {expected_size} bytes, "
            f"actual {len(cloud['data'])} bytes"
        )

    # Create NumPy structured array from binary data
    # count parameter ensures only expected number of points are read
    arr = np.frombuffer(cloud["data"], dtype=dtype, count=num_points)

    # Convert to regular dictionary (copy data to avoid modifying original)
    result = {}
    for field in cloud["fields"]:
        field_data = arr[field["name"]]

        # Handle shape of multi-element fields
        if field["count"] == 1:
            result[field["name"]] = field_data.copy()
        else:
            # Keep original shape or flatten, choose according to needs
            result[field["name"]] = field_data.copy()

    return result


def get_xyz_array(pointcloud_dict: Dict[str, np.ndarray], 
                remove_nan: bool = False) -> np.ndarray:
    """
    Extract XYZ coordinate array from converted point cloud dictionary

    Parameters:
        pointcloud_dict (Dict[str, np.ndarray]): Dictionary returned by pointcloud2_to_numpy()
        remove_nan (bool, optional): Whether to remove points containing NaN (for FLOAT32/FLOAT64 types). Defaults to False.

    Returns:
        Nx3 point coordinate array
    """
    required = ['x', 'y', 'z']
    if not all(k in pointcloud_dict for k in required):
        raise ValueError("Point cloud data missing required xyz fields")

    points = np.stack([pointcloud_dict['x'], 
                    pointcloud_dict['y'], 
                    pointcloud_dict['z']], axis=1)

    if remove_nan:
        mask = ~np.isnan(points).any(axis=1)
        points = points[mask]

    return points

def save_xyz_to_pcd(xyz_array: np.ndarray, filename: str, binary: bool = False) -> None:
    """
    Save XYZ coordinates to PCD file format (simplest option for coordinate-only data)

    Parameters:
        xyz_array (np.ndarray): Nx3 array of XYZ coordinates
        filename (str): Output PCD file path
        binary (bool, optional): If True, saves in binary format; otherwise ASCII. Defaults to False.
    """
    if xyz_array.ndim != 2 or xyz_array.shape[1] != 3:
        raise ValueError(f"xyz_array must have shape (N, 3), got {xyz_array.shape}")

    num_points = xyz_array.shape[0]
    header = [
        "# .PCD v0.7 - Point Cloud Data file format",
        "VERSION 0.7",
        "FIELDS x y z",
        "SIZE 4 4 4",
        "TYPE F F F",  # F = float32
        "COUNT 1 1 1",
        f"WIDTH {num_points}",
        "HEIGHT 1",
        "VIEWPOINT 0 0 0 1 0 0 0",
        f"POINTS {num_points}",
        f"DATA {'binary' if binary else 'ascii'}"
    ]

    if binary:
        with open(filename, 'wb') as f:
            f.write(('\n'.join(header) + '\n').encode('ascii'))
            f.write(xyz_array.astype(np.float32).tobytes())
    else:
        with open(filename, 'w') as f:
            f.write('\n'.join(header) + '\n')
            np.savetxt(f, xyz_array, fmt='%f')


def decode_compressed_image(compressed_image):
    """
    decode CompressedImage image

    Parameters:
        compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]

    Returns:
        numpy.ndarray: decoded image
    """
    image_data = compressed_image["data"]
    if compressed_image["format"] == "jpeg":
        return decode_rgb_image(image_data)
    elif compressed_image["format"] == "16UC1":
        return decode_depth_image(compressed_image)
    else:
        raise ValueError(f"Unsupport data format: {compressed_image['format']}")

def decode_rgb_image(image_data):
    """decode rgb image"""
    nparr = np.frombuffer(image_data, np.uint8)
    img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
    if img is None:
        raise ValueError("Fail to Decode RGB Image")
    return img

def decode_depth_image(image_data):
    """decode depth image"""
    depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()

    # Check if height and width exist
    if "height" not in image_data or "width" not in image_data:
        raise ValueError("Missing 'height' or 'width' in depth image metadata.")
    if image_data["height"] == 0 or image_data["width"] == 0:
        raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")

    # Parse depth image
    depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
    depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]

    return depth_img

def depth_rgb_to_pointcloud(depth, rgb, fx, fy, cx, cy, depth_scale=1.0):
    """
    Convert depth map and RGB image to point cloud

    Parameters:
        depth: (H, W) depth map
        rgb:   (H, W, 3) RGB image
        depth_scale: Depth unit scaling (use 0.001 for mm->m conversion)
    
    Returns:
        points: (N, 3) point cloud coordinate array
        colors: (N, 3) point cloud color array (0-1 range)
    """
    assert depth.shape[:2] == rgb.shape[:2]

    H, W = depth.shape

    # Pixel coordinates
    u, v = np.meshgrid(np.arange(W), np.arange(H))

    # Depth (converted to meters)
    Z = depth.astype(np.float32) * depth_scale

    # Filter invalid depths
    valid = Z > 0

    X = (u - cx) * Z / fx
    Y = (v - cy) * Z / fy

    points = np.stack((X, Y, Z), axis=-1)
    colors = rgb.astype(np.float32) / 255.0

    # Keep only valid points
    points = points[valid]
    colors = colors[valid]

    return points, colors

def check_robot_safety():
    """Check if robot is safe"""
    # Prompt important notes
    print("⚠️  Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations.")
    while True:
        key = input("Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)...")
        if key == 'y':
            print("User confirmed, continuing...")
            break
        elif key == 'n':
            print("User did not confirm, exiting program...")
            exit(1)
        else:
            print("Invalid input, please enter 'y' or 'n'")

def main():
    SHOW_IMAGE = False
    check_robot_safety()
    try:
        # Get and initialize the GalbotRobot singleton
        robot = GalbotRobot()

        # Get RGB and depth images from the left arm, depth images from the right arm, 
        # base LiDAR data, and torso IMU data
        enable_sensor_set = {SensorType.LEFT_ARM_CAMERA, # Left arm depth camera
                            SensorType.LEFT_ARM_DEPTH_CAMERA, # Left arm RGB camera
                            SensorType.BASE_LIDAR,} # Base LiDAR

        # To save resource overhead, only cameras and radar sensors passed during initialization can acquire data
        robot.init(enable_sensor_set)
        print("Initialization successful")
        # Program starts immediately, wait for data readiness
        time.sleep(5)

        # Get RGB image from the left arm
        rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
        if not rgb_image_data:
            print("No rgb image data!")
        else:
            print("get rgb image suceess")
            print(rgb_image_data['header'])
            img = decode_compressed_image(rgb_image_data)
            
            # Save RGB image
            cv2.imwrite("rgb_image_data.jpg", img)
            # Visualize RGB image
            if SHOW_IMAGE:
                cv2.namedWindow("rgb image", cv2.WINDOW_NORMAL)
                cv2.imshow("rgb image", img)
                cv2.waitKey(0)
                cv2.destroyAllWindows()

        # Get depth image from the left arm
        depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
        if not depth_data or "data" not in depth_data:
            print("Depth camera not ready")
        else:
            print("get depth data suceess")
            print(depth_data['header'])
            depth_img_raw = decode_compressed_image(depth_data)
            depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize, mapping depth values to 0-1 range
            depth_img = depth_img.astype(np.uint8)

            # Save depth image
            cv2.imwrite("depth_data.jpg", depth_img)
            # Visualize depth image
            if SHOW_IMAGE:
                cv2.namedWindow("depth image", cv2.WINDOW_NORMAL)
                cv2.imshow("depth image", depth_img)
                cv2.waitKey(0)
                cv2.destroyAllWindows()

        # Get base LiDAR data
        lidar_data = robot.get_lidar_data(SensorType.BASE_LIDAR)
        if not lidar_data:
            print("No lidar data!")
        else:
            pointcloud_dict = convert_pointcloud(lidar_data)
            xyz_points = get_xyz_array(pointcloud_dict)
            save_xyz_to_pcd(xyz_points, "output_xyz.pcd")
            print(pointcloud_dict)
            print("get lidar data success")

            # Visualize LiDAR point cloud
            if SHOW_IMAGE:
                vis = o3d.visualization.Visualizer()
                vis.create_window()
                pcd = o3d.geometry.PointCloud()
                pcd.points = o3d.utility.Vector3dVector(xyz_points)
                vis.add_geometry(pcd)
                vis.run()
                vis.destroy_window()
        
        # Get torso IMU data
        imu_data = robot.get_imu_data(SensorType.TORSO_IMU)
        if not imu_data:
            print("No imu data!")
        else:
            print("get imu data suceess")
        
        try:
            camera_info = robot.get_camera_intrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
            if not camera_info:
                print("No camera info!")
            else:
                print(camera_info)
        except Exception as e:
            camera_info = {
                "width": 1280,
                "height": 720,
                "distortion_model": "plumb_bob",
                "camera_type": "D405",
                "K": [653.4349365234375, 0.0, 639.95159912109375, 
                    0.0, 652.48858642578125, 365.29425048828125, 
                    0.0, 0.0, 1.0],
            }
        
        # Convert depth map and RGB image to point cloud and save
        if depth_data and rgb_image_data:
            points, colors = depth_rgb_to_pointcloud(
                depth_img_raw,
                img,
                fx=camera_info['K'][0],
                fy=camera_info['K'][4],
                cx=camera_info['K'][2],
                cy=camera_info['K'][5],
                depth_scale=0.1   # If depth is in mm, set to 0.001
            )
            save_xyz_to_pcd(points, "left_arm_camera_pointcloud.pcd", binary=True)
            print(f"RGB fused depth map point cloud saved to left_arm_camera_pointcloud.pcd, number of points: {points.shape[0]}")
    except Exception as e:
        print(f"❌ Exception occurred: {e}")
    finally:
        # Actively send SIGINT exit signal
        robot.request_shutdown()
        # Wait to enter shutdown state
        robot.wait_for_shutdown()
        # Release SDK resources
        robot.destroy()
        print('Resource release successful')


if __name__=="__main__":
    main()

Example 5. Execute VLA

Applicable Scenarios: Demonstrate how a VLA client uses get_synced_observation to collect synchronized observations, receives a 23-dimensional absolute-joint action chunk including the head, and rapidly streams it frame by frame through set_joint_commands. Galbot supports demonstration data collection with leader-follower arms; see the TM01 user manual. The collected MCAP data can be converted to the LeRobot dataset format with galbot-mcap2lerobot for subsequent model training. After deploying the trained model as an inference service, refer to this example for synchronized observation acquisition and action-chunk execution. For model output expressed as end-effector EE poses, refer to set_end_effector_command.

examples/python/tutorials/example5_execute_vla.py

"""Execute absolute-joint VLA action chunks with Galbot SDK interfaces.

VLA control requires observations and actions to stay aligned in time. This
example uses ``get_synced_observation`` to acquire a synchronized camera image
and robot joint state for each model request. It then uses
``set_joint_commands`` to stream the model's 23-dimensional absolute-joint
action chunk frame by frame at the configured control frequency.

For the training-data workflow, Galbot officially supports demonstration data
collection with leader-follower arms. See the TM01 user manual:
https://developer.galbot.com/docs/tm01/1.4.0/zh/tm01
The collected MCAP data can be converted to the LeRobot dataset format with
galbot-mcap2lerobot for subsequent model training:
https://github.com/GalaxyGeneralRobotics/galbot-mcap2lerobot
After deploying the trained model as an inference service, use this example as
a reference for acquiring synchronized observations and executing its action
chunks on the robot.

This example demonstrates absolute joint-position model output. If a model
instead outputs end-effector poses (EE poses such as
``[x, y, z, qx, qy, qz, qw]``), use ``set_end_effector_command`` for real-time
Cartesian control. See
``examples/g1/python/galbot_robot/set_end_effector_commands.py`` for that API.

Before running, release the emergency-stop button and clear the area around the
robot. Keep a hand near the emergency-stop button during execution.
"""

import json
import time
from pathlib import Path
from typing import Dict, List, Sequence, Tuple

import numpy as np

try:
    from galbot_sdk.g1 import (
        ControlStatus,
        G1JointGroup,
        GalbotRobot,
        JointCommand,
        SensorType,
    )
except ImportError:
    print(
        "Failed to import galbot_sdk. Install the SDK or add it to PYTHONPATH "
        "before running this example."
    )
    raise SystemExit(1)


# The order of these groups defines the meaning of every action vector:
# 5 + 2 + 7 + 1 + 7 + 1 = 23 dimensions.
ACTION_GROUPS = [
    "leg",
    "head",
    "left_arm",
    "left_gripper",
    "right_arm",
    "right_gripper",
]

GROUP_SLICES = {
    "leg": slice(0, 5),
    "head": slice(5, 7),
    "left_arm": slice(7, 14),
    "left_gripper": slice(14, 15),
    "right_arm": slice(15, 22),
    "right_gripper": slice(22, 23),
}

GROUP_JOINT_NAMES = {
    "leg": [f"leg_joint{i}" for i in range(1, 6)],
    "head": [f"head_joint{i}" for i in range(1, 3)],
    "left_arm": [f"left_arm_joint{i}" for i in range(1, 8)],
    "left_gripper": ["left_gripper_joint1"],
    "right_arm": [f"right_arm_joint{i}" for i in range(1, 8)],
    "right_gripper": ["right_gripper_joint1"],
}

CONTROL_FREQUENCY_HZ = 20.0
# Number of model action frames returned and executed in one chunk.
ACTION_CHUNK_SIZE = 30
GRIPPER_VELOCITY = 0.1
GRIPPER_EFFORT = 10.0
TASK_PROMPT = "Based on the current view, predict the next robot action."
# Shared by the G1 Python and C++ tutorial examples.
ACTION_JSON_PATH = (
    Path(__file__).resolve().parents[2] / "assets" / "tutorials" / "example5_vla.json"
)


def confirm_safe_environment() -> None:
    """Require an explicit confirmation before sending robot commands."""
    print(
        "WARNING: Release the emergency-stop button, keep the robot in working "
        "mode, and clear people and obstacles from its motion range."
    )
    answer = input("Continue with the VLA execution example? [y/N]: ").strip().lower()
    if answer not in {"y", "yes"}:
        raise SystemExit("Execution cancelled by the user.")


def action_joint_names() -> List[str]:
    """Return joint names in exactly the same order as an action vector."""
    return [
        joint_name for group in ACTION_GROUPS for joint_name in GROUP_JOINT_NAMES[group]
    ]


def collect_observation(robot: GalbotRobot) -> Dict[str, object]:
    """Collect the synchronized image and state normally sent to a VLA server."""
    observation = robot.get_synced_observation(
        [SensorType.HEAD_LEFT_CAMERA],
        True,
    )
    if not observation:
        raise RuntimeError("get_synced_observation failed")

    image_message = observation.rgb_data_map.get(SensorType.HEAD_LEFT_CAMERA)
    if image_message is None:
        raise RuntimeError("Synchronized head-left image is missing")
    if observation.joint_state is None:
        raise RuntimeError("Synchronized joint state is missing")

    state_by_name = {
        joint_state.joint_name: joint_state
        for joint_state in observation.joint_state.joint_state_vec
    }
    missing_names = [name for name in action_joint_names() if name not in state_by_name]
    if missing_names:
        raise RuntimeError(f"Joint state is missing joints: {missing_names}")

    state = np.asarray(
        [state_by_name[name].position for name in action_joint_names()],
        dtype=np.float32,
    )
    return {
        "state": state,
        "image": bytes(image_message.data),
        "prompt": TASK_PROMPT,
    }


def load_mock_actions(json_path: Path) -> np.ndarray:
    """Load the action frames returned by the simulated VLA service."""
    if not json_path.is_file():
        raise FileNotFoundError(f"Mock VLA action file does not exist: {json_path}")

    with json_path.open("r", encoding="utf-8") as file:
        payload = json.load(file)

    actions = np.asarray(payload.get("frames"), dtype=np.float32)
    if payload.get("num_frames") != len(actions):
        raise ValueError(
            "Mock num_frames does not match the number of frames in the JSON: "
            f"{payload.get('num_frames')} != {len(actions)}"
        )
    return validate_action_chunk(actions)


def mock_vla_server(
    observation: Dict[str, object],
    all_actions: np.ndarray,
    cursor: int,
    chunk_size: int = ACTION_CHUNK_SIZE,
) -> Tuple[Dict[str, np.ndarray], int]:
    """Simulate one model request and return the next JSON action chunk.

    A real VLA service would replace this function and infer actions from the
    observation. This local version directly returns consecutive absolute joint
    frames loaded from the copied client-demo JSON file.
    """
    state = np.asarray(observation["state"], dtype=np.float32)
    expected_dim = len(action_joint_names())
    if state.shape != (expected_dim,):
        raise ValueError(
            f"Observation state has shape {state.shape}; expected ({expected_dim},)"
        )

    start = int(cursor)
    end = min(start + int(chunk_size), len(all_actions))
    actions = all_actions[start:end]
    print(
        "Mock VLA server received one request: "
        f"prompt={observation['prompt']!r}, "
        f"image_bytes={len(observation['image'])}, frames={start}:{end}"
    )
    print(f"Mock VLA server returned action chunk: shape={actions.shape}")
    return {"actions": actions}, end


def move_to_first_action(robot: GalbotRobot, first_action: np.ndarray) -> None:
    """Move safely to the absolute pose used by the copied JSON trajectory."""
    # Stabilize the lower body first, then move both arms and the head together.
    for position_groups in (["leg"], ["head", "left_arm", "right_arm"]):
        joint_positions = np.concatenate(
            [first_action[GROUP_SLICES[group]] for group in position_groups]
        ).tolist()
        status = robot.set_joint_positions(
            joint_positions=joint_positions,
            joint_groups=position_groups,
            joint_names=[],
            is_blocking=True,
            speed_rad_s=0.2,
            timeout_s=10.0,
        )
        if status != ControlStatus.SUCCESS:
            raise RuntimeError(
                f"Failed to move {position_groups} to the first JSON action: {status}"
            )

    gripper_targets = (
        (G1JointGroup.left_gripper, first_action[GROUP_SLICES["left_gripper"]][0]),
        (
            G1JointGroup.right_gripper,
            first_action[GROUP_SLICES["right_gripper"]][0],
        ),
    )
    for joint_group, position in gripper_targets:
        status = robot.set_gripper_command(
            end_effector=joint_group,
            width_m=float(position),
            velocity_mps=0.1,
            effort=GRIPPER_EFFORT,
            is_blocking=True,
        )
        if status != ControlStatus.SUCCESS:
            raise RuntimeError(
                f"Failed to move {joint_group} to the first JSON action: {status}"
            )

    print("Robot reached the first pose in the mock VLA action file.")


def validate_action_chunk(actions: np.ndarray) -> np.ndarray:
    """Validate the model result before converting it to SDK commands."""
    actions = np.asarray(actions, dtype=np.float32)
    expected_dim = len(action_joint_names())
    if actions.ndim != 2 or actions.shape[1] != expected_dim:
        raise ValueError(
            "VLA actions must have shape (T, D), where "
            f"D={expected_dim}; got {actions.shape}"
        )
    if not np.all(np.isfinite(actions)):
        raise ValueError("VLA actions contain NaN or infinity")
    return actions


def build_joint_commands(action: Sequence[float]) -> List[JointCommand]:
    """Convert one model action frame to SDK ``JointCommand`` objects."""
    values = list(float(value) for value in action)
    expected_dim = len(action_joint_names())
    if len(values) != expected_dim:
        raise ValueError(f"Action dimension is {len(values)}; expected {expected_dim}")

    gripper_indices = {
        action_joint_names().index("left_gripper_joint1"),
        action_joint_names().index("right_gripper_joint1"),
    }
    commands = []
    for index, value in enumerate(values):
        command = JointCommand()
        command.position = value
        if index in gripper_indices:
            command.velocity = GRIPPER_VELOCITY
            command.effort = GRIPPER_EFFORT
        commands.append(command)
    return commands


def execute_action_chunk(robot: GalbotRobot, actions: np.ndarray) -> None:
    """Stream one VLA action chunk with the high-frequency SDK interface."""
    actions = validate_action_chunk(actions)
    if actions.shape[0] == 0:
        print("The VLA model returned an empty chunk; execution is complete.")
        return

    period_s = 1.0 / CONTROL_FREQUENCY_HZ
    next_tick = time.monotonic()
    print(
        f"Streaming {actions.shape[0]} frames at {CONTROL_FREQUENCY_HZ:.1f} Hz "
        "with set_joint_commands..."
    )

    for frame_index, action in enumerate(actions, start=1):
        status = robot.set_joint_commands(
            joint_commands=build_joint_commands(action),
            joint_groups=ACTION_GROUPS,
            joint_names=[],
            time_from_start_s=0.0,
        )
        if status != ControlStatus.SUCCESS:
            raise RuntimeError(
                f"set_joint_commands failed at frame {frame_index}: {status}"
            )

        next_tick += period_s
        remaining_s = next_tick - time.monotonic()
        if remaining_s > 0.0:
            time.sleep(remaining_s)

    print("Action chunk execution completed.")


def main() -> None:
    """Request and execute JSON action chunks until the mock service is empty."""
    confirm_safe_environment()
    robot = GalbotRobot()
    initialized = False

    try:
        if not robot.init({SensorType.HEAD_LEFT_CAMERA}, True):
            raise RuntimeError("GalbotRobot initialization failed")
        initialized = True

        # Allow the first synchronized image and joint state to become ready.
        time.sleep(5.0)

        all_actions = load_mock_actions(ACTION_JSON_PATH)
        print(f"Loaded mock VLA actions from: {ACTION_JSON_PATH}")
        print(f"Action data shape: {all_actions.shape} (full23, including head)")
        move_to_first_action(robot, all_actions[0])

        cursor = 0
        while True:
            observation = collect_observation(robot)
            response, cursor = mock_vla_server(
                observation,
                all_actions,
                cursor,
            )
            if response["actions"].shape[0] == 0:
                print("Mock VLA server has no more actions.")
                break
            execute_action_chunk(robot, response["actions"])

        print("VLA execution example finished successfully.")
    finally:
        if initialized:
            robot.request_shutdown()
            robot.wait_for_shutdown()
            robot.destroy()
            print("SDK resources released.")


if __name__ == "__main__":
    main()

Example 6. Integrated Task: Navigation + Perception + Grasping + Placement

Applicable Scenarios: A reference for complete pick-and-place applications integrating perception, motion planning and control, and navigation. It demonstrates pre-grasp, lift, retreat, navigation between picking and placement, and linear grasp/place motions with move_line.

examples/python/tutorials/example6_pick_and_place.py

"""
This tutorial demonstrates a complete pick-and-place task for a standard application scenario.
It integrates navigation, perception, motion planning, and robot control, and can be used as a
reference implementation for standard applications.
Before the task starts, the robot moves to the known 21-joint initial pose defined by this example.
After picking, the robot navigates to its current map pose with the X coordinate offset by 0.1 m,
then performs placement.
The final grasp approach and place descent use `move_line`; other end-effector motions use
`set_end_effector_pose`.
"""

try:
    import galbot_sdk.g1 as gm
    from galbot_sdk.g1 import GalbotNavigation
    from galbot_sdk.g1 import GalbotRobot
    from galbot_sdk.g1 import GalbotMotion
    from galbot_sdk.g1 import G1JointGroup, SensorType
except ImportError:
    print(
        "Import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH"
    )
    exit(1)

import os

try:
    import numpy as np
except ImportError:
    os.system("pip install numpy")
    import numpy as np

try:
    import cv2
except ImportError:
    os.system("pip install opencv-python")
    import cv2

import time
from typing import Sequence

PLACE_NAVIGATION_X_OFFSET_M = 0.1
GRASP_TRANSITION_OFFSET_M = 0.05
PLACE_POSE_BASE = [0.4, 0.3, 0.7, 0.0, 0.0, 0.0, 1.0]
# Placeholder grasp pose for this runnable tutorial. Replace it with the
# base-frame pose produced by the application's perception module.
GRASP_POSE_BASE_PLACEHOLDER = [0.4, 0.3, 0.65, 0.0, 0.0, 0.0, 1.0]
INITIAL_LEG_JOINT_NAMES = [
    "leg_joint1",
    "leg_joint2",
    "leg_joint3",
    "leg_joint4",
    "leg_joint5",
]
INITIAL_LEG_JOINT_POSITIONS = [0.4, 1.2, 0.8, 0.0, 0.0]

INITIAL_UPPER_BODY_JOINT_NAMES = [
    "head_joint1",
    "head_joint2",
    "left_arm_joint1",
    "left_arm_joint2",
    "left_arm_joint3",
    "left_arm_joint4",
    "left_arm_joint5",
    "left_arm_joint6",
    "left_arm_joint7",
    "right_arm_joint1",
    "right_arm_joint2",
    "right_arm_joint3",
    "right_arm_joint4",
    "right_arm_joint5",
    "right_arm_joint6",
    "right_arm_joint7",
]
INITIAL_UPPER_BODY_JOINT_POSITIONS = [
    0.0,
    0.0,
    1.90,
    -1.46,
    -0.54,
    -1.96,
    0.0,
    -0.4,
    0.0,
    -1.90,
    1.46,
    0.54,
    1.96,
    0.0,
    0.4,
    0.0,
]


def move_to_initial_pose(robot: GalbotRobot) -> bool:
    """Move the legs first, then move the head and both arms together."""
    leg_status = robot.set_joint_positions(
        joint_positions=INITIAL_LEG_JOINT_POSITIONS,
        joint_names=INITIAL_LEG_JOINT_NAMES,
        is_blocking=True,
        speed_rad_s=0.2,
        timeout_s=20.0,
    )
    if leg_status != gm.ControlStatus.SUCCESS:
        print(f"❌ Failed to move the legs to the initial pose: status={leg_status}")
        return False
    print(f"✅ Successfully moved the legs to the initial pose: status={leg_status}")

    upper_body_status = robot.set_joint_positions(
        joint_positions=INITIAL_UPPER_BODY_JOINT_POSITIONS,
        joint_names=INITIAL_UPPER_BODY_JOINT_NAMES,
        is_blocking=True,
        speed_rad_s=0.2,
        timeout_s=20.0,
    )
    if upper_body_status != gm.ControlStatus.SUCCESS:
        print(
            "❌ Failed to move the head and arms to the initial pose: "
            f"status={upper_body_status}"
        )
        return False

    print(
        "✅ Successfully moved the head and arms to the initial pose: "
        f"status={upper_body_status}"
    )
    return True


def decode_compressed_image(compressed_image, camera_info={}):
    """
    decode CompressedImage image

    Parameters:
        compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]

    Returns:
        numpy.ndarray: decoded image
    """
    image_data = compressed_image["data"]
    if compressed_image["format"] == "jpeg":
        return decode_rgb_image(image_data)
    elif compressed_image["format"] == "16UC1":
        return decode_depth_image(
            image_data, compressed_image["depth_scale"], camera_info
        )
    else:
        raise ValueError(f"Unsupport data format: {compressed_image['format']}")


def decode_rgb_image(image_data):
    """decode rgb image"""
    nparr = np.frombuffer(image_data, np.uint8)
    img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
    if img is None:
        raise ValueError("Fail to Decode RGB Image")
    return img


def decode_depth_image(image_data, depth_scale, camera_info):
    """decode depth image"""
    depth_img = np.frombuffer(image_data, dtype=np.uint16).copy()

    if not camera_info:
        depth_img = depth_img.reshape((720, 1280))
    else:
        depth_img = depth_img.reshape((camera_info["height"], camera_info["width"]))
    depth_img = depth_img.astype(np.float32) / depth_scale

    return depth_img


def navigation_to_goal(
    nav: GalbotNavigation, goal_pose: Sequence[float], retry_cnt: int = 3
):
    """
    Navigate to target pose

    Parameters:
        nav (GalbotNavigation): Navigation instance
        goal_pose (Sequence[float]): Target pose [x, y, z, qx, qy, qz, qw]
        retry_cnt (int, optional): Number of retries. Defaults to 3.
    """
    try:
        cur_pose = nav.get_current_pose()
        print(f"Current pose: {cur_pose}")
        if nav.check_path_reachability(goal_pose, cur_pose):
            retry_cnt = 3
            while True:
                status = nav.navigate_to_goal(
                    goal_pose, enable_collision_check=True, is_blocking=True, timeout=20
                )
                time.sleep(0.5)
                retry_cnt -= 1
                if nav.check_goal_arrival() or retry_cnt < 0:
                    break
                else:
                    print(f"Navigation failed: status={status}, retrying: {retry_cnt}")
            print("navigate_to_goal return status:", status)
            print("Has arrived:", nav.check_goal_arrival())
        else:
            print("Path unreachable or unsafe")
    except Exception as e:
        print(f"Exception occurred during navigation: {e}")


def quaternion_to_rotation_matrix(quaternion: Sequence[float]) -> np.ndarray:
    """Convert an [x, y, z, w] quaternion to a 3x3 rotation matrix."""
    quaternion_array = np.asarray(quaternion, dtype=np.float64)
    if quaternion_array.shape != (4,):
        raise ValueError("Quaternion must contain exactly four values")

    norm = np.linalg.norm(quaternion_array)
    if norm < np.finfo(np.float64).eps:
        raise ValueError("Quaternion norm must be greater than zero")

    x, y, z, w = quaternion_array / norm
    return np.array(
        [
            [
                1.0 - 2.0 * (y * y + z * z),
                2.0 * (x * y - z * w),
                2.0 * (x * z + y * w),
            ],
            [
                2.0 * (x * y + z * w),
                1.0 - 2.0 * (x * x + z * z),
                2.0 * (y * z - x * w),
            ],
            [
                2.0 * (x * z - y * w),
                2.0 * (y * z + x * w),
                1.0 - 2.0 * (x * x + y * y),
            ],
        ],
        dtype=np.float64,
    )


def pose_camera_to_base(
    robot: GalbotRobot, pose_camera: Sequence[float]
) -> Sequence[float]:
    """
    Transform camera pose to chassis coordinate system

    Parameters:
        robot (GalbotRobot): Robot instance
        pose_camera (Sequence[float]): Camera pose [x, y, z, qx, qy, qz, qw]

    Returns:
        Sequence[float]: Chassis pose [x, y, z, qx, qy, qz, qw]
    """
    source_frame = "left_arm_camera_color_optical_frame"
    target_frame = "base_link"
    base_to_cam = robot.get_transform(target_frame, source_frame)[0]
    if base_to_cam is None:
        print("Failed to get transform from camera to chassis")
        return None
    else:
        print("base_to_cam: ", base_to_cam)

    base_to_cam_mat = np.eye(4)
    base_to_cam_mat[:3, :3] = quaternion_to_rotation_matrix(base_to_cam[3:])
    base_to_cam_mat[:3, 3] = np.array(base_to_cam[:3])

    pose_base_mat = (
        base_to_cam_mat[:3, :3] @ np.array(pose_camera[:3]).reshape(3, 1)
        + base_to_cam_mat[:3, 3:]
    )

    return pose_base_mat.flatten()[:3].tolist() + [0, 0, 0, 1]


def detect_object(robot: GalbotRobot):
    try:
        # This example uses the left-arm RGB and depth cameras.
        rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA)
        depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)

        # Decode image data
        if not rgb_image_data:
            print("No rgb image data!")
        else:
            print("get rgb image suceess")
            img = decode_compressed_image(rgb_image_data)

        if not depth_data:
            print("No depth_data!")
        else:
            depth_img = decode_compressed_image(depth_data)
            print("get depth data suceess")

        # Placeholder perception result in the OpenCV camera frame (x right, y down, z forward).
        # Replace this pose with the output of an application-specific RGB-D detector.
        object_pose_camera = [0.0, 0.20, 0.29, 0.0, 0.71, 0.0, 0.71]
        print(f"object_pose_camera: {object_pose_camera}")

        # Calculate target pose in chassis coordinate system
        object_pose_base = pose_camera_to_base(robot, object_pose_camera)
        print(f"Target pose in chassis coordinate system: {object_pose_base}")

    except Exception as e:
        object_pose_base = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
        print(f"Target detection exception: {e}")

    return object_pose_base


def check_robot_safety():
    """Check if robot is safe"""
    # Prompt important notes
    print(
        "⚠️  Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations. 3. Please ensure the area around the robot is clear of obstacles."
    )
    while True:
        key = input(
            "Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)..."
        )
        if key == "y":
            print("User confirmed, continuing...")
            break
        elif key == "n":
            print("User did not confirm, exiting program...")
            exit(1)
        else:
            print("Invalid input, please enter 'y' or 'n'")


def pick_and_place(
    robot: GalbotRobot,
    nav: GalbotNavigation,
    motion: GalbotMotion,
    object_pose_base: Sequence[float],
    navigation_enabled: bool,
):
    try:
        # Attach tool to end effector
        status = motion.attach_tool(chain="left_arm", tool="galbot_gripper")
        time.sleep(0.5)
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to attach tool: status={status}")
            return False
        else:
            print(f"✅ Successfully attached tool: status={status}")

        # The target poses represent the attached gripper TCP, not the bare arm flange.
        # Tool pose mode makes set_end_effector_pose control the attached tool TCP.
        motion_params = gm.Parameter()
        motion_params.set_direct_execute(True)
        motion_params.set_blocking(True)
        motion_params.set_timeout(20.0)
        motion_params.set_tool_pose(True)
        motion_params.set_check_collision(True)
        motion_params.set_reference_frame("base_link")
        motion_params.set_actuate("with_chain_only")

        # Open left gripper
        # Set left gripper width to 0.1m, speed to 0.05m, force to 10N, will block until gripper reaches position
        status = robot.set_gripper_command(
            G1JointGroup.left_gripper, 0.1, 0.05, 10, False
        )
        time.sleep(0.5)
        print(f"✅ Successfully set left gripper width to 0.1m: status={status}")

        # This fixed grasp pose is only a placeholder for the runnable tutorial.
        # A real application should use object_pose_base from perception instead.
        print(f"Perception pose: {object_pose_base}")
        grasp_pose = list(GRASP_POSE_BASE_PLACEHOLDER)
        pre_grasp_pose = list(grasp_pose)
        pre_grasp_pose[0] -= GRASP_TRANSITION_OFFSET_M
        lift_grasp_pose = list(grasp_pose)
        lift_grasp_pose[2] += GRASP_TRANSITION_OFFSET_M
        retreat_pose = list(lift_grasp_pose)
        retreat_pose[0] -= GRASP_TRANSITION_OFFSET_M

        print(f"pre_grasp_pose: {pre_grasp_pose}")
        status = motion.set_end_effector_pose(
            target_pose=pre_grasp_pose,
            end_effector_frame="left_arm",
            reference_frame="base_link",
            enable_collision_check=True,
            is_blocking=True,
            timeout=20.0,
            params=motion_params,
        )
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to execute pre_grasp pose command: status={status}")
            return False
        print(f"✅ Successfully executed pre_grasp pose command: {pre_grasp_pose}")

        # Use move_line for the final 5 cm straight-line approach to the object.
        grasp_target = gm.MotionPlanChainTarget()
        grasp_target.chain_name = "left_arm"
        grasp_target.mode = gm.MotionPlanTargetMode.kCartesian
        grasp_target.cart.chain_name = "left_arm"
        grasp_target.cart.frame_id = "EndEffector"
        grasp_target.cart.reference_frame = "base_link"
        grasp_target.cart.pose = gm.Pose(grasp_pose)

        print(f"grasp_pose: {grasp_pose}")
        status, _ = motion.move_line([[grasp_target]], motion_params)
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to execute grasp move_line: status={status}")
            return False
        print(f"✅ Successfully executed grasp move_line: {grasp_pose}")

        # Close gripper to grasp object
        status = robot.set_gripper_command(
            G1JointGroup.left_gripper, 0.02, 0.05, 10, False
        )
        time.sleep(0.5)
        print(f"✅ Successfully closed left gripper: status={status}")

        print(f"lift_grasp_pose: {lift_grasp_pose}")
        status = motion.set_end_effector_pose(
            target_pose=lift_grasp_pose,
            end_effector_frame="left_arm",
            reference_frame="base_link",
            enable_collision_check=True,
            is_blocking=True,
            timeout=20.0,
            params=motion_params,
        )
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to execute lift_grasp pose command: status={status}")
            return False
        print(f"✅ Successfully executed lift_grasp pose command: {lift_grasp_pose}")

        print(f"retreat_pose: {retreat_pose}")
        status = motion.set_end_effector_pose(
            target_pose=retreat_pose,
            end_effector_frame="left_arm",
            reference_frame="base_link",
            enable_collision_check=True,
            is_blocking=True,
            timeout=20.0,
            params=motion_params,
        )
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to execute retreat pose command: status={status}")
            return False
        print(f"✅ Successfully executed retreat pose command: {retreat_pose}")

        # Return to the initial left-arm joint pose before navigation.
        left_arm_status = robot.set_joint_positions(
            joint_positions=INITIAL_UPPER_BODY_JOINT_POSITIONS[2:9],
            joint_names=INITIAL_UPPER_BODY_JOINT_NAMES[2:9],
            is_blocking=True,
            speed_rad_s=0.2,
            timeout_s=20.0,
        )
        if left_arm_status != gm.ControlStatus.SUCCESS:
            print(
                f"❌ Failed to restore the left-arm initial pose: status={left_arm_status}"
            )
            return False
        print(
            f"✅ Successfully restored the left-arm initial pose: status={left_arm_status}"
        )

        if navigation_enabled:
            place_navigation_goal = list(nav.get_current_pose())
            place_navigation_goal[0] += PLACE_NAVIGATION_X_OFFSET_M
            print(f"Place navigation target pose: {place_navigation_goal}")
            navigation_to_goal(nav, place_navigation_goal)
            time.sleep(2)
            print("✅ Navigation stage completed; starting Place")
        else:
            print(
                "⚠️ Navigation is unavailable; skipping navigation and continuing "
                "with Place at the current position."
            )

        # Move above the place point, then descend 5 cm in a straight line.
        place_pose = list(PLACE_POSE_BASE)
        pre_place_pose = list(place_pose)
        pre_place_pose[2] += GRASP_TRANSITION_OFFSET_M
        print(f"pre_place_pose: {pre_place_pose}")
        status = motion.set_end_effector_pose(
            target_pose=pre_place_pose,
            end_effector_frame="left_arm",
            reference_frame="base_link",
            enable_collision_check=True,
            is_blocking=True,
            timeout=20.0,
        )
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to execute pre_place pose command: status={status}")
            return False
        print(f"✅ Successfully executed pre_place pose command: {pre_place_pose}")

        place_target = gm.MotionPlanChainTarget()
        place_target.chain_name = "left_arm"
        place_target.mode = gm.MotionPlanTargetMode.kCartesian
        place_target.cart.chain_name = "left_arm"
        place_target.cart.frame_id = "EndEffector"
        place_target.cart.reference_frame = "base_link"
        place_target.cart.pose = gm.Pose(place_pose)

        print(f"place_pose: {place_pose}")
        status, _ = motion.move_line([[place_target]], motion_params)
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to execute place move_line: status={status}")
            return False
        print(f"✅ Successfully executed place move_line: {place_pose}")

        # Release target
        status = robot.set_gripper_command(
            G1JointGroup.left_gripper, 0.1, 0.05, 10, False
        )
        time.sleep(0.5)
        print(f"✅ Successfully released left gripper: status={status}")

        # Return the left arm to its initial joint pose after placing the object.
        left_arm_status = robot.set_joint_positions(
            joint_positions=INITIAL_UPPER_BODY_JOINT_POSITIONS[2:9],
            joint_names=INITIAL_UPPER_BODY_JOINT_NAMES[2:9],
            is_blocking=True,
            speed_rad_s=0.2,
            timeout_s=20.0,
        )
        if left_arm_status != gm.ControlStatus.SUCCESS:
            print(
                "❌ Failed to restore the left-arm pose after placing: "
                f"status={left_arm_status}"
            )
            return False
        print(
            "✅ Successfully restored the left-arm pose after placing: "
            f"status={left_arm_status}"
        )

        # Detach tool from end effector
        status = motion.detach_tool(chain="left_arm")
        time.sleep(0.5)
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to detach tool: status={status}")
        else:
            print(f"✅ Successfully detached tool: status={status}")
        return True
    except Exception as e:
        print(f"Exception occurred during pick_and_place: {e}")
        return False


def main():
    check_robot_safety()
    try:
        # Get robot instance
        robot = GalbotRobot()
        # Get GalbotMotion instance
        motion = GalbotMotion()
        # Get navigation instance
        nav = GalbotNavigation()

        # Enable only the RGB and depth cameras used for target detection.
        enable_sensor_set = {
            SensorType.LEFT_ARM_CAMERA,
            SensorType.LEFT_ARM_DEPTH_CAMERA,
        }

        # Initialize robot
        if robot.init(enable_sensor_set):
            print("GalbotRobot initialization successful")
        else:
            print("GalbotRobot initialization failed")
        if motion.init():
            print("GalbotMotion initialization successful")
        else:
            print("GalbotMotion initialization failed")
        if nav.init():
            print("GalbotNavigation initialization successful")
        else:
            print("GalbotNavigation initialization failed")

        # Program starts immediately, wait for data readiness
        time.sleep(1)

        if not move_to_initial_pose(robot):
            raise RuntimeError("Failed to move to the task initial pose")

        # Check localization once. If unavailable, Pick still runs and navigation
        # between Pick and Place is skipped.
        try:
            navigation_enabled = nav.is_localized()
        except Exception as e:
            navigation_enabled = False
            print(f"⚠️ Navigation status check failed: {e}")
        if navigation_enabled:
            print("✅ Robot is localized; Place navigation is enabled.")
        else:
            print(
                "⚠️ Robot is not localized; navigation will be skipped. "
                "This run demonstrates Pick and Place at the current position."
            )
        print()

        # Detect the target before Pick.
        object_pose_base = detect_object(robot)
        print()

        # Execute Pick, navigate, and then Place.
        if not pick_and_place(robot, nav, motion, object_pose_base, navigation_enabled):
            raise RuntimeError("Pick-and-place execution failed")
        print()

    except Exception as e:
        print(f"Exception occurred: {e}")
    finally:
        # Actively send SIGINT exit signal
        robot.request_shutdown()
        # Wait to enter shutdown state
        robot.wait_for_shutdown()
        # Release SDK resources
        robot.destroy()
        print("Resource release successful")


if __name__ == "__main__":
    main()