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 (S1 1.2)

Joint Group List

Robot joint group names include: ["torso", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]

Detailed Information for Each Joint Group

Joint Group	English Name	Number of Joints	Joint Name List
Head	head	2	head_joint1, head_joint2
Torso	torso	1	S1 V1: torso_base_joint; S1 V2: torso_lift_joint1
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

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/torso 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 (torso/head/arm/gripper).
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 (S1)

Joint Group	Typical Scenario
torso	Torso height/pitch adjustment
head	Head orientation / camera aiming
left_arm / right_arm	Arm reaching and manipulation
left_gripper / right_gripper	Grasp width control

Note: S1 also defines passive groups (for example, swerve chassis and camera mount groups) that are usually not used as direct set_joint_positions targets.

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
HEAD_LIDAR	head_lidar_base_link
BACK_LIDAR	back_lidar_base_link
CHASSIS_LIDAR	chassis_lidar_base_link
HEAD_IMU	head_imu_base_link
BACK_IMU	back_imu_base_link
CHASSIS_IMU	chassis_imu_base_link
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

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.s1 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.s1 import GalbotRobot
from galbot_sdk.s1 import ControlStatus

# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
if robot.init():
    print('System initialized successfully!')
else:
    print('System initialization failed!')
    exit(1)

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

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

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

# Joint groups to control; passing empty array defaults to torso, head, left_arm, right_arm (S1: torso replaces G1 leg)
joint_groups = ["head"]
# Specific joints to control; if provided, this overrides the joint_groups parameter
joint_names = []
# Joint positions; head joint group contains two joints
# Head angles: within S1 limits [-0.7854, 0.7854] and [-0.6109, 0.6109]
joint_pos = [0.2, 0.2]
# Whether to block and wait for joint angles to reach position or timeout
is_block = True
# Maximum joint speed (rad/s)
speed_rad_s = 0.1
# Maximum wait time (seconds)
timeout_s = 10.0

# Set joint positions
status = robot.set_joint_positions(
    joint_pos, joint_groups, joint_names, is_block, speed_rad_s, timeout_s
)

if status == ControlStatus.SUCCESS:
    print('Joint command set successfully!')
else:
    print('Failed to set joint command!')

# Query joint positions by group; empty array defaults to leg, head, dual-arm groups. Second parameter specifies joint names, which overrides joint_groups if provided.
ret_positions = robot.get_joint_positions(joint_groups, [])
for position in ret_positions:
    print(f"joint positions is {position}")

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

# Set joint positions
status = robot.set_joint_positions(
    joint_pos, joint_groups, joint_names, is_block, speed_rad_s, timeout_s
)

if status == ControlStatus.SUCCESS:
    print('Joint command set successfully!')
else:
    print('Failed to set joint command!')

# Query joint positions by group; empty array defaults to leg, head, dual-arm groups. Second parameter specifies joint names, which overrides joint_groups if provided.
ret_positions = robot.get_joint_positions(joint_groups, [])
for position in ret_positions:
    print(f"joint positions is {position}")

# Exit system and release SDK resources
robot.request_shutdown()
robot.wait_for_shutdown()
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.s1 import GalbotRobot, S1JointGroup
from galbot_sdk.s1 import ControlStatus


def print_gripper_state(gripper_state):
    """Print gripper status"""
    print(f"Timestamp (ns): {gripper_state.timestamp_ns}")
    print(f" width {gripper_state.width} velocity {gripper_state.velocity}"
          f" effort {gripper_state.effort} is moving {gripper_state.is_moving}")


# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
if robot.init():
    print('System initialized successfully!')
else:
    print('System initialization failed!')
    exit(1)

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

# Gripper width (m)
width_m = 0.02
# Gripper speed (m/s)
velocity_mps = 0.05
# Gripper torque (N·m)
effort = 10
# Whether to block until execution completes
is_blocking = False
# Set left gripper width to 0.02m, speed 0.05m/s, torque 10, block until execution completes
status = robot.set_gripper_command(S1JointGroup.left_gripper, width_m, velocity_mps, effort, is_blocking)

if status == ControlStatus.SUCCESS:
    print('Gripper command set successfully!')
else:
    print('Failed to set gripper command!')

# Get gripper state
gripper_state = robot.get_gripper_state(S1JointGroup.left_gripper)

if gripper_state is None:
    print('get gripper state error')
else:
    print_gripper_state(gripper_state)

# Gripper width (m)
width_m = 0.1
# Gripper speed (m/s)
velocity_mps = 0.05
# Gripper torque (N·m)
effort = 10
# Whether to block until execution completes
is_blocking = False
# Set left gripper width to 0.1m, speed 0.05m/s, torque 10, non-blocking wait for execution completes
status = robot.set_gripper_command(S1JointGroup.left_gripper, width_m, velocity_mps, effort, is_blocking)

if status == ControlStatus.SUCCESS:
    print('Gripper command set successfully!')
else:
    print('Failed to set gripper command!')

# Exit system and release SDK resources
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print('Resources released successfully')

S1 Raw End-Tool Passthrough

These examples show how to encode, transmit, validate, and decode device-specific 64-byte CAN/RS485 frames. Read S1 Raw End-Tool Passthrough before using them.

warning

WBCS must run with endtool_raw_enabled=true in [robot_info.custom_params]. Do not run a raw writer concurrently with a standard controller or another client that writes to the same TIB.

Self-Developed CAN Gripper

Initializes and moves the self-developed two-finger gripper, then monitors passive status reports from the 250 Hz receive stream.

examples/s1/python/galbot_robot/endtool_raw_grip_can.py

"""Raw-passthrough example for the self-developed CAN gripper.

The device protocol in this file is an example adapter, not part of the SDK's
generic end-tool transport contract.
"""

import argparse
import math
import struct
import sys
import threading
import time

import galbot_sdk.s1 as sdk


TX_CAN_ID = 0x70A
RX_CAN_ID = 0x10A
MIN_WIDTH_MM = 6.0
MAX_WIDTH_MM = 120.0
MIN_VELOCITY_MM_S = 1.0
MAX_VELOCITY_MM_S = 100.0
MIN_TORQUE_NM = 1.0
MAX_TORQUE_NM = 50.0
STATUS_STALE_SECONDS = 1.0
INITIAL_STATUS_WAIT_SECONDS = 1.0
STATUS_PRINT_INTERVAL_SECONDS = 0.5
STOPPED_VELOCITY_TOLERANCE_MM_S = 1.0
DEFAULT_INIT_TIMEOUT_SECONDS = 20.0
DEFAULT_MONITOR_SECONDS = 10.0
DEFAULT_POSITION_TOLERANCE_MM = 2.0

ERROR_TEXT = {
    0x00: "no error",
    0x01: "not enabled",
    0x08: "overvoltage",
    0x09: "undervoltage",
    0x0A: "overcurrent",
    0x0B: "MOS overheating",
    0x0C: "motor coil overheating",
    0x0D: "communication lost",
    0x0E: "motor overload",
    0x10: "locked rotor",
    0x11: "locked rotor and not enabled",
    0x40: "power loss after calibration",
    0x80: "communication error",
}


def encode_can(command: int, function: int, payload: bytes = b"") -> bytes:
    frame = bytearray(64)
    struct.pack_into("<II", frame, 0, 1, TX_CAN_ID)
    frame[8] = len(payload) + 2
    frame[12] = command
    frame[13] = function
    frame[14 : 14 + len(payload)] = payload
    return bytes(frame)


def encode_init() -> bytes:
    return encode_can(0x21, 0x04)


def encode_clear_error() -> bytes:
    return encode_can(0x21, 0x0A)


def encode_move(width_mm: float, velocity_mm_s: float, torque_nm: float) -> bytes:
    return encode_can(0x21, 0x05, struct.pack("<fff", width_mm, velocity_mm_s, torque_nm))


def decode_status(frame: bytes):
    if len(frame) != 64:
        return "size", None
    frame_type, can_id = struct.unpack_from("<II", frame, 0)
    if frame_type != 1:
        return "frame_type", None
    # The 250 Hz receive buffer can contain an empty CAN slot.  Treat it as an
    # idle transport snapshot instead of reporting a device/protocol error.
    if can_id == 0:
        return "idle_snapshot", None
    if can_id != RX_CAN_ID:
        return "can_id", None

    # Do not require frame[12] == 1 here.  On the current S1 EtherCAT/TIB
    # passthrough implementation, valid 0x10A gripper snapshots have byte 12
    # set to zero.  The device frame itself starts at byte 16 on RX, so use its
    # CAN ID and cmd/function fields to identify a valid status report.
    command, function = frame[16], frame[17]
    if command not in (0x22, 0x23) or function != 0x05:
        return "command", None

    position, velocity, torque = struct.unpack_from("<fff", frame, 22)
    motion_state = frame[34]
    if (
        not all(math.isfinite(value) for value in (position, velocity, torque))
        # An uninitialized gripper legitimately reports position 0 mm even
        # though 0 mm is not a legal Move command target.
        or not 0.0 <= position <= MAX_WIDTH_MM
        or abs(velocity) > MAX_VELOCITY_MM_S
        or abs(torque) > MAX_TORQUE_NM
        or motion_state > 4
    ):
        return "payload", None
    return None, (position, velocity, torque, motion_state, frame[21], command)


def format_status(generation: int, status) -> str:
    position, velocity, torque, motion_state, error_code, command = status
    report_kind = "ANSWER" if command == 0x22 else "REPORT"
    return (
        f"generation={generation} source={report_kind} position={position:.3f}mm "
        f"velocity={velocity:.3f}mm/s torque={torque:.3f}Nm "
        f"motion_state={motion_state} error=0x{error_code:02x} "
        f"({ERROR_TEXT.get(error_code, 'unknown error')})"
    )


def main() -> None:
    parser = argparse.ArgumentParser()
    parser.add_argument("--side", choices=("left", "right"), default="left")
    parser.add_argument("--width-mm", type=float, default=50.0)
    parser.add_argument("--velocity-mm-s", type=float, default=50.0)
    parser.add_argument("--torque-nm", type=float, default=20.0)
    parser.add_argument("--init-timeout", type=float, default=DEFAULT_INIT_TIMEOUT_SECONDS)
    parser.add_argument("--monitor-seconds", type=float, default=DEFAULT_MONITOR_SECONDS)
    parser.add_argument(
        "--position-tolerance-mm",
        type=float,
        default=DEFAULT_POSITION_TOLERANCE_MM,
    )
    parser.add_argument(
        "--init",
        action="store_true",
        help="explicitly home/calibrate before Move; do not use on every invocation",
    )
    parser.add_argument(
        "--skip-init",
        action="store_true",
        help=argparse.SUPPRESS,
    )
    parser.add_argument(
        "--clear-error",
        action="store_true",
        help="send RESET before optional Init/Move",
    )
    parser.add_argument(
        "--stop-standard-controller",
        action="store_true",
        help="best-effort stop of the standard controller; this is not an exclusive raw lease",
    )
    parser.add_argument(
        "--restore-controller",
        action="store_true",
        help="restart the standard gripper controller on exit (normally keep it disabled in raw mode)",
    )
    args = parser.parse_args()
    values = (
        args.width_mm,
        args.velocity_mm_s,
        args.torque_nm,
        args.init_timeout,
        args.monitor_seconds,
        args.position_tolerance_mm,
    )
    if not all(math.isfinite(value) for value in values):
        parser.error("motion parameters must be finite")
    if args.init_timeout <= 0 or args.monitor_seconds <= 0 or args.position_tolerance_mm <= 0:
        parser.error("timeouts and position tolerance must be positive")
    if not MIN_WIDTH_MM <= args.width_mm <= MAX_WIDTH_MM:
        parser.error(f"--width-mm must be in [{MIN_WIDTH_MM}, {MAX_WIDTH_MM}]")
    if not MIN_VELOCITY_MM_S <= args.velocity_mm_s <= MAX_VELOCITY_MM_S:
        parser.error(f"--velocity-mm-s must be in [{MIN_VELOCITY_MM_S}, {MAX_VELOCITY_MM_S}]")
    if not MIN_TORQUE_NM <= args.torque_nm <= MAX_TORQUE_NM:
        parser.error(f"--torque-nm must be in [{MIN_TORQUE_NM}, {MAX_TORQUE_NM}]")

    side = sdk.EndToolSide.LEFT if args.side == "left" else sdk.EndToolSide.RIGHT
    joint_group = (
        sdk.S1JointGroup.left_gripper
        if args.side == "left"
        else sdk.S1JointGroup.right_gripper
    )
    controller_group = "left_gripper" if args.side == "left" else "right_gripper"
    robot = sdk.GalbotRobot()
    if not robot.init():
        raise RuntimeError("failed to initialize the S1 SDK")

    print(
        "WARNING: same-process raw TX is serialized, but there is no "
        "cross-client resource lease."
    )
    print("WBCS must run with endtool_raw_enabled=true.")
    print(
        f"For exclusive raw control, configure {args.side}_none instead of "
        f"{controller_group} in WBCS group_lists and restart WBCS. "
        "stop_controller() only stops a trajectory; it is not an exclusive raw lease."
    )
    if args.stop_standard_controller:
        stop_status = robot.stop_controller(controller_group)
        print(f"Best-effort stop of {controller_group}: {stop_status}")

    state_changed = threading.Condition()
    state = {
        "status": None,
        "last_generation": 0,
        "last_update": 0.0,
        "latest_raw_generation": 0,
        "idle_snapshots": 0,
        "reject_reasons": {},
        "non_increasing_generations": 0,
    }

    def on_raw(data: sdk.EndToolRawData) -> None:
        if data.side != side or data.kind != sdk.EndToolRxKind.BUFFER_250HZ:
            return
        reject_reason, status = decode_status(data.frame)
        with state_changed:
            state["latest_raw_generation"] = data.generation
            if reject_reason == "idle_snapshot":
                state["idle_snapshots"] += 1
                state_changed.notify_all()
                return
            if reject_reason is not None:
                reasons = state["reject_reasons"]
                reasons[reject_reason] = reasons.get(reject_reason, 0) + 1
                state_changed.notify_all()
                return
            previous_generation = state["last_generation"]
            if data.generation and previous_generation and data.generation <= previous_generation:
                state["non_increasing_generations"] += 1
                state_changed.notify_all()
                return
            state["status"] = status
            state["last_generation"] = data.generation
            state["last_update"] = time.monotonic()
            state_changed.notify_all()

    def get_snapshot():
        with state_changed:
            snapshot = dict(state)
            snapshot["reject_reasons"] = dict(state["reject_reasons"])
            return snapshot

    def wait_for_command_ready_after(baseline_generation: int):
        deadline = time.monotonic() + args.init_timeout
        last_print_at = 0.0
        last_printed_state = None
        while time.monotonic() < deadline:
            with state_changed:
                remaining = deadline - time.monotonic()
                state_changed.wait(timeout=max(0.0, min(0.5, remaining)))
                generation = state["last_generation"]
                status = state["status"]
                if status is None or generation <= baseline_generation:
                    continue
                now = time.monotonic()
                state_key = (status[3], status[4])
                if (
                    state_key != last_printed_state
                    or now - last_print_at >= STATUS_PRINT_INTERVAL_SECONDS
                ):
                    print("Init status:", format_status(generation, status))
                    last_printed_state = state_key
                    last_print_at = now
                # The device state machine defines 2 as Ready. State 4 is the
                # completion state of a previous Move report and must not be
                # used as proof that this Init command has completed.
                if status[3] == 2:
                    return True
        return False

    def wait_for_first_status(timeout: float):
        deadline = time.monotonic() + timeout
        with state_changed:
            while state["status"] is None:
                remaining = deadline - time.monotonic()
                if remaining <= 0:
                    break
                state_changed.wait(timeout=remaining)
            return state["status"]

    handle = robot.register_endtool_raw_callback(on_raw)
    if handle == 0:
        if args.restore_controller:
            robot.start_controller(controller_group)
        robot.destroy()
        raise RuntimeError("failed to register raw callback")

    try:
        if args.clear_error:
            result = robot.send_endtool_raw_frame(side, encode_clear_error())
            if result != sdk.ControlStatus.SUCCESS:
                raise RuntimeError("failed to publish the gripper RESET frame")
            print("RESET sent; waiting 0.5s before the next command...")
            time.sleep(0.5)

        if args.init and not args.skip_init:
            init_baseline = get_snapshot()["last_generation"]
            if robot.send_endtool_raw_frame(side, encode_init()) != sdk.ControlStatus.SUCCESS:
                raise RuntimeError("failed to publish the gripper init frame")
            print(
                "Waiting for a fresh Ready status (2) after Init "
                f"(timeout={args.init_timeout:.1f}s)..."
            )
            if not wait_for_command_ready_after(init_baseline):
                snapshot = get_snapshot()
                raise RuntimeError(
                    "gripper did not report a fresh command-ready status after Init; "
                    f"latest_valid_generation={snapshot['last_generation']} "
                    f"latest_raw_generation={snapshot['latest_raw_generation']} "
                    f"idle_snapshots={snapshot['idle_snapshots']} "
                    f"malformed={snapshot['reject_reasons']}"
                )

        current_status = wait_for_first_status(INITIAL_STATUS_WAIT_SECONDS)
        if current_status is not None and (
            current_status[4] == 0x01 or current_status[3] == 0
        ):
            raise RuntimeError(
                "gripper is not initialized (device error 0x01/state 0); "
                "rerun this example with --init before sending Move"
            )

        move_baseline = get_snapshot()["last_generation"]
        result = robot.send_endtool_raw_frame(
            side, encode_move(args.width_mm, args.velocity_mm_s, args.torque_nm)
        )
        if result != sdk.ControlStatus.SUCCESS:
            raise RuntimeError("failed to publish the gripper move frame")

        print(
            "Move sent. Monitoring only fresh passive status reports for "
            f"{args.monitor_seconds:.1f}s..."
        )
        deadline = time.monotonic() + args.monitor_seconds
        printed_generation = move_baseline
        received_after_move = False
        latest_move_status = None
        raw_target_reached = False
        last_raw_print = 0.0
        last_raw_state_error = None
        last_sdk_print = 0.0
        last_sdk_position_mm = None
        while time.monotonic() < deadline:
            with state_changed:
                remaining = deadline - time.monotonic()
                state_changed.wait(timeout=max(0.0, min(0.5, remaining)))
                generation = state["last_generation"]
                status = state["status"]
            if (
                status is not None
                and generation > move_baseline
                and generation != printed_generation
            ):
                received_after_move = True
                printed_generation = generation
                latest_move_status = status
                now = time.monotonic()
                state_error = (status[3], status[4])
                if (
                    state_error != last_raw_state_error
                    or now - last_raw_print >= STATUS_PRINT_INTERVAL_SECONDS
                ):
                    print("Raw status snapshot:", format_status(generation, status))
                    last_raw_print = now
                    last_raw_state_error = state_error
                if (
                    status[4] == 0
                    and abs(status[0] - args.width_mm) <= args.position_tolerance_mm
                    and abs(status[1]) <= STOPPED_VELOCITY_TOLERANCE_MM_S
                ):
                    print(
                        f"Move completed within tolerance: requested={args.width_mm:.3f}mm "
                        f"actual={status[0]:.3f}mm (verified by Raw status)"
                    )
                    raw_target_reached = True
                    break

            sdk_state = robot.get_gripper_state(joint_group)
            if sdk_state is None:
                continue
            position_mm = sdk_state.width * 1000.0
            velocity_mm_s = sdk_state.velocity * 1000.0
            now = time.monotonic()
            if (
                last_sdk_position_mm is None
                or abs(position_mm - last_sdk_position_mm) >= 0.1
                or now - last_sdk_print >= 0.5
            ):
                print(
                    f"SDK state: position={position_mm:.3f}mm "
                    f"velocity={velocity_mm_s:.3f}mm/s effort={sdk_state.effort:.3f} "
                    f"is_moving={sdk_state.is_moving}",
                )
                last_sdk_position_mm = position_mm
                last_sdk_print = now
            if (
                abs(position_mm - args.width_mm) <= args.position_tolerance_mm
                and not sdk_state.is_moving
            ):
                print(
                    f"Move completed within tolerance: requested={args.width_mm:.3f}mm "
                    f"actual={position_mm:.3f}mm (verified by get_gripper_state)"
                )
                break

        snapshot = get_snapshot()
        if not received_after_move:
            print(
                "WARNING: no valid gripper status snapshot was received after Move. "
                "Publication success alone does not prove that the device executed the command."
            )
        elif time.monotonic() - snapshot["last_update"] > STATUS_STALE_SECONDS:
            print(
                "WARNING: the valid gripper status stream stopped; subsequent receive "
                "buffers were idle or rejected."
            )
        print(
            "RX summary: "
            f"latest_raw_generation={snapshot['latest_raw_generation']} "
            f"latest_valid_generation={snapshot['last_generation']} "
            f"idle_snapshots={snapshot['idle_snapshots']} "
            f"malformed={snapshot['reject_reasons']} "
            f"non_increasing_generations={snapshot['non_increasing_generations']}"
        )

        if raw_target_reached:
            return

        sdk_state = robot.get_gripper_state(joint_group)
        sdk_target_reached = False
        if sdk_state is not None:
            position_mm = sdk_state.width * 1000.0
            position_error = abs(position_mm - args.width_mm)
            sdk_target_reached = (
                position_error <= args.position_tolerance_mm and not sdk_state.is_moving
            )
        if not sdk_target_reached:
            raw_summary = (
                "none"
                if latest_move_status is None
                else format_status(printed_generation, latest_move_status)
            )
            sdk_summary = (
                "unavailable"
                if sdk_state is None
                else (
                    f"position={position_mm:.3f}mm error={position_error:.3f}mm "
                    f"is_moving={sdk_state.is_moving}"
                )
            )
            raise RuntimeError(
                "self-developed gripper verification failed: Move did not reach the requested "
                f"width: requested={args.width_mm:.3f}mm "
                f"tolerance={args.position_tolerance_mm:.3f}mm; "
                f"SDK state={sdk_summary}; last_raw_status={raw_summary}"
            )
    finally:
        robot.unregister_endtool_raw_callback(handle)
        if args.restore_controller:
            restore_status = robot.start_controller(controller_group)
            print(f"Best-effort restore of {controller_group}: {restore_status}")
        robot.destroy()


def run_cli() -> int:
    try:
        main()
    except RuntimeError as error:
        print(f"FAILED: {error}", file=sys.stderr)
        return 1
    return 0


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

Dahuan Two-Finger CAN Gripper

Configures and moves a Dahuan two-finger gripper and actively polls its status through the raw CAN transport.

examples/s1/python/galbot_robot/endtool_raw_dahuan2_can.py

"""Raw-passthrough example for a Dahuan two-finger CAN FD gripper."""

import argparse
import struct
import sys
import threading
import time

import galbot_sdk.s1 as sdk


TX_CAN_ID = 0x601
RX_CAN_ID = 0x201
WRITE_SINGLE_REGISTER = 0x06
READ_REGISTERS = 0x03
REGISTER_INIT = 0x0100
REGISTER_FORCE = 0x0101
REGISTER_TARGET_POSITION = 0x0103
REGISTER_SPEED = 0x0104
REGISTER_STATUS_BASE = 0x0200

ERROR_TEXT = {
    0: "no error",
    1: "under voltage",
    2: "over voltage",
    3: "over current",
    4: "over heat",
    5: "motor disconnected",
    8: "overload",
    11: "over speed",
    15: "startup error",
    32: "encoder error",
}


def encode_request(function: int, address: int, value: int) -> bytes:
    frame = bytearray(64)
    struct.pack_into("<II", frame, 0, 1, TX_CAN_ID)
    frame[8] = 7
    struct.pack_into(">BHH", frame, 12, function, address, value)
    return bytes(frame)


def decode_status(frame: bytes):
    if len(frame) != 64:
        return "size", None
    if struct.unpack_from("<I", frame, 0)[0] != 1:
        return "frame_type", None
    if frame[12] != 0x01:
        return "idle_snapshot", None
    if struct.unpack_from("<I", frame, 4)[0] != RX_CAN_ID:
        return "can_id", None
    if frame[16] != READ_REGISTERS or frame[17] < 12:
        return "function", None
    values = struct.unpack_from(">6H", frame, 18)
    current_ma = values[4] - 65536 if values[4] >= 32768 else values[4]
    return None, (*values[:4], current_ma, values[5])


def main() -> None:
    parser = argparse.ArgumentParser()
    parser.add_argument("--side", choices=("left", "right"), default="left")
    parser.add_argument("--position", type=int, default=500, help="target position in [0, 1000] permille")
    parser.add_argument("--speed", type=int, default=50, help="speed in [1, 100] percent")
    parser.add_argument("--force", type=int, default=50, help="force in [20, 100] percent")
    parser.add_argument("--init", action="store_true", help="home before setting motion parameters")
    parser.add_argument("--calibrate", action="store_true")
    parser.add_argument("--duration", type=float, default=10.0)
    parser.add_argument("--init-wait", type=float, default=4.0)
    parser.add_argument("--position-tolerance", type=int, default=20)
    parser.add_argument("--stop-standard-controller", action="store_true")
    parser.add_argument("--restore-controller", action="store_true")
    args = parser.parse_args()
    if not 0 <= args.position <= 1000:
        parser.error("--position must be in [0, 1000]")
    if not 1 <= args.speed <= 100:
        parser.error("--speed must be in [1, 100]")
    if not 20 <= args.force <= 100:
        parser.error("--force must be in [20, 100]")
    if args.duration < 0 or args.init_wait < 0 or args.position_tolerance < 0:
        parser.error("--duration and --init-wait must be non-negative")

    side = sdk.EndToolSide.LEFT if args.side == "left" else sdk.EndToolSide.RIGHT
    controller_group = "left_gripper" if args.side == "left" else "right_gripper"
    robot = sdk.GalbotRobot()
    if not robot.init():
        raise RuntimeError("failed to initialize the S1 SDK")

    print("WARNING: raw TX has no server-side exclusive writer lock.")
    print("WBCS must run with endtool_raw_enabled=true in [robot_info.custom_params].")
    if args.stop_standard_controller:
        print(f"Best-effort stop of {controller_group}: {robot.stop_controller(controller_group)}")
    changed = threading.Condition()
    state = {
        "status": None,
        "status_generation": 0,
        "status_kind": None,
        "generation_by_kind": [0, 0],
        "latest_raw_generation_by_kind": [0, 0],
        "valid": 0,
        "idle": 0,
        "non_increasing_generation": 0,
        "rejected": {},
    }

    def on_raw(data: sdk.EndToolRawData) -> None:
        # Dahuan responses may appear in either receive buffer.
        if data.side != side:
            return
        kind_index = 0 if data.kind == sdk.EndToolRxKind.BUFFER_1KHZ else 1
        reason, status = decode_status(data.frame)
        with changed:
            state["latest_raw_generation_by_kind"][kind_index] = data.generation
            if reason == "idle_snapshot":
                state["idle"] += 1
            elif reason is not None:
                state["rejected"][reason] = state["rejected"].get(reason, 0) + 1
            else:
                previous_generation = state["generation_by_kind"][kind_index]
                if (
                    data.generation
                    and previous_generation
                    and data.generation <= previous_generation
                ):
                    state["non_increasing_generation"] += 1
                    changed.notify_all()
                    return
                state["generation_by_kind"][kind_index] = data.generation
                state["status"] = status
                state["status_generation"] = data.generation
                state["status_kind"] = data.kind
                state["valid"] += 1
                init, grip, position, speed, current, error = status
                print(
                    f"Fresh status: kind={data.kind} generation={data.generation} "
                    f"init={init} grip={grip} position={position}permille "
                    f"speed={speed} current={current}mA error=0x{error:04x} "
                    f"({ERROR_TEXT.get(error, 'unknown error')})"
                )
            changed.notify_all()

    handle = robot.register_endtool_raw_callback(on_raw)
    if handle == 0:
        if args.restore_controller:
            robot.start_controller(controller_group)
        robot.destroy()
        raise RuntimeError("failed to register raw callback")

    def publish(frame: bytes, description: str) -> None:
        result = robot.send_endtool_raw_frame(side, frame)
        if result != sdk.ControlStatus.SUCCESS:
            raise RuntimeError(f"failed to publish {description}: {result}")

    def poll(seconds: float) -> None:
        deadline = time.monotonic() + seconds
        while time.monotonic() < deadline:
            publish(encode_request(READ_REGISTERS, REGISTER_STATUS_BASE, 6), "status request")
            with changed:
                changed.wait(timeout=min(0.5, max(0.0, deadline - time.monotonic())))

    try:
        if args.init or args.calibrate:
            init_value = 0x00A5 if args.calibrate else 0x0001
            publish(encode_request(WRITE_SINGLE_REGISTER, REGISTER_INIT, init_value), "init command")
            poll(args.init_wait)
        publish(encode_request(WRITE_SINGLE_REGISTER, REGISTER_SPEED, args.speed), "speed command")
        publish(encode_request(WRITE_SINGLE_REGISTER, REGISTER_FORCE, args.force), "force command")
        publish(
            encode_request(WRITE_SINGLE_REGISTER, REGISTER_TARGET_POSITION, args.position),
            "position command",
        )
        # Discard any status left by initialization polling. Only a response to
        # the active queries below may verify this motion command.
        with changed:
            state["status"] = None
            motion_status_baseline = state["valid"]
        poll(args.duration)
        with changed:
            status = state["status"]
            snapshot = dict(state)
            snapshot["rejected"] = dict(state["rejected"])
        if status is None or snapshot["valid"] <= motion_status_baseline:
            raise RuntimeError(
                "Dahuan2 device verification failed: no fresh Dahuan2 status response "
                "was received after the position command. The installed end tool may not "
                "be a Dahuan2 gripper, or its power/wiring/baud rate may be incorrect; "
                "latest_raw_generation_by_kind="
                f"{snapshot['latest_raw_generation_by_kind']} "
                f"idle_snapshots={snapshot['idle']} rejected={snapshot['rejected']}"
            )
        _, _, position, _, _, error = status
        if error != 0:
            raise RuntimeError(
                f"gripper status error=0x{error:04x} "
                f"({ERROR_TEXT.get(error, 'unknown error')})"
            )
        if abs(position - args.position) > args.position_tolerance:
            raise RuntimeError(
                "position did not reach target: "
                f"requested={args.position} actual={position} "
                f"tolerance={args.position_tolerance}permille"
            )
        print(
            f"Position reached: requested={args.position} actual={position}permille "
            f"fresh_status_count={snapshot['valid']} "
            f"non_increasing_generations={snapshot['non_increasing_generation']}"
        )
    finally:
        robot.unregister_endtool_raw_callback(handle)
        if args.restore_controller:
            robot.start_controller(controller_group)
        robot.destroy()


def run_cli() -> int:
    try:
        main()
    except RuntimeError as error:
        # Device absence, protocol mismatch and transport failures are expected
        # test outcomes. Report them without a Python traceback while keeping a
        # non-zero exit status for scripts and CI.
        print(f"FAILED: {error}", file=sys.stderr)
        return 1
    return 0


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

Kunwei RS485 Six-Axis Force Sensor

Sends periodic RS485 queries without catch-up bursts and validates, monitors, and prints six-axis force/torque responses from the 1 kHz receive stream.

examples/s1/python/galbot_robot/endtool_raw_kunwei_rs485.py

"""Raw-passthrough active-query example for a Kunwei six-axis RS485 force sensor."""

import argparse
import math
import struct
import threading
import time

import galbot_sdk.s1 as sdk


MAX_ABS_WRENCH_VALUE = 1.0e6
STALE_SECONDS = 1.0
FREEZE_SECONDS = 2.0
WARNING_INTERVAL_SECONDS = 1.0
STATS_INTERVAL_SECONDS = 5.0


def encode_query() -> bytes:
    frame = bytearray(64)
    struct.pack_into("<III", frame, 0, 2, 233, 4)
    frame[12:16] = b"\x49\xaa\x0d\x0a"
    return bytes(frame)


def decode_force(frame: bytes):
    if len(frame) != 64:
        return "size", None
    if frame[20] not in (0x48, 0x49) or frame[21] != 0xAA:
        return "prefix", None
    if frame[46] != 0x0D or frame[47] != 0x0A:
        return "terminator", None

    reading = struct.unpack_from("<6f", frame, 22)
    if any(not math.isfinite(value) or abs(value) > MAX_ABS_WRENCH_VALUE for value in reading):
        return "numeric", None
    if all(value == 0.0 for value in reading):
        return "zero", None
    return None, reading


def main() -> None:
    parser = argparse.ArgumentParser()
    parser.add_argument("--side", choices=("left", "right"), default="left")
    parser.add_argument("--duration", type=float, default=10.0)
    parser.add_argument("--query-interval-ms", type=float, default=10.0)
    parser.add_argument("--print-interval-ms", type=float, default=100.0)
    args = parser.parse_args()
    values = (args.duration, args.query_interval_ms, args.print_interval_ms)
    if not all(math.isfinite(value) and value > 0 for value in values):
        parser.error("duration and intervals must be finite and positive")

    side = sdk.EndToolSide.LEFT if args.side == "left" else sdk.EndToolSide.RIGHT
    robot = sdk.GalbotRobot()
    if not robot.init():
        raise RuntimeError("failed to initialize the S1 SDK")
    print(
        "WARNING: same-process raw TX is serialized, but there is no "
        "cross-client resource lease."
    )
    print("WBCS must run with endtool_raw_enabled=true.")

    state_lock = threading.Lock()
    state = {
        "query_ok": 0,
        "query_fail": 0,
        "rx_1khz": 0,
        "rejected": 0,
        "reject_size": 0,
        "reject_prefix": 0,
        "reject_terminator": 0,
        "reject_numeric": 0,
        "reject_zero": 0,
        "non_increasing_generation": 0,
        "duplicate_snapshots": 0,
        "valid": 0,
        "last_generation": 0,
        "last_valid": 0.0,
        "last_print": 0.0,
        "last_payload": None,
        "last_payload_change": 0.0,
        "last_freeze_warning": 0.0,
        "last_stale_warning": 0.0,
    }

    def on_raw(data: sdk.EndToolRawData) -> None:
        if data.side != side or data.kind != sdk.EndToolRxKind.BUFFER_1KHZ:
            return
        reject_reason, reading = decode_force(data.frame)
        now = time.monotonic()
        freeze_warning = None
        print_reading = False
        with state_lock:
            state["rx_1khz"] += 1
            if reject_reason is not None:
                state["rejected"] += 1
                state[f"reject_{reject_reason}"] += 1
                return

            previous_generation = state["last_generation"]
            if previous_generation and data.generation <= previous_generation:
                state["non_increasing_generation"] += 1
                return
            state["last_generation"] = data.generation

            payload = bytes(data.frame[22:46])
            if state["last_payload"] is not None and payload == state["last_payload"]:
                # The 1 kHz channel is a transport snapshot. Its generation can
                # advance while the TIB keeps replaying the last sensor payload.
                # A duplicate therefore is not a fresh force sample.
                state["duplicate_snapshots"] += 1
                if (
                    now - state["last_payload_change"] >= FREEZE_SECONDS
                    and now - state["last_freeze_warning"] >= WARNING_INTERVAL_SECONDS
                ):
                    state["last_freeze_warning"] = now
                    freeze_warning = now - state["last_payload_change"]
                return_after_lock = True
            else:
                state["last_payload"] = payload
                state["last_payload_change"] = now
                return_after_lock = False

            if not return_after_lock:
                state["last_valid"] = now
                state["valid"] += 1
                if now - state["last_print"] >= args.print_interval_ms / 1000.0:
                    state["last_print"] = now
                    print_reading = True

        if freeze_warning is not None:
            print(
                f"WARNING: force payload unchanged for {freeze_warning:.3f}s while "
                "generation advances. This may be a held TIB snapshot; an exactly "
                "steady sensor can also produce identical bytes."
            )
        if return_after_lock:
            return
        if print_reading:
            fx, fy, fz, mx, my, mz = reading
            print(
                f"generation={data.generation} Fx={fx:.4f} Fy={fy:.4f} Fz={fz:.4f}kg "
                f"Mx={mx:.4f} My={my:.4f} Mz={mz:.4f}kg*m"
            )

    handle = robot.register_endtool_raw_callback(on_raw)
    if handle == 0:
        robot.destroy()
        raise RuntimeError("failed to register raw callback")

    query = encode_query()
    query_interval = args.query_interval_ms / 1000.0
    started_at = time.monotonic()
    deadline = started_at + args.duration
    next_query_at = started_at
    next_stats_at = started_at + STATS_INTERVAL_SECONDS
    last_stats_at = started_at
    last_stats_query_count = 0
    last_stats_rx_count = 0
    try:
        while True:
            now = time.monotonic()
            if now < next_query_at:
                time.sleep(next_query_at - now)
            if time.monotonic() >= deadline:
                break

            result = robot.send_endtool_raw_frame(side, query)
            after_query = time.monotonic()
            with state_lock:
                if result == sdk.ControlStatus.SUCCESS:
                    state["query_ok"] += 1
                else:
                    state["query_fail"] += 1
                    failures = state["query_fail"]

                last_valid = state["last_valid"]
                stale_for = after_query - (last_valid if last_valid else started_at)
                stale_warning = None
                if (
                    stale_for >= STALE_SECONDS
                    and after_query - state["last_stale_warning"] >= WARNING_INTERVAL_SECONDS
                ):
                    state["last_stale_warning"] = after_query
                    stale_warning = stale_for
                snapshot = dict(state)

            if result != sdk.ControlStatus.SUCCESS and (failures == 1 or failures % 100 == 0):
                print(f"Query publish failed: {result}, failures={failures}")
            if stale_warning is not None:
                print(
                    f"WARNING: no fresh force response for {stale_warning:.3f}s: "
                    f"query_ok={snapshot['query_ok']} query_fail={snapshot['query_fail']} "
                    f"rx_1khz={snapshot['rx_1khz']} valid={snapshot['valid']}. "
                    "Check TIB, sensor power/wiring/baud rate, "
                    "endtool_raw_enabled and WBCS logs."
                )

            next_query_at += query_interval
            # Do not catch up with a burst after Publish blocks.
            if next_query_at <= after_query:
                next_query_at = after_query + query_interval

            if after_query >= next_stats_at:
                elapsed = after_query - last_stats_at
                print(
                    f"Rate: query_hz="
                    f"{(snapshot['query_ok'] - last_stats_query_count) / elapsed:.1f} "
                    f"rx_hz={(snapshot['rx_1khz'] - last_stats_rx_count) / elapsed:.1f} "
                    f"fresh={snapshot['valid']} duplicates={snapshot['duplicate_snapshots']} "
                    f"rejects(size={snapshot['reject_size']} "
                    f"prefix={snapshot['reject_prefix']} "
                    f"terminator={snapshot['reject_terminator']} "
                    f"numeric={snapshot['reject_numeric']} zero={snapshot['reject_zero']})"
                )
                last_stats_query_count = snapshot["query_ok"]
                last_stats_rx_count = snapshot["rx_1khz"]
                last_stats_at = after_query
                next_stats_at = after_query + STATS_INTERVAL_SECONDS
    finally:
        robot.unregister_endtool_raw_callback(handle)
        with state_lock:
            snapshot = dict(state)
        print(
            f"Stopped: query_ok={snapshot['query_ok']} query_fail={snapshot['query_fail']} "
            f"rx_1khz={snapshot['rx_1khz']} rejected={snapshot['rejected']} "
            f"(size={snapshot['reject_size']} prefix={snapshot['reject_prefix']} "
            f"terminator={snapshot['reject_terminator']} numeric={snapshot['reject_numeric']} "
            f"zero={snapshot['reject_zero']} "
            "non_increasing_generation="
            f"{snapshot['non_increasing_generation']}) fresh={snapshot['valid']} "
            f"duplicates={snapshot['duplicate_snapshots']}"
        )
        robot.destroy()


if __name__ == "__main__":
    main()

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.s1 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.s1 import GalbotRobot
from galbot_sdk.s1 import S1JointGroup, 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.s1 import GalbotRobot
from galbot_sdk.s1 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.s1 import GalbotRobot, S1JointGroup, 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

"""
S1 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.s1 import (
    ControlStatus,
    FrameTriad,
    GalbotRobot,
    GroupCommand,
    JointCommand,
    Pose,
    S1ControllerName,
    S1JointGroup,
    SingoriXTarget,
    TargetConfig,
    TargetGroupTrajectory,
    TargetSampling,
    TargetTaskTrajectory,
    TaskCommand,
    Twist,
    Vector3,
    TARGET_DATA_DEFAULT,
    TARGET_DATA_FRAME_POSE,
    TARGET_DATA_FRAME_TWIST,
    TARGET_TYPE_OVERRIDE,
    TARGET_TYPE_PROVERRIDE,
)


CHASSIS_TASK_NAME = S1JointGroup.swerve_chassis
CHASSIS_SUBTASK_POSE = "swerve_chassis_pose"
CHASSIS_SUBTASK_TWIST = "swerve_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_swerve_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 = [S1JointGroup.swerve_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_swerve_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 = [S1JointGroup.swerve_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_swerve_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 torso target\n"
        "  base_pose    - publish a swerve chassis pose target\n"
        "  base_twist   - publish a swerve chassis twist target with auto stop\n"
        "  mixed_pose   - publish torso + swerve chassis pose in one target\n"
        "  mixed_twist  - publish torso + swerve 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)

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

    torso_single_joint = [torso_joint_names[0]]
    joint_time_s = 3.0
    pose_time_s = 4.0
    twist_command_time_s = 0.2
    twist_duration_s = 2.0
    torso_height_m = 0.60  # torso target height in meters

    print_menu()

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

        if command == "joint":
            target = build_joint_target(S1JointGroup.torso, torso_single_joint, [torso_height_m], 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, S1ControllerName.SWERVE_CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
                continue
            target = build_swerve_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, S1ControllerName.SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
                continue
            target = build_swerve_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, S1ControllerName.SWERVE_CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
                continue
            target = merge_targets(
                [
                    build_joint_target(S1JointGroup.torso, torso_single_joint, [torso_height_m], joint_time_s),
                    build_swerve_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, S1ControllerName.SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
                continue
            target = merge_targets(
                [
                    build_joint_target(S1JointGroup.torso, torso_single_joint, [torso_height_m], joint_time_s),
                    build_swerve_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

"""
S1 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.s1 import (
    ControlStatus,
    ErrorInfo,
    FrameTriad,
    GalbotRobot,
    GroupCommand,
    JointCommand,
    Pose,
    S1ControllerName,
    S1JointGroup,
    SingoriXTarget,
    TargetConfig,
    TargetGroupTrajectory,
    TargetSampling,
    TargetTaskTrajectory,
    TaskCommand,
    Twist,
    Vector3,
    TARGET_DATA_DEFAULT,
    TARGET_DATA_FRAME_POSE,
    TARGET_DATA_FRAME_TWIST,
    TARGET_TYPE_OVERRIDE,
    TARGET_TYPE_PROVERRIDE,
)


CHASSIS_TASK_NAME = S1JointGroup.swerve_chassis
CHASSIS_SUBTASK_POSE = "swerve_chassis_pose"
CHASSIS_SUBTASK_TWIST = "swerve_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_swerve_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 = [S1JointGroup.swerve_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_swerve_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 = [S1JointGroup.swerve_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_swerve_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 torso target\n"
        "  base_pose    - request a swerve chassis pose target\n"
        "  base_twist   - request a swerve chassis twist target with auto stop\n"
        "  mixed_pose   - request torso + swerve chassis pose in one target\n"
        "  mixed_twist  - request torso + swerve 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)

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

    torso_single_joint = [torso_joint_names[0]]
    joint_time_s = 3.0
    pose_time_s = 4.0
    twist_command_time_s = 0.2
    twist_duration_s = 2.0
    torso_height_m = 0.60  # torso target height in meters

    print_menu()

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

        if command == "joint":
            target = build_joint_target(S1JointGroup.torso, torso_single_joint, [torso_height_m], joint_time_s)
            print_error_info("joint", robot.request_target(target))
            continue

        if command == "base_pose":
            if ensure_controller(robot, S1ControllerName.SWERVE_CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
                continue
            target = build_swerve_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, S1ControllerName.SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
                continue
            target = build_swerve_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, S1ControllerName.SWERVE_CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
                continue
            target = merge_targets(
                [
                    build_joint_target(S1JointGroup.torso, torso_single_joint, [torso_height_m], joint_time_s),
                    build_swerve_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, S1ControllerName.SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
                continue
            target = merge_targets(
                [
                    build_joint_target(S1JointGroup.torso, torso_single_joint, [torso_height_m], joint_time_s),
                    build_swerve_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.s1 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.s1 import GalbotRobot

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

# 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.s1 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.s1 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 ["torso", "head", "left_arm", "right_arm"] (S1: torso replaces G1 leg)
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.s1 import GalbotRobot, S1JointGroup


def print_gripper_state(gripper_state):
    """Print gripper status"""
    print(f"Timestamp (ns): {gripper_state.timestamp_ns}")
    print(f" width {gripper_state.width} velocity {gripper_state.velocity}"
          f" effort {gripper_state.effort} is moving {gripper_state.is_moving}")


# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
if robot.init():
    print('System initialized successfully!')
else:
    print('System initialization failed!')
    exit(1)

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

# Get gripper state
gripper_state = robot.get_gripper_state(S1JointGroup.left_gripper)

if gripper_state is None:
    print('get gripper state error')
else:
    print('Left gripper state:')
    print_gripper_state(gripper_state)

# Exit system and release SDK resources
robot.request_shutdown()
robot.wait_for_shutdown()
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.s1 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.s1 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()

# Available IMU sensors on S1 (choose one of the following three):
# - SensorType.HEAD_IMU: Head lidar IMU
# - SensorType.BACK_IMU: Rear lidar IMU
# - SensorType.CHASSIS_IMU: Chassis lidar IMU
robot.init({SensorType.HEAD_IMU})

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

imu_data = robot.get_imu_data(SensorType.HEAD_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 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.s1 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.s1 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"] == "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_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_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 and depth images, right arm depth image, chassis lidar data, torso IMU data
    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)
        # RGBimage
        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)
        # depth
        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'])
            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.s1 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.s1 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"] == "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 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.s1 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()

# Available lidar sensors on S1 (choose one of the following three):
# - SensorType.HEAD_LIDAR: Head lidar
# - SensorType.BACK_LIDAR: Rear lidar
# - SensorType.CHASSIS_LIDAR: Chassis lidar
enable_sensor_set = {SensorType.HEAD_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.HEAD_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.s1 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.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
    ]

    # 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.s1 import GalbotRobot
from galbot_sdk.s1 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
        -0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0,   # left_arm
        0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0        # 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.s1 import GalbotRobot
from galbot_sdk.s1 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
        -0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0,   # left_arm
        0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0        # 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"

    # 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()

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.s1 import GalbotRobot, ControlStatus, S1ControllerName, S1JointGroup


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 = S1ControllerName.LEFT_ARM_PVT_CTRL
    controller_str = "LEFT_ARM_PVT_CTRL"
    test_group = S1JointGroup.left_arm
    group_str = "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(test_group)
    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(test_group)
    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(test_group)
    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.s1 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 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.s1 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')

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 sys
import time
from typing import List, Sequence

import cv2
import numpy as np

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

_K_SPEED_RAD_S = 0.12
_K_TIMEOUT_S = 35.0

_K_PRESET_TORSO: List[float] = [0.58]
_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.7, 0.0]
_K_PRESET_RIGHT_ARM: List[float] = [-2.0, 1.5, 0.6, 1.7, 0.0, 0.7, 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 _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. Torso 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 _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
    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 (S1, HEAD_LEFT_CAMERA enabled)")
    time.sleep(5.0)

    torso_step_ok = False
    upper_step_ok = False
    demo_step_ok = False

    torso_set_status = robot.set_joint_positions(
        list(_K_PRESET_TORSO), ["torso"], [], True, _K_SPEED_RAD_S, _K_TIMEOUT_S
    )
    torso_step_ok = torso_set_status == ControlStatus.SUCCESS
    if not torso_step_ok:
        print("[FAIL] Torso set_joint_positions not SUCCESS (blocking)", file=sys.stderr)
    _print_summary("Torso set_joint_positions (blocking)", torso_step_ok)

    if torso_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: torso 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 = torso_step_ok and upper_step_ok and demo_step_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("Head camera", head_camera_capture_ok)

    installation_all_checks_passed = body_preset_step_passed 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.s1 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)


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

# 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)

# 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)

# 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)"
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.")

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

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.s1 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")

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.s1 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/s1/en/set_config_reference.md


def example_navigation(robot):
    # Navigation service: adjust replanning thresholds and select the short-distance
    # motion mode. omni_plan is G1-only (passing it on S1 returns INVALID_INPUT), so S1
    # uses adjust_motion_type instead -- 2 = rotate + crab-walk, the recommended S1 mode
    # (avoids joint collisions).
    status = robot.set_config(
        ConfigService.NAVIGATION,
        [
            ConfigItem("replan_threshold", 30.0),
            ConfigItem("no_replan_threshold", 1.0),
            ConfigItem("adjust_motion_type", 2),
        ],
    )
    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) together with
    # leg trajectory velocity/acceleration/jerk limits. Per-chain fields are keyed as
    # "{field_name}_{chain_name}"; the leg chain has only 1 joint on S1 (5 on G1), so the
    # array length here is S1-specific.
    status = robot.set_config(
        ConfigService.MOTION_PLAN,
        [
            ConfigItem("plan_timeout", 5.0),
            ConfigItem("max_velocity_leg", [0.8]),
            ConfigItem("max_acceleration_leg", [3.0]),
            ConfigItem("max_jerk_leg", [10.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.s1 import ConfigService, ControlStatus, GalbotRobot

def print_fields(title, status, fields):
    print(f"[{title}] status: {status}")
    if status != ControlStatus.SUCCESS:
        print("部分字段读取失败，以下为成功读取的字段：")
    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, ["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()

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.s1 import GalbotMotion
from galbot_sdk.s1 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 sys
import time

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

motion = GalbotMotion()
robot = GalbotRobot()


def print_pose(label, status, pose_vec, planner):
    status_str = planner.status_to_string(status)
    print(f"[{label}] Status: {status_str}")
    if status == gm.MotionStatus.SUCCESS:
        print("End-effector pose: " + " ".join(str(v) for v in pose_vec) + "\n")
    else:
        print("Calculation failed!")


def main():
    if motion.init():
        print("Planner initialized successfully!")
    else:
        print("Planner initialization failed!", file=sys.stderr)
        return -1

    if robot.init():
        print("System initialized successfully!")
    else:
        print("System initialization failed!", file=sys.stderr)
        return -1

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

    chain_joints = {
        "torso": [1.1],
        "head": [0.0000, -0.26],
        "left_arm": [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
        "right_arm": [0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0],
    }
    end_link = "left_arm_end_effector_mount_link"
    reference_frame = "base_link"
    custom_param = gm.Parameter()

    # --- test 1: default parameters (current status) ---
    try:
        print(">> Executing: Basic forward kinematics...")
        res1 = motion.forward_kinematics(end_link, reference_frame)
        print_pose("Basic version", res1[0], res1[1], motion)
    except Exception as e:
        print(f"❌ Basic version exception: {e}", file=sys.stderr)

    # --- test 2: joint state + parameters ---
    try:
        print(">> Executing: Forward kinematics with custom joints...")
        custom_joint_state = {"left_arm": chain_joints["left_arm"]}
        res2 = motion.forward_kinematics(
            end_link, reference_frame, custom_joint_state, custom_param
        )
        print_pose("Custom parameters", res2[0], res2[1], motion)
    except Exception as e:
        print(f"❌ Custom-parameter exception: {e}", file=sys.stderr)

    # --- test 3: RobotStates forward kinematics (current status, planning) ---
    try:
        print(">> Executing: Forward kinematics based on RobotStates...")
        current_state = motion.get_robot_states()
        if not current_state.whole_body_joint:
            print(
                "❌ RobotStates-based: Unable to get current body joint states; "
                "ensure WBC/sensors are ready.",
                file=sys.stderr,
            )
        else:
            res3 = motion.forward_kinematics_by_state(
                end_link,
                current_state,
                reference_frame,
                gm.Parameter(),
            )
            print_pose("Based on RobotStates", res3[0], res3[1], motion)
    except Exception as e:
        print(f"❌ RobotStates-based exception: {e}", file=sys.stderr)

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    return 0


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

Jacobian Matrix (get_jacobian)

Applicable Scenarios: Calculate the Jacobian matrix for a target chain and frame based on specified joint values. Used for velocity mapping, differential kinematics, and controller debugging.

examples/python/galbot_motion/get_jacobian.py

import time

import galbot_sdk.s1 as gm
from galbot_sdk.s1 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)}")
        # Test all kinematic chains; no chain is excluded.
        # torso/leg are virtual chains handled by the server (leg is treated the same as torso).
        if not support_chains:
            support_chains = {"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.
        # S1's get_chain_joint_state() skips leg, but leg still accepts a joint value (DOF = 1).
        fallback_dof = {"leg": 1}

        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())

Jacobian Matrix by Robot State (get_jacobian_by_state)

Applicable Scenarios: Calculate the Jacobian matrix from a complete robot state or the current robot state. Suitable for whole-body state analysis and controller verification.

examples/python/galbot_motion/get_jacobian_by_state.py

import time

import galbot_sdk.s1 as gm
from galbot_sdk.s1 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)}")
        # torso/leg are virtual chains handled by the server (leg is treated the same as torso).
        if not support_chains:
            support_chains = {"left_arm", "right_arm", "torso", "leg"}

        target_frame = "EndEffector"
        reference_frame = "base_link"

        whole_body_dof = 17
        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())

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 sys
import time

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

motion = GalbotMotion()
robot = GalbotRobot()


def print_ik_result(label, status, joint_map, planner):
    print(f"[{label}] Status feedback: {planner.status_to_string(status)}")
    if status == gm.MotionStatus.SUCCESS:
        print("✅ IK computation succeeded! Joint angles obtained:")
        for name, joints in joint_map.items():
            print(f"  - Chain [{name}]: " + " ".join(str(v) for v in joints))
    else:
        print("❌ inverse kinematics failed, checkinput targetpose.")
    print("---------------------------------------------------")


def main():
    if motion.init():
        print("Planner initialized successfully!")
    else:
        print("Planner initialization failed!", file=sys.stderr)
        return -1

    if robot.init():
        print("System initialized successfully!")
    else:
        print("System initialization failed!", file=sys.stderr)
        return -1

    time.sleep(1)

    reference_frame = "base_link"
    target_frame = "EndEffector"
    target_chain = "left_arm"
    params = gm.Parameter()

    # Get current end-effector pose from robot as target for subsequent IK scenarios
    status, target_pose = motion.get_end_effector_pose_on_chain(
        target_chain, target_frame, reference_frame
    )
    if status != gm.MotionStatus.SUCCESS or len(target_pose) != 7:
        print("Failed to get current end-effector pose, cannot continue test.", file=sys.stderr)
        return -1

    print(f"Current {target_chain} End-effector pose: " + " ".join(str(v) for v in target_pose))
    print("---------------------------------------------------")

    one_chain = [target_chain]
    chain_with_torso = [target_chain, "torso"]
    error_chains = [target_chain, "torso", "head"]

    # Scenario 1: Single-chain inverse kinematics (using default parameters)
    try:
        print(">> Running Scenario 1: Single-chain IK test...")
        res = motion.inverse_kinematics(target_pose, one_chain)
        print_ik_result("Single-chain inverse kinematics", res[0], res[1], motion)
        time.sleep(0.8)
    except Exception as e:
        print(f"Scenario 1 exception: {e}", file=sys.stderr)

    # Scenario 2: Arm chain + torso inverse kinematics
    try:
        print(">> Running Scenario 2: Arm chain + torso IK test...")
        res = motion.inverse_kinematics(
            target_pose,
            chain_with_torso,
            target_frame,
            reference_frame,
            {},
            False,
            params,
        )
        print_ik_result("Arm + torso inverse kinematics", res[0], res[1], motion)
        time.sleep(0.8)
    except Exception as e:
        print(f"Scenario 2 exception: {e}", file=sys.stderr)

    # Scenario 3: invalid chain combination (expected to return INVALID_INPUT)
    try:
        print(">> Running Scenario 3: Invalid chain-combination test...")
        res = motion.inverse_kinematics(
            target_pose,
            error_chains,
            target_frame,
            reference_frame,
            {},
            False,
            params,
        )
        print_ik_result("Invalid chain combination detection", res[0], res[1], motion)
    except Exception as e:
        print(f"Scenario 3 exception: {e}", file=sys.stderr)

    # Scenario 4: Inverse kinematics with initial reference joint values (using current robot state)
    try:
        print(">> Running Scenario 4: IK test with initial reference values...")
        current_chain_joints = motion.get_chain_joint_state()
        print("  Current joint states:")
        for name, joints in current_chain_joints.items():
            print(f"    [{name}]: " + " ".join(str(v) for v in joints))

        res = motion.inverse_kinematics(
            target_pose,
            one_chain,
            target_frame,
            reference_frame,
            current_chain_joints,
            False,
            params,
        )
        print_ik_result("Inverse kinematics with reference values", res[0], res[1], motion)
        time.sleep(0.8)
    except Exception as e:
        print(f"Scenario 4 exception: {e}", file=sys.stderr)

    # Scenario 5: Inverse kinematics based on RobotStates (get full state from robot)
    try:
        print(">> Running Scenario 5: IK test based on RobotStates...")
        robot_states = motion.get_robot_states()
        ref_state = gm.RobotStates()
        ref_state.whole_body_joint = list(robot_states.whole_body_joint)
        ref_state.base_state = list(robot_states.base_state)
        ref_state.chain_name = target_chain

        print(f"  Number of whole-body joints: {len(ref_state.whole_body_joint)}")
        print(f"  Number of chassis states: {len(ref_state.base_state)}")

        res = motion.inverse_kinematics_by_state(
            target_pose,
            one_chain,
            target_frame,
            reference_frame,
            ref_state,
            False,
            params,
        )
        print_ik_result("RobotStates inverse kinematics", res[0], res[1], motion)
    except Exception as e:
        print(f"Scenario 5 exception: {e}", file=sys.stderr)

    robot.request_shutdown()
    robot.wait_for_shutdown()
    robot.destroy()
    print("Resources released.")
    return 0


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

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.s1 as gm
from galbot_sdk.s1 import GalbotMotion, GalbotRobot

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")

time.sleep(3)

reference_frame = "base_link"
target_frame = "EndEffector"
target_chain = "right_arm"
end_ee_link = "right_arm_end_effector_mount_link"
custom_param = gm.Parameter()

# Scenario 1: Get end-effector pose (basic version)
try:
    print(">> Scenario 1: Getting the basic end-effector pose...")
    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"❌ Scenario 1 exception: {e}")

# Scenario 2: specify chain name + custom frame to get end-effector pose
current_pose = None
try:
    print(">> Scenario 2: Getting pose by specified chain name...")
    status, current_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: {current_pose}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ Scenario 2 exception: {e}")

# Scenario 3: Set end-effector pose (small offset from current pose)
try:
    print(">> Scenario 3: Setting end-effector pose...")
    if current_pose is None or len(current_pose) != 7:
        print("❌ Unable to get current pose; skipping Scenario 3")
    else:
        target_pose = list(current_pose)
        target_pose[2] -= 0.05  # Move downward 5 cm in z direction
        print(f"  Target pose: {target_pose}")

        status = motion.set_end_effector_pose(
            target_pose=target_pose,
            end_effector_frame=target_chain,
            reference_frame=reference_frame,
            enable_collision_check=False,
            is_blocking=True,
            timeout=5.0,
            params=custom_param
        )
        printStatus(status)
        if status == gm.MotionStatus.SUCCESS:
            print(f"✅ Motion execution completed")
except Exception as e:
    print(f"❌ Scenario 3 exception: {e}")

# Scenario 4: Get end-effector pose after execution to verify result
try:
    print(">> Scenario 4: Getting end-effector pose after motion...")
    status, pose = motion.get_end_effector_pose(
        end_effector_frame=end_ee_link,
        reference_frame=reference_frame
    )
    printStatus(status)
    if status == gm.MotionStatus.SUCCESS:
        print(f"✅ End-effector pose after motion: {pose}")
    time.sleep(0.8)
except Exception as e:
    print(f"❌ Scenario 4 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

import time

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

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


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"


if not confirm_robot_safety():
    print("Example cancelled.")
    raise SystemExit(0)


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(1)

chain_joints = {
    "torso":     [0.65],
    "head":      [0.0000, -0.26],
    "left_arm":  [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
    "right_arm": [0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0]
}

whole_body_joint = [
    value
    for chain in ["torso", "head", "left_arm", "right_arm"]
    for value in chain_joints[chain]
]
move_status = robot.set_joint_positions(
    whole_body_joint,
    ["torso", "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()
    raise SystemExit(1)
print("✅ Moved to the initial whole-body joint state")
time.sleep(3.0)

custom_param = gm.Parameter()

# Scenario 1: joint-space planning, target type = joint state
try:
    print(">> Scenario 1: joint-space planning (joint target)...")

    target_joint = gm.JointStates()
    target_joint.chain_name = "left_arm"
    target_positions = list(chain_joints[target_joint.chain_name])
    target_positions[4] += 0.1
    target_joint.joint_positions = target_positions
    # 若设备处于 home 位，target 就是当前点、规划轨迹会为空，故增加偏移。
    # 不使用 set_joint()：SDK 1.10.0 的该接口只接受整数，会拒绝浮点关节角。

    status, traj = motion.motion_plan(
        target=target_joint,
        enable_collision_check=False,
        params=custom_param
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Planning failed"
    if traj != {}:
        print(f"✅ Joint-space planning (joint target) success: trajectory points={len(traj[target_joint.chain_name])}")
        time.sleep(0.8)
    else:
        print(f"⚠️ SUCCESS but trajectory is empty, target may already be reached")

except Exception as e:
    print(f"ERROR: Scenario 1 exception: {e}")

# Scenario 2: joint-space planning, target type = end-effector pose (Cartesian)
try:
    print(">> Scenario 2: joint-space planning (pose target)...")

    target_pose_state = gm.PoseState()
    target_pose_state.chain_name = "left_arm"
    target_pose_state.frame_id = "EndEffector"
    target_pose_state.reference_frame = "base_link"

    status, current_pose = motion.get_end_effector_pose_on_chain(
        chain_name=target_pose_state.chain_name,
        frame_id=target_pose_state.frame_id,
        reference_frame=target_pose_state.reference_frame
    )
    assert status == gm.MotionStatus.SUCCESS and len(current_pose) == 7, \
        "Failed to get current end-effector pose"

    target_pose = list(current_pose)
    target_pose[0] += 0.04  # Move forward 4 cm from the current pose.
    target_pose[2] += 0.02  # Move upward 2 cm from the current pose.
    target_pose_state.pose = gm.Pose(target_pose)

    status, traj = motion.motion_plan(
        target=target_pose_state,
        enable_collision_check=False,
        params=custom_param
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Planning failed"
    if traj != {}:
        print(f"✅ Joint-space planning (pose target) success: trajectory points={len(traj[target_pose_state.chain_name])}")
        time.sleep(0.8)
    else:
        print(f"⚠️ SUCCESS but trajectory is empty, target may already be reached")

except Exception as e:
    print(f"ERROR: Scenario 2 exception: {e}")

# Scenario 3: joint-space planning with an explicit start state
try:
    print(">> Scenario 3: joint-space planning (explicit start)...")

    target_joint = gm.JointStates()
    target_joint.chain_name = "left_arm"
    target_joint.joint_positions = chain_joints[target_joint.chain_name]

    start_joint = gm.JointStates()
    start_joint.chain_name = "left_arm"
    start_joint.joint_positions = [0] * 7

    status, traj = motion.motion_plan(
        target=target_joint,
        start=start_joint,
        enable_collision_check=False,
        params=custom_param
    )
    printStatus(status)
    assert status == gm.MotionStatus.SUCCESS, "Planning failed"
    if traj != {}:
        print(f"✅ Joint-space planning (explicit start) success: trajectory points={len(traj[target_joint.chain_name])}")
    else:
        print(f"⚠️ SUCCESS but trajectory is empty, target may already be reached")

except Exception as e:
    print(f"ERROR: Scenario 3 exception: {e}")

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

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.s1 as gm
from galbot_sdk.s1 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 = {
        "torso": [0.65],
        "head": [0.0, -0.26],
        "left_arm": [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
        "right_arm": [0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0]
    }

     # FK results of chain_joints in base_link, showing their physical correspondence.
    chain_pose_baselink = {
        "torso": [-0.0715, 0.0000, 1.2044, 0.0000, 0.0000, 0.0000, 1.0000],
        "head": [-0.0123, 0.0046, 1.4975, -0.0002, 0.1298, -0.0001, 0.9915],
        "left_arm": [0.2851, 0.3190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920],
        "right_arm": [0.2819, -0.3212, 1.9315, 0.3701, 0.7731, 0.1567, -0.4907],
    }

    whole_body_joint = [
        num for key in ["torso", "head", "left_arm", "right_arm"]
        for num in chain_joints[key]
    ]
    move_status = robot.set_joint_positions(
        whole_body_joint,
        ["torso", "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)

    custom_param = gm.Parameter()
    target_chain = "left_arm"

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

        # Construct waypoints (3 intermediate poses)
        waypoint_poses = [
            [0.2851, 0.3190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920],
            [0.2851, 0.4190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920],
            [0.2851, 0.5190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920],
            [0.2851, 0.5190, 1.8214, -0.3816, 0.7681, -0.1496, -0.4920]
        ]

        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("⚠️ 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 joint state
        target_joint = gm.JointStates()
        target_joint.chain_name = target_chain

        # Construct waypoints (3 intermediate joint states)
        waypoints = [
            [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
            [-0.52, -0.94, -0.54, -1.92, 0.2, 0.05, 0.0],
            [-0.57, -0.94, -0.54, -1.90, 0.2, 0.10, 0.0],
            [-0.55, -0.93, -0.54, -1.98, 0.19, 0.15, 0.01]
        ]

        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, "Joint 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("⚠️ 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 sys

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

# NOTE:
# - GalbotNavigation (galbotNav) may use real-time obstacle perception/avoidance during navigation (deployment dependent).
# - GalbotMotion does NOT provide real-time obstacle perception today; Motion collision uses self-collision +
#   manually loaded obstacles (add_obstacle/attach_target_object).

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)

chain_joints = {
    "torso": [1.1],
    "head": [0.0000, -0.26],
    "left_arm": [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
    "right_arm": [0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0],
}

whole_body_joint = [
    num for key in ["torso", "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]
bad_left_arm_joint = [1.99995, -1.60004, 0.599905, -1.69994, 0, -0.799924, 0]
custom_param = gm.Parameter()

try:
    print(">> Running collision check...")

    # Construct a RobotStates list for collision checking
    check_states = []

    # status 0: RobotStates
    state0 = gm.RobotStates()
    state0.whole_body_joint = whole_body_joint
    state0.base_state = base_state
    check_states.append(state0)

    # status 1: JointStates
    state1 = gm.JointStates()
    state1.chain_name = "left_arm"
    state1.joint_positions = bad_left_arm_joint
    check_states.append(state1)

    status, collision_results = motion.check_collision(
        start=check_states,
        enable_collision_check=True,
        params=custom_param,
    )

    print(f"Status: {motion.status_to_string(status)}")

    if status == gm.MotionStatus.SUCCESS:
        print("OK: collision check finished (false=no collision, true=collision):")
        for i, collided in enumerate(collision_results):
            label = "COLLISION" if collided else "NO COLLISION"
            print(f"  - status [{i}]: {label}")
    else:
        print("ERROR: collision check returned failure.", file=sys.stderr)

except Exception as e:
    print(f"ERROR: collision check exception: {e}", file=sys.stderr)

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.s1 as gm
from galbot_sdk.s1 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 = "GE103v1"
    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.s1 as gm
from galbot_sdk.s1 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.s1 as gm
from galbot_sdk.s1 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}")

    # example: link kinematics
    if ee_link_names:
        print(f"\nRun forward kinematics using end-effector link '{ee_link_names[0]}'...")
        success, fk_result = motion.forward_kinematics(ee_link_names[0])
        if success == gm.MotionStatus.SUCCESS:
            print(f"Forward-kinematics result: {fk_result}")
        else:
            print(f"Forward-kinematics computation failed: {success}")
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.s1 as gm
from galbot_sdk.s1 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()

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.s1 as gm
from galbot_sdk.s1 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 = {
        "torso": [1.1],
        "head": [0.0000, -0.26],
        "left_arm": [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
        "right_arm": [0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0],
    }
    whole_body_joint = [num for key in ["torso", "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 = [1.99995, -1.60004, 0.599905, -1.69994, 0.0, -0.799924, 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()

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.s1 import GalbotNavigation
from galbot_sdk.s1 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.s1 import GalbotNavigation
from galbot_sdk.s1 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')

Check Path Reachability and Blocking Navigation to Goal

Applicable Scenarios: Check if the target point is reachable, and if reachable, block and wait for navigation to complete reaching the target. Convenient for simple scenarios.

examples/python/galbot_navigation/check_path_reachability.py

from galbot_sdk.s1 import GalbotNavigation
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import S1ControllerName, ControlStatus
import numpy as np
import sys

nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()

start = nav.get_current_pose()
goal = np.array([1.0, 1.0, 0.0, 0, 0, 0.4794255, 0.8775826])

res = robot.switch_controller(S1ControllerName.SWERVE_CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
    print("Failed to switch controller!")
    sys.exit(1)
else:
    print("Controller switched successfully!")

if nav.check_path_reachability(goal, start):
    status = nav.navigate_to_goal(
        goal, enable_collision_check=True, is_blocking=True, timeout=30
    )
    print("navigate_to_goal returned status:", status)
    print("Reached or not:", nav.check_goal_arrival())
else:
    print("Path unreachable or unsafe")

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.s1 import GalbotNavigation
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import S1ControllerName, ControlStatus
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(S1ControllerName.SWERVE_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.s1 import GalbotNavigation
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import S1ControllerName, ControlStatus
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(S1ControllerName.SWERVE_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.s1 import ControlStatus, GalbotNavigation, GalbotRobot, S1ControllerName

# 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(S1ControllerName.SWERVE_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.s1 import GalbotNavigation
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import NavigationTaskStatus, S1ControllerName, ControlStatus
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(S1ControllerName.SWERVE_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.s1 import GalbotRobot
from galbot_sdk.s1 import GalbotNavigation
from galbot_sdk.s1 import S1ControllerName, ControlStatus
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(S1ControllerName.SWERVE_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.s1 import GalbotNavigation
from galbot_sdk.s1 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.s1 import ControlStatus, S1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.s1 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(S1ControllerName.SWERVE_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.s1 import ControlStatus, GalbotNavigation, GalbotRobot, S1ControllerName

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(S1ControllerName.SWERVE_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.s1 import ControlStatus, GalbotNavigation, GalbotRobot, NavigationTaskStatus
from galbot_sdk.s1 import S1ControllerName

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(S1ControllerName.SWERVE_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.s1 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.s1 import ControlStatus, S1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.s1 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(S1ControllerName.SWERVE_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.s1 import ControlStatus, S1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.s1 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(S1ControllerName.SWERVE_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.6
    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)

            elapsed_ms = (time.time() - 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 Inference

Applicable Scenarios: Trigger a single stereo depth-estimation inference and retrieve the resulting depth data.

examples/python/galbot_perception/foundation_stereo_example.py

"""Foundation stereo depth example (S1): single run_once, save a pseudo-color depth image."""

import time

import cv2
import numpy as np

try:
    from galbot_sdk.s1 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.s1 import Parameter, create_parameter

# Create Parameter via constructor and set options
p = Parameter()
p.set_blocking(True)            # Set whether to block execution
p.set_check_collision(False)     # Disable collision detection
p.set_timeout(5.0)              # Set timeout (seconds)
p.set_actuate('with_chain_only')# Set drive mode
p.set_tool_pose(False)           # Whether to consider tool pose
p.set_reference_frame('base_link')

print('--- Parameter p ---')
print('blocking:', p.get_blocking())
print('collision check:', p.get_check_collision())
print('timeout:', p.get_timeout(), 's')

# 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.s1 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.s1 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/s1/python/tutorials. For walkthroughs and safety notes, see Tutorials.

Example 1. Galbot Control Started

Applicable Scenarios: Basic S1 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.s1 import GalbotRobot
    from galbot_sdk.s1 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(True, ["torso", "head", "left_arm", "right_arm"])
        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.00,  0.40, -2.06, -1.80, -2.92, -0.76, -0.03,  # left_arm
            -1.00, -0.40,  2.06,  1.80,  2.92,  0.76,  0.03   # right_arm
        ]]
        # Whether to block and wait for joints to reach position
        is_blocking = True
        # Limit maximum joint speed to 0.3rad/s
        max_speed = 0.3
        # Maximum blocking wait time
        timeout_s = 30

        # 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.s1 as gm
    from galbot_sdk.s1 import GalbotMotion
    from galbot_sdk.s1 import GalbotRobot
    from galbot_sdk.s1 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
        motion = GalbotMotion()
        robot = GalbotRobot()

        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)

        # Move whole body to predefined zero(home) configuration before arm demo.
        status = motion.move_whole_body_joint_zero(
            is_blocking=True,
            leg_head_speed_rad_s=0.2,
            leg_head_timeout_s=15.0,
            params=gm.Parameter()
        )
        printStatus(status)
        if status != gm.MotionStatus.SUCCESS:
            print("❌ move_whole_body_joint_zero failed, stop demo to avoid risky posture.")
            return
        print("✅ whole-body zero completed, start arm manipulation demo from safe pose.")

        # 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}")
            return

        # Build S1 demo target pose from zero-start current pose instead of hard-coded absolute pose.
        # This avoids using model-specific (e.g. G1-like) constants in S1 tutorial.
        chain_pose_baselink = {
            "left_arm": [
                original_pose[0] + 0.04,  # x forward 4 cm
                original_pose[1],         # keep lateral position
                original_pose[2] + 0.02,  # z up 2 cm
                original_pose[3],
                original_pose[4],
                original_pose[5],
                original_pose[6]
            ]
        }
        print(f"✅ S1 target pose (relative to zero pose): {chain_pose_baselink[target_chain]}")

        # 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.s1 import GalbotNavigation
    from galbot_sdk.s1 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.s1 import GalbotRobot
    from galbot_sdk.s1 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,
        # back LiDAR data, and back IMU data
        enable_sensor_set = {SensorType.LEFT_ARM_CAMERA, # Left arm RGB camera
                            SensorType.LEFT_ARM_DEPTH_CAMERA, # Left arm depth camera
                            SensorType.BACK_LIDAR, # Back LiDAR
                            SensorType.BACK_IMU} # Back IMU

        # To save resource overhead, only cameras and radar sensors passed during initialization can acquire data
        if robot.init(enable_sensor_set):
            print("Initialization successful")
        else:
            print("Initialization failed")
            return
        # 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 back LiDAR data
        lidar_data = robot.get_lidar_data(SensorType.BACK_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 back IMU data
        imu_data = robot.get_imu_data(SensorType.BACK_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: Convert simulated VLA outputs into safe coordinated robot joint commands.

examples/python/tutorials/example5_execute_vla.py

"""
Note: When running this example, please ensure the robot's `emergency stop button` is released;
"""
import time
import os
from typing import List, Dict, Any

try:
    import galbot_sdk.s1 as gm
    from galbot_sdk.s1 import GalbotRobot
    from galbot_sdk.s1 import GalbotMotion
    from galbot_sdk.s1 import GalbotNavigation
    from galbot_sdk.s1 import (
        SensorType, RgbOutputFormat, S1JointGroup, ControlStatus,
        Trajectory, TrajectoryPoint, JointCommand
    )
except ImportError:
    print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
    exit(1)

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


def decode_compressed_image(compressed_image: Dict[str, Any]) -> np.ndarray:
    """
    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)
    else:
        raise ValueError(f"Unsupported data format: {compressed_image['format']}")


def decode_rgb_image(image_data: bytes) -> np.ndarray:
    """
    Decode RGB image.

    Parameters:
        image_data (bytes): Raw image data

    Returns:
        numpy.ndarray: Decoded RGB image
    """
    nparr = np.frombuffer(image_data, np.uint8)
    img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
    if img is None:
        raise ValueError("Failed to decode RGB Image")
    return img


def decode_depth_image(image_data: Dict[str, Any]) -> np.ndarray:
    """
    Decode depth image.

    Parameters:
        image_data (dict): Depth image data with metadata

    Returns:
        numpy.ndarray: Decoded 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 fake_vla(rgb_data_dict: dict) -> Dict[str, np.ndarray]:
    """
    Fake VLA (Vision-Language-Action) model implementation.
    
    This function is a placeholder. In a real-world scenario, you would implement
    target detection using computer vision techniques. For this example, we assume
    a default pose.

    Parameters:
        rgb_data_dict (dict): Dictionary containing RGB image data from various sensors

    Returns:
        dict: Trajectories for different robot components (legs, head, arms)
    """
    print("Fake VLA executing...")
    
    num_points = 200

    # S1 zero pose (aligned with C++ examples and GalbotMotionS1::move_whole_body_joint_zero)
    torso_zero = np.array([1.1], dtype=np.float64)
    head_zero = np.array([0.0, -0.26], dtype=np.float64)
    left_arm_zero = np.array([-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0], dtype=np.float64)
    right_arm_zero = np.array([0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0], dtype=np.float64)

    # Torso: Move linearly from 1.1 to 0.7
    torso_traj = np.linspace([1.1], [0.7], num=num_points, dtype=np.float64)

    # Head: Apply small sinusoidal motion on the pitch joint
    phase = np.linspace(0.0, np.pi, num=num_points, dtype=np.float64)
    head_traj = np.repeat(head_zero.reshape(1, -1), num_points, axis=0)
    head_traj[:, 1] = head_zero[1] + 0.1 * np.sin(phase)

    # Dual arms: Start from zero pose and apply small linear changes only on joint 4
    left_delta = np.zeros_like(left_arm_zero)
    right_delta = np.zeros_like(right_arm_zero)
    left_delta[3] = 0.2
    right_delta[3] = -0.2

    s = np.linspace(0.0, 1.0, num=num_points, dtype=np.float64).reshape(-1, 1)
    left_arm_traj = left_arm_zero.reshape(1, -1) + s * left_delta.reshape(1, -1)
    right_arm_traj = right_arm_zero.reshape(1, -1) + s * right_delta.reshape(1, -1)

    return {
        "torso": torso_traj,
        "head": head_traj,
        "left_arm": left_arm_traj,
        "right_arm": right_arm_traj,
    }


def estimate_vla(robot: 'GalbotRobot', enable_sensor_set: set) -> Dict[str, np.ndarray]:
    """
    Estimate VLA (Vision-Language-Action) model outputs based on sensor data.

    Parameters:
        robot (GalbotRobot): GalbotRobot instance
        enable_sensor_set (set): Set of enabled sensor types

    Returns:
        dict: Joint positions for each joint group
    """
    # Get RGB images from enabled cameras
    rgb_data_dict = {}
    for sensor_type in enable_sensor_set:
        if sensor_type in [
            SensorType.LEFT_ARM_CAMERA,
            SensorType.RIGHT_ARM_CAMERA
        ]:
            # Get RGB data of sensor type
            rgb_data = robot.get_rgb_data(sensor_type, RgbOutputFormat.JPEG, True)
            time.sleep(1)
            
            # Decode RGB image
            if rgb_data:
                rgb_image = decode_compressed_image(rgb_data)
                rgb_data_dict[sensor_type] = rgb_image
            else:
                print(f"Failed to get RGB data from {sensor_type}")
        else:
            print(f"Unsupported sensor type: {sensor_type}")
    
    print("RGB data dictionary keys:", rgb_data_dict.keys())

    # Get joint position list
    joint_positions = fake_vla(rgb_data_dict)
    return joint_positions


def generate_target_point(q: List[float], target_time: float = 10.0) -> 'TrajectoryPoint':
    """
    Generate target points for trajectory execution.

    Parameters:
        q (List[float]): Joint positions
        target_time (float): Time from start in seconds (default: 10.0)

    Returns:
        TrajectoryPoint: Target trajectory point with specified joint commands
    """
    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: np.ndarray, 
    joint_groups: List[str] = None, 
    joint_names: List[str] = None, 
    dt: float = 0.008
) -> 'Trajectory':
    """
    Generate trajectory for joints.

    Parameters:
        trajectory (np.ndarray): 2D array of joint positions over time
        joint_groups (List[str]): List of joint groups
        joint_names (List[str]): List of joint names
        dt (float): Time step between trajectory points (default: 0.008)

    Returns:
        Trajectory: Generated trajectory for execution
    """
    if joint_groups is None:
        joint_groups = []
    if joint_names is None:
        joint_names = []

    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

    current_time = 0.0
    points = []
    for state in trajectory:
        current_time += dt
        # Generate target point for each joint
        traj_point = generate_target_point(state, current_time)
        points.append(traj_point)

    traj.points = points
    return traj


def print_joint_states(joint_states: List['JointState']) -> None:
    """
    Print joint states in a readable format.

    Parameters:
        joint_states (List[JointState]): List of joint states
    """
    for js in joint_states:
        print(
            f" : position = {js.position} , velocity = {js.velocity} "
            f", acceleration = {js.acceleration} , effort = {js.effort} , current = {js.current}"
        )


def check_robot_safety() -> None:
    """
    Check if the robot is safe to operate.
    Prompts the user to confirm safety conditions before proceeding.
    """
    # 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)..."
        ).lower()
        
        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() -> None:
    """
    Main function to execute the VLA (Vision-Language-Action) example.
    Initializes the robot, estimates actions based on sensor data, and executes trajectories.
    """
    check_robot_safety()

    robot = None  # Initialize robot variable for cleanup in finally block

    try:
        # Get robot instances
        robot = GalbotRobot()
        motion = GalbotMotion()
        navi = GalbotNavigation()

        # Enable required sensors - S1 only has arm cameras, not head cameras
        enable_sensor_set = {
            SensorType.LEFT_ARM_CAMERA,
            SensorType.RIGHT_ARM_CAMERA
        }
                             
        # Initialize robot components
        if not robot.init(enable_sensor_set):
            print("Robot initialization failed")
            exit(1)
        else:
            print("Robot initialization successful")
            
        if not motion.init():
            print("Motion initialization failed")
            exit(1)
        else:
            print("Motion initialization successful")
            
        if not navi.init():
            print("Navigation initialization failed")
            exit(1)
        else:
            print("Navigation initialization successful")
        
        # Wait for data preparation
        time.sleep(3)

        # Estimate VLA actions
        joint_positions = estimate_vla(robot, enable_sensor_set)
        
        # Generate target trajectory with fixed S1 order
        joint_groups = ["torso", "head", "left_arm", "right_arm"]
        joint_traj = np.concatenate([joint_positions[group] for group in joint_groups], axis=-1)
        
        # Final joint position check state
        whole_body_joint = joint_traj[-1]
        base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
        check_state = gm.RobotStates()
        check_state.whole_body_joint = whole_body_joint
        check_state.base_state = base_state
        print(f"✅ Final joint position check state: {whole_body_joint}")
        
        # Check collision
        status, collision_res = motion.check_collision(
            start=[check_state],
            enable_collision_check=True
        )
        time.sleep(1)

        if status == gm.MotionStatus.SUCCESS:
            print(f"✅ OK: collision check finished: {collision_res} (False=no collision)")

            # Get joint states before execution
            before_joint_states = robot.get_joint_states(joint_groups, [])
            print(f"Joint groups to execute: {joint_groups}")
            print(f"Joint states size before execution: {len(before_joint_states)}")
            print_joint_states(before_joint_states)

            # Execute trajectory
            trajectory = generate_target_trajectory(joint_traj, joint_groups, [])
            if trajectory is not None:
                status = robot.execute_joint_trajectory(trajectory, True)
                time.sleep(1)

                if status == ControlStatus.SUCCESS:
                    print("✅ Joint trajectory execution successful.")
                else:
                    print("❌ Joint trajectory execution failed.")
                    print(f"Diagnostic: target points = {len(trajectory.points)}, joint dimension = {len(whole_body_joint)}")

                # Check joint state
                joint_states = robot.get_joint_states(joint_groups, [])
                print_joint_states(joint_states)

                # Compute position error after execution
                error_norm_after = sum(abs(whole_body_joint[i] - joint_states[i].position) for i in range(len(whole_body_joint)))
                print(f"Absolute position error norm after execution: {error_norm_after}")
                print(f"✅ Final joint position check state after execution")
            else:
                print("❌ Generated trajectory is invalid, cannot execute.")
        else:
            print("❌ Collision check failed, will not execute the joint trajectory.")

    except Exception as e:
        print(f"An exception occurred: {e}")
        import traceback
        traceback.print_exc()
    finally:
        # Ensure proper cleanup
        if robot is not None:
            # 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 6. Integrated Task: Navigation + Perception + Grasping + Placement

Applicable Scenarios: Integrate navigation, perception, grasping, and placement into a complete autonomous task.

examples/python/tutorials/example6_pick_and_place.py

"""
Note: When running this example, please confirm that the robot's motion control service `/data/galbot/bin/service_motion_plan`,
    robot state publishing service `/data/galbot/bin/robot_state_publish`,
    navigation service `/data/galbot/bin/service_navigation_plan`
    and hand-eye calibration publishing service `/data/galbot/bin/eyehand_calib_publish` have been loaded;
"""
try:
    import galbot_sdk.s1 as gm
    from galbot_sdk.s1 import GalbotNavigation
    from galbot_sdk.s1 import GalbotRobot
    from galbot_sdk.s1 import GalbotMotion
    from galbot_sdk.s1 import S1JointGroup, SensorType, RgbOutputFormat, ControlStatus
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:
    from scipy.spatial.transform import Rotation as R
except ImportError:
    os.system("pip install scipy")
    from scipy.spatial.transform import Rotation as R

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

import time
from typing import Sequence

# mode: chassisnavigation(C++ example10 kDemoMockNavigationOnly)
DEMO_MOCK_NAVIGATION_ONLY = True

# Query concrete joint names at runtime so the example works with both S1 V1
# (torso_base_joint) and S1 V2 (torso_lift_joint1).
S1_BODY_JOINT_GROUPS_FOR_SNAPSHOT = ["torso", "head", "left_arm", "right_arm"]
S1_BODY_JOINT_COUNT = 17


def mock_navigation_log(step_description: str) -> None:
    print(f"[Mock navigation] {step_description} (navigate_to_goal not called, chassis does not move)")


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 print_gripper_state(joint_group, gripper_state):
    """
    Print gripper state

    Parameters:
        joint_group (S1JointGroup): S1JointGroup enum
        gripper_state (object): Contains 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}"
    )

def get_navigation_pose(object_goal_pose: Sequence[float], motion: GalbotMotion, arm: str = "left_arm"):
    """
    Get navigation target pose

    Parameters:
        object_goal_pose (Sequence[float]): Target pose [x, y, z, qx, qy, qz, qw]
        motion (GalbotMotion): Motion control instance
        arm (str, optional): End effector name. Defaults to "left_arm".

    Returns:
        Sequence[float]: Navigation target pose [x, y, z, qx, qy, qz, qw]
    """
    assert arm in ["left_arm", "right_arm"], "arm must be left_arm or right_arm"

    try:
        status, ee_pose_in_base = motion.get_end_effector_pose(
            end_effector_frame=f"{arm}_end_effector_mount_link",
            reference_frame="base_link"
        )
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to get end effector pose: status={status}")
            offset_y = ee_pose_in_base[1]
        else:
            print(f"✅ Successfully got end effector pose: pose={ee_pose_in_base}")
            offset_y = 0.3

        # Set chassis pose to the same z coordinate as the target pose
        base_goal_pose_mat = np.eye(4)
        base_goal_pose_mat[:3, :3] = R.from_quat(object_goal_pose[3:]).as_matrix()
        base_goal_pose_mat[:3, 3] = np.array([object_goal_pose[0], object_goal_pose[1], 0])

        # According to the relative position of the chassis and camera, move the camera navigation target 0.6m backward in the local coordinate to leave observation space for the camera
        base_goal_pose_mat = base_goal_pose_mat @ np.array([[1,0,0,-0.6],[0,1,0,-offset_y],[0,0,1,0],[0,0,0,1]])

        base_goal_pose_quat = R.from_matrix(base_goal_pose_mat[:3, :3]).as_quat()
        base_goal_pose_pos = base_goal_pose_mat[:3, 3]

        return base_goal_pose_pos.tolist() + base_goal_pose_quat.tolist()
    
    except Exception as e:
        print("Failed to get navigation target pose:", e)
        return [1, -1, 0, 0, 0, 0, 1]
    

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.
    """
    if DEMO_MOCK_NAVIGATION_ONLY:
        mock_navigation_log("navigate_to_goal skipped")
        return
    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 lower_camera_after_joint_zero(
    motion: GalbotMotion,
    target_chain: str,
    reference_frame: str,
    downward_delta_m: float = 0.08,
):
    """
    C++ lower_camera_after_joint_zero: move_whole_body_joint_zero, base end effector z.
    """
    k_min_ee_z_base_link = 0.15
    try:
        print(
            f"[Camera pose] First run move_whole_body_joint_zero (S1 zero pose consistent with SDK), then move downward relative to end effector by Δz={downward_delta_m} m"
        )
        zero_status = motion.move_whole_body_joint_zero(
            True, 0.2, 15.0, gm.Parameter()
        )
        if zero_status != gm.MotionStatus.SUCCESS:
            print(
                f"⚠️ move_whole_body_joint_zero not successful: status={zero_status}, still attempting to read end effector and fine-tune..."
            )
        else:
            print("✅ move_whole_body_joint_zero completed successfully")
        time.sleep(0.8)

        retry_cnt = 3
        cur_ee_pose = []
        while True:
            status, cur_ee_pose = motion.get_end_effector_pose_on_chain(
                chain_name=target_chain,
                frame_id="EndEffector",
                reference_frame=reference_frame,
            )
            time.sleep(0.5)
            retry_cnt -= 1

            if status == gm.MotionStatus.SUCCESS:
                print("✅ Successfully got end effector pose (after zero)")
                break
            if retry_cnt < 0:
                cur_ee_pose = [0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991]
                print("❌ Failed to get end effector pose, using default")
                break
            print(f"Failed to get end effector pose: status={status}, retrying: {retry_cnt}")

        tgt_ee_pose = list(cur_ee_pose)
        tgt_ee_pose[2] -= downward_delta_m
        if tgt_ee_pose[2] < k_min_ee_z_base_link:
            print(
                f"⚠️ Target z={tgt_ee_pose[2]} is below safety lower bound {k_min_ee_z_base_link}; clamped"
            )
            tgt_ee_pose[2] = k_min_ee_z_base_link
        print(f"Target end effector pose (after downward fine-tuning): {tgt_ee_pose}")

        retry_cnt = 3
        while True:
            status = motion.set_end_effector_pose(
                target_pose=tgt_ee_pose,
                end_effector_frame=target_chain,
                reference_frame=reference_frame,
                enable_collision_check=False,
                is_blocking=True,
                timeout=5.0,
                params=gm.Parameter(),
            )
            time.sleep(0.5)
            retry_cnt -= 1
            if status == gm.MotionStatus.SUCCESS:
                print(f"✅ Successfully set end effector pose: status={status}")
                break
            if retry_cnt < 0:
                print(f"❌ Failed to set end effector pose after retries: status={status}")
                break
            print(f"Failed to set end effector pose: status={status}, retrying: {retry_cnt}")
    except Exception as e:
        print(f"❌ Exception in lower_camera_after_joint_zero: {e}")

def detect_target(img: np.ndarray, depth_img: np.ndarray) -> Sequence[float]:
    """
    Detection target function. Input RGB image and depth image, output target pose.

    Parameters:
        img (np.ndarray): RGB image
        depth_img (np.ndarray): Depth image

    Returns:
        Sequence[float]: Target pose [x, y, z, qx, qy, qz, qw]
    """
    try:
        
        ############### NOTE ###############
        # This function is a placeholder. In a real-world scenario, you would implement
        # target detection using computer vision techniques. For this example, we assume
        # a default pose.
        ####################################

        # opencv frame: x right, y down, z forward
        default_pose = [0.0, 0.20, 0.29, 0.0, 0.71, 0.0, 0.71]
        
        return default_pose
    except Exception as e:
        print(f"Target detection exception: {e}")
        return None

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] = R.from_quat(base_to_cam[3:]).as_matrix()
    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, arm: str = "left_arm"):
    object_pose_base = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
    try:
        if arm == "left_arm":
            rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
            depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
        elif arm == "right_arm":
            rgb_image_data = robot.get_rgb_data(SensorType.RIGHT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
            depth_data = robot.get_depth_data(SensorType.RIGHT_ARM_DEPTH_CAMERA)
        else:
            raise ValueError("arm must be left_arm or right_arm")

        img = None
        depth_img = None

        if rgb_image_data and rgb_image_data.get("data"):
            print("Get rgb image success")
            raw = rgb_image_data["data"]
            if isinstance(raw, list):
                raw = bytes(raw)
            img = decode_rgb_image(raw)
        else:
            print("No rgb image data (sensor may be unavailable)")

        if (
            depth_data
            and depth_data.get("width", 0) > 0
            and depth_data.get("height", 0) > 0
            and depth_data.get("data")
        ):
            print("Get depth data success")
            camera_info = {
                "height": depth_data["height"],
                "width": depth_data["width"],
            }
            depth_data_bytes = depth_data["data"]
            if isinstance(depth_data_bytes, list):
                depth_data_bytes = bytes(depth_data_bytes)
            depth_img = decode_depth_image(
                depth_data_bytes, depth_data["depth_scale"], camera_info
            )
        else:
            print("No depth data (depth sensor may be unavailable)")

        if img is None:
            img = np.zeros((480, 640, 3), dtype=np.uint8)
        if depth_img is None:
            depth_img = np.zeros((480, 640), dtype=np.float32)

        object_pose_camera = detect_target(img, depth_img)
        if object_pose_camera is None:
            print("Target detection failed")
            return object_pose_base

        print(f"object_pose_camera: {object_pose_camera}")

        pose_b = pose_camera_to_base(robot, object_pose_camera)
        if pose_b is None:
            return object_pose_base
        object_pose_base = pose_b
        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,
    motion: GalbotMotion,
    object_pose_base: Sequence[float],
    target_chain: str,
    reference_frame: str,
):
    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}")
        else:
            print(f"✅ Successfully attached tool: status={status}")
        
        # 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(
            S1JointGroup.left_gripper, 0.1, 0.05, 10, False
        )
        time.sleep(0.5)
        print(f"✅ Successfully set left gripper width to 0.1m: status={status}")

        print("object_pose_base: ", object_pose_base)
        
        # Reach to target position
        retry_cnt = 3
        while True:
            status = motion.set_end_effector_pose(
                target_pose=object_pose_base,
                end_effector_frame=target_chain,
                reference_frame=reference_frame,
                enable_collision_check=False,
                is_blocking=True,
                timeout=5.0,
                params=gm.Parameter()
            )
            time.sleep(1)
            retry_cnt -= 1
            if status == gm.MotionStatus.SUCCESS or retry_cnt < 0:
                break
            else:
                print(f"Failed to set end effector pose: status={status}, retry count: {retry_cnt}")
        
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to set end effector pose: status={status}")
        else:
            print(f"✅ Successfully set end effector pose: status={status}")

        # Close gripper to grasp object
        status = robot.set_gripper_command(
            S1JointGroup.left_gripper, 0.02, 0.05, 10, False
        )
        time.sleep(0.5)
        print(f"✅ Successfully closed left gripper: status={status}")

        # Box scale values must all be >0 (consistent with C++)
        status = motion.attach_target_object(
            "grasped_box",
            "box",
            [0, 0, 0, 0, 0, 0, 1],
            [0.05, 0.05, 0.05],
            "",
            "left_arm",
            "ee_base",
        )
        time.sleep(0.5)
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to attach target object: status={status}")
        else:
            print(f"✅ Successfully attached target object: status={status}")

        mock_navigation_log('After grasping, "return to the starting point"')
        time.sleep(0.5)
        print("✅ (Mock) Return navigation skipped; chassis not moved")

        # Release target
        status = robot.set_gripper_command(
            S1JointGroup.left_gripper, 0.1, 0.05, 10, False
        )
        time.sleep(0.5)
        print(f"✅ Successfully released left gripper: status={status}")

        # Detach target object
        status = motion.detach_target_object("grasped_box")
        time.sleep(0.5)
        if status != gm.MotionStatus.SUCCESS:
            print(f"❌ Failed to detach target object: status={status}")
        else:
            print(f"✅ Successfully detached target object: status={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}")
    except Exception as e:
        print(f"Exception occurred during pick_and_place: {e}")
        return None


def main():
    check_robot_safety()
    try:
        # Get robot instance
        robot = GalbotRobot()
        # Get GalbotMotion instance
        motion = GalbotMotion()
        # Get navigation instance
        nav = GalbotNavigation()

        # Consistent with C++: left-arm RGB + depth only (required for demo grasping)
        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")

        time.sleep(1)

        if DEMO_MOCK_NAVIGATION_ONLY:
            print(
                "[Demo mode] Skipped: relocalize and real navigate_to_goal (chassis remains stationary)."
            )
        else:
            while True:
                if nav.is_localized():
                    print(
                        "✅ Robot localized successfully, proceeding to navigate to goal..."
                    )
                    break
                print("❌ Navigation not localized, relocalizing...")
                nav.relocalize([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
                time.sleep(2)

        mock_navigation_log('Before starting the task: "move near the target point"')

        snapshot_joint_names = robot.get_joint_names(
            True, S1_BODY_JOINT_GROUPS_FOR_SNAPSHOT
        )
        if len(snapshot_joint_names) != S1_BODY_JOINT_COUNT:
            raise RuntimeError(
                "Failed to resolve complete S1 body joint names for snapshot: "
                f"got {len(snapshot_joint_names)}, expected {S1_BODY_JOINT_COUNT}"
            )

        joint_groups_positions = robot.get_joint_positions([], snapshot_joint_names)
        print(
            f"Saved body joint snapshot: {len(joint_groups_positions)} values "
            f"(expected {len(snapshot_joint_names)})"
        )
        if len(joint_groups_positions) != len(snapshot_joint_names):
            raise RuntimeError(
                "Failed to save complete body joint snapshot: "
                f"got {len(joint_groups_positions)} values for "
                f"{len(snapshot_joint_names)} joints"
            )
        print()

        lower_camera_after_joint_zero(motion, "left_arm", "base_link", 0.08)
        print()

        object_pose_base = detect_object(robot, arm="left_arm")
        print()

        pick_and_place(robot, motion, object_pose_base, "left_arm", "base_link")
        print()

        retry_cnt = 3
        while True:
            status = robot.set_joint_positions(
                joint_groups_positions,
                [],
                snapshot_joint_names,
                True,
                0.5,
                20,
            )
            time.sleep(0.5)

            if status == ControlStatus.SUCCESS:
                print(f"✅ Successfully set joint positions: status={status}")
                break
            if retry_cnt < 0:
                print(f"❌ Failed to set joint positions after retries, status={status}")
                break
            retry_cnt -= 1
            print(f"Failed to set joint positions: retry count: {retry_cnt}")

    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()