Python 示例

本文件为 API 中公开的函数与类型提供简短示例，演示如何使用这些接口。

机器人关节名称（S1 1.2）

关节组列表

机器人关节组名称包括：["torso", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]

各关节组详细信息

关节组名称	关节组英文名	关节数量	关节名称列表
头部	head	2	head_joint1, head_joint2
躯干	torso	1	torso_base_joint
左臂	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	7	right_arm_joint1, right_arm_joint2, right_arm_joint3, right_arm_joint4, right_arm_joint5, right_arm_joint6, right_arm_joint7
左夹爪	left_gripper	1	left_gripper_joint1
右夹爪	right_gripper	1	right_gripper_joint1

如何正确使用 joint_groups

joint_groups 的设计单位是“关节组（运动链）”，而不是单个关节。这样设计是为了保证：

• 运动链一致性：头/臂/躯干等关节按链路整体校验和控制。
• 指令顺序确定性：joint_groups 会按传入顺序展开为具体关节。
• 组级约束校验：执行前会检查关节组的 active/passive 属性及输入合法性。

传参规则：

1. 需要控制整条链路时，优先使用 joint_groups（torso/head/arm/gripper）。
2. 当 joint_names 非空时，优先使用 joint_names，会覆盖 joint_groups。
3. 当 joint_groups 和 joint_names 都为空时，SDK 默认控制所有 active body 关节组。
4. 建议先调用 get_joint_group_names() 和 get_joint_names(True, groups)，避免手写字符串出错。

S1 关节组典型场景

关节组	典型场景
torso	躯干高度/俯仰调整
head	头部朝向/相机对准
left_arm / right_arm	机械臂抓取与操作
left_gripper / right_gripper	夹爪开合宽度控制

说明：S1 还定义了底盘和相机挂载等被动关节组（如 swerve_chassis、left_camera、right_camera），通常不作为 set_joint_positions 的直接目标。

传感器类型与 Frame 说明

获取传感器数据时（get_rgb_data、get_depth_data、get_imu_data、get_lidar_data），通过 SensorType 枚举指定要查询的传感器。可用的 SensorType 枚举值详见：Python API 参考 > 传感器类型。

获取传感器外参有两种方式：

• get_sensor_extrinsic(): SDK 内部已映射 frame ID，直接传入 SensorType 即可
• get_transform(): 需要显式传入 frame 名称，表格第二列为各传感器对应的 Frame ID

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

注意：调用 get_frame_names() 可获取当前所有可用的坐标系名称。

类：GalbotRobot

tips：程序启动后立刻就get，数据可能不会立刻就绪，可适当sleep几秒

获取实例并初始化（get_instance && init）

适用场景：程序启动阶段，获取机器人单例并完成 SDK 初始化。必须在调用其他 API 前执行。

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

日志函数

适用场景：使用 SDK 内置的日志系统输出日志，统一日志级别和格式。

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）

适用场景：单次点位移动，低频率的位置控制任务。该接口内部会对目标位置进行速度受限的插值规划，适合一次性移动到目标关节角度的场景。

WARNING: 此接口不适合模型推理输出的高频关节控制场景！该接口每次调用都会产生一次轨迹插值，连续调用会导致运动不连续且有延迟。

如果您在做模型推理场景，请使用 set_joint_commands 或 set_joint_commands_batch 直接下发关节指令。

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）

适用场景：控制夹爪开合。支持位置控制和力矩控制两种模式。

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

停止轨迹执行（stop_trajectory_execution）

适用场景：停止当前正在执行的关节轨迹，中断正在进行的运动。

examples/python/galbot_robot/stop_trajectory_execution.py

from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import ControlStatus
import time

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

# Stop trajectory execution
while True:
    status = robot.stop_trajectory_execution()

    # Check execution results
    if status == ControlStatus.SUCCESS:
        print('Stop trajectory execution succeeded')
        break

    print("Trajectory stop failed, retrying...")

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

设置轨迹（execute_joint_trajectory）

适用场景：执行一条预定义的关节空间轨迹，包含多个路点。SDK 会按照时间节点执行整条轨迹。

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)

适用场景：高频关节控制，例如模型推理输出（每步推理输出一帧关节指令）、自定义轨迹跟踪。直接下发关节指令给底层控制器，不做额外插值。

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_batch）

适用场景：批量下发未来多帧关节指令，用于运动预测模型一次性输出多步结果的场景，可以提高控制频率和连续性。

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_target）

适用场景：高级用户直接构造 SingoriXTarget 并通过 publish 通道下发到底层 WBCS，适合高频原始 target 发送与 joint/task mixed target 一次性 dispatch。

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_target）

适用场景：高级用户直接构造 SingoriXTarget 并通过 request 通道下发到底层 WBCS，适合需要获取服务端错误返回的 raw target 场景。

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

设置底盘速度（set_base_velocity）

适用场景：控制移动底盘的线速度和角速度，用于导航或远程遥控。

examples/python/galbot_robot/set_base_velocity.py

from galbot_sdk.s1 import GalbotRobot, ControlStatus
import time

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

# Set chassis speed
linear_velocity = [0.05, 0.0, 0.0]  # 0.5 m/s
angular_velocity = [0.0, 0.0, 0.1]  # 0.1 rad/s

duration_s = 2.0  # Automatically stop after 2 seconds
status = robot.set_base_velocity(linear_velocity, angular_velocity, duration_s)

if status == ControlStatus.SUCCESS:
    print(f"Chassis speed set successfully; will auto-stop after {duration_s} seconds.")
else:
    print("Set chassis speed failed.")

time.sleep(duration_s + 0.5)

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

适用场景：获取所有关节的完整状态信息，包括位置、速度、电流等。适用于需要完整关节信息的算法（如动力学计算、状态估计）。

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, [])
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)
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_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）

适用场景：获取机器人所有关节的名称列表，用于迭代遍历关节或配置文件生成。

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 joint positions by joint group names; returns all joints if empty
joint_group_names = ["left_arm"]
# get joint
only_active_joint = True
ret = robot.get_joint_names(only_active_joint, joint_group_names)
print("Left joint names:")
for i, name in enumerate(ret):
    print(f"{i + 1}: {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）

适用场景：获取夹爪当前位置和状态，判断夹爪当前是打开还是闭合。

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）

适用场景：获取两个坐标系之间的变换关系（位姿），用于机械臂抓取、视觉定位等场景。

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

获取 IMU 数据（get_imu_data）

适用场景：获取机器人基座 IMU 的加速度、角速度和姿态信息，用于状态估计、运动分析。

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）

适用场景：获取机器人硬件版本、固件版本等设备信息，用于调试和兼容性检查。

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_rgb_data && get_depth_data）

适用场景：获取 RGB 图像和深度图像数据，用于视觉感知、目标检测、SLAM 等任务。

examples/python/galbot_robot/get_camera_data.py

try:
    from galbot_sdk.s1 import GalbotRobot, 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"] == "rgb8":
        return decode_rgb_image(image_data)
    elif compressed_image["format"] == "16UC1":
        return decode_depth_image(compressed_image)
    else:
        raise ValueError(f"Unsupport data format: {compressed_image['format']}")

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

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

获取传感器内外参（get_camera_intrinsic && get_sensor_extrinsic）

适用场景：获取相机内参和传感器相对于机器人基座的外参，用于计算机视觉计算和 3D 重建。

examples/python/galbot_robot/get_camera_params.py

try:
    from galbot_sdk.s1 import GalbotRobot, 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"] == "rgb8":
        return decode_rgb_image(image_data)
    elif compressed_image["format"] == "16UC1":
        return decode_depth_image(compressed_image)
    else:
        raise ValueError(f"Unsupport data format: {compressed_image['format']}")

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

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

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

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

    return depth_img

def main():
    robot = GalbotRobot()

    # Get left arm RGB and depth images, right arm depth image, chassis lidar data, torso IMU data
    enable_sensor_set = {SensorType.LEFT_ARM_CAMERA, # Left arm depth camera
                        SensorType.LEFT_ARM_DEPTH_CAMERA,} # Left arm RGB camera
    robot.init(enable_sensor_set)
    print("Initialization succeeded")
    
    # Program started, waiting for data
    time.sleep(5)
    
    # Get left arm RGB image
    rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA)
    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）

适用场景：获取激光雷达点云数据，用于导航、避障、环境建模。

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_synced_observation）

适用场景：获取同步观测数据，主要用于模型推理场景下获取时序对齐的输入数据，支持彩色相机、深度相机和关节数据。

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

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

一键回零-odom坐标系（whole_body_reset_zero_odom）

适用场景：快速将全身上下所有关节运动到零位（初始姿态），基于 odom 坐标系。通常用于程序启动后的初始化。

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

一键回零-map坐标系（whole_body_reset_zero_map）

适用场景：快速将全身上下所有关节运动到零位（初始姿态），基于 map 坐标系。

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

类：GalbotMotion

获取实例并初始化

适用场景：获取运动规划模块单例并初始化。必须在使用运动规划功能前调用。

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

正运动学（使用当前状态或指定 RobotStates）

适用场景：根据给定的关节角度计算末端执行器的位姿。用于机械臂任务验证和状态分析。

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

雅可比矩阵（get_jacobian）

适用场景：根据指定关节值计算目标运动链和坐标系下的雅可比矩阵，用于速度映射、微分运动学分析和控制器调试。

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

基于机器人状态的雅可比矩阵（get_jacobian_by_state）

适用场景：基于完整机器人状态或当前机器人状态计算雅可比矩阵，适用于全身状态分析和控制器验证。

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

逆运动学（基础与基于 RobotStates）

适用场景：根据期望的末端位姿求解所需的关节角度。是机械臂抓取等操作的核心步骤。

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

获取与设置末端位姿

适用场景：直接获取当前末端位姿，或将末端移动到指定位姿。集成了逆运动学计算和执行。

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

单点运动规划（关节空间与笛卡尔空间）

适用场景：规划从当前位姿到单个目标位姿的无碰撞运动轨迹。适用于单点目标到达任务。

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

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":     [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]
}
chain_pose_baselink = {
    "left_arm":  [0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991],
    "right_arm": [0.1267, -0.2345, 0.7358, -0.0225, 0.0126, -0.0343, 0.9991]
}
custom_param = gm.Parameter()

# 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_joint.joint_positions = chain_joints[target_joint.chain_name]

    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"
    target_pose_state.pose = gm.Pose(chain_pose_baselink[target_pose_state.chain_name])

    status, traj = motion.motion_plan(
        target=target_pose_state,
        enable_collision_check=False
    )
    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()

多点轨迹规划

适用场景：规划经过多个路径点的连续运动轨迹，适用于需要经过多个中间点的复杂运动。

examples/python/galbot_motion/motion_plan_multi_waypoints.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(2)

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

# Scenario 1: Multi-waypoint planning in Cartesian space (PoseState target)
try:
    print(">> Running Scenario 1: Multi-waypoint planning in Cartesian space...")

    target_pose_state = gm.PoseState()
    target_pose_state.chain_name = target_chain

    waypoint_poses = [
        [0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991],
        [0.2267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991],
        [0.3267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991],
        [0.4267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991],
    ]

    status, traj = motion.motion_plan_multi_waypoints(
        target=target_pose_state,
        waypoint_poses=waypoint_poses,
        enable_collision_check=False,
        params=custom_param
    )
    printStatus(status)
    if status == gm.MotionStatus.SUCCESS and traj != {}:
        print(f"✅ Cartesian multi-point planning succeeded: trajectory points={len(traj[target_chain])}")
        time.sleep(0.8)
    elif status == gm.MotionStatus.SUCCESS:
        print(f"⚠️ Return status is SUCCESS, but trajectory is empty")
    else:
        print(f"❌ Cartesian multi-point planning failed")
except Exception as e:
    print(f"❌ Scenario 1 exception: {e}")

# Scenario 2: Multi-waypoint planning in joint space (JointStates target)
try:
    print(">> Running Scenario 2: Multi-waypoint planning in joint space...")

    target_joint = gm.JointStates()
    target_joint.chain_name = target_chain

    waypoints = [
        [0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991],
        [0.2267, 0.4342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991],
        [0.3267, 0.6342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991],
        [0.4267, 0.8342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991]
    ]

    status, traj = motion.motion_plan_multi_waypoints(
        target=target_joint,
        waypoint_poses=waypoints,
        enable_collision_check=False,
        params=custom_param
    )
    printStatus(status)
    if status == gm.MotionStatus.SUCCESS and traj != {}:
        print(f"✅ Joint-space multi-point planning succeeded: trajectory points={len(traj[target_chain])}")
    elif status == gm.MotionStatus.SUCCESS:
        print(f"⚠️ Return status is SUCCESS, but trajectory is empty")
    else:
        print(f"❌ Joint-space multi-point planning failed")
except Exception as e:
    print(f"❌ Scenario 2 exception: {e}")

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

碰撞检测

适用场景：检查给定关节状态是否会发生自碰撞或与环境障碍物碰撞。用于运动规划前的可行性检查。

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

附加工具

适用场景：在机器人末端添加工具（如夹具、吸盘等），更新运动规划模型，以便考虑工具质量和碰撞体积。

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")
except Exception as e:
    print(f"❌ Tool attachment exception: {e}")

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

卸载工具

适用场景：从机器人末端移除已附加的工具，恢复原始机器人模型。

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

获取连杆名称列表

适用场景：获取机器人模型中所有连杆的名称列表，用于碰撞检测和运动规划调试。

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

加载/移除环境碰撞体

适用场景：向运动规划环境中添加障碍物或移除已添加的障碍物，使运动规划考虑环境障碍。

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

类：GalbotNavigation

获取实例 / 初始化

适用场景：获取导航模块单例并初始化。必须在使用导航功能前调用。

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

重定位 / 是否已定位 / 获取当前位姿

适用场景：触发重定位，检查机器人是否已经成功定位，并获取机器人在地图中的当前位姿。

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

检查路径可达性并阻塞导航到目标

适用场景：检查目标点是否可达，如果可达则阻塞等待导航完成到达目标。简单场景使用方便。

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

非阻塞导航 + 轮询判断到达

适用场景：启动导航但不阻塞当前线程，可以在导航过程中做其他处理，需要轮询判断是否到达目标。适合需要异步处理的场景。

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

直线移动到目标 / 停止导航

适用场景：让机器人沿直线移动到目标点，或停止当前正在进行的导航任务。

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

获取导航状态 + 轮询 SUCCESS/FAILED 或超时

适用场景：获取当前导航任务的执行状态，用于非阻塞导航时轮询。

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

完整运行示例（简单流程）

适用场景：展示导航功能的完整使用流程，包括初始化、重定位、导航到目标等步骤，供参考。

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

类：Parameter

适用场景：创建运动规划参数对象，用于配置运动规划的各种参数如速度限制、加速度限制、规划时间等。

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

辅助函数

check_motion_status

适用场景：检查运动规划执行结果状态，判断规划是否成功，打印错误信息。

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

适用场景：快速创建关节状态和位姿状态对象，方便运动规划接口调用。

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)