Python 示例
本文件为 API 中公开的函数与类型提供简短示例，演示如何使用这些接口。
机器人关节名称（S1 1.2）
关节组列表
机器人关节组名称包括：["torso", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]
各关节组详细信息
| 关节组名称 | 关节组英文名 | 关节数量 | 关节名称列表 |
|---|---|---|---|
| 头部 | head | 2 | head_joint1, head_joint2 |
| 躯干 | torso | 1 | S1 V1：torso_base_joint；S1 V2：torso_lift_joint1 |
| 左臂 | 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 属性及输入合法性。
传参规则：
- 需要控制整条链路时，优先使用
joint_groups（torso/head/arm/gripper）。 - 当
joint_names非空时，优先使用joint_names，会覆盖joint_groups。 - 当
joint_groups和joint_names都为空时，SDK 默认控制所有 active body 关节组。 - 建议先调用
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')
S1 末端工具原始透传
以下示例展示如何编码、发送、校验和解析设备特定的 64 字节 CAN/RS485 帧。使用前请先阅读 S1 末端工具原始透传。
警告
WBCS 必须在 [robot_info.custom_params] 下设置 endtool_raw_enabled=true。不要让 Raw writer 与标准控制器或另一个写入相同 TIB 的客户端并发运行。
自研 CAN 夹爪
初始化并运动自研二指夹爪，然后从 250 Hz 接收流监控设备被动上报的状态。
examples/s1/python/galbot_robot/endtool_raw_grip_can.py
"""Raw-passthrough example for the self-developed CAN gripper.
The device protocol in this file is an example adapter, not part of the SDK's
generic end-tool transport contract.
"""
import argparse
import math
import struct
import sys
import threading
import time
import galbot_sdk.s1 as sdk
TX_CAN_ID = 0x70A
RX_CAN_ID = 0x10A
MIN_WIDTH_MM = 6.0
MAX_WIDTH_MM = 120.0
MIN_VELOCITY_MM_S = 1.0
MAX_VELOCITY_MM_S = 100.0
MIN_TORQUE_NM = 1.0
MAX_TORQUE_NM = 50.0
STATUS_STALE_SECONDS = 1.0
INITIAL_STATUS_WAIT_SECONDS = 1.0
STATUS_PRINT_INTERVAL_SECONDS = 0.5
STOPPED_VELOCITY_TOLERANCE_MM_S = 1.0
DEFAULT_INIT_TIMEOUT_SECONDS = 20.0
DEFAULT_MONITOR_SECONDS = 10.0
DEFAULT_POSITION_TOLERANCE_MM = 2.0
ERROR_TEXT = {
0x00: "no error",
0x01: "not enabled",
0x08: "overvoltage",
0x09: "undervoltage",
0x0A: "overcurrent",
0x0B: "MOS overheating",
0x0C: "motor coil overheating",
0x0D: "communication lost",
0x0E: "motor overload",
0x10: "locked rotor",
0x11: "locked rotor and not enabled",
0x40: "power loss after calibration",
0x80: "communication error",
}
def encode_can(command: int, function: int, payload: bytes = b"") -> bytes:
frame = bytearray(64)
struct.pack_into("<II", frame, 0, 1, TX_CAN_ID)
frame[8] = len(payload) + 2
frame[12] = command
frame[13] = function
frame[14 : 14 + len(payload)] = payload
return bytes(frame)
def encode_init() -> bytes:
return encode_can(0x21, 0x04)
def encode_clear_error() -> bytes:
return encode_can(0x21, 0x0A)
def encode_move(width_mm: float, velocity_mm_s: float, torque_nm: float) -> bytes:
return encode_can(0x21, 0x05, struct.pack("<fff", width_mm, velocity_mm_s, torque_nm))
def decode_status(frame: bytes):
if len(frame) != 64:
return "size", None
frame_type, can_id = struct.unpack_from("<II", frame, 0)
if frame_type != 1:
return "frame_type", None
# The 250 Hz receive buffer can contain an empty CAN slot. Treat it as an
# idle transport snapshot instead of reporting a device/protocol error.
if can_id == 0:
return "idle_snapshot", None
if can_id != RX_CAN_ID:
return "can_id", None
# Do not require frame[12] == 1 here. On the current S1 EtherCAT/TIB
# passthrough implementation, valid 0x10A gripper snapshots have byte 12
# set to zero. The device frame itself starts at byte 16 on RX, so use its
# CAN ID and cmd/function fields to identify a valid status report.
command, function = frame[16], frame[17]
if command not in (0x22, 0x23) or function != 0x05:
return "command", None
position, velocity, torque = struct.unpack_from("<fff", frame, 22)
motion_state = frame[34]
if (
not all(math.isfinite(value) for value in (position, velocity, torque))
# An uninitialized gripper legitimately reports position 0 mm even
# though 0 mm is not a legal Move command target.
or not 0.0 <= position <= MAX_WIDTH_MM
or abs(velocity) > MAX_VELOCITY_MM_S
or abs(torque) > MAX_TORQUE_NM
or motion_state > 4
):
return "payload", None
return None, (position, velocity, torque, motion_state, frame[21], command)
def format_status(generation: int, status) -> str:
position, velocity, torque, motion_state, error_code, command = status
report_kind = "ANSWER" if command == 0x22 else "REPORT"
return (
f"generation={generation} source={report_kind} position={position:.3f}mm "
f"velocity={velocity:.3f}mm/s torque={torque:.3f}Nm "
f"motion_state={motion_state} error=0x{error_code:02x} "
f"({ERROR_TEXT.get(error_code, 'unknown error')})"
)
def main() -> None:
parser = argparse.ArgumentParser()
parser.add_argument("--side", choices=("left", "right"), default="left")
parser.add_argument("--width-mm", type=float, default=50.0)
parser.add_argument("--velocity-mm-s", type=float, default=50.0)
parser.add_argument("--torque-nm", type=float, default=20.0)
parser.add_argument("--init-timeout", type=float, default=DEFAULT_INIT_TIMEOUT_SECONDS)
parser.add_argument("--monitor-seconds", type=float, default=DEFAULT_MONITOR_SECONDS)
parser.add_argument(
"--position-tolerance-mm",
type=float,
default=DEFAULT_POSITION_TOLERANCE_MM,
)
parser.add_argument(
"--init",
action="store_true",
help="explicitly home/calibrate before Move; do not use on every invocation",
)
parser.add_argument(
"--skip-init",
action="store_true",
help=argparse.SUPPRESS,
)
parser.add_argument(
"--clear-error",
action="store_true",
help="send RESET before optional Init/Move",
)
parser.add_argument(
"--stop-standard-controller",
action="store_true",
help="best-effort stop of the standard controller; this is not an exclusive raw lease",
)
parser.add_argument(
"--restore-controller",
action="store_true",
help="restart the standard gripper controller on exit (normally keep it disabled in raw mode)",
)
args = parser.parse_args()
values = (
args.width_mm,
args.velocity_mm_s,
args.torque_nm,
args.init_timeout,
args.monitor_seconds,
args.position_tolerance_mm,
)
if not all(math.isfinite(value) for value in values):
parser.error("motion parameters must be finite")
if args.init_timeout <= 0 or args.monitor_seconds <= 0 or args.position_tolerance_mm <= 0:
parser.error("timeouts and position tolerance must be positive")
if not MIN_WIDTH_MM <= args.width_mm <= MAX_WIDTH_MM:
parser.error(f"--width-mm must be in [{MIN_WIDTH_MM}, {MAX_WIDTH_MM}]")
if not MIN_VELOCITY_MM_S <= args.velocity_mm_s <= MAX_VELOCITY_MM_S:
parser.error(f"--velocity-mm-s must be in [{MIN_VELOCITY_MM_S}, {MAX_VELOCITY_MM_S}]")
if not MIN_TORQUE_NM <= args.torque_nm <= MAX_TORQUE_NM:
parser.error(f"--torque-nm must be in [{MIN_TORQUE_NM}, {MAX_TORQUE_NM}]")
side = sdk.EndToolSide.LEFT if args.side == "left" else sdk.EndToolSide.RIGHT
joint_group = (
sdk.S1JointGroup.left_gripper
if args.side == "left"
else sdk.S1JointGroup.right_gripper
)
controller_group = "left_gripper" if args.side == "left" else "right_gripper"
robot = sdk.GalbotRobot()
if not robot.init():
raise RuntimeError("failed to initialize the S1 SDK")
print(
"WARNING: same-process raw TX is serialized, but there is no "
"cross-client resource lease."
)
print("WBCS must run with endtool_raw_enabled=true.")
print(
f"For exclusive raw control, configure {args.side}_none instead of "
f"{controller_group} in WBCS group_lists and restart WBCS. "
"stop_controller() only stops a trajectory; it is not an exclusive raw lease."
)
if args.stop_standard_controller:
stop_status = robot.stop_controller(controller_group)
print(f"Best-effort stop of {controller_group}: {stop_status}")
state_changed = threading.Condition()
state = {
"status": None,
"last_generation": 0,
"last_update": 0.0,
"latest_raw_generation": 0,
"idle_snapshots": 0,
"reject_reasons": {},
"non_increasing_generations": 0,
}
def on_raw(data: sdk.EndToolRawData) -> None:
if data.side != side or data.kind != sdk.EndToolRxKind.BUFFER_250HZ:
return
reject_reason, status = decode_status(data.frame)
with state_changed:
state["latest_raw_generation"] = data.generation
if reject_reason == "idle_snapshot":
state["idle_snapshots"] += 1
state_changed.notify_all()
return
if reject_reason is not None:
reasons = state["reject_reasons"]
reasons[reject_reason] = reasons.get(reject_reason, 0) + 1
state_changed.notify_all()
return
previous_generation = state["last_generation"]
if data.generation and previous_generation and data.generation <= previous_generation:
state["non_increasing_generations"] += 1
state_changed.notify_all()
return
state["status"] = status
state["last_generation"] = data.generation
state["last_update"] = time.monotonic()
state_changed.notify_all()
def get_snapshot():
with state_changed:
snapshot = dict(state)
snapshot["reject_reasons"] = dict(state["reject_reasons"])
return snapshot
def wait_for_command_ready_after(baseline_generation: int):
deadline = time.monotonic() + args.init_timeout
last_print_at = 0.0
last_printed_state = None
while time.monotonic() < deadline:
with state_changed:
remaining = deadline - time.monotonic()
state_changed.wait(timeout=max(0.0, min(0.5, remaining)))
generation = state["last_generation"]
status = state["status"]
if status is None or generation <= baseline_generation:
continue
now = time.monotonic()
state_key = (status[3], status[4])
if (
state_key != last_printed_state
or now - last_print_at >= STATUS_PRINT_INTERVAL_SECONDS
):
print("Init status:", format_status(generation, status))
last_printed_state = state_key
last_print_at = now
# The device state machine defines 2 as Ready. State 4 is the
# completion state of a previous Move report and must not be
# used as proof that this Init command has completed.
if status[3] == 2:
return True
return False
def wait_for_first_status(timeout: float):
deadline = time.monotonic() + timeout
with state_changed:
while state["status"] is None:
remaining = deadline - time.monotonic()
if remaining <= 0:
break
state_changed.wait(timeout=remaining)
return state["status"]
handle = robot.register_endtool_raw_callback(on_raw)
if handle == 0:
if args.restore_controller:
robot.start_controller(controller_group)
robot.destroy()
raise RuntimeError("failed to register raw callback")
try:
if args.clear_error:
result = robot.send_endtool_raw_frame(side, encode_clear_error())
if result != sdk.ControlStatus.SUCCESS:
raise RuntimeError("failed to publish the gripper RESET frame")
print("RESET sent; waiting 0.5s before the next command...")
time.sleep(0.5)
if args.init and not args.skip_init:
init_baseline = get_snapshot()["last_generation"]
if robot.send_endtool_raw_frame(side, encode_init()) != sdk.ControlStatus.SUCCESS:
raise RuntimeError("failed to publish the gripper init frame")
print(
"Waiting for a fresh Ready status (2) after Init "
f"(timeout={args.init_timeout:.1f}s)..."
)
if not wait_for_command_ready_after(init_baseline):
snapshot = get_snapshot()
raise RuntimeError(
"gripper did not report a fresh command-ready status after Init; "
f"latest_valid_generation={snapshot['last_generation']} "
f"latest_raw_generation={snapshot['latest_raw_generation']} "
f"idle_snapshots={snapshot['idle_snapshots']} "
f"malformed={snapshot['reject_reasons']}"
)
current_status = wait_for_first_status(INITIAL_STATUS_WAIT_SECONDS)
if current_status is not None and (
current_status[4] == 0x01 or current_status[3] == 0
):
raise RuntimeError(
"gripper is not initialized (device error 0x01/state 0); "
"rerun this example with --init before sending Move"
)
move_baseline = get_snapshot()["last_generation"]
result = robot.send_endtool_raw_frame(
side, encode_move(args.width_mm, args.velocity_mm_s, args.torque_nm)
)
if result != sdk.ControlStatus.SUCCESS:
raise RuntimeError("failed to publish the gripper move frame")
print(
"Move sent. Monitoring only fresh passive status reports for "
f"{args.monitor_seconds:.1f}s..."
)
deadline = time.monotonic() + args.monitor_seconds
printed_generation = move_baseline
received_after_move = False
latest_move_status = None
raw_target_reached = False
last_raw_print = 0.0
last_raw_state_error = None
last_sdk_print = 0.0
last_sdk_position_mm = None
while time.monotonic() < deadline:
with state_changed:
remaining = deadline - time.monotonic()
state_changed.wait(timeout=max(0.0, min(0.5, remaining)))
generation = state["last_generation"]
status = state["status"]
if (
status is not None
and generation > move_baseline
and generation != printed_generation
):
received_after_move = True
printed_generation = generation
latest_move_status = status
now = time.monotonic()
state_error = (status[3], status[4])
if (
state_error != last_raw_state_error
or now - last_raw_print >= STATUS_PRINT_INTERVAL_SECONDS
):
print("Raw status snapshot:", format_status(generation, status))
last_raw_print = now
last_raw_state_error = state_error
if (
status[4] == 0
and abs(status[0] - args.width_mm) <= args.position_tolerance_mm
and abs(status[1]) <= STOPPED_VELOCITY_TOLERANCE_MM_S
):
print(
f"Move completed within tolerance: requested={args.width_mm:.3f}mm "
f"actual={status[0]:.3f}mm (verified by Raw status)"
)
raw_target_reached = True
break
sdk_state = robot.get_gripper_state(joint_group)
if sdk_state is None:
continue
position_mm = sdk_state.width * 1000.0
velocity_mm_s = sdk_state.velocity * 1000.0
now = time.monotonic()
if (
last_sdk_position_mm is None
or abs(position_mm - last_sdk_position_mm) >= 0.1
or now - last_sdk_print >= 0.5
):
print(
f"SDK state: position={position_mm:.3f}mm "
f"velocity={velocity_mm_s:.3f}mm/s effort={sdk_state.effort:.3f} "
f"is_moving={sdk_state.is_moving}",
)
last_sdk_position_mm = position_mm
last_sdk_print = now
if (
abs(position_mm - args.width_mm) <= args.position_tolerance_mm
and not sdk_state.is_moving
):
print(
f"Move completed within tolerance: requested={args.width_mm:.3f}mm "
f"actual={position_mm:.3f}mm (verified by get_gripper_state)"
)
break
snapshot = get_snapshot()
if not received_after_move:
print(
"WARNING: no valid gripper status snapshot was received after Move. "
"Publication success alone does not prove that the device executed the command."
)
elif time.monotonic() - snapshot["last_update"] > STATUS_STALE_SECONDS:
print(
"WARNING: the valid gripper status stream stopped; subsequent receive "
"buffers were idle or rejected."
)
print(
"RX summary: "
f"latest_raw_generation={snapshot['latest_raw_generation']} "
f"latest_valid_generation={snapshot['last_generation']} "
f"idle_snapshots={snapshot['idle_snapshots']} "
f"malformed={snapshot['reject_reasons']} "
f"non_increasing_generations={snapshot['non_increasing_generations']}"
)
if raw_target_reached:
return
sdk_state = robot.get_gripper_state(joint_group)
sdk_target_reached = False
if sdk_state is not None:
position_mm = sdk_state.width * 1000.0
position_error = abs(position_mm - args.width_mm)
sdk_target_reached = (
position_error <= args.position_tolerance_mm and not sdk_state.is_moving
)
if not sdk_target_reached:
raw_summary = (
"none"
if latest_move_status is None
else format_status(printed_generation, latest_move_status)
)
sdk_summary = (
"unavailable"
if sdk_state is None
else (
f"position={position_mm:.3f}mm error={position_error:.3f}mm "
f"is_moving={sdk_state.is_moving}"
)
)
raise RuntimeError(
"self-developed gripper verification failed: Move did not reach the requested "
f"width: requested={args.width_mm:.3f}mm "
f"tolerance={args.position_tolerance_mm:.3f}mm; "
f"SDK state={sdk_summary}; last_raw_status={raw_summary}"
)
finally:
robot.unregister_endtool_raw_callback(handle)
if args.restore_controller:
restore_status = robot.start_controller(controller_group)
print(f"Best-effort restore of {controller_group}: {restore_status}")
robot.destroy()
def run_cli() -> int:
try:
main()
except RuntimeError as error:
print(f"FAILED: {error}", file=sys.stderr)
return 1
return 0
if __name__ == "__main__":
raise SystemExit(run_cli())
大寰二指 CAN 夹爪
通过 Raw CAN 传输配置和运动大寰二指夹爪，并主动轮询设备状态。
examples/s1/python/galbot_robot/endtool_raw_dahuan2_can.py
"""Raw-passthrough example for a Dahuan two-finger CAN FD gripper."""
import argparse
import struct
import sys
import threading
import time
import galbot_sdk.s1 as sdk
TX_CAN_ID = 0x601
RX_CAN_ID = 0x201
WRITE_SINGLE_REGISTER = 0x06
READ_REGISTERS = 0x03
REGISTER_INIT = 0x0100
REGISTER_FORCE = 0x0101
REGISTER_TARGET_POSITION = 0x0103
REGISTER_SPEED = 0x0104
REGISTER_STATUS_BASE = 0x0200
ERROR_TEXT = {
0: "no error",
1: "under voltage",
2: "over voltage",
3: "over current",
4: "over heat",
5: "motor disconnected",
8: "overload",
11: "over speed",
15: "startup error",
32: "encoder error",
}
def encode_request(function: int, address: int, value: int) -> bytes:
frame = bytearray(64)
struct.pack_into("<II", frame, 0, 1, TX_CAN_ID)
frame[8] = 7
struct.pack_into(">BHH", frame, 12, function, address, value)
return bytes(frame)
def decode_status(frame: bytes):
if len(frame) != 64:
return "size", None
if struct.unpack_from("<I", frame, 0)[0] != 1:
return "frame_type", None
if frame[12] != 0x01:
return "idle_snapshot", None
if struct.unpack_from("<I", frame, 4)[0] != RX_CAN_ID:
return "can_id", None
if frame[16] != READ_REGISTERS or frame[17] < 12:
return "function", None
values = struct.unpack_from(">6H", frame, 18)
current_ma = values[4] - 65536 if values[4] >= 32768 else values[4]
return None, (*values[:4], current_ma, values[5])
def main() -> None:
parser = argparse.ArgumentParser()
parser.add_argument("--side", choices=("left", "right"), default="left")
parser.add_argument("--position", type=int, default=500, help="target position in [0, 1000] permille")
parser.add_argument("--speed", type=int, default=50, help="speed in [1, 100] percent")
parser.add_argument("--force", type=int, default=50, help="force in [20, 100] percent")
parser.add_argument("--init", action="store_true", help="home before setting motion parameters")
parser.add_argument("--calibrate", action="store_true")
parser.add_argument("--duration", type=float, default=10.0)
parser.add_argument("--init-wait", type=float, default=4.0)
parser.add_argument("--position-tolerance", type=int, default=20)
parser.add_argument("--stop-standard-controller", action="store_true")
parser.add_argument("--restore-controller", action="store_true")
args = parser.parse_args()
if not 0 <= args.position <= 1000:
parser.error("--position must be in [0, 1000]")
if not 1 <= args.speed <= 100:
parser.error("--speed must be in [1, 100]")
if not 20 <= args.force <= 100:
parser.error("--force must be in [20, 100]")
if args.duration < 0 or args.init_wait < 0 or args.position_tolerance < 0:
parser.error("--duration and --init-wait must be non-negative")
side = sdk.EndToolSide.LEFT if args.side == "left" else sdk.EndToolSide.RIGHT
controller_group = "left_gripper" if args.side == "left" else "right_gripper"
robot = sdk.GalbotRobot()
if not robot.init():
raise RuntimeError("failed to initialize the S1 SDK")
print("WARNING: raw TX has no server-side exclusive writer lock.")
print("WBCS must run with endtool_raw_enabled=true in [robot_info.custom_params].")
if args.stop_standard_controller:
print(f"Best-effort stop of {controller_group}: {robot.stop_controller(controller_group)}")
changed = threading.Condition()
state = {
"status": None,
"status_generation": 0,
"status_kind": None,
"generation_by_kind": [0, 0],
"latest_raw_generation_by_kind": [0, 0],
"valid": 0,
"idle": 0,
"non_increasing_generation": 0,
"rejected": {},
}
def on_raw(data: sdk.EndToolRawData) -> None:
# Dahuan responses may appear in either receive buffer.
if data.side != side:
return
kind_index = 0 if data.kind == sdk.EndToolRxKind.BUFFER_1KHZ else 1
reason, status = decode_status(data.frame)
with changed:
state["latest_raw_generation_by_kind"][kind_index] = data.generation
if reason == "idle_snapshot":
state["idle"] += 1
elif reason is not None:
state["rejected"][reason] = state["rejected"].get(reason, 0) + 1
else:
previous_generation = state["generation_by_kind"][kind_index]
if (
data.generation
and previous_generation
and data.generation <= previous_generation
):
state["non_increasing_generation"] += 1
changed.notify_all()
return
state["generation_by_kind"][kind_index] = data.generation
state["status"] = status
state["status_generation"] = data.generation
state["status_kind"] = data.kind
state["valid"] += 1
init, grip, position, speed, current, error = status
print(
f"Fresh status: kind={data.kind} generation={data.generation} "
f"init={init} grip={grip} position={position}permille "
f"speed={speed} current={current}mA error=0x{error:04x} "
f"({ERROR_TEXT.get(error, 'unknown error')})"
)
changed.notify_all()
handle = robot.register_endtool_raw_callback(on_raw)
if handle == 0:
if args.restore_controller:
robot.start_controller(controller_group)
robot.destroy()
raise RuntimeError("failed to register raw callback")
def publish(frame: bytes, description: str) -> None:
result = robot.send_endtool_raw_frame(side, frame)
if result != sdk.ControlStatus.SUCCESS:
raise RuntimeError(f"failed to publish {description}: {result}")
def poll(seconds: float) -> None:
deadline = time.monotonic() + seconds
while time.monotonic() < deadline:
publish(encode_request(READ_REGISTERS, REGISTER_STATUS_BASE, 6), "status request")
with changed:
changed.wait(timeout=min(0.5, max(0.0, deadline - time.monotonic())))
try:
if args.init or args.calibrate:
init_value = 0x00A5 if args.calibrate else 0x0001
publish(encode_request(WRITE_SINGLE_REGISTER, REGISTER_INIT, init_value), "init command")
poll(args.init_wait)
publish(encode_request(WRITE_SINGLE_REGISTER, REGISTER_SPEED, args.speed), "speed command")
publish(encode_request(WRITE_SINGLE_REGISTER, REGISTER_FORCE, args.force), "force command")
publish(
encode_request(WRITE_SINGLE_REGISTER, REGISTER_TARGET_POSITION, args.position),
"position command",
)
# Discard any status left by initialization polling. Only a response to
# the active queries below may verify this motion command.
with changed:
state["status"] = None
motion_status_baseline = state["valid"]
poll(args.duration)
with changed:
status = state["status"]
snapshot = dict(state)
snapshot["rejected"] = dict(state["rejected"])
if status is None or snapshot["valid"] <= motion_status_baseline:
raise RuntimeError(
"Dahuan2 device verification failed: no fresh Dahuan2 status response "
"was received after the position command. The installed end tool may not "
"be a Dahuan2 gripper, or its power/wiring/baud rate may be incorrect; "
"latest_raw_generation_by_kind="
f"{snapshot['latest_raw_generation_by_kind']} "
f"idle_snapshots={snapshot['idle']} rejected={snapshot['rejected']}"
)
_, _, position, _, _, error = status
if error != 0:
raise RuntimeError(
f"gripper status error=0x{error:04x} "
f"({ERROR_TEXT.get(error, 'unknown error')})"
)
if abs(position - args.position) > args.position_tolerance:
raise RuntimeError(
"position did not reach target: "
f"requested={args.position} actual={position} "
f"tolerance={args.position_tolerance}permille"
)
print(
f"Position reached: requested={args.position} actual={position}permille "
f"fresh_status_count={snapshot['valid']} "
f"non_increasing_generations={snapshot['non_increasing_generation']}"
)
finally:
robot.unregister_endtool_raw_callback(handle)
if args.restore_controller:
robot.start_controller(controller_group)
robot.destroy()
def run_cli() -> int:
try:
main()
except RuntimeError as error:
# Device absence, protocol mismatch and transport failures are expected
# test outcomes. Report them without a Python traceback while keeping a
# non-zero exit status for scripts and CI.
print(f"FAILED: {error}", file=sys.stderr)
return 1
return 0
if __name__ == "__main__":
raise SystemExit(run_cli())
坤维 RS485 六维力传感器
以不追赶突发的方式周期发送 RS485 查询，并从 1 kHz 接收流校验、监控和打印六维力/力矩数据。
examples/s1/python/galbot_robot/endtool_raw_kunwei_rs485.py
"""Raw-passthrough active-query example for a Kunwei six-axis RS485 force sensor."""
import argparse
import math
import struct
import threading
import time
import galbot_sdk.s1 as sdk
MAX_ABS_WRENCH_VALUE = 1.0e6
STALE_SECONDS = 1.0
FREEZE_SECONDS = 2.0
WARNING_INTERVAL_SECONDS = 1.0
STATS_INTERVAL_SECONDS = 5.0
def encode_query() -> bytes:
frame = bytearray(64)
struct.pack_into("<III", frame, 0, 2, 233, 4)
frame[12:16] = b"\x49\xaa\x0d\x0a"
return bytes(frame)
def decode_force(frame: bytes):
if len(frame) != 64:
return "size", None
if frame[20] not in (0x48, 0x49) or frame[21] != 0xAA:
return "prefix", None
if frame[46] != 0x0D or frame[47] != 0x0A:
return "terminator", None
reading = struct.unpack_from("<6f", frame, 22)
if any(not math.isfinite(value) or abs(value) > MAX_ABS_WRENCH_VALUE for value in reading):
return "numeric", None
if all(value == 0.0 for value in reading):
return "zero", None
return None, reading
def main() -> None:
parser = argparse.ArgumentParser()
parser.add_argument("--side", choices=("left", "right"), default="left")
parser.add_argument("--duration", type=float, default=10.0)
parser.add_argument("--query-interval-ms", type=float, default=10.0)
parser.add_argument("--print-interval-ms", type=float, default=100.0)
args = parser.parse_args()
values = (args.duration, args.query_interval_ms, args.print_interval_ms)
if not all(math.isfinite(value) and value > 0 for value in values):
parser.error("duration and intervals must be finite and positive")
side = sdk.EndToolSide.LEFT if args.side == "left" else sdk.EndToolSide.RIGHT
robot = sdk.GalbotRobot()
if not robot.init():
raise RuntimeError("failed to initialize the S1 SDK")
print(
"WARNING: same-process raw TX is serialized, but there is no "
"cross-client resource lease."
)
print("WBCS must run with endtool_raw_enabled=true.")
state_lock = threading.Lock()
state = {
"query_ok": 0,
"query_fail": 0,
"rx_1khz": 0,
"rejected": 0,
"reject_size": 0,
"reject_prefix": 0,
"reject_terminator": 0,
"reject_numeric": 0,
"reject_zero": 0,
"non_increasing_generation": 0,
"duplicate_snapshots": 0,
"valid": 0,
"last_generation": 0,
"last_valid": 0.0,
"last_print": 0.0,
"last_payload": None,
"last_payload_change": 0.0,
"last_freeze_warning": 0.0,
"last_stale_warning": 0.0,
}
def on_raw(data: sdk.EndToolRawData) -> None:
if data.side != side or data.kind != sdk.EndToolRxKind.BUFFER_1KHZ:
return
reject_reason, reading = decode_force(data.frame)
now = time.monotonic()
freeze_warning = None
print_reading = False
with state_lock:
state["rx_1khz"] += 1
if reject_reason is not None:
state["rejected"] += 1
state[f"reject_{reject_reason}"] += 1
return
previous_generation = state["last_generation"]
if previous_generation and data.generation <= previous_generation:
state["non_increasing_generation"] += 1
return
state["last_generation"] = data.generation
payload = bytes(data.frame[22:46])
if state["last_payload"] is not None and payload == state["last_payload"]:
# The 1 kHz channel is a transport snapshot. Its generation can
# advance while the TIB keeps replaying the last sensor payload.
# A duplicate therefore is not a fresh force sample.
state["duplicate_snapshots"] += 1
if (
now - state["last_payload_change"] >= FREEZE_SECONDS
and now - state["last_freeze_warning"] >= WARNING_INTERVAL_SECONDS
):
state["last_freeze_warning"] = now
freeze_warning = now - state["last_payload_change"]
return_after_lock = True
else:
state["last_payload"] = payload
state["last_payload_change"] = now
return_after_lock = False
if not return_after_lock:
state["last_valid"] = now
state["valid"] += 1
if now - state["last_print"] >= args.print_interval_ms / 1000.0:
state["last_print"] = now
print_reading = True
if freeze_warning is not None:
print(
f"WARNING: force payload unchanged for {freeze_warning:.3f}s while "
"generation advances. This may be a held TIB snapshot; an exactly "
"steady sensor can also produce identical bytes."
)
if return_after_lock:
return
if print_reading:
fx, fy, fz, mx, my, mz = reading
print(
f"generation={data.generation} Fx={fx:.4f} Fy={fy:.4f} Fz={fz:.4f}kg "
f"Mx={mx:.4f} My={my:.4f} Mz={mz:.4f}kg*m"
)
handle = robot.register_endtool_raw_callback(on_raw)
if handle == 0:
robot.destroy()
raise RuntimeError("failed to register raw callback")
query = encode_query()
query_interval = args.query_interval_ms / 1000.0
started_at = time.monotonic()
deadline = started_at + args.duration
next_query_at = started_at
next_stats_at = started_at + STATS_INTERVAL_SECONDS
last_stats_at = started_at
last_stats_query_count = 0
last_stats_rx_count = 0
try:
while True:
now = time.monotonic()
if now < next_query_at:
time.sleep(next_query_at - now)
if time.monotonic() >= deadline:
break
result = robot.send_endtool_raw_frame(side, query)
after_query = time.monotonic()
with state_lock:
if result == sdk.ControlStatus.SUCCESS:
state["query_ok"] += 1
else:
state["query_fail"] += 1
failures = state["query_fail"]
last_valid = state["last_valid"]
stale_for = after_query - (last_valid if last_valid else started_at)
stale_warning = None
if (
stale_for >= STALE_SECONDS
and after_query - state["last_stale_warning"] >= WARNING_INTERVAL_SECONDS
):
state["last_stale_warning"] = after_query
stale_warning = stale_for
snapshot = dict(state)
if result != sdk.ControlStatus.SUCCESS and (failures == 1 or failures % 100 == 0):
print(f"Query publish failed: {result}, failures={failures}")
if stale_warning is not None:
print(
f"WARNING: no fresh force response for {stale_warning:.3f}s: "
f"query_ok={snapshot['query_ok']} query_fail={snapshot['query_fail']} "
f"rx_1khz={snapshot['rx_1khz']} valid={snapshot['valid']}. "
"Check TIB, sensor power/wiring/baud rate, "
"endtool_raw_enabled and WBCS logs."
)
next_query_at += query_interval
# Do not catch up with a burst after Publish blocks.
if next_query_at <= after_query:
next_query_at = after_query + query_interval
if after_query >= next_stats_at:
elapsed = after_query - last_stats_at
print(
f"Rate: query_hz="
f"{(snapshot['query_ok'] - last_stats_query_count) / elapsed:.1f} "
f"rx_hz={(snapshot['rx_1khz'] - last_stats_rx_count) / elapsed:.1f} "
f"fresh={snapshot['valid']} duplicates={snapshot['duplicate_snapshots']} "
f"rejects(size={snapshot['reject_size']} "
f"prefix={snapshot['reject_prefix']} "
f"terminator={snapshot['reject_terminator']} "
f"numeric={snapshot['reject_numeric']} zero={snapshot['reject_zero']})"
)
last_stats_query_count = snapshot["query_ok"]
last_stats_rx_count = snapshot["rx_1khz"]
last_stats_at = after_query
next_stats_at = after_query + STATS_INTERVAL_SECONDS
finally:
robot.unregister_endtool_raw_callback(handle)
with state_lock:
snapshot = dict(state)
print(
f"Stopped: query_ok={snapshot['query_ok']} query_fail={snapshot['query_fail']} "
f"rx_1khz={snapshot['rx_1khz']} rejected={snapshot['rejected']} "
f"(size={snapshot['reject_size']} prefix={snapshot['reject_prefix']} "
f"terminator={snapshot['reject_terminator']} numeric={snapshot['reject_numeric']} "
f"zero={snapshot['reject_zero']} "
"non_increasing_generation="
f"{snapshot['non_increasing_generation']}) fresh={snapshot['valid']} "
f"duplicates={snapshot['duplicate_snapshots']}"
)
robot.destroy()
if __name__ == "__main__":
main()
停止轨迹执行（stop_trajectory_execution）
适用场景：停止当前正在执行的关节轨迹，中断正在进行的运动。
examples/python/galbot_robot/stop_trajectory_execution.py
import signal
import threading
import time
from galbot_sdk.s1 import (
ControlStatus,
GalbotRobot,
JointCommand,
Trajectory,
TrajectoryPoint,
)
# Set when the trajectory call has returned, so the worker stops waiting for a
# Ctrl-C that is never coming.
trajectory_done = threading.Event()
# Set once the worker has requested shutdown, so the main flow does not repeat it.
shutdown_requested = threading.Event()
def make_head_trajectory():
trajectory = Trajectory()
trajectory.joint_names = ["head_joint2"]
point = TrajectoryPoint()
point.time_from_start_second = 10.0
command = JointCommand()
command.position = 0.4
point.joint_command_vec = [command]
trajectory.points = [point]
return trajectory
def stop_trajectory(robot):
print("Worker thread: stopping trajectory execution...")
status = robot.stop_trajectory_execution()
if status == ControlStatus.SUCCESS:
print("Worker thread: trajectory stop succeeded")
else:
print(f"Worker thread: trajectory stop FAILED: {status}")
shutdown_requested.set()
def stop_trajectory_on_ctrl_c(robot):
"""Call stop from a worker while the main thread waits for the trajectory."""
# sigwait() cannot be cancelled, so wait in slices: when the trajectory
# finishes on its own no Ctrl-C ever arrives, and the worker still has to
# exit or the join() in the main flow would block forever.
while not trajectory_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is None:
continue
stop_trajectory(robot)
return
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the area is clear.")
if input("Start trajectory execution? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(2)
print("Initialization succeeded")
# execute_joint_trajectory blocks in the main thread. Ctrl-C is consumed by
# the worker, which stops the motion and then shuts the SDK down.
print("Trajectory is executing. Press Ctrl-C to stop it.")
stop_thread = threading.Thread(target=stop_trajectory_on_ctrl_c, args=(robot,))
stop_thread.start()
status = robot.execute_joint_trajectory(make_head_trajectory(), is_blocking=True)
trajectory_done.set()
print(f"Trajectory execution returned: {status}")
stop_thread.join()
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")
设置轨迹（execute_joint_trajectory）
适用场景：执行一条预定义的关节空间轨迹，包含多个路点。SDK 会按照时间节点执行整条轨迹。
warning
构造 Trajectory / TrajectoryPoint 时，请如下方示例一样用普通 Python list 收集后再整体赋值。直接对 traj.points、traj_point.joint_command_vec 等属性调用 .append() 不会生效，会导致接口返回 INVALID_INPUT 且机器人不动。详见故障排除。
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）
适用场景：批量下发未来多帧关节指令，用于运动预测模型一次性输出多步结果的场景，可以提高控制频率和连续性。
warning
本接口同样接收 Trajectory 对象，构造方式与 execute_joint_trajectory 一致：请用普通 Python list 收集后再整体赋值。直接对 traj.points、traj_point.joint_command_vec 等属性调用 .append() 不会生效，会导致接口返回 INVALID_INPUT 且机器人不动。详见故障排除。
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()
获取并设置底盘速度（get_base_velocity && set_base_velocity）
duration_s=0 只发送一次；正值会阻塞调用线程，从首次成功发送后以 10Hz 续发指定时长，到期只停止发送，不调用 stop_base()。负数、非有限值及不可表示的时长返回 INVALID_INPUT。实际停止取决于底层看门狗，SUCCESS 不代表底盘已停止。不要在续发期间并发调用其他底盘命令，它们不会取消续发；需要自行控制停止时使用单次发送模式。
适用场景：控制移动底盘的线速度和角速度，用于导航或远程遥控。
examples/python/galbot_robot/get_set_base_velocity.py
from galbot_sdk.s1 import GalbotRobot, ControlStatus
import time
def print_base_velocity(base_velocity_info: dict):
"""
base_velocity_info: dict, includes:
- 'linear_velocity': [vx, vy, vz] Linear velocity (m/s)
- 'angular_velocity': [wx, wy, wz] Angular velocity (rad/s)
"""
if not base_velocity_info:
print("Base velocity data is empty")
return
# Print linear and angular velocity in the same format as the standalone example.
linear_velocity = base_velocity_info.get("linear_velocity", [])
if linear_velocity:
print(
f"Linear velocity (m/s): vx={linear_velocity[0]}, vy={linear_velocity[1]}, "
f"vz={linear_velocity[2]}"
)
angular_velocity = base_velocity_info.get("angular_velocity", [])
if angular_velocity:
print(
f"Angular velocity (rad/s): wx={angular_velocity[0]}, wy={angular_velocity[1]}, "
f"wz={angular_velocity[2]}"
)
# Get GalbotRobot
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")
# Read base velocity before issuing the motion command.
base_velocity_before = robot.get_base_velocity()
if base_velocity_before:
print("Base velocity before command:")
print_base_velocity(base_velocity_before)
else:
print("Failed to get base velocity before command.")
# Set chassis speed
linear_velocity = [0.2, 0.0, 0.0] # 0.2 m/s
angular_velocity = [0.0, 0.0, 0.0] # 0.0 rad/s
duration_s = 3.0 # Block while publishing at 10 Hz for 2 seconds; no stop is sent.
status = robot.set_base_velocity(linear_velocity, angular_velocity, duration_s)
if status == ControlStatus.SUCCESS:
print(f"Velocity publishing completed after {duration_s} seconds; stopping depends on the watchdog.")
else:
print("Set chassis speed failed.")
# Observe immediately; publishing completion does not imply a stopped base.
# Read base velocity again after the command has finished.
base_velocity_after = robot.get_base_velocity()
if base_velocity_after:
print("Base velocity after command:")
print_base_velocity(base_velocity_after)
else:
print("Failed to get base velocity after command.")
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取关节状态（get_joint_states）
适用场景：获取所有关节的完整状态信息，包括位置、速度、电流等。适用于需要完整关节信息的算法（如动力学计算、状态估计）。以下示例以左臂为例。
examples/python/galbot_robot/get_joint_states.py
import time
from galbot_sdk.s1 import GalbotRobot
def print_joint_states(joint_states):
"""
joint_state_vec: List of JointState; each object has position, velocity, acceleration, effort, current
"""
for js in joint_states:
print(f" : position = {js.position} , velocity = {js.velocity} "
f", acceleration = {js.acceleration} , effort = {js.effort} , current = {js.current}")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get joint states by joint group names; returns all joints if empty
joint_group_names = ["left_arm"]
ret = robot.get_joint_states(joint_group_names, [])
if not ret:
print(f"No joint states returned for joint groups {joint_group_names}")
else:
print_joint_states(ret)
# Get specified joint states; if provided, overrides joint group input
joint_names = ["left_arm_joint1", "left_arm_joint2"]
state_ret = robot.get_joint_states([], joint_names)
if not state_ret:
print(f"No joint states returned for joint names {joint_names}")
else:
print_joint_states(state_ret)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取关节位置（get_joint_positions）
适用场景：仅获取当前关节位置。当你只需要关节位置信息时使用此接口，比 get_joint_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 specified joint names; joint groups include ["torso", "head", "left_arm", "right_arm"] (S1: torso replaces G1 leg)
joint_group_names = ["head"]
only_active_joint = True # Get active joints
head_joint_names = robot.get_joint_names(only_active_joint, joint_group_names)
print("Head joint names:")
for i, name in enumerate(head_joint_names):
print(f"{i}: {name}")
# Passing an empty list returns all joint group information by default
all_joint_names = robot.get_joint_names(only_active_joint, [])
print("All joint names:")
for i, name in enumerate(all_joint_names):
print(f"{i}: {name}")
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取夹爪状态（get_gripper_state）
适用场景：获取夹爪当前位置和状态，判断夹爪当前是打开还是闭合。
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, RgbOutputFormat, SensorType
except ImportError:
print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
import os
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Dict
def decode_compressed_image(compressed_image):
"""
decode CompressedImage image
Parameters:
compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"] == "jpeg":
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(compressed_image)
else:
raise ValueError(f"Unsupport data format: {compressed_image['format']}")
def decode_rgb_image(image_data):
"""decode rgb image"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Fail to Decode RGB Image")
return img
def decode_ir_image(ir_image):
"""decode IR (infrared) grayscale image.
Supports JPEG-compressed mono8 and raw mono8 byte streams.
"""
data = ir_image["data"]
fmt = ir_image.get("format", "")
# Raw mono8: plain pixel bytes, use height/width directly
if fmt == "mono8":
h = ir_image.get("height", 0)
w = ir_image.get("width", 0)
if h == 0 or w == 0:
raise ValueError(f"decode_ir_image: height/width unavailable for mono8, got {h}x{w}")
return np.frombuffer(data, dtype=np.uint8).reshape(h, w).copy()
# Compressed (JPEG / PNG)
nparr = np.frombuffer(data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_GRAYSCALE)
if img is None:
raise ValueError(f"decode_ir_image: imdecode failed, format='{fmt}', size={len(data)}")
return img
def decode_depth_image(image_data):
"""decode depth image"""
depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()
# Check whether height and width exist
if "height" not in image_data or "width" not in image_data:
raise ValueError("Missing 'height' or 'width' in depth image metadata.")
if image_data["height"] == 0 or image_data["width"] == 0:
raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")
# Parse depth image
depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]
return depth_img
def main():
SHOW_IMAGE = False
robot = GalbotRobot()
# Get left arm RGB and depth images, right arm depth image, chassis lidar data, torso IMU data
enable_sensor_set = {SensorType.LEFT_ARM_CAMERA,
SensorType.LEFT_ARM_DEPTH_CAMERA,
SensorType.LEFT_ARM_INFRA_CAMERA_1,
SensorType.LEFT_ARM_INFRA_CAMERA_2,}
robot.init(enable_sensor_set)
print("Initialization succeeded")
# Program started, waiting for data
time.sleep(5)
# Get left arm RGB image
rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
if not rgb_image_data:
print("No rgb image data!")
else:
print("get rgb image suceess")
print(rgb_image_data['header'])
img = decode_compressed_image(rgb_image_data)
# Save RGB image
cv2.imwrite("rgb_image_data.jpg", img)
# RGBimage
if SHOW_IMAGE:
cv2.namedWindow("rgb image", cv2.WINDOW_NORMAL)
cv2.imshow("rgb image", img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Get left arm depth image
depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not depth_data or "data" not in depth_data:
print("Depth camera not ready")
else:
print("get depth data suceess")
print(depth_data['header'])
depth_img_raw = decode_compressed_image(depth_data)
depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize depth values to 0-1 range
depth_img = depth_img.astype(np.uint8)
# Save depth image
cv2.imwrite("depth_data.jpg", depth_img)
# depth
if SHOW_IMAGE:
cv2.namedWindow("depth image", cv2.WINDOW_NORMAL)
cv2.imshow("depth image", depth_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Get left arm IR images
for sensor_type, filename in [
(SensorType.LEFT_ARM_INFRA_CAMERA_1, "ir_infra1.jpg"),
(SensorType.LEFT_ARM_INFRA_CAMERA_2, "ir_infra2.jpg"),
]:
ir_data = robot.get_ir_data(sensor_type)
if not ir_data or "data" not in ir_data:
print(f"IR camera {sensor_type} not ready (ir_enabled may be false)")
else:
print(f"get ir data {sensor_type} success")
print(ir_data['header'])
ir_img = decode_ir_image(ir_data)
cv2.imwrite(filename, ir_img)
if SHOW_IMAGE:
cv2.namedWindow(f"ir image {sensor_type}", cv2.WINDOW_NORMAL)
cv2.imshow(f"ir image {sensor_type}", ir_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
if __name__=="__main__":
main()
订阅视频数据（subscribe_video_data）
适用场景：订阅指定传感器的 H.264 视频流并通过回调接收编码帧，适用于录像、实时处理、网络转发等需要基于推送的压缩视频交付场景。
examples/python/galbot_robot/subscribe_video_data.py
import threading
import time
try:
from galbot_sdk.s1 import GalbotRobot, SensorStatus, SensorType
except ImportError:
print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
OUTPUT_PATH = "head_left.h264"
DURATION = 10
output_file = None
frame_count = 0
frame_count_lock = threading.Lock()
# Important:
# The video callback is executed by the SDK internal dispatch thread.
# Do not perform time-consuming operations in this callback; keep it
# short and non-blocking. Long-running work may delay video frame delivery
# for this subscription.
def video_data_callback(video_data: dict):
global frame_count
header = video_data.get("header", {})
data = video_data.get("data", b"")
if output_file is not None and data:
# This sample callback only writes the raw H.264 stream to file without expensive transcoding here.
output_file.write(data)
with frame_count_lock:
frame_count += 1
current_count = frame_count
if current_count == 1 or current_count % 30 == 0:
print(
"frame_count={}, timestamp_ns={}, format={}, bytes={}".format(
current_count,
header.get("timestamp_ns"),
video_data.get("format"),
len(data),
)
)
def main():
global output_file
robot = GalbotRobot()
# Enable only the camera used by this example.
if not robot.init({SensorType.HEAD_LEFT_CAMERA}):
print("GalbotRobot initialization failed")
return
print("Initialization succeeded")
output_file = open(OUTPUT_PATH, "wb")
print(f"writing H.264 stream to {OUTPUT_PATH}")
status = robot.subscribe_video_data(SensorType.HEAD_LEFT_CAMERA, video_data_callback)
if status != SensorStatus.SUCCESS:
print(f"subscribe_video_data failed, status={status}")
output_file.close()
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
print("subscribe_video_data success")
try:
time.sleep(DURATION)
except KeyboardInterrupt:
print("Interrupted by user")
finally:
# Unsubscribe clears the user callback without recreating SDK reader resources.
status = robot.unsubscribe_video_data(SensorType.HEAD_LEFT_CAMERA)
print(f"unsubscribe_video_data status={status}")
if output_file is not None:
output_file.close()
with frame_count_lock:
total_frames = frame_count
print(f"received {total_frames} frames")
print(f"saved H.264 stream to {OUTPUT_PATH}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
if __name__ == "__main__":
main()
获取传感器内外参（get_camera_intrinsic && get_sensor_extrinsic）
适用场景：获取相机内参和传感器相对于机器人基座的外参，用于计算机视觉计算和 3D 重建。
examples/python/galbot_robot/get_camera_params.py
try:
from galbot_sdk.s1 import GalbotRobot, RgbOutputFormat, SensorType
except ImportError:
print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
import os
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Dict
def decode_compressed_image(compressed_image):
"""
decode CompressedImage image
Parameters:
compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"] == "jpeg":
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(compressed_image)
else:
raise ValueError(f"Unsupport data format: {compressed_image['format']}")
def decode_rgb_image(image_data):
"""decode rgb image"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Fail to Decode RGB Image")
return img
def decode_depth_image(image_data):
"""decode depth image"""
depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()
# Check whether height and width exist
if "height" not in image_data or "width" not in image_data:
raise ValueError("Missing 'height' or 'width' in depth image metadata.")
if image_data["height"] == 0 or image_data["width"] == 0:
raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")
# Parse depth image
depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]
return depth_img
def main():
robot = GalbotRobot()
# Get left arm RGB and depth images, right arm depth image, chassis lidar data, torso IMU data
enable_sensor_set = {SensorType.LEFT_ARM_CAMERA, # Left arm depth camera
SensorType.LEFT_ARM_DEPTH_CAMERA,} # Left arm RGB camera
robot.init(enable_sensor_set)
print("Initialization succeeded")
# Program started, waiting for data
time.sleep(5)
# Get left arm RGB image
rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
if not rgb_image_data:
print("No rgb image data!")
else:
print("get rgb image suceess")
print(rgb_image_data['header'])
img = decode_compressed_image(rgb_image_data)
# Save RGB image
cv2.imwrite("rgb_image_data.jpg", img)
# Get left arm camera intrinsics
camera_intrinsics = robot.get_camera_intrinsic(SensorType.LEFT_ARM_CAMERA)
if not camera_intrinsics:
print("No camera intrinsics data!")
else:
print("get camera intrinsics suceess")
print(camera_intrinsics)
# Get left arm depth image
depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not depth_data or "data" not in depth_data:
print("Depth camera not ready")
else:
print("get depth data suceess")
print(depth_data['header'])
depth_img_raw = decode_compressed_image(depth_data)
depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize depth values to 0-1 range
depth_img = depth_img.astype(np.uint8)
# Save depth image
cv2.imwrite("depth_data.jpg", depth_img)
# Get left-arm depth camera intrinsics
camera_intrinsics = robot.get_camera_intrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not camera_intrinsics:
print("No camera intrinsics data!")
else:
print("get camera intrinsics suceess")
print(camera_intrinsics)
# Extrinsics
time.sleep(2)
camera_extrinsics, timestamp_ns = robot.get_sensor_extrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not camera_extrinsics:
print("No camera extrinsics data!")
else:
print("get camera extrinsics suceess")
print(camera_extrinsics)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
if __name__=="__main__":
main()
获取雷达数据（get_lidar_data）
适用场景：获取激光雷达点云数据，用于导航、避障、环境建模。
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.
# Sync-mode RGB payloads are tightly packed CPU-owned NV12, not JPEG.
print("[Synced RGB]")
rgb_map: dict[SensorType, RgbData] = obs.rgb_data_map
anchor_camera = cameras[0]
anchor_frame: RgbData | None = rgb_map.get(anchor_camera, None)
anchor_ts_ns = (
anchor_frame.header.timestamp_ns if anchor_frame is not None else None
)
print(f"anchor_camera={anchor_camera}, anchor_timestamp_ns={anchor_ts_ns}")
for cam in cameras:
frame: RgbData | None = rgb_map.get(cam, None)
cam_ts_ns = frame.header.timestamp_ns if frame is not None else None
frame_format = frame.format if frame is not None else "N/A"
dimensions = f"{frame.width}x{frame.height}" if frame is not None else "N/A"
payload_bytes = len(frame.data) if frame is not None else 0
delta_ms_str = "N/A"
if anchor_ts_ns is not None and cam_ts_ns is not None:
delta_ms_str = f"{(cam_ts_ns - anchor_ts_ns) * NS_TO_MS:.3f}"
print(
f"camera={cam}, timestamp_ns={cam_ts_ns if cam_ts_ns is not None else 'N/A'}, "
f"delta_to_anchor_ms={delta_ms_str}, format={frame_format}, "
f"dimensions={dimensions}, bytes={payload_bytes}"
)
# 5) Read optional joint snapshot from typed field: obs.joint_state
# joint_state is JointStateMessage | None.
joint_state: JointStateMessage | None = obs.joint_state
print("[Joint]")
joint_ts_ns = joint_state.timestamp_ns if joint_state is not None else None
joint_delta_ms_str = "N/A"
if anchor_ts_ns is not None and joint_ts_ns is not None:
joint_delta_ms_str = f"{(joint_ts_ns - anchor_ts_ns) * NS_TO_MS:.3f}"
print(
"joint_timestamp_ns:",
joint_ts_ns if joint_ts_ns is not None else "N/A",
"delta_to_anchor_ms:",
joint_delta_ms_str,
)
# 6) Each element in joint_state_vec is typed JointState and contains joint_name.
joint_state_vec: list[JointState] = (
joint_state.joint_state_vec if joint_state is not None else []
)
print("joint_count:", len(joint_state_vec))
for i, js in enumerate(joint_state_vec[:5]):
print(
f" [{i}] joint_name={js.joint_name}, "
f"position={js.position}, "
f"velocity={js.velocity}, "
f"effort={js.effort}"
)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
一键回零-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()
控制器管理
适用场景：查询控制器状态，并切换或管理机器人各部件使用的控制器。
examples/python/galbot_robot/controller_management_example.py
import time
from galbot_sdk.s1 import GalbotRobot, ControlStatus, S1ControllerName, S1JointGroup
def print_active_controller(group_name, controller_name):
print(f"Active controller for {group_name}: {controller_name}")
def print_status(operation, status):
if status == ControlStatus.SUCCESS:
print(f"{operation}: SUCCESS")
else:
# Assuming status can be cast to int or printed directly
print(f"{operation}: FAILED (Status: {status})")
def main():
# Get robot instance and init
robot = GalbotRobot()
print("Initializing robot...")
if robot.init():
print("System initialized successfully!")
else:
print("System initialization failed!")
return
# Wait for data readiness
time.sleep(1)
# Define the controller we want to test
# Using LEFT_ARM_PVT_CTRL as the test subject
test_controller = S1ControllerName.LEFT_ARM_PVT_CTRL
controller_str = "LEFT_ARM_PVT_CTRL"
test_group = S1JointGroup.left_arm
group_str = "left_arm"
print("\n--- Starting Controller Management Example ---")
# 1. Get active controller before switching
active_controller = robot.get_active_controller(test_group)
print_active_controller(group_str, active_controller)
# 2. Switch controller
print(f"\n[Test 1] Switching to {controller_str}...")
status = robot.switch_controller(test_controller)
print_status("switch_controller", status)
time.sleep(0.5)
active_controller = robot.get_active_controller(test_group)
print_active_controller(group_str, active_controller)
# 3. Stop controller
print(f"\n[Test 2] Stopping controller for group {group_str}...")
# Using string overload/version for group
status = robot.stop_controller(test_group)
print_status("stop_controller", status)
time.sleep(0.5)
# 4. Release controller
print(f"\n[Test 3] Releasing controller for group {group_str}...")
status = robot.release_controller(test_group)
print_status("release_controller", status)
time.sleep(0.5)
# 5. Acquire controller
print(f"\n[Test 4] Acquiring {controller_str}...")
status = robot.acquire_controller(test_controller)
print_status("acquire_controller", status)
time.sleep(0.5)
# 6. Start controller
print(f"\n[Test 5] Starting controller for group {group_str}...")
status = robot.start_controller(test_group)
print_status("start_controller", status)
time.sleep(0.5)
print("\n--- Controller Management Example Finished ---")
# Shutdown
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
if __name__ == "__main__":
main()
停止控制器（stop_controller）
适用场景：停止正在执行运动的活动控制器。
examples/python/galbot_robot/stop_controller.py
"""Stop an active head controller from another Python thread."""
import signal
import threading
import time
from galbot_sdk.s1 import GalbotRobot
motion_done = threading.Event()
shutdown_requested = threading.Event()
def move_head_joints(robot, target_position):
status = robot.set_joint_positions(
[target_position], [], ["head_joint2"], False, 0.05, 15.0
)
print(f"Head motion command sent: {status}")
def stop_controller_on_ctrl_c(robot, group_name):
while not motion_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is not None:
break
else:
return
print(f"Worker thread: stopping {group_name} controller...")
status = robot.stop_controller(group_name)
print(f"Worker thread: stop_controller returned: {status}")
shutdown_requested.set()
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the area is clear.")
if input("Start head motion? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(1)
group_name = "head"
# Send a non-blocking motion command, then stop its controller after Ctrl-C.
print("Head is moving. Press Ctrl-C to stop its controller.")
stop_thread = threading.Thread(
target=stop_controller_on_ctrl_c, args=(robot, group_name)
)
stop_thread.start()
move_head_joints(robot, 0.4)
# The command is non-blocking, so retain the Ctrl-C listener for its full
# command timeout unless it has already requested shutdown.
shutdown_requested.wait(timeout=15.0)
motion_done.set()
stop_thread.join()
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
获取里程计数据（get_odom）
适用场景：读取里程计报告的机器人底盘位姿和运动状态。
examples/python/galbot_robot/get_odom.py
import time
from galbot_sdk.s1 import GalbotRobot
def print_odom_data(odom_data: dict):
"""
Print odometry data
odom_data: dict, includes:
- 'timestamp_ns': Timestamp (ns)
- 'position': [x, y, z] Position (m)
- 'orientation': [qx, qy, qz, qw] Quaternion orientation
- 'linear_velocity': [vx, vy, vz] Linear velocity (m/s)
- 'angular_velocity': [wx, wy, wz] Angular velocity (rad/s)
"""
if not odom_data:
print("Odom data is empty")
return
print(f"Timestamp (ns): {odom_data.get('timestamp_ns')}")
position = odom_data.get("position", [])
if position:
print(f"Position (m): x={position[0]}, y={position[1]}, z={position[2]}")
orientation = odom_data.get("orientation", [])
if orientation:
print(f"Orientation (quaternion): qx={orientation[0]}, qy={orientation[1]}, "
f"qz={orientation[2]}, qw={orientation[3]}")
linear_velocity = odom_data.get("linear_velocity", [])
if linear_velocity:
print(f"Linear velocity (m/s): vx={linear_velocity[0]}, vy={linear_velocity[1]}, "
f"vz={linear_velocity[2]}")
angular_velocity = odom_data.get("angular_velocity", [])
if angular_velocity:
print(f"Angular velocity (rad/s): wx={angular_velocity[0]}, wy={angular_velocity[1]}, "
f"wz={angular_velocity[2]}")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get odometry data
odom_data = robot.get_odom()
if not odom_data:
print("No odom data!")
else:
print("Odometry data:")
print_odom_data(odom_data)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
安装验证
适用场景：验证 SDK 安装、机器人连接、初始化以及基础接口是否可用。
examples/python/galbot_robot/installation_welcome_verify.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from __future__ import annotations
import sys
import time
from typing import List, Sequence
import cv2
import numpy as np
from galbot_sdk.s1 import (
ControlStatus,
GalbotRobot,
JointCommand,
SensorType, RgbOutputFormat,
Trajectory,
TrajectoryPoint,
)
_K_SPEED_RAD_S = 0.12
_K_TIMEOUT_S = 35.0
_K_PRESET_TORSO: List[float] = [0.58]
_K_PRESET_HEAD: List[float] = [0.0, 0.0]
_K_PRESET_LEFT_ARM: List[float] = [2.0, -1.5, -0.6, -1.7, 0.0, -0.7, 0.0]
_K_PRESET_RIGHT_ARM: List[float] = [-2.0, 1.5, 0.6, 1.7, 0.0, 0.7, 0.0]
_K_HEAD_ARMS_DEMO_GROUPS: List[str] = ["head", "left_arm", "right_arm"]
_K_HEAD_ARMS_DEMO_DT_S = 0.06
_K_HEAD_ARMS_DEMO_SEG1 = 28
_K_HEAD_ARMS_DEMO_SEG2 = 32
_K_HEAD_ARMS_DEMO_SEG3 = 28
_K_HEAD_SWAY_YAW_POS_RAD = 0.22
_K_HEAD_SWAY_YAW_NEG_RAD = -0.22
def _print_summary(step_description: str, step_passed: bool) -> None:
print(("[PASS] " if step_passed else "[FAIL] ") + step_description)
def make_point(joint_positions_rad: Sequence[float], time_from_start_sec: float) -> TrajectoryPoint:
point = TrajectoryPoint()
point.time_from_start_second = time_from_start_sec
cmds: List[JointCommand] = []
for q in joint_positions_rad:
jc = JointCommand()
jc.position = float(q)
cmds.append(jc)
point.joint_command_vec = cmds
return point
def build_trajectory(
joint_waypoints_rad: List[List[float]], joint_groups: List[str], time_step_sec: float
) -> Trajectory:
trajectory = Trajectory()
trajectory.joint_groups = joint_groups
trajectory.joint_names = []
time_from_start_sec = 0.0
points: List[TrajectoryPoint] = []
for waypoint in joint_waypoints_rad:
time_from_start_sec += time_step_sec
points.append(make_point(waypoint, time_from_start_sec))
trajectory.points = points
return trajectory
def linspace_rows(
start_positions_rad: Sequence[float], end_positions_rad: Sequence[float], num_samples: int
) -> List[List[float]]:
if num_samples < 2:
return [list(start_positions_rad)]
out: List[List[float]] = []
n = num_samples - 1
for sample_idx in range(num_samples):
blend = sample_idx / n
row = [
float(start_positions_rad[j]) + blend * (float(end_positions_rad[j]) - float(start_positions_rad[j]))
for j in range(len(start_positions_rad))
]
out.append(row)
return out
def append_rows(dest: List[List[float]], src: List[List[float]]) -> None:
dest.extend(src)
def upper_body_preset_pose_rad() -> List[float]:
return list(_K_PRESET_HEAD) + list(_K_PRESET_LEFT_ARM) + list(_K_PRESET_RIGHT_ARM)
def build_slow_head_arms_demo_waypoints_rad() -> List[List[float]]:
"""Slow head yaw left-right (joint2 fixed); arms shift slightly with the sway. Torso not in trajectory."""
home = upper_body_preset_pose_rad()
pose_yaw_pos = (
[_K_HEAD_SWAY_YAW_POS_RAD, 0.0]
+ [2.0 + 0.07, -1.5 + 0.04, -0.6, -1.7, 0.0, -0.8 + 0.04, 0.0]
+ [-2.0 - 0.07, 1.5 - 0.04, 0.6, 1.7, 0.0, 0.8 - 0.04, 0.0]
)
pose_yaw_neg = (
[_K_HEAD_SWAY_YAW_NEG_RAD, 0.0]
+ [2.0 - 0.05, -1.5 - 0.03, -0.6, -1.7, 0.0, -0.8 - 0.03, 0.0]
+ [-2.0 + 0.05, 1.5 + 0.03, 0.6, 1.7, 0.0, 0.8 + 0.03, 0.0]
)
rows: List[List[float]] = []
append_rows(rows, linspace_rows(home, pose_yaw_pos, _K_HEAD_ARMS_DEMO_SEG1))
append_rows(rows, linspace_rows(pose_yaw_pos, pose_yaw_neg, _K_HEAD_ARMS_DEMO_SEG2))
append_rows(rows, linspace_rows(pose_yaw_neg, home, _K_HEAD_ARMS_DEMO_SEG3))
return rows
def _decode_rgb_image_bytes(image_data: bytes) -> np.ndarray | None:
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
return img
def decode_head_rgb(rgb_dict: dict) -> np.ndarray | None:
if not rgb_dict or "data" not in rgb_dict:
return None
data = rgb_dict["data"]
if not data:
return None
return _decode_rgb_image_bytes(bytes(data))
def main() -> int:
body_preset_step_passed = False
head_camera_capture_ok = False
robot = GalbotRobot()
required_sensors = {SensorType.HEAD_LEFT_CAMERA}
if not robot.init(required_sensors):
print("GalbotRobot::init failed", file=sys.stderr)
return 1
print("Robot initialized (S1, HEAD_LEFT_CAMERA enabled)")
time.sleep(5.0)
torso_step_ok = False
upper_step_ok = False
demo_step_ok = False
torso_set_status = robot.set_joint_positions(
list(_K_PRESET_TORSO), ["torso"], [], True, _K_SPEED_RAD_S, _K_TIMEOUT_S
)
torso_step_ok = torso_set_status == ControlStatus.SUCCESS
if not torso_step_ok:
print("[FAIL] Torso set_joint_positions not SUCCESS (blocking)", file=sys.stderr)
_print_summary("Torso set_joint_positions (blocking)", torso_step_ok)
if torso_step_ok:
upper_body_joint_targets_rad: List[float] = []
upper_body_joint_targets_rad.extend(_K_PRESET_HEAD)
upper_body_joint_targets_rad.extend(_K_PRESET_LEFT_ARM)
upper_body_joint_targets_rad.extend(_K_PRESET_RIGHT_ARM)
upper_body_set_status = robot.set_joint_positions(
upper_body_joint_targets_rad,
_K_HEAD_ARMS_DEMO_GROUPS,
[],
True,
_K_SPEED_RAD_S,
_K_TIMEOUT_S,
)
upper_step_ok = upper_body_set_status == ControlStatus.SUCCESS
if not upper_step_ok:
print(
"[FAIL] head/left_arm/right_arm set_joint_positions not SUCCESS (blocking)",
file=sys.stderr,
)
_print_summary("Upper set_joint_positions head+arms (blocking)", upper_step_ok)
if upper_step_ok:
demo_waypoints_rad = build_slow_head_arms_demo_waypoints_rad()
demo_trajectory = build_trajectory(
demo_waypoints_rad, _K_HEAD_ARMS_DEMO_GROUPS, _K_HEAD_ARMS_DEMO_DT_S
)
demo_exec_status = robot.execute_joint_trajectory(demo_trajectory, True)
demo_step_ok = demo_exec_status == ControlStatus.SUCCESS
if not demo_step_ok:
print("[FAIL] Head/arms execute_joint_trajectory not SUCCESS (blocking)", file=sys.stderr)
_print_summary("Head/arms demo trajectory (blocking)", demo_step_ok)
else:
print(
"[INFO] Skipping head/arms demo: upper set_joint_positions did not return SUCCESS.",
file=sys.stderr,
)
_print_summary("Head/arms demo trajectory (blocking)", False)
else:
print(
"[INFO] Skipping upper-body preset and head/arms demo: torso set_joint_positions not SUCCESS.",
file=sys.stderr,
)
_print_summary("Upper set_joint_positions head+arms (blocking)", False)
_print_summary("Head/arms demo trajectory (blocking)", False)
body_preset_step_passed = torso_step_ok and upper_step_ok and demo_step_ok
head_rgb_data = robot.get_rgb_data(SensorType.HEAD_LEFT_CAMERA, RgbOutputFormat.JPEG, True)
if not head_rgb_data:
print("[FAIL] get_rgb_data returned null", file=sys.stderr)
head_camera_capture_ok = False
elif isinstance(head_rgb_data, dict):
raw = head_rgb_data.get("data")
raw_len = len(raw) if raw is not None and hasattr(raw, "__len__") else 0
if raw_len == 0:
print("[FAIL] head camera data missing or empty", file=sys.stderr)
head_camera_capture_ok = False
else:
head_camera_capture_ok = True
fmt = head_rgb_data.get("format", "")
print(f"Head camera: {raw_len} bytes format={fmt}")
head_image_mat = decode_head_rgb(head_rgb_data)
if head_image_mat is not None and head_image_mat.size > 0:
print(f" decoded size {head_image_mat.shape[1]}x{head_image_mat.shape[0]}")
else:
head_camera_capture_ok = True
print("Head camera: get_rgb_data returned non-empty object")
_print_summary("Head camera data", head_camera_capture_ok)
print("\n======== Summary ========")
_print_summary("Motion verify (blocking)", body_preset_step_passed)
_print_summary("Head camera", head_camera_capture_ok)
installation_all_checks_passed = body_preset_step_passed and head_camera_capture_ok
print("\nOverall: PASS\n" if installation_all_checks_passed else "\nOverall: FAIL\n")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0 if installation_all_checks_passed else 1
if __name__ == "__main__":
sys.exit(main())
设置底盘位姿（set_base_pose）
适用场景：控制移动底盘在指定参考坐标系下运动至目标位姿。
examples/python/galbot_robot/set_base_pose.py
import time
from galbot_sdk.s1 import ControlStatus, GalbotRobot, Pose
def set_yaw_orientation(pose, yaw):
import math
half_yaw = 0.5 * yaw
pose.orientation.x = 0.0
pose.orientation.y = 0.0
pose.orientation.z = math.sin(half_yaw)
pose.orientation.w = math.cos(half_yaw)
robot = GalbotRobot()
robot.init()
time.sleep(2)
print("Initialization succeeded")
# Test 1: set base pose using Pose.
pose = Pose()
pose.position.x = 0.3 # Move to x = 0.3 m in the odom frame.
pose.position.y = 0.0
pose.position.z = 0.0
set_yaw_orientation(pose, 0.0)
status = robot.set_base_pose(pose, True, 15.0)
if status == ControlStatus.SUCCESS:
print("[Test 1] Pose-struct set succeeded.")
else:
print("[Test 1] Pose-struct set failed or timed out.")
time.sleep(2)
# Test 2: set base pose using frame ids. "rel(0)" is relative to the current base pose.
x = -0.3 # Move backward by 0.3 m from the current pose.
y = 0.0
yaw = 0.0
frame_id = "rel(0)"
reference_frame_id = "odom"
is_blocking = True
timeout_s = 15.0
status = robot.set_base_pose(x, y, yaw, frame_id, reference_frame_id, is_blocking, timeout_s)
if status == ControlStatus.SUCCESS:
print("[Test 2] Relative set succeeded.")
else:
print("[Test 2] Relative set failed or timed out.")
time.sleep(2)
# Test 3: set base pose using explicit interpolation time.
x = 0.0 # Keep the third target at zero relative displacement.
y = 0.0
yaw = 0.0
# frame_id = "base_link"
frame_id = "rel(0)"
reference_frame_id = "odom"
time_from_start_s = 5.0
is_blocking = True
timeout_s = 15.0
status = robot.set_base_pose(
x,
y,
yaw,
frame_id,
reference_frame_id,
time_from_start_s,
is_blocking,
timeout_s,
)
if status == ControlStatus.SUCCESS:
print("[Test 3] Full-control set succeeded.")
else:
print("[Test 3] Full-control set failed or timed out.")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
停止底盘运动（stop_base）
适用场景：在底盘未由导航模块管理时，立即停止当前底盘运动指令。
examples/python/galbot_robot/stop_base.py
import signal
import threading
import time
from galbot_sdk.s1 import ControlStatus, GalbotRobot
motion_done = threading.Event()
shutdown_requested = threading.Event()
def stop_base_on_ctrl_c():
"""Notify the main thread to stop publishing velocity after Ctrl-C."""
while not motion_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is not None:
shutdown_requested.set()
return
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the area is clear.")
if input("Start chassis motion? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
# Block SIGINT before SDK initialization so all SDK threads inherit the mask.
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")
print("Chassis is moving. Press Ctrl-C to stop it.")
stop_thread = threading.Thread(target=stop_base_on_ctrl_c)
stop_thread.start()
start_time = time.monotonic()
while not shutdown_requested.is_set() and time.monotonic() - start_time < 5.0:
next_publish_time = time.monotonic() + 0.1
status = robot.set_base_velocity(
[0.1, 0.0, 0.0], [0.0, 0.0, 0.0], duration_s=0.0
)
if status != ControlStatus.SUCCESS:
print(f"Failed to send chassis velocity: {status}")
break
shutdown_requested.wait(
timeout=max(0.0, min(next_publish_time, start_time + 5.0) - time.monotonic())
)
motion_done.set()
stop_thread.join()
print("Sending chassis stop command...")
for _ in range(10):
status = robot.stop_base()
if status == ControlStatus.SUCCESS:
print("Chassis motion stopped successfully")
break
print("Chassis stop failed, retrying...")
time.sleep(0.2)
else:
print("Chassis stop timed out")
# AsyncShutdown raises SIGINT on its calling thread, so unblock it here first.
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")
设置机器人配置（set_config）
适用场景：通过统一配置接口设置支持的机器人服务或运行参数。
examples/python/galbot_robot/set_config.py
import time
from galbot_sdk.s1 import ConfigService, ConfigItem, ControlStatus, GalbotRobot
# Full list of settable fields (navigation service, camera services, motion
# planning service and control service), their meaning, type and valid range:
# docs/s1/en/set_config_reference.md
def example_navigation(robot):
# Navigation service: adjust replanning thresholds and select the short-distance
# motion mode. omni_plan is G1-only (passing it on S1 returns INVALID_INPUT), so S1
# uses adjust_motion_type instead -- 2 = rotate + crab-walk, the recommended S1 mode
# (avoids joint collisions).
status = robot.set_config(
ConfigService.NAVIGATION,
[
ConfigItem("replan_threshold", 30.0),
ConfigItem("no_replan_threshold", 1.0),
ConfigItem("adjust_motion_type", 2),
],
)
if status == ControlStatus.SUCCESS:
print("[Navigation] set_config succeeded. Restart the device for the new config to take effect.")
else:
print(f"[Navigation] set_config failed: {status}. Check the SDK log for details.")
def example_camera(robot):
# Front head camera: set resolution.
# color_width/color_height must be set together in the same call, and the
# combination must be one of the documented valid resolutions.
status = robot.set_config(
ConfigService.FRONT_HEAD_CAMERA,
[
ConfigItem("color_width", 1280),
ConfigItem("color_height", 992),
],
)
if status == ControlStatus.SUCCESS:
print("[Camera] set_config succeeded. Restart the device for the new config to take effect.")
else:
print(f"[Camera] set_config failed: {status}. Check the SDK log for details.")
def example_motion_plan(robot):
# Motion planning service: set a non-per-chain field (plan_timeout) together with
# leg trajectory velocity/acceleration/jerk limits. Per-chain fields are keyed as
# "{field_name}_{chain_name}"; the leg chain has only 1 joint on S1 (5 on G1), so the
# array length here is S1-specific.
status = robot.set_config(
ConfigService.MOTION_PLAN,
[
ConfigItem("plan_timeout", 5.0),
ConfigItem("max_velocity_leg", [0.8]),
ConfigItem("max_acceleration_leg", [3.0]),
ConfigItem("max_jerk_leg", [10.0]),
],
)
if status == ControlStatus.SUCCESS:
print("[MotionPlan] set_config succeeded. Restart the device for the new config to take effect.")
else:
print(f"[MotionPlan] set_config failed: {status}. Check the SDK log for details.")
def example_control(robot):
# Control service: report_error_skip is a general parameter shared by
# both G-series and S1 (robot_config.toml).
status = robot.set_config(
ConfigService.CONTROL,
[
ConfigItem("report_error_skip", 1),
],
)
if status == ControlStatus.SUCCESS:
print("[Control] set_config succeeded. Restart the device for the new config to take effect.")
else:
print(f"[Control] set_config failed: {status}. Check the SDK log for details.")
if __name__ == "__main__":
robot = GalbotRobot()
robot.init()
time.sleep(2)
print("Initialization succeeded")
example_navigation(robot)
example_camera(robot)
example_motion_plan(robot)
example_control(robot)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
读取机器人配置（get_config）
适用场景：读取当前场景的持久化配置；也可使用 use_default=True 读取机器人内置默认配置，用于对比当前配置与默认值。
examples/python/galbot_robot/get_config.py
import time
from galbot_sdk.s1 import ConfigService, ControlStatus, GalbotRobot
def print_fields(title, status, fields):
print(f"[{title}] status: {status}")
if status != ControlStatus.SUCCESS:
print("部分字段读取失败，以下为成功读取的字段：")
for field in fields:
print(f" {field.key} = {field.value}")
if __name__ == "__main__":
robot = GalbotRobot()
robot.init()
time.sleep(2)
status, fields = robot.get_config(ConfigService.CONTROL, ["report_error_skip", "report_sensor_skip"])
print_fields("Control selected fields", status, fields)
status, fields = robot.get_config(ConfigService.CONTROL, ["report_error_skip", "report_sensor_skip"], use_default=True)
print_fields("Control built-in defaults", status, fields)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
类：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).
def confirm_robot_safety():
print("WARNING: The robot will move to the initial joint state. Release the emergency stop and clear nearby obstacles.")
return input("Continue? (y/n): ").strip().lower() == "y"
if not confirm_robot_safety():
print("Example cancelled.")
raise SystemExit(0)
motion = GalbotMotion()
robot = GalbotRobot()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Result: SUCCESS")
elif(status == gm.MotionStatus.TIMEOUT):
print("Result: TIMEOUT")
elif(status == gm.MotionStatus.FAULT):
print("Result: FAULT")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Result: INVALID_INPUT")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Result: INIT_FAILED")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Result: IN_PROGRESS")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Result: STOPPED_UNREACHED")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Result: DATA_FETCH_FAILED")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Result: PUBLISH_FAIL")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Result: COMM_DISCONNECTED")
if motion.init():
print("GalbotMotion init OK")
else:
print("GalbotMotion init FAILED")
if robot.init():
print("GalbotRobot init OK")
else:
print("GalbotRobot init FAILED")
# Wait for data to be ready.
time.sleep(1)
chain_joints = {
"torso": [0.65],
"head": [0.0000, -0.26],
"left_arm": [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
"right_arm": [0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0]
}
whole_body_joint = [
value
for chain in ["torso", "head", "left_arm", "right_arm"]
for value in chain_joints[chain]
]
move_status = robot.set_joint_positions(
whole_body_joint,
["torso", "head", "left_arm", "right_arm"],
[],
True,
0.1,
30.0,
)
if move_status != gm.ControlStatus.SUCCESS:
print(f"❌ Failed to move to the initial joint state: {move_status}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
raise SystemExit(1)
print("✅ Moved to the initial whole-body joint state")
time.sleep(3.0)
custom_param = gm.Parameter()
# Scenario 1: joint-space planning, target type = joint state
try:
print(">> Scenario 1: joint-space planning (joint target)...")
target_joint = gm.JointStates()
target_joint.chain_name = "left_arm"
target_positions = list(chain_joints[target_joint.chain_name])
target_positions[4] += 0.1
target_joint.joint_positions = target_positions
# 若设备处于 home 位，target 就是当前点、规划轨迹会为空，故增加偏移。
# 不使用 set_joint()：SDK 1.10.0 的该接口只接受整数，会拒绝浮点关节角。
status, traj = motion.motion_plan(
target=target_joint,
enable_collision_check=False,
params=custom_param
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Planning failed"
if traj != {}:
print(f"✅ Joint-space planning (joint target) success: trajectory points={len(traj[target_joint.chain_name])}")
time.sleep(0.8)
else:
print(f"⚠️ SUCCESS but trajectory is empty, target may already be reached")
except Exception as e:
print(f"ERROR: Scenario 1 exception: {e}")
# Scenario 2: joint-space planning, target type = end-effector pose (Cartesian)
try:
print(">> Scenario 2: joint-space planning (pose target)...")
target_pose_state = gm.PoseState()
target_pose_state.chain_name = "left_arm"
target_pose_state.frame_id = "EndEffector"
target_pose_state.reference_frame = "base_link"
status, current_pose = motion.get_end_effector_pose_on_chain(
chain_name=target_pose_state.chain_name,
frame_id=target_pose_state.frame_id,
reference_frame=target_pose_state.reference_frame
)
assert status == gm.MotionStatus.SUCCESS and len(current_pose) == 7, \
"Failed to get current end-effector pose"
target_pose = list(current_pose)
target_pose[0] += 0.04 # Move forward 4 cm from the current pose.
target_pose[2] += 0.02 # Move upward 2 cm from the current pose.
target_pose_state.pose = gm.Pose(target_pose)
status, traj = motion.motion_plan(
target=target_pose_state,
enable_collision_check=False,
params=custom_param
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Planning failed"
if traj != {}:
print(f"✅ Joint-space planning (pose target) success: trajectory points={len(traj[target_pose_state.chain_name])}")
time.sleep(0.8)
else:
print(f"⚠️ SUCCESS but trajectory is empty, target may already be reached")
except Exception as e:
print(f"ERROR: Scenario 2 exception: {e}")
# Scenario 3: joint-space planning with an explicit start state
try:
print(">> Scenario 3: joint-space planning (explicit start)...")
target_joint = gm.JointStates()
target_joint.chain_name = "left_arm"
target_joint.joint_positions = chain_joints[target_joint.chain_name]
start_joint = gm.JointStates()
start_joint.chain_name = "left_arm"
start_joint.joint_positions = [0] * 7
status, traj = motion.motion_plan(
target=target_joint,
start=start_joint,
enable_collision_check=False,
params=custom_param
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Planning failed"
if traj != {}:
print(f"✅ Joint-space planning (explicit start) success: trajectory points={len(traj[target_joint.chain_name])}")
else:
print(f"⚠️ SUCCESS but trajectory is empty, target may already be reached")
except Exception as e:
print(f"ERROR: Scenario 3 exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
多点轨迹规划
适用场景：规划经过多个路径点的连续运动轨迹，适用于需要经过多个中间点的复杂运动。
examples/python/galbot_motion/motion_plan_multi_waypoints.py
import time
import galbot_sdk.s1 as gm
from galbot_sdk.s1 import GalbotMotion, GalbotRobot
def confirm_robot_safety():
print("WARNING: The robot will move to the initial joint state. Release the emergency stop and clear nearby obstacles.")
return input("Continue? (y/n): ").strip().lower() == "y"
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, execution timed out")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, a fault occurred and execution cannot continue")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, failed to create internal communication components")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, in motion but not yet in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, data transmission failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, connection failed")
def main():
if not confirm_robot_safety():
print("Example cancelled.")
return
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# Program started, waiting for data
time.sleep(2)
chain_joints = {
"torso": [0.65],
"head": [0.0, -0.26],
"left_arm": [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
"right_arm": [0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0]
}
# FK results of chain_joints in base_link, showing their physical correspondence.
chain_pose_baselink = {
"torso": [-0.0715, 0.0000, 1.2044, 0.0000, 0.0000, 0.0000, 1.0000],
"head": [-0.0123, 0.0046, 1.4975, -0.0002, 0.1298, -0.0001, 0.9915],
"left_arm": [0.2851, 0.3190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920],
"right_arm": [0.2819, -0.3212, 1.9315, 0.3701, 0.7731, 0.1567, -0.4907],
}
whole_body_joint = [
num for key in ["torso", "head", "left_arm", "right_arm"]
for num in chain_joints[key]
]
move_status = robot.set_joint_positions(
whole_body_joint,
["torso", "head", "left_arm", "right_arm"],
[],
True,
0.1,
30.0,
)
if move_status != gm.ControlStatus.SUCCESS:
print(f"❌ Failed to move to the initial joint state: {move_status}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
print("✅ Moved to the initial whole-body joint state")
time.sleep(3.0)
custom_param = gm.Parameter()
target_chain = "left_arm"
# Scenario 1: Multi-waypoint planning in Cartesian space (PoseState target)
try:
# Construct target pose
target_pose_state = gm.PoseState()
target_pose_state.chain_name = target_chain
# Construct waypoints (3 intermediate poses)
waypoint_poses = [
[0.2851, 0.3190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920],
[0.2851, 0.4190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920],
[0.2851, 0.5190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920],
[0.2851, 0.5190, 1.8214, -0.3816, 0.7681, -0.1496, -0.4920]
]
status, traj = motion.motion_plan_multi_waypoints(
target=target_pose_state,
waypoint_poses=waypoint_poses,
enable_collision_check=False,
params=custom_param
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Cartesian multi-waypoint single-chain planning failed"
if traj != {}:
print(f"✅ Cartesian waypoint single-chain planning succeeded: trajectory points={len(traj[target_pose_state.chain_name])}")
time.sleep(0.8)
else:
print("⚠️ Return status is SUCCESS, but trajectory is empty; possibly already reached, check whether the target matches current state or is within tolerance")
except Exception as e:
print(f"❌ Cartesian multi-point motion planning exception: {e}")
# Scenario 2: Multi-waypoint planning in joint space (JointStates target)
try:
# Construct target joint state
target_joint = gm.JointStates()
target_joint.chain_name = target_chain
# Construct waypoints (3 intermediate joint states)
waypoints = [
[-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
[-0.52, -0.94, -0.54, -1.92, 0.2, 0.05, 0.0],
[-0.57, -0.94, -0.54, -1.90, 0.2, 0.10, 0.0],
[-0.55, -0.93, -0.54, -1.98, 0.19, 0.15, 0.01]
]
status, traj = motion.motion_plan_multi_waypoints(
target=target_joint,
waypoint_poses=waypoints,
enable_collision_check=False,
params=custom_param
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Joint multi-waypoint single-chain planning failed"
if traj != {}:
print(f"✅ Joint-waypoint single-chain planning succeeded: trajectory points={len(traj[target_joint.chain_name])}")
else:
print("⚠️ Return status is SUCCESS, but trajectory is empty; possibly already reached, check whether the target matches current state or is within tolerance")
except Exception as e:
print(f"❌ Joint-space multi-point motion planning exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
碰撞检测
适用场景：检查给定关节状态是否会发生自碰撞或与环境障碍物碰撞。用于运动规划前的可行性检查。
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")
# Clean up the attached tool model to avoid affecting subsequent examples.
status = motion.detach_tool(chain=chain_name)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "detach failed"
print(f"✅ Tool detached successfully")
except Exception as e:
print(f"❌ Tool operation exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
卸载工具
适用场景：从机器人末端移除已附加的工具，恢复原始机器人模型。
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()
详细碰撞检测（check_collision_detail）
适用场景：查看详细碰撞结果，确定发生碰撞的机器人连杆或环境物体。
examples/python/galbot_motion/check_collision_detail.py
import sys
import galbot_sdk.s1 as gm
from galbot_sdk.s1 import GalbotMotion, GalbotNavigation, GalbotRobot
def print_collision_details(motion, title, status, collision_infos):
print(f"{title} status: {motion.status_to_string(status)}")
if status != gm.MotionStatus.SUCCESS:
return
if not collision_infos:
print(" no collision pair returned")
return
for i, info in enumerate(collision_infos):
label = "COLLISION" if info.is_collision else "NO COLLISION"
print(
f" - pair [{i}]: {label}, {info.link1} <-> {info.link2}, "
f"distance: {info.distance}, type: {info.collision_type}"
)
motion = GalbotMotion()
robot = GalbotRobot()
navigation = GalbotNavigation()
if not motion.init():
print("GalbotMotion init FAILED", file=sys.stderr)
sys.exit(1)
if not robot.init():
print("GalbotRobot init FAILED", file=sys.stderr)
sys.exit(1)
if not navigation.init():
print("GalbotNavigation init FAILED", file=sys.stderr)
sys.exit(1)
params = gm.Parameter()
params.set_timeout(5.0)
try:
print(">> Detailed collision check: current robot state")
status, collision_infos = motion.check_collision_detail(
robot_states=[],
is_check_once=False,
is_log=False,
params=params,
)
print_collision_details(motion, "current robot state", status, collision_infos)
chain_joints = {
"torso": [1.1],
"head": [0.0000, -0.26],
"left_arm": [-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0],
"right_arm": [0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0],
}
whole_body_joint = [num for key in ["torso", "head", "left_arm", "right_arm"] for num in chain_joints[key]]
whole_body_state = gm.RobotStates()
whole_body_state.whole_body_joint = whole_body_joint
whole_body_state.base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
left_arm_state = gm.JointStates()
left_arm_state.chain_name = "left_arm"
left_arm_state.joint_positions = [1.99995, -1.60004, 0.599905, -1.69994, 0.0, -0.799924, 0.0]
print(">> Detailed collision check: explicit robot states")
status, collision_infos = motion.check_collision_detail(
robot_states=[whole_body_state, left_arm_state],
is_check_once=False,
is_log=True,
params=params,
)
print_collision_details(motion, "explicit robot states", status, collision_infos)
except Exception as e:
print(f"ERROR: check_collision_detail exception: {e}", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
类：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")
停止导航（stop_navigation）
适用场景：停止正在执行的导航任务。
examples/python/galbot_navigation/stop_navigation.py
"""Stop a blocking navigation request from another Python thread."""
import signal
import threading
import time
import numpy as np
from galbot_sdk.s1 import ControlStatus, GalbotNavigation, GalbotRobot, S1ControllerName
# Set when the navigation call has returned, so the worker stops waiting for a
# Ctrl-C that is never coming.
navigation_done = threading.Event()
def stop_navigation_on_ctrl_c(navigation):
# sigwait() cannot be cancelled, so wait in slices: when the navigation
# finishes on its own no Ctrl-C ever arrives, and the worker still has to
# exit or the join() in the main flow would block forever.
while not navigation_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is None:
continue
print("Worker thread: stopping navigation...")
succeeded, detail = navigation.stop_navigation()
if succeeded:
print("Worker thread: navigation stop succeeded")
else:
print(f"Worker thread: navigation stop FAILED: {detail}")
return
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the path is clear.")
if input("Start straight navigation? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
# Block SIGINT before SDK initialization so all SDK threads inherit the mask.
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
navigation = GalbotNavigation()
robot.init()
navigation.init()
time.sleep(3) # Wait for the PNS service to enter its working state.
if (
robot.switch_controller(S1ControllerName.SWERVE_CHASSIS_POSE_CTRL)
!= ControlStatus.SUCCESS
):
print("Failed to switch controller!")
else:
# move_straight_to is sent through PNS and is covered by stop_navigation.
# It does not require map localization, so it is a reliable stop example.
print("Robot is moving straight. Press Ctrl-C to stop navigation.")
stop_thread = threading.Thread(
target=stop_navigation_on_ctrl_c, args=(navigation,)
)
stop_thread.start()
result = navigation.move_straight_to(
np.array([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]),
is_blocking=True,
timeout=10,
)
navigation_done.set()
print(f"move_straight_to returned: {result}")
stop_thread.join()
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
获取导航状态 + 轮询 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()
携带物体时的导航包围盒过滤
适用场景：机器人携带附着物体导航时，过滤对应的导航碰撞几何体。
examples/python/galbot_navigation/bounding_box_filter.py
from galbot_sdk.s1 import GalbotNavigation
from galbot_sdk.s1 import GalbotRobot
def print_boxes(boxes):
print(f"Current bounding boxes: {len(boxes)}")
for index, box in enumerate(boxes):
print(f"--- Box [{index}] ---")
print("Tag:", box["box_tag"])
print("Parent:", box["parent_link_name"])
print("Size:", box["box_size"])
print("Pose:", box["box_pose"])
robot = GalbotRobot()
robot.init()
nav = GalbotNavigation()
nav.init()
# Add a box so navigation does not treat the corresponding fused points as obstacles.
box_info = {
"box_size": [0.4, 0.3, 0.28],
"box_pose": [0.45, 0.0, 0.25, 0.0, 0.0, 0.0, 1.0],
"box_tag": 1001,
"parent_link_name": "base_link",
}
attached_box_info = {
"box_size": [0.4, 0.3, 0.28],
"box_pose": [0.2, 0.0, -0.2, 0.0, 0.0, 0.0, 1.0],
"box_tag": 2001,
"parent_link_name": "left_arm_end_effector_mount_link",
}
success, status = nav.add_bounding_box(box_info)
print("add_bounding_box:", success, status)
print_boxes(nav.get_bounding_box())
success, status = nav.attach_box_to_link(attached_box_info, [])
print("attach_box_to_link:", success, status)
success, status = nav.detach_box_from_link(2001)
print("detach_box_from_link:", success, status)
success, status = nav.remove_bounding_box(1001)
print("remove_bounding_box:", success, status)
print_boxes(nav.get_bounding_box())
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
沿轨迹导航（navigate_along_trajectory）
适用场景：执行由一组有序底盘位姿构成的导航轨迹。
examples/python/galbot_navigation/navigate_along_trajectory.py
from galbot_sdk.s1 import ControlStatus, S1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.s1 import NavigationTaskStatus, Pose
import numpy as np
import sys
import time
kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kTaskTimeoutSec = 100.0
def navigation_target_status_to_string(status):
if status == NavigationTaskStatus.UNKNOWN:
return "UNKNOWN"
if status == NavigationTaskStatus.RUNNING:
return "RUNNING"
if status == NavigationTaskStatus.SUCCESS:
return "SUCCESS"
if status == NavigationTaskStatus.FAILED:
return "FAILED"
if status == NavigationTaskStatus.INTERRUPTED:
return "INTERRUPTED"
if status == NavigationTaskStatus.OCCUPIED:
return "OCCUPIED"
if status == NavigationTaskStatus.COLLISION:
return "COLLISION"
if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
return "CLOSE_TO_OBSTACLE"
return "UNKNOWN"
def format_pose(pose):
values = [
pose.position.x,
pose.position.y,
pose.position.z,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
]
return "[" + ", ".join(f"{value:g}" for value in values) + "]"
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def wait_for_localization(navigation, init_pose):
retry = 0
while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
navigation.relocalize(init_pose)
time.sleep(kLocalizationRetrySleepMs / 1000.0)
retry += 1
return navigation.is_localized()
def print_task_snapshot(navigation, handle, snapshot, include_pose=True):
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
if include_pose:
print_current_pose(navigation, indent=" ")
def monitor_task_until_terminal(navigation, handle, timeout_s):
start_time = time.time()
while True:
snapshot = navigation.get_navigation_target_status(handle.task_id)
print_task_snapshot(navigation, handle, snapshot)
if snapshot.status in {
NavigationTaskStatus.SUCCESS,
NavigationTaskStatus.FAILED,
NavigationTaskStatus.INTERRUPTED,
NavigationTaskStatus.OCCUPIED,
NavigationTaskStatus.COLLISION,
NavigationTaskStatus.CLOSE_TO_OBSTACLE,
}:
return
if (time.time() - start_time) >= timeout_s:
print(" timeout reached while waiting for task completion.")
return
time.sleep(0.5)
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!")
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!")
return -1
res = robot.switch_controller(S1ControllerName.SWERVE_CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
return -1
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
waypoints = [
Pose([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]),
Pose([0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]),
Pose([1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]),
]
time.sleep(3)
if not wait_for_localization(navigation, init_pose):
print("localization failed", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return -1
time.sleep(2)
print("set target: waypoints")
for i, waypoint in enumerate(waypoints):
print(f" waypoint_{i}: {format_pose(waypoint)}")
frame_id = "map"
speed_ratio = 1.0
enable_collision_check = False
handle = navigation.navigate_along_trajectory(waypoints, frame_id, speed_ratio, enable_collision_check)
if handle.request_sent:
monitor_task_until_terminal(navigation, handle, kTaskTimeoutSec)
else:
print("trajectory task submission failed")
navigation.stop_navigation()
print("\nfinal task summary")
snapshot = navigation.get_navigation_target_status(handle.task_id)
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
sys.exit(main())
导航至目标 V2（navigate_to_goal_v2）
适用场景：使用扩展目标接口及其附加选项导航至指定目标位姿。
examples/python/galbot_navigation/navigate_to_goal_v2.py
from galbot_sdk.s1 import ControlStatus, GalbotNavigation, GalbotRobot, S1ControllerName
import numpy as np
import sys
import time
kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def wait_for_localization(navigation, init_pose):
retry = 0
while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
navigation.relocalize(init_pose)
time.sleep(kLocalizationRetrySleepMs / 1000.0)
retry += 1
return navigation.is_localized()
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!", file=sys.stderr)
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!", file=sys.stderr)
return -1
res = robot.switch_controller(S1ControllerName.SWERVE_CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!", file=sys.stderr)
return -1
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
time.sleep(3)
if not wait_for_localization(navigation, init_pose):
print("localization failed", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return -1
print_current_pose(navigation)
relative_goal = np.array([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
max_vel = np.array([0.5, 0.5, 0.5], dtype=np.float64)
pose_frame = "base_link"
enable_collision_check = True
is_blocking = True
timeout_s = 20.0
omni_plan = False
print("navigate_to_goal_v2 relative target in base_link frame")
success, status = navigation.navigate_to_goal_v2(
relative_goal,
max_vel,
pose_frame=pose_frame,
enable_collision_check=enable_collision_check,
is_blocking=is_blocking,
timeout=timeout_s,
omni_plan=omni_plan,
)
print(f"navigate_to_goal_v2 result: success={success}, status={status}")
print_current_pose(navigation)
navigation.stop_navigation()
print("navigation stopped")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("example completed ...")
return 0
if __name__ == "__main__":
sys.exit(main())
速度导航（navigate_with_velocity）
适用场景：通过速度指令连续控制底盘平移和旋转。
examples/python/galbot_navigation/navigate_with_velocity.py
from galbot_sdk.s1 import ControlStatus, GalbotNavigation, GalbotRobot, NavigationTaskStatus
from galbot_sdk.s1 import S1ControllerName
import sys
import time
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def init_sdk():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!", file=sys.stderr)
return None, None
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!", file=sys.stderr)
return None, None
res = robot.switch_controller(S1ControllerName.SWERVE_CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!", file=sys.stderr)
return None, None
time.sleep(3)
print_current_pose(navigation)
return navigation, robot
def shutdown_sdk(navigation, robot):
navigation.stop_navigation()
print_current_pose(navigation)
print("navigation stopped")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("example completed ...")
def monitor_navigation(navigation, timeout_s, poll_interval_s=0.5):
start = time.time()
while True:
status = navigation.get_navigation_status()
elapsed = time.time() - start
print(f" status: {status.name}, elapsed: {elapsed:.1f}s")
print_current_pose(navigation, indent=" ")
if status in (
NavigationTaskStatus.SUCCESS,
NavigationTaskStatus.FAILED,
NavigationTaskStatus.INTERRUPTED,
NavigationTaskStatus.OCCUPIED,
NavigationTaskStatus.COLLISION,
NavigationTaskStatus.CLOSE_TO_OBSTACLE,
):
break
if elapsed >= timeout_s:
print(" monitor timeout reached")
break
time.sleep(poll_interval_s)
def test_base_vel_cmd():
navigation, robot = init_sdk()
if navigation is None or robot is None:
return -1
vx = 0.3
vy = 0.0
vyaw = 0.0
duration_s = 6.0
enable_collision_check = True
print(
f"navigate_with_velocity: vx={vx}, vy={vy}, "
f"vyaw={vyaw}, duration_s={duration_s}"
)
success, status = navigation.navigate_with_velocity(
vx, vy, vyaw, duration_s, enable_collision_check
)
print(f"navigate_with_velocity result: success={success}, status={status}")
monitor_navigation(navigation, timeout_s=duration_s + 2.0)
shutdown_sdk(navigation, robot)
return 0
def test_change_vel_cmd():
navigation, robot = init_sdk()
if navigation is None or robot is None:
return -1
print("send first velocity command")
success, status = navigation.navigate_with_velocity(0.3, 0.0, 0.0, 6.0, True)
print(f"navigate_with_velocity result 1: success={success}, status={status}")
monitor_navigation(navigation, timeout_s=4.0)
print("send second velocity command with y direction")
success, status = navigation.navigate_with_velocity(0.1, 0.2, 0.0, 6.0, True)
print(f"navigate_with_velocity result 2: success={success}, status={status}")
monitor_navigation(navigation, timeout_s=8.0)
shutdown_sdk(navigation, robot)
return 0
def main():
return test_base_vel_cmd()
# return test_change_vel_cmd()
if __name__ == "__main__":
sys.exit(main())
导航配置
适用场景：读取和更新导航模块使用的运行配置。
examples/python/galbot_navigation/navigation_set_get_config.py
from galbot_sdk.s1 import GalbotNavigation, GalbotRobot
import numpy as np
import sys
def print_result(name, result):
success, status = result
print(f"{name}: success={success}, status={status}")
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!", file=sys.stderr)
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!", file=sys.stderr)
return -1
print("dump navigation configs before setting parameters")
print_result("dump_navigation_configs", navigation.dump_navigation_configs())
# valid range: [0.05, 1.5]
vel_limit = np.array([0.5, 0.5, 0.5], dtype=np.float64)
# valid range: [0.05, 6.0]
acc_limit = np.array([0.5, 0.5, 0.5], dtype=np.float64)
# valid range: [0.05, 12.0]
jerk_limit = np.array([5.0, 5.0, 5.0], dtype=np.float64)
# valid range: [0.03, 2.0]
arrival_threshold = np.array([0.05, 0.05, 0.05], dtype=np.float64)
timeout_s = 30.0
print_result(
"set_navigation_velocity_limit",
navigation.set_navigation_velocity_limit(vel_limit),
)
print_result(
"set_navigation_kinematics_limits",
navigation.set_navigation_kinematics_limits(vel_limit, acc_limit, jerk_limit),
)
print_result("set_navigation_timeout", navigation.set_navigation_timeout(timeout_s))
print_result(
"set_navigation_arrival_threshold",
navigation.set_navigation_arrival_threshold(arrival_threshold),
)
print("dump navigation configs after setting parameters")
print_result("dump_navigation_configs", navigation.dump_navigation_configs())
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
sys.exit(main())
多路点导航（navigation_through_waypoints）
适用场景：让机器人按照指定顺序依次访问多个导航路点。
examples/python/galbot_navigation/navigation_through_waypoints.py
from galbot_sdk.s1 import ControlStatus, S1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.s1 import NavigationTaskStatus, Pose, Waypoint, WaypointParams
import numpy as np
import sys
import time
kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kTaskTimeoutSec = 100.0
def navigation_target_status_to_string(status):
if status == NavigationTaskStatus.UNKNOWN:
return "UNKNOWN"
if status == NavigationTaskStatus.RUNNING:
return "RUNNING"
if status == NavigationTaskStatus.SUCCESS:
return "SUCCESS"
if status == NavigationTaskStatus.FAILED:
return "FAILED"
if status == NavigationTaskStatus.INTERRUPTED:
return "INTERRUPTED"
if status == NavigationTaskStatus.OCCUPIED:
return "OCCUPIED"
if status == NavigationTaskStatus.COLLISION:
return "COLLISION"
if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
return "CLOSE_TO_OBSTACLE"
return "UNKNOWN"
def format_pose(pose):
values = [
pose.position.x,
pose.position.y,
pose.position.z,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
]
return "[" + ", ".join(f"{value:g}" for value in values) + "]"
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def wait_for_localization(navigation, init_pose):
retry = 0
while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
navigation.relocalize(init_pose)
time.sleep(kLocalizationRetrySleepMs / 1000.0)
retry += 1
return navigation.is_localized()
def print_task_snapshot(navigation, handle, snapshot):
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
print_current_pose(navigation, indent=" ")
def monitor_task_until_terminal(navigation, handle, timeout_s):
start_time = time.time()
while True:
snapshot = navigation.get_navigation_target_status(handle.task_id)
print_task_snapshot(navigation, handle, snapshot)
now = time.time()
if snapshot.status in {
NavigationTaskStatus.SUCCESS,
NavigationTaskStatus.FAILED,
NavigationTaskStatus.INTERRUPTED,
NavigationTaskStatus.OCCUPIED,
NavigationTaskStatus.COLLISION,
NavigationTaskStatus.CLOSE_TO_OBSTACLE,
}:
return
if (now - start_time) >= timeout_s:
print(" timeout reached while waiting for task completion.")
return
time.sleep(1.0)
def make_waypoint(pose, params=None):
if params is None:
params = WaypointParams()
return Waypoint(pose, params)
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!")
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!")
return -1
res = robot.switch_controller(S1ControllerName.SWERVE_CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
return -1
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
waypoint_2_params = WaypointParams()
waypoint_2_params.arrival_position_threshold_x = 0.02
waypoint_2_params.arrival_position_threshold_y = 0.02
waypoint_2_params.arrival_orientation_threshold = 0.08
waypoint_2_params.velocity_scale = 0.2
waypoint_3_params = WaypointParams()
waypoint_3_params.arrival_position_threshold_x = 0.04
waypoint_3_params.arrival_position_threshold_y = 0.04
waypoint_3_params.arrival_orientation_threshold = 0.05
waypoint_3_params.velocity_scale = 0.8
waypoint_3_params.acceleration_scale = 0.8
waypoint_3_params.jerk_scale = 0.8
waypoints = [
make_waypoint(Pose([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])),
make_waypoint(Pose([0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]), waypoint_2_params),
make_waypoint(Pose([1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]), waypoint_3_params),
]
time.sleep(3)
if not wait_for_localization(navigation, init_pose):
print("localization failed", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return -1
time.sleep(2)
print("set target: waypoints")
for i, waypoint in enumerate(waypoints):
print(f" waypoint_{i}: {format_pose(waypoint.pose)}")
frame_id = "map"
enable_collision_check = True
handle = navigation.navigate_through_waypoints(waypoints, frame_id, enable_collision_check)
if handle.request_sent:
monitor_task_until_terminal(navigation, handle, kTaskTimeoutSec)
else:
print("navigation failed to submit task")
navigation.stop_navigation()
print("\nfinal task summary")
snapshot = navigation.get_navigation_target_status(handle.task_id)
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
sys.exit(main())
设置导航目标（set_navigation_target）
适用场景：在启动或管理导航任务前，单独设置导航目标。
examples/python/galbot_navigation/set_navigation_target.py
from galbot_sdk.s1 import ControlStatus, S1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.s1 import NavigationTaskStatus, Pose
import numpy as np
import sys
import time
kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kPollIntervalMs = 500
kMonitorDurationMs = 2000
def navigation_target_status_to_string(status):
if status == NavigationTaskStatus.UNKNOWN:
return "UNKNOWN"
if status == NavigationTaskStatus.RUNNING:
return "RUNNING"
if status == NavigationTaskStatus.SUCCESS:
return "SUCCESS"
if status == NavigationTaskStatus.FAILED:
return "FAILED"
if status == NavigationTaskStatus.INTERRUPTED:
return "INTERRUPTED"
if status == NavigationTaskStatus.OCCUPIED:
return "OCCUPIED"
if status == NavigationTaskStatus.COLLISION:
return "COLLISION"
if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
return "CLOSE_TO_OBSTACLE"
return "UNKNOWN"
def format_pose(pose):
values = [
pose.position.x,
pose.position.y,
pose.position.z,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
]
return "[" + ", ".join(f"{value:g}" for value in values) + "]"
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def print_task_snapshot(navigation, handle, snapshot, include_pose=True):
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
if include_pose:
print_current_pose(navigation, indent=" ")
def wait_for_localization(navigation, init_pose):
retry = 0
while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
navigation.relocalize(init_pose)
time.sleep(kLocalizationRetrySleepMs / 1000.0)
retry += 1
return navigation.is_localized()
def print_task_summary(navigation, handles):
print("\nfinal task summary")
for handle in handles:
snapshot = navigation.get_navigation_target_status(handle.task_id)
print_task_snapshot(navigation, handle, snapshot, include_pose=False)
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!")
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!")
return -1
res = robot.switch_controller(S1ControllerName.SWERVE_CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
return -1
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
time.sleep(3)
if not wait_for_localization(navigation, init_pose):
print("localization failed", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return -1
frame_id = "map"
speed_ratio = 0.6
enable_collision_check = True
target_points = [
Pose([1.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
Pose([0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
Pose([0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
]
handles = []
for i, target in enumerate(target_points):
print(f"set target: dynamic_goal_{i}, target_pose: {format_pose(target)}")
handle = navigation.set_navigation_target(target, frame_id, speed_ratio, enable_collision_check)
handles.append(handle)
start_time = time.time()
while True:
snapshot = navigation.get_navigation_target_status(handle.task_id)
print_task_snapshot(navigation, handle, snapshot)
elapsed_ms = (time.time() - start_time) * 1000.0
if elapsed_ms >= kMonitorDurationMs:
break
time.sleep(kPollIntervalMs / 1000.0)
print_task_summary(navigation, handles)
print("\nstop navigation and shutdown")
navigation.stop_navigation()
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
sys.exit(main())
类：GalbotPerception
高精度双目深度估计单次推理
适用场景：触发一次双目深度估计推理，并获取生成的深度数据。
examples/python/galbot_perception/foundation_stereo_example.py
"""Foundation stereo depth example (S1): single run_once, save a pseudo-color depth image."""
import time
import cv2
import numpy as np
try:
from galbot_sdk.s1 import (
GalbotPerception,
GalbotRobot,
PerceptionModule,
)
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
raise
OUTPUT_IMAGE_PATH = "foundation_stereo_depth.png"
def main():
robot = GalbotRobot()
try:
if not robot.init():
print("Robot init failed")
return
print("Robot init OK")
perception = GalbotPerception()
if not perception.init({PerceptionModule.FOUNDATION_STEREO, PerceptionModule.LIGHT_STEREO}):
print("Perception init failed")
return
print("Perception init OK")
time.sleep(12) # Wait for perception models to load
print("Triggering single inference...")
if not perception.run_once(PerceptionModule.FOUNDATION_STEREO):
print("run_once failed to send command")
return
print("Waiting for inference result...")
if not perception.wait_for_new_result(
PerceptionModule.FOUNDATION_STEREO, timeout_s=6.0
):
print("Timed out waiting for inference result")
return
ok, result = perception.get_latest_result(PerceptionModule.FOUNDATION_STEREO)
if not ok:
print("get_latest_result failed")
return
print(result.get_result_info())
depth_map = result.instance_mask
if depth_map is None:
print("No depth map (instance_mask is empty)")
return
print(f"Depth map shape: {depth_map.shape}, dtype: {depth_map.dtype}")
print(f"Depth value range: [{np.nanmin(depth_map)}, {np.nanmax(depth_map)}]")
depth_f = depth_map.astype(np.float32)
valid = depth_f[depth_f > 0]
if valid.size > 0:
vmin, vmax = np.percentile(valid, [1, 99])
normalized = np.clip((depth_f - vmin) / (vmax - vmin + 1e-6), 0, 1)
else:
normalized = np.zeros_like(depth_f)
colored = cv2.applyColorMap(
(normalized * 255).astype(np.uint8), cv2.COLORMAP_TURBO
)
if cv2.imwrite(OUTPUT_IMAGE_PATH, colored):
print(f"Saved: {OUTPUT_IMAGE_PATH}")
else:
print(f"Failed to save: {OUTPUT_IMAGE_PATH}")
finally:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
类：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)
教程源码示例
以下可运行教程维护在 examples/s1/python/tutorials 中；使用步骤与安全说明请参阅入门教程。
示例 1：Galbot 控制入门
适用场景：S1 最基础的关节控制：执行双臂比心动作并恢复初始姿态。运行前请确认机器人处于工作模式且急停按钮已旋开；夹爪控制及更多控制案例请参阅本页 galbot_robot 下的示例。
examples/python/tutorials/example1_galbot_control_started.py
"""
Note: When running this example, please ensure the robot's `emergency stop button` is released;
"""
import time
try:
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import ControlStatus
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
exit(1)
def demo_heart_pose(robot: GalbotRobot,
joint_group_names: list,
position_seq: list,
is_blocking: bool,
max_speed: float,
timeout_s: float,
retry_count: int = 3
):
"""
Robot heart gesture demonstration function
Parameters:
robot (GalbotRobot): GalbotRobot instance
joint_group_names (list): List of joint group names to control
position_seq (list): List of joint group angle sequences to set
is_blocking (bool): Whether to set angles in blocking mode
max_speed (float): Maximum speed
timeout_s (float): Timeout time (seconds)
retry_count (int, optional): Number of retries, default is 3
Returns:
None
"""
# Get current joint group angles for subsequent restoration
original_pos = robot.get_joint_positions(joint_group_names, [])
print(f"Current angles of joint group {joint_group_names}: {original_pos}")
# Start heart gesture
pos_idx = 0
print("Starting heart gesture...")
while True:
time.sleep(1)
pos = position_seq[pos_idx]
control_status = robot.set_joint_positions(
pos, joint_group_names, [], is_blocking, max_speed, timeout_s
)
# If setting fails, retry 3 times
retry_cnt = retry_count
while control_status != ControlStatus.SUCCESS and retry_cnt > 0:
print(f"Setting angles for joint group {joint_group_names} failed, retrying {retry_cnt}...")
retry_cnt = retry_cnt - 1
time.sleep(1)
control_status = robot.set_joint_positions(
pos, joint_group_names, [], is_blocking, max_speed, timeout_s
)
# If successful, switch to next pose sequence
if control_status == ControlStatus.SUCCESS:
print(f"Setting angles for joint group {joint_group_names} successful")
pos_idx = pos_idx + 1
# If failed, break the loop
else:
print(f"Setting angles for joint group {joint_group_names} failed")
break
# If all pose sequences are completed, break the loop
if pos_idx > len(position_seq) - 1:
break
# Get current joint group angles
print("Showing heart gesture for 15 seconds, then restoring original pose...")
time.sleep(5)
# Restore original pose joint group angles
control_status = robot.set_joint_positions(
original_pos, joint_group_names, [], is_blocking, max_speed, timeout_s
)
# If setting fails, retry 5 times
retry_cnt = retry_count
while control_status != ControlStatus.SUCCESS and retry_cnt > 0:
print(f"Setting angles for joint group {joint_group_names} failed, retrying {retry_cnt}...")
retry_cnt = retry_cnt - 1
time.sleep(2)
control_status = robot.set_joint_positions(
original_pos, joint_group_names, [], is_blocking, max_speed, timeout_s
)
# If successful, restore original pose
if control_status == ControlStatus.SUCCESS:
print(f"Restoring angles for joint group {joint_group_names} successful")
else:
print(f"Restoring angles for joint group {joint_group_names} failed")
def check_robot_safety():
"""Check if the robot is safe"""
# Prompt for precautions
print("⚠️ Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing execution...")
break
elif key == 'n':
print("User not confirmed, program exiting...")
exit(1)
else:
print("Input error, please enter 'y' or 'n'")
def main():
check_robot_safety()
try:
# Get robot instance
robot = GalbotRobot()
# Initialize robot
state = robot.init()
if not state:
print(f"Initialization failed")
exit(1)
else:
print(f"Initialization successful")
print(f"Is robot running: {robot.is_running()}")
# Wait for data preparation
time.sleep(3)
# Get list of joint names
joint_names = robot.get_joint_names(True, ["torso", "head", "left_arm", "right_arm"])
if len(joint_names) > 0:
print(f"List of joint names: {joint_names}")
else:
print(f"Failed to get list of joint names")
# Get joint positions using joint group names, empty returns all joints by default
joint_group_names = ["left_arm", "right_arm"]
# Left and right arm heart gesture sequence
position_seq = [[
1.00, 0.40, -2.06, -1.80, -2.92, -0.76, -0.03, # left_arm
-1.00, -0.40, 2.06, 1.80, 2.92, 0.76, 0.03 # right_arm
]]
# Whether to block and wait for joints to reach position
is_blocking = True
# Limit maximum joint speed to 0.3rad/s
max_speed = 0.3
# Maximum blocking wait time
timeout_s = 30
# Perform heartbeat gesture
demo_heart_pose(robot, joint_group_names, position_seq,
is_blocking, max_speed, timeout_s)
except Exception as e:
print(f"An exception occurred: {e}")
finally:
# Actively send SIGINT shutdown signal
robot.request_shutdown()
# Wait to enter shutdown state
robot.wait_for_shutdown()
# Release SDK resources
robot.destroy()
print('Resource release successful')
if __name__ == "__main__":
main()
示例 2：机械臂操作
适用场景：学习机械臂正逆运动学，并使用规划结果控制机械臂。
examples/python/tutorials/example2_arm_manipulation.py
"""
Note: When running this example, please ensure the robot's motion control service `/data/galbot/bin/service_motion_plan`,
robot state publishing service `/data/galbot/bin/robot_state_publish`,
and hand-eye calibration publishing service `/data/galbot/bin/eyehand_calib_publish` are loaded;
"""
try:
import galbot_sdk.s1 as gm
from galbot_sdk.s1 import GalbotMotion
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import ControlStatus
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
exit(1)
import os
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Sequence, Dict
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, Execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, Execution timeout")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, Fault occurred, unable to continue execution")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, Input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, Internal communication component creation failed")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, Moving but not in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, Stopped but did not reach target")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, Data acquisition failed")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, Data sending failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, Connection failed")
def quat_normalize(q: np.ndarray) -> np.ndarray:
q = np.array(q, dtype=np.float64)
return q / np.linalg.norm(q)
def quat_conjugate(q: np.ndarray) -> np.ndarray:
"""
Calculate the conjugate of a quaternion
Parameters:
q (np.ndarray): Input quaternion [x, y, z, w]
Returns:
np.ndarray: Conjugate of the quaternion [x, y, z, w]
"""
qx, qy, qz, qw = q
return np.array([-qx, -qy, -qz, qw])
def quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
"""
Calculate the product of two quaternions
Parameters:
q1 (np.ndarray): First quaternion [x, y, z, w]
q2 (np.ndarray): Second quaternion [x, y, z, w]
Returns:
np.ndarray: Product of the two quaternions [x, y, z, w]
"""
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
return np.array([
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2,
w1*w2 - x1*x2 - y1*y2 - z1*z2
])
def orientation_error_angle(A: np.ndarray, B: np.ndarray) -> float:
"""
Calculate the rotation angle error between two quaternions (radians)
Parameters:
A (np.ndarray): First quaternion [x, y, z, w]
B (np.ndarray): Second quaternion [x, y, z, w]
Returns:
float: Rotation angle error (radians)
"""
qA = quat_normalize(A[3:7])
qB = quat_normalize(B[3:7])
q_err = quat_multiply(qB, quat_conjugate(qA))
q_err = quat_normalize(q_err)
# Numerical stability
qw = np.clip(q_err[3], -1.0, 1.0)
angle = 2 * np.arccos(qw)
return angle # Unit: radians
def calculate_error(pose1: Sequence[float], pose2: Sequence[float]) -> Dict[str, float]:
"""
Calculate position error and rotation error between two poses (radians)
Parameters:
pose1 (Sequence[float]): First pose [x, y, z, qx, qy, qz, qw]
pose2 (Sequence[float]): Second pose [x, y, z, qx, qy, qz, qw]
Returns:
dict: Dictionary containing position error (meters) and rotation error (radians)
"""
A, B = np.array(pose1), np.array(pose2)
pos_err = np.linalg.norm(A[:3] - B[:3])
rot_err = orientation_error_angle(A, B)
return {
"position_error_norm": pos_err,
"orientation_error_rad": rot_err,
"orientation_error_deg": np.degrees(rot_err)
}
def check_robot_safety():
"""Check if the robot is safe"""
# Prompt for precautions
print("⚠️ Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing execution...")
break
elif key == 'n':
print("User not confirmed, program exiting...")
exit(1)
else:
print("Input error, please enter 'y' or 'n'")
def main():
check_robot_safety()
try:
# Get GalbotMotion singleton and initialize
motion = GalbotMotion()
robot = GalbotRobot()
if motion.init():
print("GalbotMotion initialization successful")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialization successful")
else:
print("GalbotRobot initialization failed")
# Program starts immediately, wait for data readiness time
time.sleep(3)
# Move whole body to predefined zero(home) configuration before arm demo.
status = motion.move_whole_body_joint_zero(
is_blocking=True,
leg_head_speed_rad_s=0.2,
leg_head_timeout_s=15.0,
params=gm.Parameter()
)
printStatus(status)
if status != gm.MotionStatus.SUCCESS:
print("❌ move_whole_body_joint_zero failed, stop demo to avoid risky posture.")
return
print("✅ whole-body zero completed, start arm manipulation demo from safe pose.")
# Define target chain name, target pose, reference pose, end link
target_frame = "EndEffector"
reference_frame = "base_link"
target_chain = "left_arm"
end_link = "left_arm_end_effector_mount_link"
# 1. Get current left_arm end-effector pose
try:
status, original_pose = motion.get_end_effector_pose_on_chain(
chain_name=target_chain,
frame_id=target_frame,
reference_frame=reference_frame
)
assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
print(f"✅ Current {target_chain} end-effector pose: {original_pose}")
time.sleep(0.8)
except Exception as e:
print(f"❌ Exception getting end-effector pose by chain name: {e}")
return
# Build S1 demo target pose from zero-start current pose instead of hard-coded absolute pose.
# This avoids using model-specific (e.g. G1-like) constants in S1 tutorial.
chain_pose_baselink = {
"left_arm": [
original_pose[0] + 0.04, # x forward 4 cm
original_pose[1], # keep lateral position
original_pose[2] + 0.02, # z up 2 cm
original_pose[3],
original_pose[4],
original_pose[5],
original_pose[6]
]
}
print(f"✅ S1 target pose (relative to zero pose): {chain_pose_baselink[target_chain]}")
# 2. Solve joint angles based on target pose IK and verify the solution
# 2.1 Solve joint angles joint_angles_ik for target pose through IK
try:
status, joint_angles_ik = motion.inverse_kinematics(
target_pose=chain_pose_baselink[target_chain],
chain_names=[target_chain],
target_frame=target_frame,
reference_frame=reference_frame,
enable_collision_check=False # Disable collision detection
)
assert status == gm.MotionStatus.SUCCESS, "IK solving failed"
print(f"✅ Target {target_chain} IK solving successful joint_angles_ik: {joint_angles_ik}")
time.sleep(1)
except Exception as e:
print(f"❌ IK solving exception: {e}")
# 2.2 Set end-effector pose to target pose tgt_pose_ik by setting joint group angles joint_angles_ik
try:
status = robot.set_joint_positions(
joint_angles_ik[target_chain],
[target_chain],
[],
True,
0.1,
20.0,
)
assert status == ControlStatus.SUCCESS, "Setting joint group angles failed"
print(f"✅ Setting {target_chain} joint group angles successful.")
time.sleep(1)
except Exception as e:
print(f"❌ Setting {target_chain} joint group angles exception: {e}")
# 2.3 Verify whether the set joint group angles are consistent with the solved angles
try:
status, tgt_pose_ik = motion.get_end_effector_pose_on_chain(
chain_name=target_chain,
frame_id=target_frame,
reference_frame=reference_frame
)
assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
print(f"✅ Getting {target_chain} end-effector pose successful: {tgt_pose_ik}")
time.sleep(1)
error = calculate_error(tgt_pose_ik, chain_pose_baselink[target_chain])
print(f"End-effector pose error: {error}")
except Exception as e:
print(f"❌ Getting {target_chain} end-effector pose exception: {e}")
# 2.4 Verify whether the end-effector pose tgt_pose_fk corresponding to joint group angles joint_angles_ik solved by FK is consistent with target pose tgt_pose_ik
try:
status, tgt_pose_fk = motion.forward_kinematics(
target_frame=end_link,
reference_frame=reference_frame,
joint_state=joint_angles_ik,
params=gm.Parameter()
)
assert status == gm.MotionStatus.SUCCESS, "FK solving failed"
print(f"✅ Target {target_chain} FK solving successful: {tgt_pose_fk}")
time.sleep(1)
error = calculate_error(tgt_pose_fk, chain_pose_baselink[target_chain])
print(f"FK solving error: {error}")
except Exception as e:
print(f"❌ FK solving exception: {e}")
time.sleep(3)
print()
# 3. Restore to original pose by setting end-effector pose
# 3.1 Set end-effector pose to restore to original pose
try:
status = motion.set_end_effector_pose(
target_pose=original_pose,
end_effector_frame=target_chain,
reference_frame=reference_frame,
enable_collision_check=False,
is_blocking=True,
timeout=5.0,
params=gm.Parameter()
)
assert status == gm.MotionStatus.SUCCESS, "Setting end-effector pose failed"
print(f"✅ Setting end-effector pose successful: status={status}")
time.sleep(1)
except Exception as e:
print(f"❌ Setting {target_chain} end-effector pose exception: {e}")
# 3.2 Get end-effector pose and verify whether it has been restored to original pose
try:
status, original_pose_rec = motion.get_end_effector_pose_on_chain(
chain_name=target_chain,
frame_id=target_frame,
reference_frame=reference_frame
)
assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
print(f"✅ Getting {target_chain} end-effector pose successful: {original_pose_rec}")
time.sleep(0.8)
error = calculate_error(original_pose_rec, original_pose)
print(f"Restore end-effector pose error: {error}")
except Exception as e:
print(f"❌ Setting end-effector pose exception: {e}")
except Exception as e:
print(f"❌ Main program exception: {e}")
finally:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__=="__main__":
main()
示例 3：机器人导航
适用场景：初始化导航、完成机器人定位并导航至目标位姿。
examples/python/tutorials/example3_robot_navigation.py
"""
Note: When running this example, please confirm that the robot's navigation function `/data/galbot/bin/service_navigation_plan` has been loaded;
"""
try:
from galbot_sdk.s1 import GalbotNavigation
from galbot_sdk.s1 import GalbotRobot
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it's in PYTHONPATH")
exit(1)
import os
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Sequence, Dict
def quat_normalize(q: np.ndarray) -> np.ndarray:
q = np.array(q, dtype=np.float64)
return q / np.linalg.norm(q)
def quat_conjugate(q: np.ndarray) -> np.ndarray:
"""
Calculate the conjugate of a quaternion
Parameters:
q (np.ndarray): Input quaternion [x, y, z, w]
Returns:
np.ndarray: Conjugate of the quaternion [x, y, z, w]
"""
qx, qy, qz, qw = q
return np.array([-qx, -qy, -qz, qw])
def quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
"""
Calculate the product of two quaternions
Parameters:
q1 (np.ndarray): First quaternion [x, y, z, w]
q2 (np.ndarray): Second quaternion [x, y, z, w]
Returns:
np.ndarray: Product of the two quaternions [x, y, z, w]
"""
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
return np.array([
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2,
w1*w2 - x1*x2 - y1*y2 - z1*z2
])
def quat_to_matrix(q: Sequence[float]) -> np.ndarray:
"""
Convert quaternion [x, y, z, w] to a 3x3 rotation matrix.
"""
x, y, z, w = quat_normalize(q)
xx, yy, zz = x * x, y * y, z * z
xy, xz, yz = x * y, x * z, y * z
wx, wy, wz = w * x, w * y, w * z
return np.array([
[1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
[2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
[2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)],
], dtype=np.float64)
def matrix_to_quat(m: np.ndarray) -> np.ndarray:
"""
Convert a 3x3 rotation matrix to quaternion [x, y, z, w].
"""
m = np.asarray(m, dtype=np.float64)
trace = float(np.trace(m))
if trace > 0.0:
s = np.sqrt(trace + 1.0) * 2.0
w = 0.25 * s
x = (m[2, 1] - m[1, 2]) / s
y = (m[0, 2] - m[2, 0]) / s
z = (m[1, 0] - m[0, 1]) / s
elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0
w = (m[2, 1] - m[1, 2]) / s
x = 0.25 * s
y = (m[0, 1] + m[1, 0]) / s
z = (m[0, 2] + m[2, 0]) / s
elif m[1, 1] > m[2, 2]:
s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0
w = (m[0, 2] - m[2, 0]) / s
x = (m[0, 1] + m[1, 0]) / s
y = 0.25 * s
z = (m[1, 2] + m[2, 1]) / s
else:
s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0
w = (m[1, 0] - m[0, 1]) / s
x = (m[0, 2] + m[2, 0]) / s
y = (m[1, 2] + m[2, 1]) / s
z = 0.25 * s
return quat_normalize([x, y, z, w])
def orientation_error_angle(A: np.ndarray, B: np.ndarray) -> float:
"""
Calculate the rotation angle error between two quaternions (in radians)
Parameters:
A (np.ndarray): First quaternion [x, y, z, w]
B (np.ndarray): Second quaternion [x, y, z, w]
Returns:
float: Rotation angle error (in radians)
"""
qA = quat_normalize(A[3:7])
qB = quat_normalize(B[3:7])
q_err = quat_multiply(qB, quat_conjugate(qA))
q_err = quat_normalize(q_err)
# Numerically stable
qw = np.clip(q_err[3], -1.0, 1.0)
angle = 2 * np.arccos(qw)
return angle # Unit: radians
def calculate_error(pose1: Sequence[float], pose2: Sequence[float]) -> Dict[str, float]:
"""
Calculate the position error and rotation error between two poses (in radians)
Parameters:
pose1 (Sequence[float]): First pose, [x, y, z, qx, qy, qz, qw]
pose2 (Sequence[float]): Second pose, [x, y, z, qx, qy, qz, qw]
Returns:
dict: Dictionary containing position error (in meters) and rotation error (in radians)
"""
A, B = np.array(pose1), np.array(pose2)
pos_err = np.linalg.norm(A[:3] - B[:3])
rot_err = orientation_error_angle(A, B)
return {
"position_error_norm": pos_err,
"orientation_error_rad": rot_err,
"orientation_error_deg": np.degrees(rot_err)
}
def local_pose_to_global(start_pose: Sequence[float], local_pose: Sequence[float]):
"""
Convert local pose to global pose
Parameters:
start_pose (Sequence[float]): Start pose, [x, y, z, qx, qy, qz, qw]
local_pose (Sequence[float]): Local pose, [x, y, z, qx, qy, qz, qw]
Returns:
Sequence[float]: Global pose, [x, y, z, qx, qy, qz, qw]
"""
start_mat = np.eye(4)
start_mat[:3, :3] = quat_to_matrix(start_pose[3:7])
start_mat[:3, 3] = [start_pose[0], start_pose[1], start_pose[2]]
local_mat = np.eye(4)
local_mat[:3, :3] = quat_to_matrix(local_pose[3:7])
local_mat[:3, 3] = [local_pose[0], local_pose[1], local_pose[2]]
global_mat = start_mat @ local_mat
return global_mat[:3, 3].tolist() + matrix_to_quat(global_mat[:3, :3]).tolist()
def demo_square_move(robot: GalbotRobot, nav: GalbotNavigation):
"""
Demonstrate robot moving in a square pattern in navigation environment
Parameters:
robot (GalbotRobot): Robot instance
nav (GalbotNavigation): Navigation instance
"""
try:
start_pose = nav.get_current_pose()
except Exception as e:
print(f"Failed to get current pose: {e}")
return
# Move forward 0.5m, turn left 90 degrees
local_pose = [0.5, 0.0, 0.0, 0.0, 0.0, 0.707, 0.707]
try:
# Move forward 0.5m each time, turn left 90 degrees, repeat 4 times to form a square
for _ in range(4):
# Calculate target pose
cur_pose = nav.get_current_pose()
goal_pose = local_pose_to_global(cur_pose, local_pose)
# Check if path is reachable
if nav.check_path_reachability(goal_pose, cur_pose):
# Navigate to target pose
retry_cnt = 3
while True:
status = nav.navigate_to_goal(goal_pose, enable_collision_check=True, is_blocking=True, timeout=30)
time.sleep(0.5)
retry_cnt -= 1
if nav.check_goal_arrival() or retry_cnt < 0:
break
else:
print(f"Navigation failed, retrying...{retry_cnt}")
print("navigate_to_goal return status:", status)
print("Has arrived:", nav.check_goal_arrival())
else:
print("Path unreachable or unsafe")
cur_pose = nav.get_current_pose()
print(f"Current pose: {cur_pose}, Error from start pose: {calculate_error(cur_pose, start_pose)}")
except Exception as e:
print(f"Exception occurred during navigation: {e}")
def move_to_original(robot: GalbotRobot, nav: GalbotNavigation):
"""
Demonstrate robot returning to start pose in navigation environment
Parameters:
robot (GalbotRobot): Robot instance
nav (GalbotNavigation): Navigation instance
"""
cur_pose = nav.get_current_pose()
goal_pose = [0, 0, 0, 0, 0, 0, 1]
try:
if nav.check_path_reachability(goal_pose, cur_pose):
retry_cnt = 3
while True:
status = nav.navigate_to_goal(goal_pose, enable_collision_check=True, is_blocking=True, timeout=30)
time.sleep(0.5)
retry_cnt -= 1
if nav.check_goal_arrival() or retry_cnt < 0:
break
else:
print(f"Navigation failed, retrying...{retry_cnt}")
print("navigate_to_goal return status:", status)
print("Has arrived:", nav.check_goal_arrival())
else:
print("Path unreachable or unsafe")
except Exception as e:
print(f"Exception occurred during navigation: {e}")
def check_robot_safety():
"""Check if robot is safe"""
# Prompt important notes
print("⚠️ Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations; 3. Please ensure the area around the robot is clear of obstacles.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing...")
break
elif key == 'n':
print("User did not confirm, exiting program...")
exit(1)
else:
print("Invalid input, please enter 'y' or 'n'")
def main():
check_robot_safety()
try:
# Get robot instance
robot = GalbotRobot()
# Get navigation instance
nav = GalbotNavigation()
# Initialize robot
if robot.init():
print("Robot initialization successful")
else:
print("Robot initialization failed")
# Initialize navigation
if nav.init():
print("Navigation initialization successful")
else:
print("Navigation initialization failed")
# Wait for data preparation
time.sleep(1)
# Check initial localization status
is_localized = nav.is_localized()
if not is_localized:
print("Localization failed, attempting to re-localize: Please move the robot to the origin of the map!")
time.sleep(3)
while not is_localized:
nav.relocalize([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
time.sleep(0.5)
is_localized = nav.is_localized()
# square_move
demo_square_move(robot, nav)
except Exception as e:
print(f"Exception occurred: {e}")
finally:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
示例 4：传感器数据采集
适用场景：采集相机、深度、雷达、IMU 等机器人传感器数据。
examples/python/tutorials/example4_sensor_data_collect.py
"""
Note: When running this example, please confirm that the robot's left arm camera driver `/data/galbot/bin/left_arm_camera_capture`
and radar driver `/data/galbot/bin/service_lidar_capture` have been loaded;
"""
try:
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import RgbOutputFormat, SensorType
except ImportError:
print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
import os
try:
import open3d as o3d
except ImportError:
os.system("pip install open3d")
import open3d as o3d
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Dict
def convert_pointcloud(cloud):
"""
Convert cloud dict to NumPy array dictionary
Parameters:
cloud (dict): PointCloud2 protobuf message object
Returns:
Dictionary: {field_name: NumPy array}
- Single-element fields: shape (N,)
- Multi-element fields: shape (N, count) or (N,)
- N = width * height (total number of points)
"""
if not cloud:
return {}
num_points = cloud["height"] * cloud["width"]
if num_points == 0:
return {}
DTYPE_MAP = {
1: np.int8,
2: np.uint8,
3: np.int16,
4: np.uint16,
5: np.int32,
6: np.uint32,
7: np.float32,
8: np.float64
}
dtype_list = []
for field in cloud["fields"]:
# Get base data type
np_dtype_class = DTYPE_MAP.get(field["datatype"])
if np_dtype_class is None:
raise ValueError(f"Unsupported data type: {field['datatype']}")
dtype_inst = np.dtype(np_dtype_class)
# Handle byte order (endianness)
if dtype_inst.itemsize > 1:
byteorder = '>' if cloud["is_bigendian"] else '<'
dtype_inst = dtype_inst.newbyteorder(byteorder)
# Add to dtype list
if field["count"] == 1:
dtype_list.append((field["name"], dtype_inst))
else:
# Multi-element fields (e.g., rgb)
dtype_list.append((field["name"], dtype_inst, field["count"]))
# Create structured dtype
dtype = np.dtype(dtype_list)
# Data integrity check
expected_size = num_points * cloud["point_step"]
if len(cloud["data"]) < expected_size:
raise ValueError(
f"Insufficient data length: expected {expected_size} bytes, "
f"actual {len(cloud['data'])} bytes"
)
# Create NumPy structured array from binary data
# count parameter ensures only expected number of points are read
arr = np.frombuffer(cloud["data"], dtype=dtype, count=num_points)
# Convert to regular dictionary (copy data to avoid modifying original)
result = {}
for field in cloud["fields"]:
field_data = arr[field["name"]]
# Handle shape of multi-element fields
if field["count"] == 1:
result[field["name"]] = field_data.copy()
else:
# Keep original shape or flatten, choose according to needs
result[field["name"]] = field_data.copy()
return result
def get_xyz_array(pointcloud_dict: Dict[str, np.ndarray],
remove_nan: bool = False) -> np.ndarray:
"""
Extract XYZ coordinate array from converted point cloud dictionary
Parameters:
pointcloud_dict (Dict[str, np.ndarray]): Dictionary returned by pointcloud2_to_numpy()
remove_nan (bool, optional): Whether to remove points containing NaN (for FLOAT32/FLOAT64 types). Defaults to False.
Returns:
Nx3 point coordinate array
"""
required = ['x', 'y', 'z']
if not all(k in pointcloud_dict for k in required):
raise ValueError("Point cloud data missing required xyz fields")
points = np.stack([pointcloud_dict['x'],
pointcloud_dict['y'],
pointcloud_dict['z']], axis=1)
if remove_nan:
mask = ~np.isnan(points).any(axis=1)
points = points[mask]
return points
def save_xyz_to_pcd(xyz_array: np.ndarray, filename: str, binary: bool = False) -> None:
"""
Save XYZ coordinates to PCD file format (simplest option for coordinate-only data)
Parameters:
xyz_array (np.ndarray): Nx3 array of XYZ coordinates
filename (str): Output PCD file path
binary (bool, optional): If True, saves in binary format; otherwise ASCII. Defaults to False.
"""
if xyz_array.ndim != 2 or xyz_array.shape[1] != 3:
raise ValueError(f"xyz_array must have shape (N, 3), got {xyz_array.shape}")
num_points = xyz_array.shape[0]
header = [
"# .PCD v0.7 - Point Cloud Data file format",
"VERSION 0.7",
"FIELDS x y z",
"SIZE 4 4 4",
"TYPE F F F", # F = float32
"COUNT 1 1 1",
f"WIDTH {num_points}",
"HEIGHT 1",
"VIEWPOINT 0 0 0 1 0 0 0",
f"POINTS {num_points}",
f"DATA {'binary' if binary else 'ascii'}"
]
if binary:
with open(filename, 'wb') as f:
f.write(('\n'.join(header) + '\n').encode('ascii'))
f.write(xyz_array.astype(np.float32).tobytes())
else:
with open(filename, 'w') as f:
f.write('\n'.join(header) + '\n')
np.savetxt(f, xyz_array, fmt='%f')
def decode_compressed_image(compressed_image):
"""
decode CompressedImage image
Parameters:
compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"] == "jpeg":
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(compressed_image)
else:
raise ValueError(f"Unsupport data format: {compressed_image['format']}")
def decode_rgb_image(image_data):
"""decode rgb image"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Fail to Decode RGB Image")
return img
def decode_depth_image(image_data):
"""decode depth image"""
depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()
# Check if height and width exist
if "height" not in image_data or "width" not in image_data:
raise ValueError("Missing 'height' or 'width' in depth image metadata.")
if image_data["height"] == 0 or image_data["width"] == 0:
raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")
# Parse depth image
depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]
return depth_img
def depth_rgb_to_pointcloud(depth, rgb, fx, fy, cx, cy, depth_scale=1.0):
"""
Convert depth map and RGB image to point cloud
Parameters:
depth: (H, W) depth map
rgb: (H, W, 3) RGB image
depth_scale: Depth unit scaling (use 0.001 for mm->m conversion)
Returns:
points: (N, 3) point cloud coordinate array
colors: (N, 3) point cloud color array (0-1 range)
"""
assert depth.shape[:2] == rgb.shape[:2]
H, W = depth.shape
# Pixel coordinates
u, v = np.meshgrid(np.arange(W), np.arange(H))
# Depth (converted to meters)
Z = depth.astype(np.float32) * depth_scale
# Filter invalid depths
valid = Z > 0
X = (u - cx) * Z / fx
Y = (v - cy) * Z / fy
points = np.stack((X, Y, Z), axis=-1)
colors = rgb.astype(np.float32) / 255.0
# Keep only valid points
points = points[valid]
colors = colors[valid]
return points, colors
def check_robot_safety():
"""Check if robot is safe"""
# Prompt important notes
print("⚠️ Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing...")
break
elif key == 'n':
print("User did not confirm, exiting program...")
exit(1)
else:
print("Invalid input, please enter 'y' or 'n'")
def main():
SHOW_IMAGE = False
check_robot_safety()
try:
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
# Get RGB and depth images from the left arm, depth images from the right arm,
# back LiDAR data, and back IMU data
enable_sensor_set = {SensorType.LEFT_ARM_CAMERA, # Left arm RGB camera
SensorType.LEFT_ARM_DEPTH_CAMERA, # Left arm depth camera
SensorType.BACK_LIDAR, # Back LiDAR
SensorType.BACK_IMU} # Back IMU
# To save resource overhead, only cameras and radar sensors passed during initialization can acquire data
if robot.init(enable_sensor_set):
print("Initialization successful")
else:
print("Initialization failed")
return
# Program starts immediately, wait for data readiness
time.sleep(5)
# Get RGB image from the left arm
rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
if not rgb_image_data:
print("No rgb image data!")
else:
print("get rgb image suceess")
print(rgb_image_data['header'])
img = decode_compressed_image(rgb_image_data)
# Save RGB image
cv2.imwrite("rgb_image_data.jpg", img)
# Visualize RGB image
if SHOW_IMAGE:
cv2.namedWindow("rgb image", cv2.WINDOW_NORMAL)
cv2.imshow("rgb image", img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Get depth image from the left arm
depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not depth_data or "data" not in depth_data:
print("Depth camera not ready")
else:
print("get depth data suceess")
print(depth_data['header'])
depth_img_raw = decode_compressed_image(depth_data)
depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize, mapping depth values to 0-1 range
depth_img = depth_img.astype(np.uint8)
# Save depth image
cv2.imwrite("depth_data.jpg", depth_img)
# Visualize depth image
if SHOW_IMAGE:
cv2.namedWindow("depth image", cv2.WINDOW_NORMAL)
cv2.imshow("depth image", depth_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Get back LiDAR data
lidar_data = robot.get_lidar_data(SensorType.BACK_LIDAR)
if not lidar_data:
print("No lidar data!")
else:
pointcloud_dict = convert_pointcloud(lidar_data)
xyz_points = get_xyz_array(pointcloud_dict)
save_xyz_to_pcd(xyz_points, "output_xyz.pcd")
print(pointcloud_dict)
print("get lidar data success")
# Visualize LiDAR point cloud
if SHOW_IMAGE:
vis = o3d.visualization.Visualizer()
vis.create_window()
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz_points)
vis.add_geometry(pcd)
vis.run()
vis.destroy_window()
# Get back IMU data
imu_data = robot.get_imu_data(SensorType.BACK_IMU)
if not imu_data:
print("No imu data!")
else:
print("get imu data suceess")
try:
camera_info = robot.get_camera_intrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not camera_info:
print("No camera info!")
else:
print(camera_info)
except Exception as e:
camera_info = {
"width": 1280,
"height": 720,
"distortion_model": "plumb_bob",
"camera_type": "D405",
"K": [653.4349365234375, 0.0, 639.95159912109375,
0.0, 652.48858642578125, 365.29425048828125,
0.0, 0.0, 1.0],
}
# Convert depth map and RGB image to point cloud and save
if depth_data and rgb_image_data:
points, colors = depth_rgb_to_pointcloud(
depth_img_raw,
img,
fx=camera_info['K'][0],
fy=camera_info['K'][4],
cx=camera_info['K'][2],
cy=camera_info['K'][5],
depth_scale=0.1 # If depth is in mm, set to 0.001
)
save_xyz_to_pcd(points, "left_arm_camera_pointcloud.pcd", binary=True)
print(f"RGB fused depth map point cloud saved to left_arm_camera_pointcloud.pcd, number of points: {points.shape[0]}")
except Exception as e:
print(f"❌ Exception occurred: {e}")
finally:
# Actively send SIGINT exit signal
robot.request_shutdown()
# Wait to enter shutdown state
robot.wait_for_shutdown()
# Release SDK resources
robot.destroy()
print('Resource release successful')
if __name__=="__main__":
main()
示例 5：执行 VLA
适用场景：将模拟 VLA 输出转换为安全、协调的机器人关节指令。
examples/python/tutorials/example5_execute_vla.py
"""
Note: When running this example, please ensure the robot's `emergency stop button` is released;
"""
import time
import os
from typing import List, Dict, Any
try:
import galbot_sdk.s1 as gm
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import GalbotMotion
from galbot_sdk.s1 import GalbotNavigation
from galbot_sdk.s1 import (
SensorType, RgbOutputFormat, S1JointGroup, ControlStatus,
Trajectory, TrajectoryPoint, JointCommand
)
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
exit(1)
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
def decode_compressed_image(compressed_image: Dict[str, Any]) -> np.ndarray:
"""
Decode CompressedImage image.
Parameters:
compressed_image (dict): image dict, keys: [header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"] == "jpeg":
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(image_data)
else:
raise ValueError(f"Unsupported data format: {compressed_image['format']}")
def decode_rgb_image(image_data: bytes) -> np.ndarray:
"""
Decode RGB image.
Parameters:
image_data (bytes): Raw image data
Returns:
numpy.ndarray: Decoded RGB image
"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Failed to decode RGB Image")
return img
def decode_depth_image(image_data: Dict[str, Any]) -> np.ndarray:
"""
Decode depth image.
Parameters:
image_data (dict): Depth image data with metadata
Returns:
numpy.ndarray: Decoded depth image
"""
depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()
# Check if height and width exist
if "height" not in image_data or "width" not in image_data:
raise ValueError("Missing 'height' or 'width' in depth image metadata.")
if image_data["height"] == 0 or image_data["width"] == 0:
raise ValueError(
f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata."
)
# Parse depth image
depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]
return depth_img
def fake_vla(rgb_data_dict: dict) -> Dict[str, np.ndarray]:
"""
Fake VLA (Vision-Language-Action) model implementation.
This function is a placeholder. In a real-world scenario, you would implement
target detection using computer vision techniques. For this example, we assume
a default pose.
Parameters:
rgb_data_dict (dict): Dictionary containing RGB image data from various sensors
Returns:
dict: Trajectories for different robot components (legs, head, arms)
"""
print("Fake VLA executing...")
num_points = 200
# S1 zero pose (aligned with C++ examples and GalbotMotionS1::move_whole_body_joint_zero)
torso_zero = np.array([1.1], dtype=np.float64)
head_zero = np.array([0.0, -0.26], dtype=np.float64)
left_arm_zero = np.array([-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0], dtype=np.float64)
right_arm_zero = np.array([0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0], dtype=np.float64)
# Torso: Move linearly from 1.1 to 0.7
torso_traj = np.linspace([1.1], [0.7], num=num_points, dtype=np.float64)
# Head: Apply small sinusoidal motion on the pitch joint
phase = np.linspace(0.0, np.pi, num=num_points, dtype=np.float64)
head_traj = np.repeat(head_zero.reshape(1, -1), num_points, axis=0)
head_traj[:, 1] = head_zero[1] + 0.1 * np.sin(phase)
# Dual arms: Start from zero pose and apply small linear changes only on joint 4
left_delta = np.zeros_like(left_arm_zero)
right_delta = np.zeros_like(right_arm_zero)
left_delta[3] = 0.2
right_delta[3] = -0.2
s = np.linspace(0.0, 1.0, num=num_points, dtype=np.float64).reshape(-1, 1)
left_arm_traj = left_arm_zero.reshape(1, -1) + s * left_delta.reshape(1, -1)
right_arm_traj = right_arm_zero.reshape(1, -1) + s * right_delta.reshape(1, -1)
return {
"torso": torso_traj,
"head": head_traj,
"left_arm": left_arm_traj,
"right_arm": right_arm_traj,
}
def estimate_vla(robot: 'GalbotRobot', enable_sensor_set: set) -> Dict[str, np.ndarray]:
"""
Estimate VLA (Vision-Language-Action) model outputs based on sensor data.
Parameters:
robot (GalbotRobot): GalbotRobot instance
enable_sensor_set (set): Set of enabled sensor types
Returns:
dict: Joint positions for each joint group
"""
# Get RGB images from enabled cameras
rgb_data_dict = {}
for sensor_type in enable_sensor_set:
if sensor_type in [
SensorType.LEFT_ARM_CAMERA,
SensorType.RIGHT_ARM_CAMERA
]:
# Get RGB data of sensor type
rgb_data = robot.get_rgb_data(sensor_type, RgbOutputFormat.JPEG, True)
time.sleep(1)
# Decode RGB image
if rgb_data:
rgb_image = decode_compressed_image(rgb_data)
rgb_data_dict[sensor_type] = rgb_image
else:
print(f"Failed to get RGB data from {sensor_type}")
else:
print(f"Unsupported sensor type: {sensor_type}")
print("RGB data dictionary keys:", rgb_data_dict.keys())
# Get joint position list
joint_positions = fake_vla(rgb_data_dict)
return joint_positions
def generate_target_point(q: List[float], target_time: float = 10.0) -> 'TrajectoryPoint':
"""
Generate target points for trajectory execution.
Parameters:
q (List[float]): Joint positions
target_time (float): Time from start in seconds (default: 10.0)
Returns:
TrajectoryPoint: Target trajectory point with specified joint commands
"""
joint_position = TrajectoryPoint()
joint_position.time_from_start_second = target_time
joint_command_vec = []
for joint in q:
joint_cmd = JointCommand()
joint_cmd.position = joint
joint_command_vec.append(joint_cmd)
joint_position.joint_command_vec = joint_command_vec
return joint_position
def generate_target_trajectory(
trajectory: np.ndarray,
joint_groups: List[str] = None,
joint_names: List[str] = None,
dt: float = 0.008
) -> 'Trajectory':
"""
Generate trajectory for joints.
Parameters:
trajectory (np.ndarray): 2D array of joint positions over time
joint_groups (List[str]): List of joint groups
joint_names (List[str]): List of joint names
dt (float): Time step between trajectory points (default: 0.008)
Returns:
Trajectory: Generated trajectory for execution
"""
if joint_groups is None:
joint_groups = []
if joint_names is None:
joint_names = []
if trajectory is None or np.ndim(trajectory) != 2 or len(trajectory) == 0:
return None
# Create Trajectory
traj = Trajectory()
traj.joint_groups = joint_groups
traj.joint_names = joint_names
current_time = 0.0
points = []
for state in trajectory:
current_time += dt
# Generate target point for each joint
traj_point = generate_target_point(state, current_time)
points.append(traj_point)
traj.points = points
return traj
def print_joint_states(joint_states: List['JointState']) -> None:
"""
Print joint states in a readable format.
Parameters:
joint_states (List[JointState]): List of joint states
"""
for js in joint_states:
print(
f" : position = {js.position} , velocity = {js.velocity} "
f", acceleration = {js.acceleration} , effort = {js.effort} , current = {js.current}"
)
def check_robot_safety() -> None:
"""
Check if the robot is safe to operate.
Prompts the user to confirm safety conditions before proceeding.
"""
# Prompt for precautions
print(
"⚠️ Note: 1. Please ensure the robot's emergency stop button is released; "
"2. Please ensure there are no obstacles in front, back, left, and right "
"of the robot to avoid unexpected situations."
)
while True:
key = input(
"Please confirm that the robot's emergency stop button is released "
"and there are no obstacles. Continue? (y/n)..."
).lower()
if key == 'y':
print("User confirmed, continuing execution...")
break
elif key == 'n':
print("User not confirmed, program exiting...")
exit(1)
else:
print("Input error, please enter 'y' or 'n'")
def main() -> None:
"""
Main function to execute the VLA (Vision-Language-Action) example.
Initializes the robot, estimates actions based on sensor data, and executes trajectories.
"""
check_robot_safety()
robot = None # Initialize robot variable for cleanup in finally block
try:
# Get robot instances
robot = GalbotRobot()
motion = GalbotMotion()
navi = GalbotNavigation()
# Enable required sensors - S1 only has arm cameras, not head cameras
enable_sensor_set = {
SensorType.LEFT_ARM_CAMERA,
SensorType.RIGHT_ARM_CAMERA
}
# Initialize robot components
if not robot.init(enable_sensor_set):
print("Robot initialization failed")
exit(1)
else:
print("Robot initialization successful")
if not motion.init():
print("Motion initialization failed")
exit(1)
else:
print("Motion initialization successful")
if not navi.init():
print("Navigation initialization failed")
exit(1)
else:
print("Navigation initialization successful")
# Wait for data preparation
time.sleep(3)
# Estimate VLA actions
joint_positions = estimate_vla(robot, enable_sensor_set)
# Generate target trajectory with fixed S1 order
joint_groups = ["torso", "head", "left_arm", "right_arm"]
joint_traj = np.concatenate([joint_positions[group] for group in joint_groups], axis=-1)
# Final joint position check state
whole_body_joint = joint_traj[-1]
base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
check_state = gm.RobotStates()
check_state.whole_body_joint = whole_body_joint
check_state.base_state = base_state
print(f"✅ Final joint position check state: {whole_body_joint}")
# Check collision
status, collision_res = motion.check_collision(
start=[check_state],
enable_collision_check=True
)
time.sleep(1)
if status == gm.MotionStatus.SUCCESS:
print(f"✅ OK: collision check finished: {collision_res} (False=no collision)")
# Get joint states before execution
before_joint_states = robot.get_joint_states(joint_groups, [])
print(f"Joint groups to execute: {joint_groups}")
print(f"Joint states size before execution: {len(before_joint_states)}")
print_joint_states(before_joint_states)
# Execute trajectory
trajectory = generate_target_trajectory(joint_traj, joint_groups, [])
if trajectory is not None:
status = robot.execute_joint_trajectory(trajectory, True)
time.sleep(1)
if status == ControlStatus.SUCCESS:
print("✅ Joint trajectory execution successful.")
else:
print("❌ Joint trajectory execution failed.")
print(f"Diagnostic: target points = {len(trajectory.points)}, joint dimension = {len(whole_body_joint)}")
# Check joint state
joint_states = robot.get_joint_states(joint_groups, [])
print_joint_states(joint_states)
# Compute position error after execution
error_norm_after = sum(abs(whole_body_joint[i] - joint_states[i].position) for i in range(len(whole_body_joint)))
print(f"Absolute position error norm after execution: {error_norm_after}")
print(f"✅ Final joint position check state after execution")
else:
print("❌ Generated trajectory is invalid, cannot execute.")
else:
print("❌ Collision check failed, will not execute the joint trajectory.")
except Exception as e:
print(f"An exception occurred: {e}")
import traceback
traceback.print_exc()
finally:
# Ensure proper cleanup
if robot is not None:
# Actively send SIGINT shutdown signal
robot.request_shutdown()
# Wait to enter shutdown state
robot.wait_for_shutdown()
# Release SDK resources
robot.destroy()
print('Resource release successful')
if __name__ == "__main__":
main()
示例 6：综合任务：导航+感知+抓取+放置
适用场景：将导航、感知、抓取和放置组合为完整自主任务。
examples/python/tutorials/example6_pick_and_place.py
"""
Note: When running this example, please confirm that the robot's motion control service `/data/galbot/bin/service_motion_plan`,
robot state publishing service `/data/galbot/bin/robot_state_publish`,
navigation service `/data/galbot/bin/service_navigation_plan`
and hand-eye calibration publishing service `/data/galbot/bin/eyehand_calib_publish` have been loaded;
"""
try:
import galbot_sdk.s1 as gm
from galbot_sdk.s1 import GalbotNavigation
from galbot_sdk.s1 import GalbotRobot
from galbot_sdk.s1 import GalbotMotion
from galbot_sdk.s1 import S1JointGroup, SensorType, RgbOutputFormat, ControlStatus
except ImportError:
print("Import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
import os
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
try:
from scipy.spatial.transform import Rotation as R
except ImportError:
os.system("pip install scipy")
from scipy.spatial.transform import Rotation as R
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
import time
from typing import Sequence
# mode: chassisnavigation(C++ example10 kDemoMockNavigationOnly)
DEMO_MOCK_NAVIGATION_ONLY = True
# Query concrete joint names at runtime so the example works with both S1 V1
# (torso_base_joint) and S1 V2 (torso_lift_joint1).
S1_BODY_JOINT_GROUPS_FOR_SNAPSHOT = ["torso", "head", "left_arm", "right_arm"]
S1_BODY_JOINT_COUNT = 17
def mock_navigation_log(step_description: str) -> None:
print(f"[Mock navigation] {step_description} (navigate_to_goal not called, chassis does not move)")
def decode_compressed_image(compressed_image, camera_info={}):
"""
decode CompressedImage image
Parameters:
compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"] == "jpeg":
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(image_data, compressed_image["depth_scale"], camera_info)
else:
raise ValueError(f"Unsupport data format: {compressed_image['format']}")
def decode_rgb_image(image_data):
"""decode rgb image"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Fail to Decode RGB Image")
return img
def decode_depth_image(image_data, depth_scale, camera_info):
"""decode depth image"""
depth_img = np.frombuffer(image_data, dtype=np.uint16).copy()
if not camera_info:
depth_img = depth_img.reshape((720, 1280))
else:
depth_img = depth_img.reshape((camera_info["height"], camera_info["width"]))
depth_img = depth_img.astype(np.float32) / depth_scale
return depth_img
def print_gripper_state(joint_group, gripper_state):
"""
Print gripper state
Parameters:
joint_group (S1JointGroup): S1JointGroup enum
gripper_state (object): Contains timestamp_ns, width, velocity, effort, is_moving
"""
print(f"Timestamp (ns): {gripper_state.timestamp_ns}")
print(
f"width {gripper_state.width} "
f"velocity {gripper_state.velocity} "
f"effort {gripper_state.effort} "
f"is moving {gripper_state.is_moving}"
)
def get_navigation_pose(object_goal_pose: Sequence[float], motion: GalbotMotion, arm: str = "left_arm"):
"""
Get navigation target pose
Parameters:
object_goal_pose (Sequence[float]): Target pose [x, y, z, qx, qy, qz, qw]
motion (GalbotMotion): Motion control instance
arm (str, optional): End effector name. Defaults to "left_arm".
Returns:
Sequence[float]: Navigation target pose [x, y, z, qx, qy, qz, qw]
"""
assert arm in ["left_arm", "right_arm"], "arm must be left_arm or right_arm"
try:
status, ee_pose_in_base = motion.get_end_effector_pose(
end_effector_frame=f"{arm}_end_effector_mount_link",
reference_frame="base_link"
)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to get end effector pose: status={status}")
offset_y = ee_pose_in_base[1]
else:
print(f"✅ Successfully got end effector pose: pose={ee_pose_in_base}")
offset_y = 0.3
# Set chassis pose to the same z coordinate as the target pose
base_goal_pose_mat = np.eye(4)
base_goal_pose_mat[:3, :3] = R.from_quat(object_goal_pose[3:]).as_matrix()
base_goal_pose_mat[:3, 3] = np.array([object_goal_pose[0], object_goal_pose[1], 0])
# According to the relative position of the chassis and camera, move the camera navigation target 0.6m backward in the local coordinate to leave observation space for the camera
base_goal_pose_mat = base_goal_pose_mat @ np.array([[1,0,0,-0.6],[0,1,0,-offset_y],[0,0,1,0],[0,0,0,1]])
base_goal_pose_quat = R.from_matrix(base_goal_pose_mat[:3, :3]).as_quat()
base_goal_pose_pos = base_goal_pose_mat[:3, 3]
return base_goal_pose_pos.tolist() + base_goal_pose_quat.tolist()
except Exception as e:
print("Failed to get navigation target pose:", e)
return [1, -1, 0, 0, 0, 0, 1]
def navigation_to_goal(nav: GalbotNavigation, goal_pose: Sequence[float], retry_cnt: int = 3):
"""
Navigate to target pose
Parameters:
nav (GalbotNavigation): Navigation instance
goal_pose (Sequence[float]): Target pose [x, y, z, qx, qy, qz, qw]
retry_cnt (int, optional): Number of retries. Defaults to 3.
"""
if DEMO_MOCK_NAVIGATION_ONLY:
mock_navigation_log("navigate_to_goal skipped")
return
try:
cur_pose = nav.get_current_pose()
print(f"Current pose: {cur_pose}")
if nav.check_path_reachability(goal_pose, cur_pose):
retry_cnt = 3
while True:
status = nav.navigate_to_goal(goal_pose, enable_collision_check=True, is_blocking=True, timeout=20)
time.sleep(0.5)
retry_cnt -= 1
if nav.check_goal_arrival() or retry_cnt < 0:
break
else:
print(f"Navigation failed: status={status}, retrying: {retry_cnt}")
print("navigate_to_goal return status:", status)
print("Has arrived:", nav.check_goal_arrival())
else:
print("Path unreachable or unsafe")
except Exception as e:
print(f"Exception occurred during navigation: {e}")
def lower_camera_after_joint_zero(
motion: GalbotMotion,
target_chain: str,
reference_frame: str,
downward_delta_m: float = 0.08,
):
"""
C++ lower_camera_after_joint_zero: move_whole_body_joint_zero, base end effector z.
"""
k_min_ee_z_base_link = 0.15
try:
print(
f"[Camera pose] First run move_whole_body_joint_zero (S1 zero pose consistent with SDK), then move downward relative to end effector by Δz={downward_delta_m} m"
)
zero_status = motion.move_whole_body_joint_zero(
True, 0.2, 15.0, gm.Parameter()
)
if zero_status != gm.MotionStatus.SUCCESS:
print(
f"⚠️ move_whole_body_joint_zero not successful: status={zero_status}, still attempting to read end effector and fine-tune..."
)
else:
print("✅ move_whole_body_joint_zero completed successfully")
time.sleep(0.8)
retry_cnt = 3
cur_ee_pose = []
while True:
status, cur_ee_pose = motion.get_end_effector_pose_on_chain(
chain_name=target_chain,
frame_id="EndEffector",
reference_frame=reference_frame,
)
time.sleep(0.5)
retry_cnt -= 1
if status == gm.MotionStatus.SUCCESS:
print("✅ Successfully got end effector pose (after zero)")
break
if retry_cnt < 0:
cur_ee_pose = [0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991]
print("❌ Failed to get end effector pose, using default")
break
print(f"Failed to get end effector pose: status={status}, retrying: {retry_cnt}")
tgt_ee_pose = list(cur_ee_pose)
tgt_ee_pose[2] -= downward_delta_m
if tgt_ee_pose[2] < k_min_ee_z_base_link:
print(
f"⚠️ Target z={tgt_ee_pose[2]} is below safety lower bound {k_min_ee_z_base_link}; clamped"
)
tgt_ee_pose[2] = k_min_ee_z_base_link
print(f"Target end effector pose (after downward fine-tuning): {tgt_ee_pose}")
retry_cnt = 3
while True:
status = motion.set_end_effector_pose(
target_pose=tgt_ee_pose,
end_effector_frame=target_chain,
reference_frame=reference_frame,
enable_collision_check=False,
is_blocking=True,
timeout=5.0,
params=gm.Parameter(),
)
time.sleep(0.5)
retry_cnt -= 1
if status == gm.MotionStatus.SUCCESS:
print(f"✅ Successfully set end effector pose: status={status}")
break
if retry_cnt < 0:
print(f"❌ Failed to set end effector pose after retries: status={status}")
break
print(f"Failed to set end effector pose: status={status}, retrying: {retry_cnt}")
except Exception as e:
print(f"❌ Exception in lower_camera_after_joint_zero: {e}")
def detect_target(img: np.ndarray, depth_img: np.ndarray) -> Sequence[float]:
"""
Detection target function. Input RGB image and depth image, output target pose.
Parameters:
img (np.ndarray): RGB image
depth_img (np.ndarray): Depth image
Returns:
Sequence[float]: Target pose [x, y, z, qx, qy, qz, qw]
"""
try:
############### NOTE ###############
# This function is a placeholder. In a real-world scenario, you would implement
# target detection using computer vision techniques. For this example, we assume
# a default pose.
####################################
# opencv frame: x right, y down, z forward
default_pose = [0.0, 0.20, 0.29, 0.0, 0.71, 0.0, 0.71]
return default_pose
except Exception as e:
print(f"Target detection exception: {e}")
return None
def pose_camera_to_base(robot: GalbotRobot, pose_camera: Sequence[float]) -> Sequence[float]:
"""
Transform camera pose to chassis coordinate system
Parameters:
robot (GalbotRobot): Robot instance
pose_camera (Sequence[float]): Camera pose [x, y, z, qx, qy, qz, qw]
Returns:
Sequence[float]: Chassis pose [x, y, z, qx, qy, qz, qw]
"""
source_frame="left_arm_camera_color_optical_frame"
target_frame="base_link"
base_to_cam = robot.get_transform(target_frame, source_frame)[0]
if base_to_cam is None:
print("Failed to get transform from camera to chassis")
return None
else:
print("base_to_cam: ", base_to_cam)
base_to_cam_mat = np.eye(4)
base_to_cam_mat[:3, :3] = R.from_quat(base_to_cam[3:]).as_matrix()
base_to_cam_mat[:3, 3] = np.array(base_to_cam[:3])
pose_base_mat = base_to_cam_mat[:3, :3] @ np.array(pose_camera[:3]).reshape(3, 1) + base_to_cam_mat[:3, 3:]
return pose_base_mat.flatten()[:3].tolist() + [0, 0, 0, 1]
def detect_object(robot: GalbotRobot, arm: str = "left_arm"):
object_pose_base = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
try:
if arm == "left_arm":
rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
elif arm == "right_arm":
rgb_image_data = robot.get_rgb_data(SensorType.RIGHT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
depth_data = robot.get_depth_data(SensorType.RIGHT_ARM_DEPTH_CAMERA)
else:
raise ValueError("arm must be left_arm or right_arm")
img = None
depth_img = None
if rgb_image_data and rgb_image_data.get("data"):
print("Get rgb image success")
raw = rgb_image_data["data"]
if isinstance(raw, list):
raw = bytes(raw)
img = decode_rgb_image(raw)
else:
print("No rgb image data (sensor may be unavailable)")
if (
depth_data
and depth_data.get("width", 0) > 0
and depth_data.get("height", 0) > 0
and depth_data.get("data")
):
print("Get depth data success")
camera_info = {
"height": depth_data["height"],
"width": depth_data["width"],
}
depth_data_bytes = depth_data["data"]
if isinstance(depth_data_bytes, list):
depth_data_bytes = bytes(depth_data_bytes)
depth_img = decode_depth_image(
depth_data_bytes, depth_data["depth_scale"], camera_info
)
else:
print("No depth data (depth sensor may be unavailable)")
if img is None:
img = np.zeros((480, 640, 3), dtype=np.uint8)
if depth_img is None:
depth_img = np.zeros((480, 640), dtype=np.float32)
object_pose_camera = detect_target(img, depth_img)
if object_pose_camera is None:
print("Target detection failed")
return object_pose_base
print(f"object_pose_camera: {object_pose_camera}")
pose_b = pose_camera_to_base(robot, object_pose_camera)
if pose_b is None:
return object_pose_base
object_pose_base = pose_b
print(f"Target pose in chassis coordinate system: {object_pose_base}")
except Exception as e:
object_pose_base = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
print(f"Target detection exception: {e}")
return object_pose_base
def check_robot_safety():
"""Check if robot is safe"""
# Prompt important notes
print("⚠️ Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations. 3. Please ensure the area around the robot is clear of obstacles.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing...")
break
elif key == 'n':
print("User did not confirm, exiting program...")
exit(1)
else:
print("Invalid input, please enter 'y' or 'n'")
def pick_and_place(
robot: GalbotRobot,
motion: GalbotMotion,
object_pose_base: Sequence[float],
target_chain: str,
reference_frame: str,
):
try:
# Attach tool to end effector
status = motion.attach_tool(chain="left_arm", tool="galbot_gripper")
time.sleep(0.5)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to attach tool: status={status}")
else:
print(f"✅ Successfully attached tool: status={status}")
# Open left gripper
# Set left gripper width to 0.1m, speed to 0.05m, force to 10N, will block until gripper reaches position
status = robot.set_gripper_command(
S1JointGroup.left_gripper, 0.1, 0.05, 10, False
)
time.sleep(0.5)
print(f"✅ Successfully set left gripper width to 0.1m: status={status}")
print("object_pose_base: ", object_pose_base)
# Reach to target position
retry_cnt = 3
while True:
status = motion.set_end_effector_pose(
target_pose=object_pose_base,
end_effector_frame=target_chain,
reference_frame=reference_frame,
enable_collision_check=False,
is_blocking=True,
timeout=5.0,
params=gm.Parameter()
)
time.sleep(1)
retry_cnt -= 1
if status == gm.MotionStatus.SUCCESS or retry_cnt < 0:
break
else:
print(f"Failed to set end effector pose: status={status}, retry count: {retry_cnt}")
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to set end effector pose: status={status}")
else:
print(f"✅ Successfully set end effector pose: status={status}")
# Close gripper to grasp object
status = robot.set_gripper_command(
S1JointGroup.left_gripper, 0.02, 0.05, 10, False
)
time.sleep(0.5)
print(f"✅ Successfully closed left gripper: status={status}")
# Box scale values must all be >0 (consistent with C++)
status = motion.attach_target_object(
"grasped_box",
"box",
[0, 0, 0, 0, 0, 0, 1],
[0.05, 0.05, 0.05],
"",
"left_arm",
"ee_base",
)
time.sleep(0.5)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to attach target object: status={status}")
else:
print(f"✅ Successfully attached target object: status={status}")
mock_navigation_log('After grasping, "return to the starting point"')
time.sleep(0.5)
print("✅ (Mock) Return navigation skipped; chassis not moved")
# Release target
status = robot.set_gripper_command(
S1JointGroup.left_gripper, 0.1, 0.05, 10, False
)
time.sleep(0.5)
print(f"✅ Successfully released left gripper: status={status}")
# Detach target object
status = motion.detach_target_object("grasped_box")
time.sleep(0.5)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to detach target object: status={status}")
else:
print(f"✅ Successfully detached target object: status={status}")
# Detach tool from end effector
status = motion.detach_tool(chain="left_arm")
time.sleep(0.5)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to detach tool: status={status}")
else:
print(f"✅ Successfully detached tool: status={status}")
except Exception as e:
print(f"Exception occurred during pick_and_place: {e}")
return None
def main():
check_robot_safety()
try:
# Get robot instance
robot = GalbotRobot()
# Get GalbotMotion instance
motion = GalbotMotion()
# Get navigation instance
nav = GalbotNavigation()
# Consistent with C++: left-arm RGB + depth only (required for demo grasping)
enable_sensor_set = {
SensorType.LEFT_ARM_CAMERA,
SensorType.LEFT_ARM_DEPTH_CAMERA,
}
# Initialize robot
if robot.init(enable_sensor_set):
print("GalbotRobot initialization successful")
else:
print("GalbotRobot initialization failed")
if motion.init():
print("GalbotMotion initialization successful")
else:
print("GalbotMotion initialization failed")
if nav.init():
print("GalbotNavigation initialization successful")
else:
print("GalbotNavigation initialization failed")
time.sleep(1)
if DEMO_MOCK_NAVIGATION_ONLY:
print(
"[Demo mode] Skipped: relocalize and real navigate_to_goal (chassis remains stationary)."
)
else:
while True:
if nav.is_localized():
print(
"✅ Robot localized successfully, proceeding to navigate to goal..."
)
break
print("❌ Navigation not localized, relocalizing...")
nav.relocalize([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
time.sleep(2)
mock_navigation_log('Before starting the task: "move near the target point"')
snapshot_joint_names = robot.get_joint_names(
True, S1_BODY_JOINT_GROUPS_FOR_SNAPSHOT
)
if len(snapshot_joint_names) != S1_BODY_JOINT_COUNT:
raise RuntimeError(
"Failed to resolve complete S1 body joint names for snapshot: "
f"got {len(snapshot_joint_names)}, expected {S1_BODY_JOINT_COUNT}"
)
joint_groups_positions = robot.get_joint_positions([], snapshot_joint_names)
print(
f"Saved body joint snapshot: {len(joint_groups_positions)} values "
f"(expected {len(snapshot_joint_names)})"
)
if len(joint_groups_positions) != len(snapshot_joint_names):
raise RuntimeError(
"Failed to save complete body joint snapshot: "
f"got {len(joint_groups_positions)} values for "
f"{len(snapshot_joint_names)} joints"
)
print()
lower_camera_after_joint_zero(motion, "left_arm", "base_link", 0.08)
print()
object_pose_base = detect_object(robot, arm="left_arm")
print()
pick_and_place(robot, motion, object_pose_base, "left_arm", "base_link")
print()
retry_cnt = 3
while True:
status = robot.set_joint_positions(
joint_groups_positions,
[],
snapshot_joint_names,
True,
0.5,
20,
)
time.sleep(0.5)
if status == ControlStatus.SUCCESS:
print(f"✅ Successfully set joint positions: status={status}")
break
if retry_cnt < 0:
print(f"❌ Failed to set joint positions after retries, status={status}")
break
retry_cnt -= 1
print(f"Failed to set joint positions: retry count: {retry_cnt}")
except Exception as e:
print(f"Exception occurred: {e}")
finally:
# Actively send SIGINT exit signal
robot.request_shutdown()
# Wait to enter shutdown state
robot.wait_for_shutdown()
# Release SDK resources
robot.destroy()
print('Resource release successful')
if __name__ == "__main__":
main()