Python 示例
本文件为 API 中公开的函数与类型提供简短示例，演示如何使用这些接口。
机器人关节名称（G1 2.2）
关节组列表
机器人关节组名称包括：["head", "left_arm", "right_arm", "leg", "left_gripper", "right_gripper", "left_suction_cup", "right_suction_cup"]
各关节组详细信息
| 关节组名称 | 关节组英文名 | 关节数量 | 关节名称列表 |
|---|---|---|---|
| 头部 | head | 2 | head_joint1, head_joint2 |
| 腿部 | leg | 5 | leg_joint1, leg_joint2, leg_joint3, leg_joint4, leg_joint5 |
| 左臂 | 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 |
| 左吸盘 | left_suction_cup | 1 | left_suction_cup_joint1 |
| 右吸盘 | right_suction_cup | 1 | right_suction_cup_joint1 |
如何正确使用 joint_groups
joint_groups 的设计单位是“关节组（运动链）”，而不是单个关节。这样设计是为了保证：
- 运动链一致性：头/臂/腿等关节按链路整体校验和控制。
- 指令顺序确定性：
joint_groups会按传入顺序展开为具体关节。 - 组级约束校验：执行前会检查关节组的 active/passive 属性及输入合法性。
传参规则：
- 需要控制整条链路时，优先使用
joint_groups（head/arm/leg/gripper/suction cup）。 - 当
joint_names非空时，优先使用joint_names，会覆盖joint_groups。 - 当
joint_groups和joint_names都为空时，SDK 默认控制所有 active body 关节组。 - 建议先调用
get_joint_group_names()和get_joint_names(True, groups)，避免手写字符串出错。
G1 关节组典型场景
| 关节组 | 典型场景 |
|---|---|
head | 头部朝向/相机对准 |
left_arm / right_arm | 机械臂抓取与操作 |
leg | 下肢姿态调整 |
left_gripper / right_gripper | 夹爪开合宽度控制 |
left_suction_cup / right_suction_cup | 吸盘吸放作业 |
传感器类型与 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 |
LEFT_ARM_CAMERA | left_arm_camera_color_optical_frame |
LEFT_ARM_DEPTH_CAMERA | left_arm_camera_color_optical_frame |
RIGHT_ARM_CAMERA | right_arm_camera_color_optical_frame |
RIGHT_ARM_DEPTH_CAMERA | right_arm_camera_color_optical_frame |
BASE_LIDAR | lidar_base_link |
TORSO_IMU | imu_base_link |
BASE_ULTRASONIC | —（无 TF frame） |
LEFT_FRONT_SURROUND_CAMERA | left_front_surround_color_optical_frame |
RIGHT_FRONT_SURROUND_CAMERA | right_front_surround_color_optical_frame |
LEFT_REAR_SURROUND_CAMERA | left_rear_surround_color_optical_frame |
RIGHT_REAR_SURROUND_CAMERA | right_rear_surround_color_optical_frame |
注意：调用 get_frame_names() 可获取当前所有可用的坐标系名称。
类：GalbotRobot
tips：程序启动后立刻就get，数据可能不会立刻就绪，可适当sleep几秒
获取实例并初始化（get_instance && init）
适用场景：程序启动阶段，获取机器人单例并完成 SDK 初始化。必须在调用其他 API 前执行。
examples/python/galbot_robot/get_instance.py
from galbot_sdk.g1 import GalbotRobot
import time
# Get GalbotRobot
robot = GalbotRobot()
state = robot.init()
if not state:
print("Initialization failed")
else:
print("Initialization succeeded")
while robot.is_running():
# business logic
time.sleep(1)
break
# Send exit signal to exit the program
robot.request_shutdown()
# Wait for exit state
robot.wait_for_shutdown()
# Release SDK-related resources
robot.destroy()
日志函数
适用场景：使用 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.g1 import GalbotRobot
from galbot_sdk.g1 import ControlStatus
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
print('Initialization succeeded')
# Program started, waiting for data
time.sleep(2)
# Set head joints to 0.2, 0.2, block and wait for motion to complete, max timeout 10s
joint_pos = [0.2, 0.2]
# Set head joint group; if empty, defaults to whole body joints ["leg", "head", "left_arm", "right_arm"]
joint_groups = ["head"]
# Whether to block until joints reach target
is_blocking = True
# Limit joint max speed to 0.1rad/s
max_speed = 0.1
# Maximum blocking wait time
timeout_s = 10
status = robot.set_joint_positions(
joint_pos, joint_groups, [], is_blocking, max_speed, timeout_s
)
if status != ControlStatus.SUCCESS:
print("Joint angle setting failed")
else:
print('Joint angle setting succeeded')
time.sleep(1)
# Use specific joint names for control; this parameter overrides joint_groups
joint_names = ["head_joint1", "head_joint2"]
joint_pos = [0.0, 0.0]
status = robot.set_joint_positions(
joint_pos, [], joint_names, is_blocking, max_speed, timeout_s
)
if status != ControlStatus.SUCCESS:
print("Joint angle setting failed")
else:
print('Joint angle setting succeeded')
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
设置夹爪指令（set_gripper_command）
适用场景：控制夹爪开合。支持位置控制和力矩控制两种模式。
examples/python/galbot_robot/set_gripper_command.py
import time
from galbot_sdk.g1 import GalbotRobot, G1JointGroup, ControlStatus
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
print('Initialization succeeded')
# Program started, waiting for data
time.sleep(2)
# Set left gripper width to 0.02m, speed 0.05m, torque 10N, block until gripper reaches position
status = robot.set_gripper_command(
G1JointGroup.left_gripper, 0.02, 0.05, 10, True
)
if status != ControlStatus.SUCCESS:
print("Set gripper failed")
else:
print('Set gripper success')
# Set left gripper width to 0.1m, speed 0.05m, torque 10N, block until gripper reaches position
status = robot.set_gripper_command(
G1JointGroup.left_gripper, 0.1, 0.05, 10, True
)
if status != ControlStatus.SUCCESS:
print("Set gripper failed")
else:
print('Set gripper success')
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
设置吸盘指令（set_suction_cup_command）
适用场景：控制吸盘吸放物体，获取吸盘当前的开启/关闭状态。
examples/python/galbot_robot/set_suction_cup_command.py
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import G1JointGroup, ControlStatus
import time
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
time.sleep(1)
print('Initialization succeeded')
# Set suction cup joint group (right suction cup)
joint_group = G1JointGroup.right_suction_cup
# Whether to activate suction cup
activate = True # True: activate suction cup, False: deactivate suction cup
# Send suction cup control command
status = robot.set_suction_cup_command(
joint_group,
activate
)
# Check execution results
if status != ControlStatus.SUCCESS:
print("Set suction cup failed")
else:
print("Set suction cup succeeded")
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
停止轨迹执行（stop_trajectory_execution）
适用场景：停止当前正在执行的关节轨迹，中断正在进行的运动。
examples/python/galbot_robot/stop_trajectory_execution.py
import signal
import threading
import time
from galbot_sdk.g1 import (
ControlStatus,
GalbotRobot,
JointCommand,
Trajectory,
TrajectoryPoint,
)
# Set when the trajectory call has returned, so the worker stops waiting for a
# Ctrl-C that is never coming.
trajectory_done = threading.Event()
# Set once the worker has requested shutdown, so the main flow does not repeat it.
shutdown_requested = threading.Event()
def make_head_trajectory():
trajectory = Trajectory()
trajectory.joint_names = ["head_joint2"]
point = TrajectoryPoint()
point.time_from_start_second = 10.0
command = JointCommand()
command.position = 0.4
point.joint_command_vec = [command]
trajectory.points = [point]
return trajectory
def stop_trajectory(robot):
print("Worker thread: stopping trajectory execution...")
status = robot.stop_trajectory_execution()
if status == ControlStatus.SUCCESS:
print("Worker thread: trajectory stop succeeded")
else:
print(f"Worker thread: trajectory stop FAILED: {status}")
shutdown_requested.set()
def stop_trajectory_on_ctrl_c(robot):
"""Call stop from a worker while the main thread waits for the trajectory."""
# sigwait() cannot be cancelled, so wait in slices: when the trajectory
# finishes on its own no Ctrl-C ever arrives, and the worker still has to
# exit or the join() in the main flow would block forever.
while not trajectory_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is None:
continue
stop_trajectory(robot)
return
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the area is clear.")
if input("Start trajectory execution? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(2)
print("Initialization succeeded")
# execute_joint_trajectory blocks in the main thread. Ctrl-C is consumed by
# the worker, which stops the motion and then shuts the SDK down.
print("Trajectory is executing. Press Ctrl-C to stop it.")
stop_thread = threading.Thread(target=stop_trajectory_on_ctrl_c, args=(robot,))
stop_thread.start()
status = robot.execute_joint_trajectory(make_head_trajectory(), is_blocking=True)
trajectory_done.set()
print(f"Trajectory execution returned: {status}")
stop_thread.join()
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")
设置轨迹（execute_joint_trajectory）
适用场景：执行一条预定义的关节空间轨迹，包含多个路点。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.g1 import GalbotRobot
from galbot_sdk.g1 import G1JointGroup, ControlStatus, Trajectory, TrajectoryPoint, JointCommand
import time
import numpy as np
from typing import List
# Generate trajectory point with position and time info
def generate_target_point(q: List[float], target_time: float = 10):
"""Generate target for joints"""
joint_position = TrajectoryPoint()
joint_position.time_from_start_second = target_time
joint_command_vec = []
for joint in q:
joint_cmd = JointCommand()
joint_cmd.position = joint
joint_command_vec.append(joint_cmd)
joint_position.joint_command_vec = joint_command_vec
return joint_position
def generate_target_trajectory(trajectory, joint_groups=[], joint_names=[], dt=0.008):
"""Generate trajectory for joints"""
if trajectory is None or np.ndim(trajectory) != 2 or len(trajectory) == 0:
return None
# Create Trajectory
traj = Trajectory()
traj.joint_groups = joint_groups
traj.joint_names = joint_names
time = 0.0
points = []
for state in trajectory:
time += dt
# Create single trajectory point
traj_point = generate_target_point(state, time)
points.append(traj_point)
traj.points = points
return traj
# Trajectory execution function
def traj_exec():
# Get GalbotRobot singleton and initialize; only needs to be initialized once
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")
head_traj = np.linspace(
[0.0, 0.0],
[0.5, 0.0],
num=200,
)
# Whether to block and wait for trajectory execution to complete
is_block = True
# Specify which joint group trajectory to execute
joint_groups = ["head"]
# Execute specified joint trajectory; if provided, overrides joint_groups parameters
joint_names = []
status = robot.execute_joint_trajectory(generate_target_trajectory(head_traj.tolist(), joint_groups, joint_names), is_block)
# Check execution results
if status != ControlStatus.SUCCESS:
print("Trajectory execution failed")
else:
print("Trajectory execution succeeded")
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
traj_exec()
设置关节指令 (set_joint_commands)
适用场景：高频关节控制，例如模型推理输出（每步推理输出一帧关节指令）、自定义轨迹跟踪。直接下发关节指令给底层控制器，不做额外插值。
examples/python/galbot_robot/set_joint_commands_example.py
import time
import math
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import JointCommand
def head_high_frequency_control():
"""
Head high-frequency control example
"""
control_frequency = 100.0 # Hz
dt = 1.0 / control_frequency
duration = 4.0 # Control for 4 seconds
amplitude = 0.3 # Maximum oscillation amplitude (rad)
frequency = 0.5 # Sine frequency (Hz)
# Joint group name to control
joint_groups = ["head"]
# Fill this field to control specific joints, which will override joint_groups. If empty, controls all joints in joint_groups by default
joint_names = []
print("Start high-frequency head control")
joint_commands = [JointCommand(), JointCommand()]
start_time = time.time()
while True:
current_time = time.time() - start_time
if current_time > duration:
break
# Generate sine trajectory
target_position = amplitude * math.sin(
2 * math.pi * frequency * current_time
)
# Set head joint angles
joint_commands[0].position = target_position
joint_commands[1].position = target_position
print(f"current: {current_time:.2f}s, target: {target_position:.3f} rad")
# Expected arrival time
time_from_start_sec = 0.0
execution_status = GalbotRobot().set_joint_commands(
joint_commands,
joint_groups,
joint_names,
time_from_start_sec
)
# Sleep at a fixed interval
time.sleep(dt)
print("end")
def main():
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
if robot.init():
print("System initialized successfully!")
else:
print("System initialization failed!")
return
# Program started, waiting for data
time.sleep(2)
head_high_frequency_control()
# Exit system and release SDK resources
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Program exited")
if __name__ == "__main__":
main()
批量设置关节指令（set_joint_commands_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.g1 import GalbotRobot, G1JointGroup, ControlStatus, Trajectory, TrajectoryPoint, JointCommand
import time
import numpy as np
from typing import List
# Generate trajectory point with position and time info
def generate_target_point(q: List[float], target_time: float = 10):
"""Generate target for joints"""
joint_position = TrajectoryPoint()
joint_position.time_from_start_second = target_time
joint_command_vec = []
for joint in q:
joint_cmd = JointCommand()
joint_cmd.position = joint
joint_cmd.velocity = 0.0
joint_cmd.acceleration = 0.0
joint_cmd.effort = 0.0
# joint_cmd.Kp = 0.0
# joint_cmd.Kd = 0.0
joint_command_vec.append(joint_cmd)
joint_position.joint_command_vec = joint_command_vec
return joint_position
def generate_batch_trajectory(trajectory, joint_groups=[], joint_names=[], dt=0.008):
"""Generate batch trajectory for joints"""
if trajectory is None or np.ndim(trajectory) != 2 or len(trajectory) == 0:
return None
# Create Trajectory
traj = Trajectory()
traj.joint_groups = joint_groups
traj.joint_names = joint_names
time = 0.0
points = []
for state in trajectory:
time += dt
# Create single trajectory point
traj_point = generate_target_point(state, time)
points.append(traj_point)
traj.points = points
return traj
# Batch set-joint-command function
def batch_commands_exec():
# Get GalbotRobot singleton and initialize; only needs to be initialized once
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")
# Generate batched trajectory data (joint commands at multiple time points)
head_traj = np.linspace(
[0.0, 0.0],
[0.5, 0.0],
num=10, # Number of batch trajectory points
)
# Specify which joint group trajectory to execute
joint_groups = ["head"]
# Execute specified joint trajectory; if provided, overrides joint_groups parameters
joint_names = []
# Batch set joint commands (non-blocking, returns immediately)
status = robot.set_joint_commands_batch(generate_batch_trajectory(head_traj.tolist(), joint_groups, joint_names))
# Check execution results
if status != ControlStatus.SUCCESS:
print("Batch command submission failed")
else:
print("Batch commands submitted, executing in background (non-blocking)")
# Wait for a while to let the command execute
time.sleep(1)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
batch_commands_exec()
直接发布原始目标（publish_target）
适用场景：高级用户直接构造 SingoriXTarget 并通过 publish 通道下发到底层 WBCS，适合高频原始 target 发送与 joint/task mixed target 一次性 dispatch。
examples/python/galbot_robot/publish_target_example.py
"""
G1 PublishTarget menu example for SDK mirror SingoriXTarget.
This example shows how to construct `SingoriXTarget` directly at the Python SDK
layer and send it to the low-level WBCS through `publish_target()`.
1. Joint-space commands are written into `target_group_trajectory_map`
2. Chassis pose / twist style task-space commands are written into `target_task_trajectory_map`
3. One `SingoriXTarget` can contain both joint trajectory and task trajectory
4. The current SDK does not automatically switch the chassis controller, so
pose / twist / mixed scenes explicitly call `switch_controller(...)`
5. The base twist scene automatically sends a zero-twist target after the
configured duration to stop the chassis
"""
import math
import time
from galbot_sdk.g1 import (
ControlStatus,
G1ControllerName,
G1JointGroup,
GalbotRobot,
GroupCommand,
JointCommand,
Pose,
SingoriXTarget,
TargetConfig,
TargetGroupTrajectory,
TargetSampling,
TargetTaskTrajectory,
TaskCommand,
Twist,
Vector3,
FrameTriad,
TARGET_DATA_DEFAULT,
TARGET_DATA_FRAME_POSE,
TARGET_DATA_FRAME_TWIST,
TARGET_TYPE_DEFAULT,
TARGET_TYPE_OVERRIDE,
TARGET_TYPE_PROVERRIDE,
)
CHASSIS_TASK_NAME = "chassis"
CHASSIS_SUBTASK_POSE = "chassis_pose"
CHASSIS_SUBTASK_TWIST = "chassis_twist"
def now_ns():
return time.time_ns()
def status_to_string(status):
return getattr(status, "name", str(status))
def yaw_to_quaternion(yaw):
return [0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5)]
def make_empty_target():
target = SingoriXTarget()
target.header.timestamp_ns = now_ns()
target.header.frame_id = "base_link"
return target
def make_group_target_config():
config = TargetConfig()
config.target_data = TARGET_DATA_DEFAULT
config.target_type = TARGET_TYPE_PROVERRIDE
config.target_sampling = TargetSampling.TARGET_SAMPLING_DEFAULT
config.target_priority = 1
return config
def make_pose_target_config():
config = TargetConfig()
config.target_data = TARGET_DATA_FRAME_POSE
# config.target_type = TARGET_TYPE_PROVERRIDE
config.target_type = TARGET_TYPE_OVERRIDE
config.target_sampling = TargetSampling.TARGET_SAMPLING_LINEAR_INTERPOLATE
config.target_priority = 1
return config
def make_twist_target_config():
config = TargetConfig()
config.target_data = TARGET_DATA_FRAME_TWIST
config.target_type = TARGET_TYPE_OVERRIDE
config.target_sampling = TargetSampling.TARGET_SAMPLING_DIRECT_PASS
config.target_priority = 1
return config
def make_joint_command(position):
command = JointCommand()
command.position = position
command.velocity = 0.0
command.acceleration = 0.0
command.effort = 0.0
return command
def make_vector3(x, y, z):
vec = Vector3()
vec.x = x
vec.y = y
vec.z = z
return vec
def build_joint_target(group_name, joint_names, positions, time_from_start_s):
target = make_empty_target()
group_traj = TargetGroupTrajectory()
group_traj.target_config = make_group_target_config()
group_traj.joint_names = list(joint_names)
command = GroupCommand()
command.time_from_start_s = time_from_start_s
command.joint_commands = [make_joint_command(position) for position in positions]
group_traj.group_commands = [command]
target.target_group_trajectory_map = {group_name: group_traj}
return target
def build_chassis_pose_target(x, y, yaw, time_from_start_s, frame_id="rel(0)", reference_frame_id="odom"):
target = make_empty_target()
task_traj = TargetTaskTrajectory()
task_traj.target_config = make_pose_target_config()
task_traj.group_names = [G1JointGroup.chassis]
task_traj.subtask_names = [f"{CHASSIS_SUBTASK_POSE}_{now_ns()}"]
triad = FrameTriad()
triad.header.timestamp_ns = now_ns()
triad.header.frame_id = frame_id
triad.body_frame_id = "base_link"
triad.reference_frame_id = reference_frame_id
triad.pose = Pose([x, y, 0.0], yaw_to_quaternion(yaw))
command = TaskCommand()
command.time_from_start_s = time_from_start_s
command.subtask_commands = [triad]
task_traj.task_commands = [command]
target.target_task_trajectory_map = {CHASSIS_TASK_NAME: task_traj}
return target
def build_chassis_twist_target(vx, vy, wz, time_from_start_s):
target = make_empty_target()
task_traj = TargetTaskTrajectory()
task_traj.target_config = make_twist_target_config()
task_traj.group_names = [G1JointGroup.chassis]
task_traj.subtask_names = [f"{CHASSIS_SUBTASK_TWIST}_{now_ns()}"]
twist = Twist()
twist.linear = make_vector3(vx, vy, 0.0)
twist.angular = make_vector3(0.0, 0.0, wz)
triad = FrameTriad()
triad.header.timestamp_ns = now_ns()
triad.header.frame_id = "base_link"
triad.body_frame_id = "base_link"
triad.reference_frame_id = "base_link"
triad.twist = twist
command = TaskCommand()
command.time_from_start_s = time_from_start_s
command.subtask_commands = [triad]
task_traj.task_commands = [command]
target.target_task_trajectory_map = {CHASSIS_TASK_NAME: task_traj}
return target
def build_stop_twist_target():
return build_chassis_twist_target(0.0, 0.0, 0.0, 0.1)
def merge_targets(targets):
merged = make_empty_target()
group_map = {}
task_map = {}
for target in targets:
group_map.update(target.target_group_trajectory_map)
task_map.update(target.target_task_trajectory_map)
merged.target_group_trajectory_map = group_map
merged.target_task_trajectory_map = task_map
return merged
def ensure_controller(robot, controller_name):
status = robot.switch_controller(controller_name)
print(f"switch_controller({controller_name}): {status_to_string(status)}")
return status
def run_twist_scene(robot, scene_name, target, twist_duration_s):
print(f"{scene_name}: start moving for {twist_duration_s} seconds")
motion_status = robot.publish_target(target)
print(f"{scene_name} publish_target status: {status_to_string(motion_status)}")
if motion_status != ControlStatus.SUCCESS:
return motion_status
time.sleep(twist_duration_s)
print(f"{scene_name}: send stop twist target")
stop_status = robot.publish_target(build_stop_twist_target())
print(f"{scene_name} stop status: {status_to_string(stop_status)}")
return stop_status
def print_menu():
print(
"\nAvailable commands:\n"
" joint - publish a joint-only head target\n"
" base_pose - publish a chassis pose target\n"
" base_twist - publish a chassis twist target with auto stop\n"
" mixed_pose - publish head + left_arm + chassis pose in one target\n"
" mixed_twist - publish head + left_arm + chassis twist in one target\n"
" quit - exit example\n"
)
def main():
robot = GalbotRobot()
if not robot.init():
print("robot init failed")
return
time.sleep(2)
head_joint_names = robot.get_joint_names(True, [G1JointGroup.head])
left_arm_joint_names = robot.get_joint_names(True, [G1JointGroup.left_arm])
if not head_joint_names or not left_arm_joint_names:
print("failed to fetch active head/left_arm joints")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
head_single_joint = [head_joint_names[0]]
arm_single_joint = [left_arm_joint_names[0]]
joint_time_s = 3.0
pose_time_s = 4.0
twist_command_time_s = 0.2
twist_duration_s = 2.0
print_menu()
while True:
command = input("Enter command: ").strip()
if command == "quit":
break
if command == "joint":
target = build_joint_target(G1JointGroup.head, head_single_joint, [0.2], joint_time_s)
status = robot.publish_target(target)
print(f"joint publish_target status: {status_to_string(status)}")
continue
if command == "base_pose":
if ensure_controller(robot, G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
continue
target = build_chassis_pose_target(0.2, 0.0, 0.0, pose_time_s)
status = robot.publish_target(target)
print(f"base_pose publish_target status: {status_to_string(status)}")
continue
if command == "base_twist":
if ensure_controller(robot, G1ControllerName.CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
continue
target = build_chassis_twist_target(0.05, 0.0, 0.0, twist_command_time_s)
run_twist_scene(robot, "base_twist", target, twist_duration_s)
continue
if command == "mixed_pose":
if ensure_controller(robot, G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
continue
target = merge_targets(
[
build_joint_target(G1JointGroup.head, head_single_joint, [0.2], joint_time_s),
build_joint_target(G1JointGroup.left_arm, arm_single_joint, [0.1], joint_time_s),
build_chassis_pose_target(0.1, 0.0, 0.0, pose_time_s),
]
)
status = robot.publish_target(target)
print(f"mixed_pose publish_target status: {status_to_string(status)}")
continue
if command == "mixed_twist":
if ensure_controller(robot, G1ControllerName.CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
continue
target = merge_targets(
[
build_joint_target(G1JointGroup.head, head_single_joint, [-0.2], joint_time_s),
build_joint_target(G1JointGroup.left_arm, arm_single_joint, [-0.1], joint_time_s),
build_chassis_twist_target(0.05, 0.0, 0.0, twist_command_time_s),
]
)
run_twist_scene(robot, "mixed_twist", target, twist_duration_s)
continue
print(f"Unknown command: {command}")
print_menu()
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
直接请求原始目标（request_target）
适用场景：高级用户直接构造 SingoriXTarget 并通过 request 通道下发到底层 WBCS，适合需要获取服务端错误返回的 raw target 场景。
examples/python/galbot_robot/request_target_example.py
"""
G1 RequestTarget menu example for SDK mirror SingoriXTarget.
This example shows how to construct `SingoriXTarget` directly at the Python SDK
layer and send it to the low-level WBCS through `request_target()`.
1. Joint-space commands are written into `target_group_trajectory_map`
2. Chassis pose / twist style task-space commands are written into `target_task_trajectory_map`
3. One `SingoriXTarget` can contain both joint trajectory and task trajectory
4. The current SDK does not automatically switch the chassis controller, so
pose / twist / mixed scenes explicitly call `switch_controller(...)`
5. The base twist scene automatically sends a zero-twist target after the
configured duration to stop the chassis
"""
import math
import time
from galbot_sdk.g1 import (
ControlStatus,
ErrorInfo,
G1ControllerName,
G1JointGroup,
GalbotRobot,
GroupCommand,
JointCommand,
Pose,
SingoriXTarget,
TargetConfig,
TargetGroupTrajectory,
TargetSampling,
TargetTaskTrajectory,
TaskCommand,
Twist,
Vector3,
FrameTriad,
TARGET_DATA_DEFAULT,
TARGET_DATA_FRAME_POSE,
TARGET_DATA_FRAME_TWIST,
TARGET_TYPE_DEFAULT,
TARGET_TYPE_OVERRIDE,
TARGET_TYPE_PROVERRIDE,
)
CHASSIS_TASK_NAME = "chassis"
CHASSIS_SUBTASK_POSE = "chassis_pose"
CHASSIS_SUBTASK_TWIST = "chassis_twist"
def now_ns():
return time.time_ns()
def yaw_to_quaternion(yaw):
return [0.0, 0.0, math.sin(yaw * 0.5), math.cos(yaw * 0.5)]
def make_empty_target():
target = SingoriXTarget()
target.header.timestamp_ns = now_ns()
target.header.frame_id = "base_link"
return target
def make_group_target_config():
config = TargetConfig()
config.target_data = TARGET_DATA_DEFAULT
config.target_type = TARGET_TYPE_PROVERRIDE
config.target_sampling = TargetSampling.TARGET_SAMPLING_DEFAULT
config.target_priority = 1
return config
def make_pose_target_config():
config = TargetConfig()
config.target_data = TARGET_DATA_FRAME_POSE
config.target_type = TARGET_TYPE_OVERRIDE
config.target_sampling = TargetSampling.TARGET_SAMPLING_LINEAR_INTERPOLATE
config.target_priority = 1
return config
def make_twist_target_config():
config = TargetConfig()
config.target_data = TARGET_DATA_FRAME_TWIST
config.target_type = TARGET_TYPE_OVERRIDE
config.target_sampling = TargetSampling.TARGET_SAMPLING_DIRECT_PASS
config.target_priority = 1
return config
def make_joint_command(position):
command = JointCommand()
command.position = position
command.velocity = 0.0
command.acceleration = 0.0
command.effort = 0.0
return command
def make_vector3(x, y, z):
vec = Vector3()
vec.x = x
vec.y = y
vec.z = z
return vec
def build_joint_target(group_name, joint_names, positions, time_from_start_s):
target = make_empty_target()
group_traj = TargetGroupTrajectory()
group_traj.target_config = make_group_target_config()
group_traj.joint_names = list(joint_names)
command = GroupCommand()
command.time_from_start_s = time_from_start_s
command.joint_commands = [make_joint_command(position) for position in positions]
group_traj.group_commands = [command]
target.target_group_trajectory_map = {group_name: group_traj}
return target
def build_chassis_pose_target(x, y, yaw, time_from_start_s, frame_id="rel(0)", reference_frame_id="odom"):
target = make_empty_target()
task_traj = TargetTaskTrajectory()
task_traj.target_config = make_pose_target_config()
task_traj.group_names = [G1JointGroup.chassis]
task_traj.subtask_names = [f"{CHASSIS_SUBTASK_POSE}_{now_ns()}"]
triad = FrameTriad()
triad.header.timestamp_ns = now_ns()
triad.header.frame_id = frame_id
triad.body_frame_id = "base_link"
triad.reference_frame_id = reference_frame_id
triad.pose = Pose([x, y, 0.0], yaw_to_quaternion(yaw))
command = TaskCommand()
command.time_from_start_s = time_from_start_s
command.subtask_commands = [triad]
task_traj.task_commands = [command]
target.target_task_trajectory_map = {CHASSIS_TASK_NAME: task_traj}
return target
def build_chassis_twist_target(vx, vy, wz, time_from_start_s):
target = make_empty_target()
task_traj = TargetTaskTrajectory()
task_traj.target_config = make_twist_target_config()
task_traj.group_names = [G1JointGroup.chassis]
task_traj.subtask_names = [f"{CHASSIS_SUBTASK_TWIST}_{now_ns()}"]
twist = Twist()
twist.linear = make_vector3(vx, vy, 0.0)
twist.angular = make_vector3(0.0, 0.0, wz)
triad = FrameTriad()
triad.header.timestamp_ns = now_ns()
triad.header.frame_id = "base_link"
triad.body_frame_id = "base_link"
triad.reference_frame_id = "base_link"
triad.twist = twist
command = TaskCommand()
command.time_from_start_s = time_from_start_s
command.subtask_commands = [triad]
task_traj.task_commands = [command]
target.target_task_trajectory_map = {CHASSIS_TASK_NAME: task_traj}
return target
def build_stop_twist_target():
return build_chassis_twist_target(0.0, 0.0, 0.0, 0.1)
def merge_targets(targets):
merged = make_empty_target()
group_map = {}
task_map = {}
for target in targets:
group_map.update(target.target_group_trajectory_map)
task_map.update(target.target_task_trajectory_map)
merged.target_group_trajectory_map = group_map
merged.target_task_trajectory_map = task_map
return merged
def print_error_info(scene_name, error_info):
if error_info is None:
print(f"{scene_name}: request_target returned None")
return
if not isinstance(error_info, ErrorInfo):
print(f"{scene_name}: unexpected response type {type(error_info)}")
return
if not error_info.error_vec:
print(f"{scene_name}: request_target success, service returned no errors")
return
print(f"{scene_name}: request_target returned {len(error_info.error_vec)} error entries:")
for error in error_info.error_vec:
print(
f" component={error.commpent}, code={error.error_code}, description={error.description}"
)
def ensure_controller(robot, controller_name):
status = robot.switch_controller(controller_name)
print(f"switch_controller({controller_name}): {getattr(status, 'name', status)}")
return status
def run_twist_scene(robot, scene_name, target, twist_duration_s):
print(f"{scene_name}: start moving for {twist_duration_s} seconds")
print_error_info(scene_name, robot.request_target(target))
time.sleep(twist_duration_s)
print(f"{scene_name}: send stop twist target")
print_error_info(f"{scene_name}_stop", robot.request_target(build_stop_twist_target()))
def print_menu():
print(
"\nAvailable commands:\n"
" joint - request a joint-only head target\n"
" base_pose - request a chassis pose target\n"
" base_twist - request a chassis twist target with auto stop\n"
" mixed_pose - request head + left_arm + chassis pose in one target\n"
" mixed_twist - request head + left_arm + chassis twist in one target\n"
" quit - exit example\n"
)
def main():
robot = GalbotRobot()
if not robot.init():
print("robot init failed")
return
time.sleep(2)
head_joint_names = robot.get_joint_names(True, [G1JointGroup.head])
if not head_joint_names:
print("failed to fetch active head joints")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
head_single_joint = [head_joint_names[0]]
joint_time_s = 3.0
pose_time_s = 4.0
twist_command_time_s = 0.2
twist_duration_s = 2.0
print_menu()
while True:
command = input("Enter command: ").strip()
if command == "quit":
break
if command == "joint":
target = build_joint_target(G1JointGroup.head, head_single_joint, [0.2], joint_time_s)
print_error_info("joint", robot.request_target(target))
continue
if command == "base_pose":
if ensure_controller(robot, G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
continue
target = build_chassis_pose_target(0.2, 0.0, 0.0, pose_time_s)
print_error_info("base_pose", robot.request_target(target))
continue
if command == "base_twist":
if ensure_controller(robot, G1ControllerName.CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
continue
target = build_chassis_twist_target(0.05, 0.0, 0.0, twist_command_time_s)
run_twist_scene(robot, "base_twist", target, twist_duration_s)
continue
if command == "mixed_pose":
if ensure_controller(robot, G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
continue
target = merge_targets(
[
build_joint_target(G1JointGroup.head, head_single_joint, [0.2], joint_time_s),
build_chassis_pose_target(0.1, 0.0, 0.0, pose_time_s),
]
)
print_error_info("mixed_pose", robot.request_target(target))
continue
if command == "mixed_twist":
if ensure_controller(robot, G1ControllerName.CHASSIS_TWIST_CTRL) != ControlStatus.SUCCESS:
continue
target = merge_targets(
[
build_joint_target(G1JointGroup.head, head_single_joint, [-0.2], joint_time_s),
build_chassis_twist_target(0.05, 0.0, 0.0, twist_command_time_s),
]
)
run_twist_scene(robot, "mixed_twist", target, twist_duration_s)
continue
print(f"Unknown command: {command}")
print_menu()
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
获取并设置底盘速度（get_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.g1 import GalbotRobot, ControlStatus
import time
def print_base_velocity(base_velocity_info: dict):
"""
base_velocity_info: dict, includes:
- 'linear_velocity': [vx, vy, vz] Linear velocity (m/s)
- 'angular_velocity': [wx, wy, wz] Angular velocity (rad/s)
"""
if not base_velocity_info:
print("Base velocity data is empty")
return
# Print linear and angular velocity in the same format as the standalone example.
linear_velocity = base_velocity_info.get("linear_velocity", [])
if linear_velocity:
print(
f"Linear velocity (m/s): vx={linear_velocity[0]}, vy={linear_velocity[1]}, "
f"vz={linear_velocity[2]}"
)
angular_velocity = base_velocity_info.get("angular_velocity", [])
if angular_velocity:
print(
f"Angular velocity (rad/s): wx={angular_velocity[0]}, wy={angular_velocity[1]}, "
f"wz={angular_velocity[2]}"
)
# Get GalbotRobot
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")
# Read base velocity before issuing the motion command.
base_velocity_before = robot.get_base_velocity()
if base_velocity_before:
print("Base velocity before command:")
print_base_velocity(base_velocity_before)
else:
print("Failed to get base velocity before command.")
# Set chassis speed
linear_velocity = [0.2, 0.0, 0.0] # 0.2 m/s
angular_velocity = [0.0, 0.0, 0.0] # 0.0 rad/s
duration_s = 3.0 # Block while publishing at 10 Hz for 2 seconds; no stop is sent.
status = robot.set_base_velocity(linear_velocity, angular_velocity, duration_s)
if status == ControlStatus.SUCCESS:
print(f"Velocity publishing completed after {duration_s} seconds; stopping depends on the watchdog.")
else:
print("Set chassis speed failed.")
# Observe immediately; publishing completion does not imply a stopped base.
# Read base velocity again after the command has finished.
base_velocity_after = robot.get_base_velocity()
if base_velocity_after:
print("Base velocity after command:")
print_base_velocity(base_velocity_after)
else:
print("Failed to get base velocity after command.")
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取关节状态（get_joint_states）
适用场景：获取所有关节的完整状态信息，包括位置、速度、电流等。适用于需要完整关节信息的算法（如动力学计算、状态估计）。以下示例以左臂为例。
examples/python/galbot_robot/get_joint_states.py
import time
from galbot_sdk.g1 import GalbotRobot
def print_joint_states(joint_states):
"""
joint_state_vec: List of JointState; each object has
joint_name, position, velocity, acceleration, effort, current
"""
for js in joint_states:
print(f" : joint_name = {js.joint_name} , position = {js.position} , velocity = {js.velocity} "
f", acceleration = {js.acceleration} , effort = {js.effort} , current = {js.current}"
f", timestamp_ns = {js.timestamp_ns}")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get joint states by joint group names; returns all joints if empty
joint_group_names = ["left_arm"]
ret = robot.get_joint_states(joint_group_names, [])
if not ret:
print(f"No joint states returned for joint groups {joint_group_names}")
else:
print_joint_states(ret)
# Get specified joint states; if provided, overrides joint group input
joint_names = ["left_arm_joint1", "left_arm_joint2"]
state_ret = robot.get_joint_states([], joint_names)
if not state_ret:
print(f"No joint states returned for joint names {joint_names}")
else:
print_joint_states(state_ret)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取关节位置（get_joint_positions）
适用场景：仅获取当前关节位置。当你只需要关节位置信息时使用此接口，比 get_joint_states 更轻量。
examples/python/galbot_robot/get_joint_positions.py
import time
from galbot_sdk.g1 import GalbotRobot
def print_joint_positions(joint_positions):
print(f"pos count is {len(joint_positions)}")
for pos in joint_positions:
print(pos)
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get joint positions by joint group names; returns all joints if empty
joint_group_names = ["left_arm"]
ret = robot.get_joint_positions(joint_group_names, [])
print("Left arm joint positions:")
print_joint_positions(ret)
# Get specified joint positions; if provided, overrides joint group input
joint_names = ["left_arm_joint1", "left_arm_joint2"]
state_ret = robot.get_joint_positions([], joint_names)
print("Left arm joints 1 and 2 positions:")
print_joint_positions(state_ret)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取关节名称（get_joint_names）
适用场景：获取机器人所有关节的名称列表，用于迭代遍历关节或配置文件生成。
examples/python/galbot_robot/get_joint_names.py
import time
from galbot_sdk.g1 import GalbotRobot
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get specified joint names; joint groups include ["leg", "head", "left_arm", "right_arm"]
joint_group_names = ["head"]
only_active_joint = True # Get active joints
head_joint_names = robot.get_joint_names(only_active_joint, joint_group_names)
print("Head joint names:")
for i, name in enumerate(head_joint_names):
print(f"{i}: {name}")
# Passing an empty list returns all joint group information by default
all_joint_names = robot.get_joint_names(only_active_joint, [])
print("All joint names:")
for i, name in enumerate(all_joint_names):
print(f"{i}: {name}")
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取夹爪状态（get_gripper_state）
适用场景：获取夹爪当前位置和状态，判断夹爪当前是打开还是闭合。
examples/python/galbot_robot/get_gripper_state.py
import time
from galbot_sdk.g1 import GalbotRobot, G1JointGroup
def print_gripper_state(joint_group, gripper_state):
"""
joint_group: G1JointGroup enum value
gripper_state: object including timestamp_ns, width, velocity, effort, is_moving
"""
print(f"Timestamp (ns): {gripper_state.timestamp_ns}")
print(
f"width {gripper_state.width} "
f"velocity {gripper_state.velocity} "
f"effort {gripper_state.effort} "
f"is moving {gripper_state.is_moving}"
)
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Set gripper joint group (left gripper)
joint_group = G1JointGroup.left_gripper
# Get gripper state
gripper_state = robot.get_gripper_state(joint_group)
if gripper_state is None:
print("get gripper state error")
else:
print("Left gripper state is as follows:")
print_gripper_state(joint_group, gripper_state)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取吸盘状态（get_suction_cup_state）
适用场景：获取吸盘当前是否处于吸合状态，以及是否成功检测到物体。
examples/python/galbot_robot/get_suction_cup_state.py
import time
from galbot_sdk.g1 import GalbotRobot, G1JointGroup
def print_suction_cup_state(suction_cup_state):
"""
suction_cup_state: object including timestamp_ns, pressure, activation, action_state
"""
group_name = joint_group.name
print(f"Timestamp (ns): {suction_cup_state.timestamp_ns}")
print(
f"pressure {suction_cup_state.pressure} "
f"activation {suction_cup_state.activation} "
f"action state {int(suction_cup_state.action_state)}"
)
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Set suction cup joint group (right suction cup)
joint_group = G1JointGroup.right_suction_cup
# Get suction cup state
suction_cup_state = robot.get_suction_cup_state(joint_group)
if suction_cup_state is None:
print("get suction cup error")
else:
print("Right suction cup status:")
print_suction_cup_state(suction_cup_state)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取坐标变换（get_transform）
适用场景：获取两个坐标系之间的变换关系（位姿），用于机械臂抓取、视觉定位等场景。
examples/python/galbot_robot/get_transform.py
import time
from galbot_sdk.g1 import GalbotRobot
def print_pose(pose_vec):
"""
pose_vec: list of floats
"""
print("pose_vec = [" + ", ".join(str(p) for p in pose_vec) + "]")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Set target frame and source frame
target_frame = "base_link"
source_frame = "left_arm_link1"
timestamp_ns = 0 # 0 means fetch the latest TF transform value
# Get coordinate transform
ret_val = robot.get_transform(target_frame, source_frame)
if not ret_val[0]:
print("get_transform error")
else:
print("tf_timestamp_ns:", ret_val[1])
print_pose(ret_val[0])
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取 IMU 数据（get_imu_data）
适用场景：获取机器人基座 IMU 的加速度、角速度和姿态信息，用于状态估计、运动分析。
examples/python/galbot_robot/get_imu_data.py
import time
from galbot_sdk.g1 import GalbotRobot, SensorType
from typing import Dict
def print_imu_data(imu_data: dict):
"""
imu_data: dict, includes:
- 'timestamp_ns'
- 'accel' : {'x', 'y', 'z'}
- 'gyro' : {'x', 'y', 'z'}
- 'magnet' : {'x', 'y', 'z'}
"""
if not imu_data:
print("IMU data is empty")
return
print(f"Timestamp (ns): {imu_data.get('timestamp_ns')}")
accel = imu_data.get("accel", {})
gyro = imu_data.get("gyro", {})
magnet = imu_data.get("magnet", {})
print(
f"Accelerometer: x={accel.get('x')}, "
f"y={accel.get('y')}, "
f"z={accel.get('z')}"
)
print(
f"Gyroscope: x={gyro.get('x')}, "
f"y={gyro.get('y')}, "
f"z={gyro.get('z')}"
)
print(
f"Magnetometer: x={magnet.get('x')}, "
f"y={magnet.get('y')}, "
f"z={magnet.get('z')}"
)
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
# - SensorType::TORSO_IMU: torso IMU
# - SensorType::LIDAR_IMU: lidar IMU
# - SensorType::CHASSIS_IMU: Chassis lidar IMU
robot.init({SensorType.CHASSIS_IMU})
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
imu_data = robot.get_imu_data(SensorType.CHASSIS_IMU)
if not imu_data:
print("No imu data!")
else:
print("IMU data:")
print_imu_data(imu_data)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取设备信息（get_device_information）
适用场景：获取机器人硬件版本、固件版本等设备信息，用于调试和兼容性检查。
examples/python/galbot_robot/get_device_information.py
import time
from galbot_sdk.g1 import GalbotRobot
from typing import Dict
def print_device_info(device_info: dict):
"""
device_info: dict, including the following fields:
- 'model': Device model (str)
- 'serial_number': Serial number (str)
- 'firmware_version': Firmware version (str)
- 'hardware_version': Hardware version (str)
- 'manufacturer': Manufacturer (str)
"""
if not device_info:
print("Device information is empty")
return
print("Device information:")
print(f" Model: {device_info.get('model', 'N/A')}")
print(f" Serial number: {device_info.get('serial_number', 'N/A')}")
print(f" Firmware version: {device_info.get('firmware_version', 'N/A')}")
print(f" Hardware version: {device_info.get('hardware_version', 'N/A')}")
print(f" : {device_info.get('manufacturer', 'N/A')}")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
device_info = robot.get_device_information()
if not device_info:
print("Failed to get device information!")
else:
print("Device information retrieved successfully!")
print_device_info(device_info)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取电池信息（get_bms_information）
适用场景：获取电池电压、电量等信息，用于低电量检测和状态监控。
examples/python/galbot_robot/get_bms_information.py
import time
from galbot_sdk.g1 import GalbotRobot
def print_bms_information(bms_info: dict):
"""
bms_info: dict, includes:
- 'voltage' : float (V)
- 'current' : float (A)
- 'battery_level' : float (0-100)
- 'temperature' : float (°C)
- 'charging_status' : str or int
- 'health_status' : str or int
- 'capacity' : float (Ah)
"""
if not bms_info:
print("BMS information is empty")
return
print(f"Voltage (V): {bms_info.get('voltage')}")
print(f"Current (A): {bms_info.get('current')}")
print(f"Battery level (%): {bms_info.get('battery_level')}")
print(f"Temperature (C): {bms_info.get('temperature')}")
print(f"Charging status: {bms_info.get('charging_status')}")
print(f"Health status: {bms_info.get('health_status')}")
print(f"Capacity (Ah): {bms_info.get('capacity')}")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(3)
print("Initialization succeeded")
bms_info = robot.get_bms_information()
print_bms_information(bms_info)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")
获取相机图像数据（get_rgb_data && get_depth_data）
适用场景：获取 RGB 图像和深度图像数据，用于视觉感知、目标检测、SLAM 等任务。
examples/python/galbot_robot/get_camera_data.py
try:
from galbot_sdk.g1 import GalbotRobot, RgbOutputFormat, SensorType
except ImportError:
print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
import os
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Dict
def decode_compressed_image(compressed_image):
"""
decode CompressedImage image
Parameters:
compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"].lower() in ("jpeg", "jpg", "rgb8"):
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(compressed_image)
else:
raise ValueError(f"Unsupport data format: {compressed_image['format']}")
def decode_ir_image(ir_image):
"""decode IR (infrared) grayscale image.
Supports JPEG-compressed mono8 and raw mono8 byte streams.
"""
data = ir_image["data"]
fmt = ir_image.get("format", "")
# Raw mono8: plain pixel bytes, use height/width directly
if fmt == "mono8":
h = ir_image.get("height", 0)
w = ir_image.get("width", 0)
if h == 0 or w == 0:
raise ValueError(f"decode_ir_image: height/width unavailable for mono8, got {h}x{w}")
return np.frombuffer(data, dtype=np.uint8).reshape(h, w).copy()
# Compressed (JPEG / PNG)
nparr = np.frombuffer(data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_GRAYSCALE)
if img is None:
raise ValueError(f"decode_ir_image: imdecode failed, format='{fmt}', size={len(data)}")
return img
def decode_rgb_image(image_data):
"""decode rgb image"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Fail to Decode RGB Image")
return img
def decode_depth_image(image_data):
"""decode depth image"""
depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()
# Check whether height and width exist
if "height" not in image_data or "width" not in image_data:
raise ValueError("Missing 'height' or 'width' in depth image metadata.")
if image_data["height"] == 0 or image_data["width"] == 0:
raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")
# Parse depth image
depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]
return depth_img
def main():
SHOW_IMAGE = False
robot = GalbotRobot()
# Get left arm RGB, depth and IR images
enable_sensor_set = {SensorType.LEFT_ARM_CAMERA,
SensorType.LEFT_ARM_DEPTH_CAMERA,
SensorType.LEFT_ARM_INFRA_CAMERA_1,
SensorType.LEFT_ARM_INFRA_CAMERA_2,}
robot.init(enable_sensor_set)
print("Initialization succeeded")
# Program started, waiting for data
time.sleep(5)
# Get left arm RGB image
rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
if not rgb_image_data:
print("No rgb image data!")
else:
print("get rgb image suceess")
print(rgb_image_data['header'])
img = decode_compressed_image(rgb_image_data)
# Save RGB image
cv2.imwrite("rgb_image_data.jpg", img)
# 可视化RGB图像
if SHOW_IMAGE:
cv2.namedWindow("rgb image", cv2.WINDOW_NORMAL)
cv2.imshow("rgb image", img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Get left arm depth image
depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not depth_data or "data" not in depth_data:
print("Depth camera not ready")
else:
print("get depth data suceess")
print(depth_data['header'])
depth_img_raw = decode_compressed_image(depth_data)
depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize depth values to 0-1 range
depth_img = depth_img.astype(np.uint8)
# Save depth image
cv2.imwrite("depth_data.jpg", depth_img)
# 可视化深度图
if SHOW_IMAGE:
cv2.namedWindow("depth image", cv2.WINDOW_NORMAL)
cv2.imshow("depth image", depth_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Get left arm IR images
for sensor_type, filename in [
(SensorType.LEFT_ARM_INFRA_CAMERA_1, "ir_infra1.jpg"),
(SensorType.LEFT_ARM_INFRA_CAMERA_2, "ir_infra2.jpg"),
]:
ir_data = robot.get_ir_data(sensor_type)
if not ir_data or "data" not in ir_data:
print(f"IR camera {sensor_type} not ready (ir_enabled may be false)")
else:
print(f"get ir data {sensor_type} success")
print(ir_data['header'])
print(ir_data['format'])
ir_img = decode_ir_image(ir_data)
cv2.imwrite(filename, ir_img)
if SHOW_IMAGE:
cv2.namedWindow(f"ir image {sensor_type}", cv2.WINDOW_NORMAL)
cv2.imshow(f"ir image {sensor_type}", ir_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
if __name__=="__main__":
main()
订阅视频数据（subscribe_video_data）
适用场景：订阅指定传感器的 H.264 视频流并通过回调接收编码帧，适用于录像、实时处理、网络转发等需要基于推送的压缩视频交付场景。
examples/python/galbot_robot/subscribe_video_data.py
import threading
import time
try:
from galbot_sdk.g1 import GalbotRobot, SensorStatus, SensorType
except ImportError:
print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
OUTPUT_PATH = "head_left.h264"
DURATION = 10
output_file = None
frame_count = 0
frame_count_lock = threading.Lock()
# Important:
# The video callback is executed by the SDK internal dispatch thread.
# Do not perform time-consuming operations in this callback; keep it
# short and non-blocking. Long-running work may delay video frame delivery
# for this subscription.
def video_data_callback(video_data: dict):
global frame_count
header = video_data.get("header", {})
data = video_data.get("data", b"")
if output_file is not None and data:
# This sample callback only writes the raw H.264 stream to file without expensive transcoding here.
output_file.write(data)
with frame_count_lock:
frame_count += 1
current_count = frame_count
if current_count == 1 or current_count % 30 == 0:
print(
"frame_count={}, timestamp_ns={}, format={}, bytes={}".format(
current_count,
header.get("timestamp_ns"),
video_data.get("format"),
len(data),
)
)
def main():
global output_file
robot = GalbotRobot()
# Enable only the camera used by this example.
if not robot.init({SensorType.HEAD_LEFT_CAMERA}):
print("GalbotRobot initialization failed")
return
print("Initialization succeeded")
output_file = open(OUTPUT_PATH, "wb")
print(f"writing H.264 stream to {OUTPUT_PATH}")
status = robot.subscribe_video_data(SensorType.HEAD_LEFT_CAMERA, video_data_callback)
if status != SensorStatus.SUCCESS:
print(f"subscribe_video_data failed, status={status}")
output_file.close()
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
print("subscribe_video_data success")
try:
time.sleep(DURATION)
except KeyboardInterrupt:
print("Interrupted by user")
finally:
# Unsubscribe clears the user callback without recreating SDK reader resources.
status = robot.unsubscribe_video_data(SensorType.HEAD_LEFT_CAMERA)
print(f"unsubscribe_video_data status={status}")
if output_file is not None:
output_file.close()
with frame_count_lock:
total_frames = frame_count
print(f"received {total_frames} frames")
print(f"saved H.264 stream to {OUTPUT_PATH}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
if __name__ == "__main__":
main()
获取传感器内外参（get_camera_intrinsic && get_sensor_extrinsic）
适用场景：获取相机内参和传感器相对于机器人基座的外参，用于计算机视觉计算和 3D 重建。
examples/python/galbot_robot/get_camera_params.py
try:
from galbot_sdk.g1 import GalbotRobot, RgbOutputFormat, SensorType
except ImportError:
print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
import os
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Dict
def decode_compressed_image(compressed_image):
"""
decode CompressedImage image
Parameters:
compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"].lower() in ("jpeg", "jpg", "rgb8"):
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(compressed_image)
else:
raise ValueError(f"Unsupport data format: {compressed_image['format']}")
def decode_rgb_image(image_data):
"""decode rgb image"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Fail to Decode RGB Image")
return img
def decode_depth_image(image_data):
"""decode depth image"""
depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()
# Check whether height and width exist
if "height" not in image_data or "width" not in image_data:
raise ValueError("Missing 'height' or 'width' in depth image metadata.")
if image_data["height"] == 0 or image_data["width"] == 0:
raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")
# Parse depth image
depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]
return depth_img
def main():
robot = GalbotRobot()
# Get left arm RGB and depth images, right arm depth image, chassis lidar data, torso IMU data
enable_sensor_set = {SensorType.LEFT_ARM_CAMERA, # Left arm depth camera
SensorType.LEFT_ARM_DEPTH_CAMERA,} # Left arm RGB camera
robot.init(enable_sensor_set)
print("Initialization succeeded")
# Program started, waiting for data
time.sleep(5)
# Get left arm RGB image
rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
if not rgb_image_data:
print("No rgb image data!")
else:
print("get rgb image suceess")
print(rgb_image_data['header'])
img = decode_compressed_image(rgb_image_data)
# Save RGB image
cv2.imwrite("rgb_image_data.jpg", img)
# Get left arm camera intrinsics
camera_intrinsics = robot.get_camera_intrinsic(SensorType.LEFT_ARM_CAMERA)
if not camera_intrinsics:
print("No camera intrinsics data!")
else:
print("get camera intrinsics suceess")
print(camera_intrinsics)
# Get left arm depth image
depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not depth_data or "data" not in depth_data:
print("Depth camera not ready")
else:
print("get depth data suceess")
print(depth_data['header'])
depth_img_raw = decode_compressed_image(depth_data)
depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize depth values to 0-1 range
depth_img = depth_img.astype(np.uint8)
# Save depth image
cv2.imwrite("depth_data.jpg", depth_img)
# Get left-arm depth camera intrinsics
camera_intrinsics = robot.get_camera_intrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not camera_intrinsics:
print("No camera intrinsics data!")
else:
print("get camera intrinsics suceess")
print(camera_intrinsics)
# Extrinsics
time.sleep(2)
camera_extrinsics, timestamp_ns = robot.get_sensor_extrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not camera_extrinsics:
print("No camera extrinsics data!")
else:
print("get camera extrinsics suceess")
print(camera_extrinsics)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
if __name__=="__main__":
main()
获取雷达数据（get_lidar_data）
适用场景：获取激光雷达点云数据，用于导航、避障、环境建模。
examples/python/galbot_robot/get_lidar_data.py
from galbot_sdk.g1 import GalbotRobot, SensorType
from typing import Dict
import time
import numpy as np
def convert_pointcloud(cloud):
"""
Convert cloud dict to NumPy array dictionary
Args:
pointcloud_msg: PointCloud2 protobuf message object
Returns:
Dictionary: {field_name: NumPy array}
- Single-element fields: shape (N,)
- Multi-element fields: shape (N, count) or (N,)
- N = width * height (total number of points)
"""
if not cloud:
return {}
num_points = cloud["height"] * cloud["width"]
if num_points == 0:
return {}
DTYPE_MAP = {
1: np.int8,
2: np.uint8,
3: np.int16,
4: np.uint16,
5: np.int32,
6: np.uint32,
7: np.float32,
8: np.float64
}
dtype_list = []
for field in cloud["fields"]:
# Get base data type
np_dtype_class = DTYPE_MAP.get(field["datatype"])
if np_dtype_class is None:
raise ValueError(f"Unsupported data type: {field['datatype']}")
dtype_inst = np.dtype(np_dtype_class)
# Handle byte order (endianness)
if dtype_inst.itemsize > 1:
byteorder = '>' if cloud["is_bigendian"] else '<'
dtype_inst = dtype_inst.newbyteorder(byteorder)
# Add to dtype list
if field["count"] == 1:
dtype_list.append((field["name"], dtype_inst))
else:
# Multi-element fields (e.g., rgb)
dtype_list.append((field["name"], dtype_inst, field["count"]))
# Create structured dtype
dtype = np.dtype(dtype_list)
# Data integrity check
expected_size = num_points * cloud["point_step"]
if len(cloud["data"]) < expected_size:
raise ValueError(
f"Insufficient data length: expected {expected_size} bytes, "
f"actual {len(cloud['data'])} bytes"
)
# Create NumPy structured array from binary data
# count parameter ensures only expected number of points are read
arr = np.frombuffer(cloud["data"], dtype=dtype, count=num_points)
# Convert to regular dictionary (copy data to avoid modifying original)
result = {}
for field in cloud["fields"]:
field_data = arr[field["name"]]
# Handle shape of multi-element fields
if field["count"] == 1:
result[field["name"]] = field_data.copy()
else:
# Keep original shape or flatten, choose according to needs
result[field["name"]] = field_data.copy()
return result
def get_xyz_array(pointcloud_dict: Dict[str, np.ndarray],
remove_nan: bool = False) -> np.ndarray:
"""
Extract XYZ coordinate array from converted point cloud dictionary
Args:
pointcloud_dict: Dictionary returned by pointcloud2_to_numpy()
remove_nan: Whether to remove points containing NaN (for FLOAT32/FLOAT64 types)
Returns:
Nx3 point coordinate array
"""
required = ['x', 'y', 'z']
if not all(k in pointcloud_dict for k in required):
raise ValueError("Point cloud data missing required xyz fields")
points = np.stack([pointcloud_dict['x'],
pointcloud_dict['y'],
pointcloud_dict['z']], axis=1)
if remove_nan:
mask = ~np.isnan(points).any(axis=1)
points = points[mask]
return points
def save_xyz_to_pcd(xyz_array: np.ndarray, filename: str, binary: bool = False) -> None:
"""
Save XYZ coordinates to PCD file format (simplest option for coordinate-only data)
Args:
xyz_array: Nx3 array of XYZ coordinates
filename: Output PCD file path
binary: If True, saves in binary format; otherwise ASCII
"""
if xyz_array.ndim != 2 or xyz_array.shape[1] != 3:
raise ValueError(f"xyz_array must have shape (N, 3), got {xyz_array.shape}")
num_points = xyz_array.shape[0]
header = [
"# .PCD v0.7 - Point Cloud Data file format",
"VERSION 0.7",
"FIELDS x y z",
"SIZE 4 4 4",
"TYPE F F F", # F = float32
"COUNT 1 1 1",
f"WIDTH {num_points}",
"HEIGHT 1",
"VIEWPOINT 0 0 0 1 0 0 0",
f"POINTS {num_points}",
f"DATA {'binary' if binary else 'ascii'}"
]
if binary:
with open(filename, 'wb') as f:
f.write(('\n'.join(header) + '\n').encode('ascii'))
f.write(xyz_array.astype(np.float32).tobytes())
else:
with open(filename, 'w') as f:
f.write('\n'.join(header) + '\n')
np.savetxt(f, xyz_array, fmt='%f')
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
enable_sensor_set = {SensorType.BASE_LIDAR}
# To save resource overhead, only cameras and LiDAR sensors passed during initialization can retrieve data
robot.init(enable_sensor_set)
print("Initialization succeeded")
# Program started, waiting for data
time.sleep(4)
cloud = robot.get_lidar_data(SensorType.BASE_LIDAR)
if not cloud:
print("No lidar data!")
else:
pointcloud_dict = convert_pointcloud(cloud)
xyz_points = get_xyz_array(pointcloud_dict)
save_xyz_to_pcd(xyz_points, "output_xyz.pcd")
print(pointcloud_dict)
print("get lidar data success")
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取同步观测数据（get_synced_observation）
适用场景：获取同步观测数据，主要用于模型推理场景下获取时序对齐的输入数据，支持彩色相机、深度相机和关节数据。
examples/python/galbot_robot/get_synced_observation.py
import time
from galbot_sdk.g1 import (
GalbotRobot,
JointState,
JointStateMessage,
RgbData,
SensorType,
SyncedObservation,
)
NS_TO_MS = 1e-6
def main():
# 1) Initialize robot with sync mode enabled.
# The 2nd init arg must be True, otherwise get_synced_observation() is unavailable.
robot = GalbotRobot()
ok = robot.init(
{
SensorType.HEAD_LEFT_CAMERA,
SensorType.LEFT_ARM_CAMERA,
SensorType.RIGHT_ARM_CAMERA,
},
True, # enable_sync_mode
)
if not ok:
print("init failed")
robot.destroy()
return
time.sleep(2)
# 2) Build camera list for synchronization.
# cameras[0] is the anchor camera; other cameras are matched by nearest timestamp.
cameras = [
SensorType.LEFT_ARM_CAMERA, # anchor
SensorType.RIGHT_ARM_CAMERA, # nearest-neighbor
SensorType.HEAD_LEFT_CAMERA, # nearest-neighbor
]
# 3) Call API (typed return).
obs: SyncedObservation | None = robot.get_synced_observation(cameras, True)
if not obs:
print("get_synced_observation failed")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
# 4) Read synced RGB frames from typed field: obs.rgb_data_map.
# Sync-mode RGB payloads are tightly packed CPU-owned NV12, not JPEG.
print("[Synced RGB]")
rgb_map: dict[SensorType, RgbData] = obs.rgb_data_map
anchor_camera = cameras[0]
anchor_frame: RgbData | None = rgb_map.get(anchor_camera, None)
anchor_ts_ns = (
anchor_frame.header.timestamp_ns if anchor_frame is not None else None
)
print(f"anchor_camera={anchor_camera}, anchor_timestamp_ns={anchor_ts_ns}")
for cam in cameras:
frame: RgbData | None = rgb_map.get(cam, None)
cam_ts_ns = frame.header.timestamp_ns if frame is not None else None
frame_format = frame.format if frame is not None else "N/A"
dimensions = f"{frame.width}x{frame.height}" if frame is not None else "N/A"
payload_bytes = len(frame.data) if frame is not None else 0
delta_ms_str = "N/A"
if anchor_ts_ns is not None and cam_ts_ns is not None:
delta_ms_str = f"{(cam_ts_ns - anchor_ts_ns) * NS_TO_MS:.3f}"
print(
f"camera={cam}, timestamp_ns={cam_ts_ns if cam_ts_ns is not None else 'N/A'}, "
f"delta_to_anchor_ms={delta_ms_str}, format={frame_format}, "
f"dimensions={dimensions}, bytes={payload_bytes}"
)
# 5) Read optional joint snapshot from typed field: obs.joint_state
# joint_state is JointStateMessage | None.
joint_state: JointStateMessage | None = obs.joint_state
print("[Joint]")
joint_ts_ns = joint_state.timestamp_ns if joint_state is not None else None
joint_delta_ms_str = "N/A"
if anchor_ts_ns is not None and joint_ts_ns is not None:
joint_delta_ms_str = f"{(joint_ts_ns - anchor_ts_ns) * NS_TO_MS:.3f}"
print(
"joint_timestamp_ns:",
joint_ts_ns if joint_ts_ns is not None else "N/A",
"delta_to_anchor_ms:",
joint_delta_ms_str,
)
# 6) Each element in joint_state_vec is typed JointState and contains joint_name.
joint_state_vec: list[JointState] = (
joint_state.joint_state_vec if joint_state is not None else []
)
print("joint_count:", len(joint_state_vec))
for i, js in enumerate(joint_state_vec[:5]):
print(
f" [{i}] joint_name={js.joint_name}, "
f"position={js.position}, "
f"velocity={js.velocity}, "
f"effort={js.effort}"
)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
一键回零-odom坐标系（whole_body_reset_zero_odom）
适用场景：快速将全身上下所有关节运动到零位（初始姿态），基于 odom 坐标系。通常用于程序启动后的初始化。
examples/python/galbot_robot/whole_body_reset_zero_odom_example.py
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import GalbotMotion
import time
def main():
robot = GalbotRobot()
motion = GalbotMotion()
if not robot.init():
print("GalbotRobot init failed.")
return
if not motion.init():
print("GalbotMotion init failed.")
return
time.sleep(2)
# Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
whole_body_joint_1 = [
0.25, 1.1, 0.85, 0.0, 0.0, # leg
0.5, 0.5, # head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.2, # left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.2 # right_arm
]
# Base pose command odom(x, y, yaw)
base_x_1 = 0.2
base_y_1 = 0.0
base_yaw_1 = 0.0
# Optional frames (frame_id: base_link/odom/map, reference_frame_id: odom/map)
frame_id = "base_link"
reference_frame_id = "odom"
# Chassis pose interpolation time (seconds), used to generate a smooth chassis trajectory
base_time_s = 15.0
time.sleep(1)
# reset to zero
result = robot.zero_whole_body_and_base(
frame_id,
reference_frame_id,
True,
0.2,
15.0,
)
print("Zero joint status:", result[0])
print("Zero base status:", result[1])
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
一键回零-map坐标系（whole_body_reset_zero_map）
适用场景：快速将全身上下所有关节运动到零位（初始姿态），基于 map 坐标系。
examples/python/galbot_robot/whole_body_reset_zero_map_example.py
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import GalbotMotion
import time
def main():
robot = GalbotRobot()
motion = GalbotMotion()
if not robot.init():
print("GalbotRobot init failed.")
return
if not motion.init():
print("GalbotMotion init failed.")
return
time.sleep(2)
# Whole-body joints: leg(5) + head(2) + left_arm(7) + right_arm(7)
whole_body_joint_1 = [
0.25, 1.1, 0.85, 0.0, 0.0, # leg
0.5, 0.5, # head
2.0, -1.55, -0.55, -1.7, -0.0, -0.8, 0.2, # left_arm
-2.0, 1.55, 0.55, 1.7, 0.0, 0.8, 0.2 # right_arm
]
# Base pose command map(x, y, yaw) Note: adjust based on the robot's actual localization in the map frame
base_x_1 = -0.4
base_y_1 = 0.226593
base_yaw_1 = 0.0
# Optional frames (frame_id: base_link/odom/map, reference_frame_id: odom/map)
frame_id = "base_link"
reference_frame_id = "map"
time.sleep(1)
# reset to zero
result = robot.zero_whole_body_and_base(
frame_id,
reference_frame_id,
True,
0.2,
15.0,
)
print("Zero joint status:", result[0])
print("Zero base status:", result[1])
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
紧急停止与恢复（emergency_stop / resume_from_emergency_stop）
适用场景：立即切断所有电机驱动，用于需要快速断电的安全场景；随后在触发紧急停止后重新使能所有电机，恢复机器人正常运行状态。
examples/python/galbot_robot/emergency_stop_and_resume_example.py
import signal
import threading
import time
from galbot_sdk.g1 import ControlStatus, GalbotRobot
motion_done = threading.Event()
emergency_triggered = threading.Event()
def move_head_joints(robot, target_position):
status = robot.set_joint_positions(
[target_position], [], ["head_joint2"], True, 0.05, 15.0
)
print(f"Head motion returned: {status}")
def emergency_stop_on_ctrl_c(robot):
while not motion_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is not None:
break
else:
return
print("Worker thread: triggering software emergency stop...")
emergency_triggered.set()
status = robot.emergency_stop()
print(f"Worker thread: emergency_stop returned: {status}")
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the area is clear.")
if input("Start head emergency-stop test? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")
# Move an arm in the main thread, then emergency-stop it after Ctrl-C.
print("Head is moving. Press Ctrl-C to trigger software emergency stop.")
stop_thread = threading.Thread(target=emergency_stop_on_ctrl_c, args=(robot,))
stop_thread.start()
target_position = 0.4
move_head_joints(robot, target_position)
motion_done.set()
stop_thread.join()
if emergency_triggered.is_set():
print("Waiting 3 seconds before resuming from emergency stop...")
time.sleep(3)
print("Resuming from software emergency stop...")
status = robot.resume_from_emergency_stop()
if status == ControlStatus.SUCCESS:
print("Resume from emergency stop successfully.")
print("Waiting 10 seconds for the robot to stabilize after recovery...")
time.sleep(10)
print("Resuming head motion to the original target position...")
move_head_joints(robot, target_position)
else:
print("Resume from emergency stop failed.")
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")
全身控制末端执行器命令（set_end_effector_command / clear_end_effector_command）
适用场景：需要根据高频模型推理结果实时控制末端笛卡尔位姿的场景。使用 clear_end_effector_command 可停止并清除当前末端实时控制指令。
examples/python/galbot_robot/set_end_effector_commands.py
"""
WBC end-effector command example
Demonstrates the whole-body control end-effector trajectory API:
- set_end_effector_command: publish pose targets (each pose is [x,y,z,qx,qy,qz,qw])
- clear_end_effector_command: clear end-effector task commands on the WBC channel
reference_frames: omit or [] to use ``world`` for every pose; otherwise one string per pose,
typically ``world`.
"""
import sys
import os
import time
# Add pybind library path to PYTHONPATH
script_path = os.path.abspath(__file__)
sdk_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(script_path))))
# Add package path (for galbot_sdk module)
package_path = os.path.join(sdk_root, "pybind", "package")
# Add lib path (for .so files)
lib_path = os.path.join(sdk_root, "pybind", "output", "linux-x86_64-gcc940", "lib")
for p in [package_path, lib_path]:
if p not in sys.path:
sys.path.insert(0, p)
print(f"Added paths: {package_path}, {lib_path}")
from galbot_sdk.g1 import GalbotRobot
def main():
# Get GalbotRobot singleton and initialize
robot = GalbotRobot()
robot.init()
print("Initialization successful")
time.sleep(2)
ee_info = robot.get_wbc_end_effector_poses()# Before using this function, ensure that `using_wbc` in the configuration file is set to `true`.
print("\nCurrent WBC end-effector poses:")
for frame, pose in ee_info.items():
print(f" Frame: {frame}, Pose: {pose}")
ree_pose = ee_info.get("ree_pose") # keys: "ree_pose", "lee_pose", "head_pose"
if ree_pose and len(ree_pose) >= 7:
target_pose = list(ree_pose[:7])
target_pose[0] -= 0.1
# Example: absolute pose — omit reference_frames to default every pose to "world"
robot.set_end_effector_command(
poses=[target_pose],
end_effector_frames=["right_arm_end_effector_mount_link"],
)
print('\nPublished end-effector command (default reference_frames → world)')
time.sleep(3)
robot.clear_end_effector_command() # clear and return to default reference joint pose
# robot.clear_end_effector_command(hold=True) # clear and hold current joint pose
# Send shutdown signal
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resource cleanup successful")
if __name__ == "__main__":
main()
音频播放（play_audio / stop_audio）
适用场景：通过机器人扬声器播放音频文件，并停止当前音频播放任务。
examples/python/galbot_robot/audio_example.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import time
import struct
import math
import os
from galbot_sdk.g1 import GalbotRobot
# global variable
audio_callback_count = 0
last_audio_type = None
# Command help table
COMMAND_TABLE = [
{
"cmd": "start_microphone_stream_input",
"description": "Start microphone streaming audio input feature",
"parameters": "No parameters",
"example": "start_microphone_stream_input"
},
{
"cmd": "stop_microphone_stream_input",
"description": "Stop the specified microphone streaming audio input",
"parameters": "No parameters",
"example": "stop_microphone_stream_input"
},
{
"cmd": "write_audio_stream_output",
"description": "PCM audiodata audio output",
"parameters": "Enter audio file name to play (16K 16bit Mono PCM audio file path):",
"example": "write_audio_stream_output"
},
{
"cmd": "stop_audio_stream_output",
"description": "Stop the specified audio output stream or all active audio output streams",
"parameters": "No parameters",
"example": "stop_audio_stream_output"
},
{
"cmd": "set_volume",
"description": "Set system global volume",
"parameters": "Please enter volume setting parameter (0-100)",
"example": "set_volume"
},
{
"cmd": "get_volume",
"description": "Get current system global volume",
"parameters": "No parameters",
"example": "get_volume"
},
{
"cmd": "help",
"description": "Show help",
"parameters": "No parameters",
"example": "help"
},
{
"cmd": "q",
"description": "Exit program",
"parameters": "No parameters",
"example": "q"
}
]
# command
def print_command_help():
print("\n================ Command Help Table =================")
print(f"{'Command':<40} | {'Description':<30} | {'Parameters':<20} | {'example':<30}")
print("-" * 130)
for cmd in COMMAND_TABLE:
print(
f"{cmd['cmd']:<40} | {cmd['description']:<30} | {cmd['parameters']:<20} | {cmd['example']:<30}")
print("=============================================\n")
# Safely read float input
def safe_read_float(prompt: str = "") -> float:
"""
Safely get float input
Args:
prompt: prompt text
Returns:
Valid float input
"""
if prompt:
print(prompt, end='', flush=True)
try:
value = float(input())
return value
except ValueError:
print("Input error, please enter a valid float.")
return None
except EOFError:
print("\nInput ended")
return None
# Safely read integer input
def safe_read_int(prompt: str = "") -> int:
"""
Safely get integer input
Args:
prompt: prompt text
Returns:
Valid integer input
"""
if prompt:
print(prompt, end='', flush=True)
try:
value = int(input())
return value
except ValueError:
print("Input error, please enter a valid integer.")
return None
except EOFError:
print("\nInput ended")
return None
# Safely read filename input and handle input errors
def safe_read_filename(prompt: str = "") -> str:
"""
Safely get filename input
Args:
prompt: prompt text
Returns:
Valid filename input
"""
if prompt:
print(prompt, end='', flush=True)
try:
filename = input()
return filename
except ValueError:
print("Input error, please enter a valid file name.")
# Save audio data
def save_audio_to_file(filename: str, audio_data: bytes):
"""
Save audio data
Args:
filename: file name
audio_data: audio data (bytes)
"""
try:
with open(filename, 'ab') as f:
f.write(audio_data)
# print(f"✓ Audio appended and saved to {filename}")
except Exception as e:
print(f"✗ Failed to save file: {e}")
# Read audio data from file
def read_audio_from_file(filename: str, chunk_size: int = 2560) -> list:
"""
Read audio data from file
Args:
filename: file name
chunk_size: chunk size (bytes)
Returns:
Audio data block list
"""
try:
audio_chunks = []
with open(filename, 'rb') as f:
while True:
chunk = f.read(chunk_size)
if not chunk:
break
audio_chunks.append(chunk)
print(f"✓ Read {len(audio_chunks)} chunks from {filename}")
return audio_chunks
except FileNotFoundError:
print(f"✗ File not found: {filename}")
return []
except Exception as e:
print(f"✗ Failed to read file: {e}")
return []
# Audio callback function
def audio_callback(audio_data: dict):
"""
Audio data callback function
Args:
audio_data: audio data dictionary including header, type, format, data
"""
global audio_callback_count, last_audio_type
if not audio_data:
return
header = audio_data.get("header", {})
timestamp_ns = header.get("timestamp_ns", 0)
audio_type = audio_data.get("type", "unknown")
audio_format = audio_data.get("format", "unknown")
data_size = len(audio_data.get("data", b""))
audio_callback_count += 1
last_audio_type = audio_type
# Print audio data info
if audio_type == "vad_begin":
print(f"\n[VAD_BEGIN] Voice activity detection started")
pass
elif audio_type == "vad_end":
print(f"[VAD_END] Voice activity detection ended")
pass
elif audio_type == "vad_chunk":
# print(f"[AUDIO_CHUNK] Received {data_size} bytes of {audio_format} audio data (timestamp_ns: {timestamp_ns})")
save_audio_to_file("mic_vad.pcm", audio_data.get("data", b""))
pass
elif audio_type == "denoise_chunk":
# print(f"[DENOISE_AUDIO] Received {data_size} bytes of denoised audio (timestamp_ns: {timestamp_ns})")
save_audio_to_file("mic_denoise.pcm", audio_data.get("data", b""))
pass
elif audio_type == "waken_up":
print(f"\n[WAKEN_UP] Wake-up event triggered: ", audio_data.get("data", ""))
pass
else:
print(f"[{audio_type}] Received audio data, size: {data_size} bytes")
pass
def main():
# Get GalbotRobot singleton
base_instance = GalbotRobot()
ok = base_instance.init()
if not ok:
print("Initialization failed")
return -1
# Waiting for init done
time.sleep(5)
print("\n========================================")
print(" Galbot audio test tool started")
print("========================================\n")
print_command_help()
microphone_stream_id = ""
speaker_stream_id = "speaker_audio_output_test"
while base_instance.is_running():
print("Enter operation to execute:", end='', flush=True)
cmd_line = input().strip()
if not cmd_line:
continue
parts = cmd_line.split()
cmd = parts[0]
try:
if cmd == "help":
print_command_help()
elif cmd == "start_microphone_stream_input":
print("Starting microphone streaming audio input...")
global audio_callback_count, last_audio_type
microphone_stream_id = base_instance.start_microphone_stream_input(
callback=audio_callback,
chunk_size=2560,
use_raw_audio=False
)
audio_callback_count = 0
last_audio_type = None
print(f"✓ Microphone started successfully, stream ID: {microphone_stream_id}")
elif cmd == "stop_microphone_stream_input":
if microphone_stream_id is None:
print("No active microphone stream")
else:
print(f"Stopping microphone stream (ID: {microphone_stream_id})...")
base_instance.stop_microphone_stream_input(microphone_stream_id)
print(f"✓ Microphone stopped")
print(f" - Number of callbacks: {audio_callback_count}")
print(f" - Last audio type: {last_audio_type}")
microphone_stream_id = None
elif cmd == "write_audio_stream_output":
audio_output_file = safe_read_filename("Enter audio file name to play (16K 16bit Mono PCM audio file path): ")
print("Reading audio file...")
# Read and play audio
audio_chunks = read_audio_from_file(audio_output_file, chunk_size=2560)
if audio_chunks:
print(f"Playing audio stream ID: {speaker_stream_id}")
for i, chunk in enumerate(audio_chunks):
success = base_instance.write_audio_stream_output(chunk, speaker_stream_id)
if not success:
print(f"✗ Playback failed for chunk {i+1}")
break
time.sleep(0.05)
print(f"✓ Audio playback completed")
elif cmd == "stop_audio_stream_output":
print("Stopping audio output stream...")
base_instance.stop_audio_stream_output(speaker_stream_id)
print("✓ Audio output stream stopped")
elif cmd == "set_volume":
volume = safe_read_float("Please enter volume setting parameter (0-100): ")
if volume is None or volume < 0 or volume > 100:
print(f"✗ Volume must be between 0-100; input value: {volume}")
continue
if (not base_instance.set_volume(volume)):
print(f"✗ Failed to set volume({volume})")
print(f"✓ Volume set to: {volume}%")
elif cmd == "get_volume":
volume = base_instance.get_volume()
print(f"Current system volume: {volume}%")
elif cmd == "q":
print("Program exiting...")
break
else:
print(f"Unknown command: {cmd}, enter 'help' to see available commands")
except (ValueError, TypeError) as e:
print(f"Parameters error: {e}")
except Exception as e:
print(f"Command execution exception: {e}")
base_instance.request_shutdown()
base_instance.wait_for_shutdown()
base_instance.destroy()
print("Program exited")
if __name__ == "__main__":
main()
控制器管理
适用场景：查询控制器状态，并切换或管理机器人各部件使用的控制器。
examples/python/galbot_robot/controller_management_example.py
import time
from galbot_sdk.g1 import GalbotRobot, ControlStatus, G1ControllerName, G1JointGroup
def print_active_controller(group_name, controller_name):
print(f"Active controller for {group_name}: {controller_name}")
def print_status(operation, status):
if status == ControlStatus.SUCCESS:
print(f"{operation}: SUCCESS")
else:
# Assuming status can be cast to int or printed directly
print(f"{operation}: FAILED (Status: {status})")
def main():
# Get robot instance and init
robot = GalbotRobot()
print("Initializing robot...")
if robot.init():
print("System initialized successfully!")
else:
print("System initialization failed!")
return
# Wait for data readiness
time.sleep(1)
# Define the controller we want to test
# Using LEFT_ARM_PVT_CTRL as the test subject
test_controller = G1ControllerName.LEFT_ARM_PVT_CTRL
controller_str = "LEFT_ARM_PVT_CTRL"
# Using string for display and G1JointGroup enum for querying
group_str = "left_arm"
test_group = G1JointGroup.left_arm
print("\n--- Starting Controller Management Example ---")
# 1. Get active controller before switching
active_controller = robot.get_active_controller(test_group)
print_active_controller(group_str, active_controller)
# 2. Switch controller
print(f"\n[Test 1] Switching to {controller_str}...")
status = robot.switch_controller(test_controller)
print_status("switch_controller", status)
time.sleep(0.5)
active_controller = robot.get_active_controller(test_group)
print_active_controller(group_str, active_controller)
# 3. Stop controller
print(f"\n[Test 2] Stopping controller for group {group_str}...")
# Using string overload/version for group
status = robot.stop_controller(group_str)
print_status("stop_controller", status)
time.sleep(0.5)
# 4. Release controller
print(f"\n[Test 3] Releasing controller for group {group_str}...")
status = robot.release_controller(group_str)
print_status("release_controller", status)
time.sleep(0.5)
# 5. Acquire controller
print(f"\n[Test 4] Acquiring {controller_str}...")
status = robot.acquire_controller(test_controller)
print_status("acquire_controller", status)
time.sleep(0.5)
# 6. Start controller
print(f"\n[Test 5] Starting controller for group {group_str}...")
status = robot.start_controller(group_str)
print_status("start_controller", status)
time.sleep(0.5)
print("\n--- Controller Management Example Finished ---")
# Shutdown
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
if __name__ == "__main__":
main()
停止控制器（stop_controller）
适用场景：停止正在执行运动的活动控制器。
examples/python/galbot_robot/stop_controller.py
"""Stop an active head controller from another Python thread."""
import signal
import threading
import time
from galbot_sdk.g1 import GalbotRobot
motion_done = threading.Event()
shutdown_requested = threading.Event()
def move_head_joints(robot, target_position):
status = robot.set_joint_positions(
[target_position], [], ["head_joint2"], False, 0.05, 15.0
)
print(f"Head motion command sent: {status}")
def stop_controller_on_ctrl_c(robot, group_name):
while not motion_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is not None:
break
else:
return
print(f"Worker thread: stopping {group_name} controller...")
status = robot.stop_controller(group_name)
print(f"Worker thread: stop_controller returned: {status}")
shutdown_requested.set()
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the area is clear.")
if input("Start head motion? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(1)
group_name = "head"
# Send a non-blocking motion command, then stop its controller after Ctrl-C.
print("Head is moving. Press Ctrl-C to stop its controller.")
stop_thread = threading.Thread(
target=stop_controller_on_ctrl_c, args=(robot, group_name)
)
stop_thread.start()
move_head_joints(robot, 0.4)
# The command is non-blocking, so retain the Ctrl-C listener for its full
# command timeout unless it has already requested shutdown.
shutdown_requested.wait(timeout=15.0)
motion_done.set()
stop_thread.join()
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
获取并设置灵巧手状态（get_dexhand_state && set_dexhand_command）
适用场景：读取已配置灵巧手的关节状态和运行状态并向已配置的灵巧手发送目标关节指令。
examples/python/galbot_robot/get_set_dexhand_state.py
import time
from galbot_sdk.g1 import GalbotRobot, G1JointGroup, ControlStatus, JointCommand, DexHandType
CYCLE_COUNT = 3
COMMAND_INTERVAL_SECONDS = 1.0
INIT_WAIT_SECONDS = 2.0
# The following demo positions are copied from the standalone set_dexhand_command example.
# Different dexterous hand models expose different joint counts and units, so each model
# needs its own open/close target list.
INSPIRE_RH56DFX_POSITION_OPEN = [
1000.0, # finger1
1000.0, # finger2
1000.0, # finger3
1000.0, # finger4
1000.0, # finger5
1000.0, # finger6
]
INSPIRE_RH56DFX_POSITION_CLOSE = [
850.0, # finger1
350.0, # finger2
60.0, # finger3
60.0, # finger4
60.0, # finger5
60.0, # finger6
]
INSPIRE_RH56F2_POSITION_OPEN = [
1750.0, # thumb_bend
1550.0, # thumb_rotate
1740.0, # index
1740.0, # middle
1740.0, # ring
1740.0, # little
]
INSPIRE_RH56F2_POSITION_CLOSE = [
1600.0, # thumb_bend
1250.0, # thumb_rotate
950.0, # index
950.0, # middle
950.0, # ring
950.0, # little
]
BRAINCO_POSITION_OPEN = [
100.0, # finger1
100.0, # finger2
100.0, # finger3
100.0, # finger4
100.0, # finger5
100.0, # finger6
]
BRAINCO_POSITION_CLOSE = [
33.3, # finger1
100.0, # finger2
86.7, # finger3
86.7, # finger4
86.7, # finger5
86.7, # finger6
]
LINKER_L20_POSITION_OPEN = [
1.3543754995475996, # thumb_roll
1.562069680534925, # thumb_yaw
0.23561944901923448, # index_yaw
0.23561944901923448, # middle_yaw
0.23561944901923448, # ring_yaw
0.23561944901923448, # little_yaw
0.82030474843733492, # thumb_root
1.2217304763960306, # index_root
1.2217304763960306, # middle_root
1.2217304763960306, # ring_root
1.2217304763960306, # little_root
1.2042771838760873, # thumb_tip
1.7453292519943295, # index_tip
1.7453292519943295, # middle_tip
1.7453292519943295, # ring_tip
1.7453292519943295, # little_tip
]
LINKER_L20_POSITION_CLOSE = [
0.85, # thumb_roll
1.55, # thumb_yaw
0.00, # index_yaw
0.00, # middle_yaw
0.00, # ring_yaw
0.00, # little_yaw
0.40, # thumb_root
0.00, # index_root
0.00, # middle_root
0.00, # ring_root
0.00, # little_root
0.35, # thumb_tip
0.00, # index_tip
0.00, # middle_tip
0.00, # ring_tip
0.00, # little_tip
]
SHARPA_POSITION_OPEN = [
0.0, # thumb CMC_FE
0.0, # thumb CMC_AA
0.0, # thumb MCP_FE
0.0, # thumb MCP_AA
0.0, # thumb IP
0.0, # index MCP_FE
0.0, # index MCP_AA
0.0, # index PIP
0.0, # index DIP
0.0, # middle MCP_FE
0.0, # middle MCP_AA
0.0, # middle PIP
0.0, # middle DIP
0.0, # ring MCP_FE
0.0, # ring MCP_AA
0.0, # ring PIP
0.0, # ring DIP
0.0, # pinky CMC
0.0, # pinky MCP_FE
0.0, # pinky MCP_AA
0.0, # pinky PIP
0.0, # pinky DIP
]
SHARPA_POSITION_CLOSE = [
0.87, # thumb CMC_FE
0.0, # thumb CMC_AA
0.44, # thumb MCP_FE
0.0, # thumb MCP_AA
0.87, # thumb IP
0.78, # index MCP_FE
0.0, # index MCP_AA
0.87, # index PIP
0.70, # index DIP
0.78, # middle MCP_FE
0.0, # middle MCP_AA
0.87, # middle PIP
0.70, # middle DIP
0.78, # ring MCP_FE
0.0, # ring MCP_AA
0.87, # ring PIP
0.70, # ring DIP
0.13, # pinky CMC
0.78, # pinky MCP_FE
0.0, # pinky MCP_AA
0.87, # pinky PIP
0.70, # pinky DIP
]
DEXHAND_TYPE_MAP = {
"inspire": DexHandType.INSPIRE,
"inspire_rh56dfx": DexHandType.INSPIRE_RH56DFX,
"inspire_rh56f2": DexHandType.INSPIRE_RH56F2,
"brainco": DexHandType.BRAINCO,
"revo": DexHandType.BRAINCO,
"sharpa": DexHandType.SHARPA,
"linker_l20": DexHandType.LINKER_L20,
}
EXIT_INPUTS = {"q", "quit", "exit"}
SUPPORTED_TYPE_TEXT = ", ".join(DEXHAND_TYPE_MAP.keys())
def print_supported_types() -> None:
"""Print all dexhand model names accepted by this combined get/set example."""
print("Supported dexterous hand types:")
print(" inspire Compatibility alias for inspire_rh56f2")
print(" inspire_rh56dfx Inspire RH56DFX dexterous hand")
print(" inspire_rh56f2 Inspire RH56F2 dexterous hand")
print(" brainco BrainCo dexterous hand")
print(" revo Alias for brainco")
print(" sharpa Sharpa dexterous hand")
print(" linker_l20 Linker Hand L20 dexterous hand")
def read_dexhand_type(side_name: str):
"""Read one hand type before robot initialization so invalid input fails early."""
while True:
raw_value = input(f"Enter {side_name} dexterous hand type (or q to quit): ").strip()
key = raw_value.lower()
if key in EXIT_INPUTS:
return None, None
if key in DEXHAND_TYPE_MAP:
return DEXHAND_TYPE_MAP[key], key
print(f"Unsupported dexterous hand type: {raw_value}")
print(f"Please choose from: {SUPPORTED_TYPE_TEXT}")
def make_dexhand_command(positions) -> list:
"""Convert a list of target positions into SDK JointCommand objects."""
commands = []
for position in positions:
cmd = JointCommand()
cmd.position = position
commands.append(cmd)
return commands
def dexhand_motion_positions(dexhand_type: DexHandType, action: str) -> list:
"""Return the demo open or close position list for the selected hand model."""
is_open = action == "open"
if dexhand_type == DexHandType.INSPIRE_RH56DFX:
return INSPIRE_RH56DFX_POSITION_OPEN if is_open else INSPIRE_RH56DFX_POSITION_CLOSE
if dexhand_type in (DexHandType.INSPIRE, DexHandType.INSPIRE_RH56F2):
return INSPIRE_RH56F2_POSITION_OPEN if is_open else INSPIRE_RH56F2_POSITION_CLOSE
if dexhand_type == DexHandType.BRAINCO:
return BRAINCO_POSITION_OPEN if is_open else BRAINCO_POSITION_CLOSE
if dexhand_type == DexHandType.SHARPA:
return SHARPA_POSITION_OPEN if is_open else SHARPA_POSITION_CLOSE
if dexhand_type == DexHandType.LINKER_L20:
return LINKER_L20_POSITION_OPEN if is_open else LINKER_L20_POSITION_CLOSE
raise ValueError(f"Unsupported dexhand type: {dexhand_type}")
def print_dexhand_state(hand_name: str, dexhand_state, dexhand_type: DexHandType) -> None:
"""Print joint states, and print force sensors for Sharpa hands when available."""
type_label = "sharpa" if dexhand_type == DexHandType.SHARPA else "dexterous"
print(f"{hand_name} {type_label} hand state:")
print(f"Timestamp (ns): {dexhand_state.timestamp_ns}")
joint_state_vec = dexhand_state.joint_state.joint_state_vec
print(f" Joint states ({len(joint_state_vec)} joints):")
for i, js in enumerate(joint_state_vec):
if dexhand_type == DexHandType.SHARPA:
# Sharpa state does not use the generic acceleration field in the same way.
print(
f" joint{i + 1}: position={js.position:.4f}, velocity={js.velocity:.4f}, "
f"effort={js.effort:.4f}, current={js.current:.4f}"
)
else:
# Some SDK backends fill joint_name; otherwise keep a stable fallback label.
joint_label = js.joint_name if js.joint_name else f"{hand_name.lower()}_dexhand_joint{i + 1}"
print(
f" {joint_label}: "
f"position={js.position:.4f}, velocity={js.velocity:.4f}, "
f"acceleration={js.acceleration:.4f}, "
f"effort={js.effort:.4f}, current={js.current:.4f}"
)
if dexhand_type != DexHandType.SHARPA:
return
force_map = dexhand_state.force_sensor_map
if not force_map:
print(" (no force sensor data)")
else:
print(f" Force sensors ({len(force_map)} sensors):")
for sensor_name, effort in force_map.items():
print(
f" {sensor_name} @ {effort.timestamp_ns}: "
f"Fx={effort.force.x:.4f}, Fy={effort.force.y:.4f}, Fz={effort.force.z:.4f}, "
f"Mx={effort.torque.x:.4f}, My={effort.torque.y:.4f}, Mz={effort.torque.z:.4f}"
)
def main():
print("Starting get_set_dexhand_state example")
print_supported_types()
# Ask the user to select the actual dexterous hand model installed on each side.
left_type, left_label = read_dexhand_type("left")
if left_type is None:
print("Example canceled before robot initialization")
return
right_type, right_label = read_dexhand_type("right")
if right_type is None:
print("Example canceled before robot initialization")
return
robot_initialized = False
robot = GalbotRobot()
try:
# Initialize the robot SDK before sending commands or reading states.
robot.init()
robot_initialized = True
print("Initialization succeeded")
time.sleep(INIT_WAIT_SECONDS)
# Run one initial open command, then run three close/open demo cycles.
completed = True
for action_index, action in enumerate(["open"] + ["close", "open"] * CYCLE_COUNT):
if action_index > 0 and action == "close":
cycle_index = (action_index + 1) // 2
print(f"Running dexterous hand cycle {cycle_index}/{CYCLE_COUNT}")
# Send the target position command to the left dexterous hand.
left_positions = dexhand_motion_positions(left_type, action)
left_command = make_dexhand_command(left_positions)
left_status = robot.set_dexhand_command(
G1JointGroup.left_dexhand,
left_command,
left_type,
False,
)
if left_status != ControlStatus.SUCCESS:
print(
f"Failed to {action} left {left_label} dexterous hand "
f"({len(left_command)} joints), status={left_status}"
)
completed = False
else:
print(f"Left {left_label} dexterous hand {action} command sent ({len(left_command)} joints)")
# Send the same action to the right dexterous hand with its own model type.
right_positions = dexhand_motion_positions(right_type, action)
right_command = make_dexhand_command(right_positions)
right_status = robot.set_dexhand_command(
G1JointGroup.right_dexhand,
right_command,
right_type,
False,
)
if right_status != ControlStatus.SUCCESS:
print(
f"Failed to {action} right {right_label} dexterous hand "
f"({len(right_command)} joints), status={right_status}"
)
completed = False
else:
print(f"Right {right_label} dexterous hand {action} command sent ({len(right_command)} joints)")
# Give the hardware or simulator time to execute the command before reading state.
time.sleep(COMMAND_INTERVAL_SECONDS)
# Read back the left hand state after the command has been applied.
left_state = robot.get_dexhand_state(G1JointGroup.left_dexhand, left_type)
if left_state is None:
print("get left dexterous hand state error")
completed = False
else:
print_dexhand_state("Left", left_state, left_type)
# Read back the right hand state after the command has been applied.
right_state = robot.get_dexhand_state(G1JointGroup.right_dexhand, right_type)
if right_state is None:
print("get right dexterous hand state error")
completed = False
else:
print_dexhand_state("Right", right_state, right_type)
if completed:
print("get_set_dexhand_state example completed")
else:
print("get_set_dexhand_state example completed with one or more errors")
finally:
# Release SDK resources only after the robot has been initialized.
if robot_initialized:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
if __name__ == "__main__":
main()
获取力传感器数据（get_force_sensor_data）
适用场景：获取力和力矩测量值，用于接触检测及力感知操作。
examples/python/galbot_robot/get_force_sensor_data.py
import time
from galbot_sdk.g1 import GalbotRobot, GalbotOneFoxtrotSensor
def force_sensor_type_to_string(sensor_type: GalbotOneFoxtrotSensor) -> str:
"""Convert force sensor type to string"""
type_map = {
GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE: "LEFT_WRIST_FORCE",
GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE: "RIGHT_WRIST_FORCE",
}
return type_map.get(sensor_type, "UNKNOWN_FORCE_SENSOR")
def print_force_data(sensor_type: GalbotOneFoxtrotSensor, force_data: dict):
"""
Print force sensor data
force_data: dict, including the following fields:
- 'timestamp_ns': Timestamp (ns)
- 'force': {'x', 'y', 'z'} Force (N)
- 'torque': {'x', 'y', 'z'} Torque (N·m)
"""
print(f"--- {force_sensor_type_to_string(sensor_type)} ---")
if not force_data:
print(" Force data is empty")
return
print(f" Timestamp (ns): {force_data.get('timestamp_ns')}")
force = force_data.get("force", {})
print(f" Force (N): fx={force.get('x')}, fy={force.get('y')}, fz={force.get('z')}")
torque = force_data.get("torque", {})
print(f" Torque (Nm): tx={torque.get('x')}, ty={torque.get('y')}, tz={torque.get('z')}")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get left wrist force sensor data
print("\n===== Get left wrist force sensor data =====")
left_force_data = robot.get_force_sensor_data(GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE)
print_force_data(GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE, left_force_data)
# Get calibrated left-arm wrench expressed in base_link
print("\n===== Get calibrated left wrist force data in base_link =====")
calibrated_left_force_data = robot.get_force_sensor_data(
GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE, calibrated=True, ref_frame="base_link"
)
print_force_data(GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE, calibrated_left_force_data)
# Get right wrist force sensor data
print("\n===== Get right wrist force sensor data =====")
right_force_data = robot.get_force_sensor_data(GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE)
print_force_data(GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE, right_force_data)
# Keep calibrated right-arm wrench in its native end-effector mount frame
print("\n===== Get calibrated right wrist force data in mount frame =====")
calibrated_right_force_data = robot.get_force_sensor_data(
GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE,
calibrated=True,
ref_frame="right_arm_end_effector_mount_link",
)
print_force_data(GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE, calibrated_right_force_data)
# Iterate and get all force sensor data
print("\n===== Get all force sensor data =====")
force_sensor_types = [
GalbotOneFoxtrotSensor.LEFT_WRIST_FORCE,
GalbotOneFoxtrotSensor.RIGHT_WRIST_FORCE,
]
for sensor_type in force_sensor_types:
force_data = robot.get_force_sensor_data(sensor_type)
print_force_data(sensor_type, force_data)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取里程计数据（get_odom）
适用场景：读取里程计报告的机器人底盘位姿和运动状态。
examples/python/galbot_robot/get_odom.py
import time
from galbot_sdk.g1 import GalbotRobot
def print_odom_data(odom_data: dict):
"""
Print odometry data
odom_data: dict, includes:
- 'timestamp_ns': Timestamp (ns)
- 'position': [x, y, z] Position (m)
- 'orientation': [qx, qy, qz, qw] Quaternion orientation
- 'linear_velocity': [vx, vy, vz] Linear velocity (m/s)
- 'angular_velocity': [wx, wy, wz] Angular velocity (rad/s)
"""
if not odom_data:
print("Odom data is empty")
return
print(f"Timestamp (ns): {odom_data.get('timestamp_ns')}")
position = odom_data.get("position", [])
if position:
print(f"Position (m): x={position[0]}, y={position[1]}, z={position[2]}")
orientation = odom_data.get("orientation", [])
if orientation:
print(f"Orientation (quaternion): qx={orientation[0]}, qy={orientation[1]}, "
f"qz={orientation[2]}, qw={orientation[3]}")
linear_velocity = odom_data.get("linear_velocity", [])
if linear_velocity:
print(f"Linear velocity (m/s): vx={linear_velocity[0]}, vy={linear_velocity[1]}, "
f"vz={linear_velocity[2]}")
angular_velocity = odom_data.get("angular_velocity", [])
if angular_velocity:
print(f"Angular velocity (rad/s): wx={angular_velocity[0]}, wy={angular_velocity[1]}, "
f"wz={angular_velocity[2]}")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
robot.init()
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get odometry data
odom_data = robot.get_odom()
if not odom_data:
print("No odom data!")
else:
print("Odometry data:")
print_odom_data(odom_data)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
获取超声波数据（get_ultrasonic_data）
适用场景：读取超声波测距数据，用于近距离障碍物感知。
examples/python/galbot_robot/get_ultrasonic_data.py
import time
from galbot_sdk.g1 import GalbotRobot, SensorType, UltrasonicType
def ultrasonic_type_to_string(ultrasonic_type: UltrasonicType) -> str:
"""Convert ultrasonic sensor type to string"""
type_map = {
UltrasonicType.FRONT_LEFT: "FRONT_LEFT",
UltrasonicType.FRONT_RIGHT: "FRONT_RIGHT",
UltrasonicType.RIGHT_LEFT: "RIGHT_LEFT",
UltrasonicType.RIGHT_RIGHT: "RIGHT_RIGHT",
UltrasonicType.BACK_LEFT: "BACK_LEFT",
UltrasonicType.BACK_RIGHT: "BACK_RIGHT",
UltrasonicType.LEFT_LEFT: "LEFT_LEFT",
UltrasonicType.LEFT_RIGHT: "LEFT_RIGHT",
}
return type_map.get(ultrasonic_type, "UNKNOWN_ULTRASONIC")
def print_ultrasonic_data(ultrasonic_type: UltrasonicType, ultrasonic_data: dict):
"""
Print ultrasonic sensor data
ultrasonic_data: dict, including the following fields:
- 'timestamp_ns': Timestamp (ns)
- 'distance': Distance (m)
"""
print(f"--- {ultrasonic_type_to_string(ultrasonic_type)} ---")
if not ultrasonic_data:
print(" Ultrasonic data is empty")
return
print(f" Timestamp (ns): {ultrasonic_data.get('timestamp_ns')}")
print(f" Distance (m): {ultrasonic_data.get('distance')}")
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
# Initialize sensors; only sensors passed during initialization can retrieve data to save resources
enable_sensor_set = {SensorType.BASE_ULTRASONIC}
robot.init(enable_sensor_set)
# Program started, waiting for data
time.sleep(1)
print("Initialization succeeded")
# Get single ultrasonic sensor data
print("\n===== Get single ultrasonic sensor data =====")
ultrasonic_data = robot.get_ultrasonic_data(UltrasonicType.FRONT_LEFT)
print_ultrasonic_data(UltrasonicType.FRONT_LEFT, ultrasonic_data)
# Iterate to get all 8 ultrasonic sensor data
print("\n===== Get all ultrasonic sensor data =====")
ultrasonic_types = [
UltrasonicType.FRONT_LEFT,
UltrasonicType.FRONT_RIGHT,
UltrasonicType.RIGHT_LEFT,
UltrasonicType.RIGHT_RIGHT,
UltrasonicType.BACK_LEFT,
UltrasonicType.BACK_RIGHT,
UltrasonicType.LEFT_LEFT,
UltrasonicType.LEFT_RIGHT,
]
for ultrasonic_type in ultrasonic_types:
data = robot.get_ultrasonic_data(ultrasonic_type)
print_ultrasonic_data(ultrasonic_type, data)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
安装验证
适用场景：验证 SDK 安装、机器人连接、初始化以及基础接口是否可用。
examples/python/galbot_robot/installation_welcome_verify.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from __future__ import annotations
import os
import sys
import time
from typing import List, Sequence
import cv2
import numpy as np
from galbot_sdk.g1 import (
ControlStatus,
GalbotRobot,
JointCommand,
SensorType, RgbOutputFormat,
Trajectory,
TrajectoryPoint,
)
_K_SPEED_RAD_S = 0.12
_K_TIMEOUT_S = 20.0
_K_WELCOME_PCM_REL: str = ""
_K_PRESET_LEG: List[float] = [0.5, 1.5, 1.0, 0.0, 0.0]
_K_PRESET_HEAD: List[float] = [0.0, 0.0]
_K_PRESET_LEFT_ARM: List[float] = [2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0]
_K_PRESET_RIGHT_ARM: List[float] = [-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0]
_K_HEAD_ARMS_DEMO_GROUPS: List[str] = ["head", "left_arm", "right_arm"]
_K_HEAD_ARMS_DEMO_DT_S = 0.06
_K_HEAD_ARMS_DEMO_SEG1 = 28
_K_HEAD_ARMS_DEMO_SEG2 = 32
_K_HEAD_ARMS_DEMO_SEG3 = 28
_K_HEAD_SWAY_YAW_POS_RAD = 0.22
_K_HEAD_SWAY_YAW_NEG_RAD = -0.22
def _getenv_string(key: str) -> str:
v = os.environ.get(key)
return v if v else ""
def resolve_welcome_pcm_path() -> str:
env_pcm = _getenv_string("GALBOT_WELCOME_PCM")
if env_pcm:
return env_pcm
return _K_WELCOME_PCM_REL
def _print_summary(step_description: str, step_passed: bool) -> None:
print(("[PASS] " if step_passed else "[FAIL] ") + step_description)
def make_point(joint_positions_rad: Sequence[float], time_from_start_sec: float) -> TrajectoryPoint:
point = TrajectoryPoint()
point.time_from_start_second = time_from_start_sec
cmds: List[JointCommand] = []
for q in joint_positions_rad:
jc = JointCommand()
jc.position = float(q)
cmds.append(jc)
point.joint_command_vec = cmds
return point
def build_trajectory(
joint_waypoints_rad: List[List[float]], joint_groups: List[str], time_step_sec: float
) -> Trajectory:
trajectory = Trajectory()
trajectory.joint_groups = joint_groups
trajectory.joint_names = []
time_from_start_sec = 0.0
points: List[TrajectoryPoint] = []
for waypoint in joint_waypoints_rad:
time_from_start_sec += time_step_sec
points.append(make_point(waypoint, time_from_start_sec))
trajectory.points = points
return trajectory
def linspace_rows(
start_positions_rad: Sequence[float], end_positions_rad: Sequence[float], num_samples: int
) -> List[List[float]]:
if num_samples < 2:
return [list(start_positions_rad)]
out: List[List[float]] = []
n = num_samples - 1
for sample_idx in range(num_samples):
blend = sample_idx / n
row = [
float(start_positions_rad[j]) + blend * (float(end_positions_rad[j]) - float(start_positions_rad[j]))
for j in range(len(start_positions_rad))
]
out.append(row)
return out
def append_rows(dest: List[List[float]], src: List[List[float]]) -> None:
dest.extend(src)
def upper_body_preset_pose_rad() -> List[float]:
return list(_K_PRESET_HEAD) + list(_K_PRESET_LEFT_ARM) + list(_K_PRESET_RIGHT_ARM)
def build_slow_head_arms_demo_waypoints_rad() -> List[List[float]]:
"""Slow head yaw left-right (joint2 fixed); arms shift slightly with the sway. Legs not in trajectory."""
home = upper_body_preset_pose_rad()
pose_yaw_pos = (
[_K_HEAD_SWAY_YAW_POS_RAD, 0.0]
+ [2.0 + 0.07, -1.5 + 0.04, -0.6, -1.7, 0.0, -0.8 + 0.04, 0.0]
+ [-2.0 - 0.07, 1.5 - 0.04, 0.6, 1.7, 0.0, 0.8 - 0.04, 0.0]
)
pose_yaw_neg = (
[_K_HEAD_SWAY_YAW_NEG_RAD, 0.0]
+ [2.0 - 0.05, -1.5 - 0.03, -0.6, -1.7, 0.0, -0.8 - 0.03, 0.0]
+ [-2.0 + 0.05, 1.5 + 0.03, 0.6, 1.7, 0.0, 0.8 + 0.03, 0.0]
)
rows: List[List[float]] = []
append_rows(rows, linspace_rows(home, pose_yaw_pos, _K_HEAD_ARMS_DEMO_SEG1))
append_rows(rows, linspace_rows(pose_yaw_pos, pose_yaw_neg, _K_HEAD_ARMS_DEMO_SEG2))
append_rows(rows, linspace_rows(pose_yaw_neg, home, _K_HEAD_ARMS_DEMO_SEG3))
return rows
def play_pcm(robot: GalbotRobot, pcm_file_path: str) -> bool:
k_pcm_read_chunk_bytes = 2560
audio_stream_id = "install_welcome_pcm"
try:
with open(pcm_file_path, "rb") as pcm_input:
while True:
chunk = pcm_input.read(k_pcm_read_chunk_bytes)
if not chunk:
break
if not robot.write_audio_stream_output(chunk, audio_stream_id):
print("[audio] write_audio_stream_output failed", file=sys.stderr)
# robot.stop_audio_stream_output(audio_stream_id)
return False
time.sleep(0.05)
except OSError as e:
print(f"[audio] Failed to open/read PCM: {pcm_file_path} ({e})", file=sys.stderr)
return False
# robot.stop_audio_stream_output(audio_stream_id)
return True
def _decode_rgb_image_bytes(image_data: bytes) -> np.ndarray | None:
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
return img
def decode_head_rgb(rgb_dict: dict) -> np.ndarray | None:
if not rgb_dict or "data" not in rgb_dict:
return None
data = rgb_dict["data"]
if not data:
return None
return _decode_rgb_image_bytes(bytes(data))
def main() -> int:
body_preset_step_passed = False
welcome_pcm_playback_ok = False
head_camera_capture_ok = False
robot = GalbotRobot()
required_sensors = {SensorType.HEAD_LEFT_CAMERA}
if not robot.init(required_sensors):
print("GalbotRobot::init failed", file=sys.stderr)
return 1
print("Robot initialized (HEAD_LEFT_CAMERA enabled)")
time.sleep(5.0)
if not robot.set_volume(100.0):
print(
"[WARN] set_volume(100) failed; welcome PCM may play quietly",
file=sys.stderr,
)
leg_step_ok = False
upper_step_ok = False
demo_step_ok = False
leg_set_status = robot.set_joint_positions(
list(_K_PRESET_LEG), ["leg"], [], True, _K_SPEED_RAD_S, _K_TIMEOUT_S
)
leg_step_ok = leg_set_status == ControlStatus.SUCCESS
if not leg_step_ok:
print("[FAIL] Leg set_joint_positions not SUCCESS (blocking)", file=sys.stderr)
_print_summary("Leg set_joint_positions (blocking)", leg_step_ok)
if leg_step_ok:
upper_body_joint_targets_rad: List[float] = []
upper_body_joint_targets_rad.extend(_K_PRESET_HEAD)
upper_body_joint_targets_rad.extend(_K_PRESET_LEFT_ARM)
upper_body_joint_targets_rad.extend(_K_PRESET_RIGHT_ARM)
upper_body_set_status = robot.set_joint_positions(
upper_body_joint_targets_rad,
_K_HEAD_ARMS_DEMO_GROUPS,
[],
True,
_K_SPEED_RAD_S,
_K_TIMEOUT_S,
)
upper_step_ok = upper_body_set_status == ControlStatus.SUCCESS
if not upper_step_ok:
print(
"[FAIL] head/left_arm/right_arm set_joint_positions not SUCCESS (blocking)",
file=sys.stderr,
)
_print_summary("Upper set_joint_positions head+arms (blocking)", upper_step_ok)
if upper_step_ok:
demo_waypoints_rad = build_slow_head_arms_demo_waypoints_rad()
demo_trajectory = build_trajectory(
demo_waypoints_rad, _K_HEAD_ARMS_DEMO_GROUPS, _K_HEAD_ARMS_DEMO_DT_S
)
demo_exec_status = robot.execute_joint_trajectory(demo_trajectory, True)
demo_step_ok = demo_exec_status == ControlStatus.SUCCESS
if not demo_step_ok:
print("[FAIL] Head/arms execute_joint_trajectory not SUCCESS (blocking)", file=sys.stderr)
_print_summary("Head/arms demo trajectory (blocking)", demo_step_ok)
else:
print(
"[INFO] Skipping head/arms demo: upper set_joint_positions did not return SUCCESS.",
file=sys.stderr,
)
_print_summary("Head/arms demo trajectory (blocking)", False)
else:
print(
"[INFO] Skipping upper-body preset and head/arms demo: leg set_joint_positions not SUCCESS.",
file=sys.stderr,
)
_print_summary("Upper set_joint_positions head+arms (blocking)", False)
_print_summary("Head/arms demo trajectory (blocking)", False)
body_preset_step_passed = leg_step_ok and upper_step_ok and demo_step_ok
resolved_welcome_pcm_path = resolve_welcome_pcm_path()
print("PCM path: " + resolved_welcome_pcm_path)
if not resolved_welcome_pcm_path:
print(
"[FAIL] Welcome PCM path is empty: set GALBOT_WELCOME_PCM (no built-in default path).",
file=sys.stderr,
)
welcome_pcm_playback_ok = False
else:
try:
with open(resolved_welcome_pcm_path, "rb"):
pass
except OSError as e:
print(
f"[FAIL] PCM file not found or not readable: {resolved_welcome_pcm_path} ({e})",
file=sys.stderr,
)
welcome_pcm_playback_ok = False
else:
welcome_pcm_playback_ok = play_pcm(robot, resolved_welcome_pcm_path)
_print_summary("Welcome PCM playback", welcome_pcm_playback_ok)
head_rgb_data = robot.get_rgb_data(SensorType.HEAD_LEFT_CAMERA, RgbOutputFormat.JPEG, True)
if not head_rgb_data:
print("[FAIL] get_rgb_data returned null", file=sys.stderr)
head_camera_capture_ok = False
elif isinstance(head_rgb_data, dict):
raw = head_rgb_data.get("data")
raw_len = len(raw) if raw is not None and hasattr(raw, "__len__") else 0
if raw_len == 0:
print("[FAIL] head camera data missing or empty", file=sys.stderr)
head_camera_capture_ok = False
else:
head_camera_capture_ok = True
fmt = head_rgb_data.get("format", "")
print(f"Head camera: {raw_len} bytes format={fmt}")
head_image_mat = decode_head_rgb(head_rgb_data)
if head_image_mat is not None and head_image_mat.size > 0:
print(f" decoded size {head_image_mat.shape[1]}x{head_image_mat.shape[0]}")
else:
head_camera_capture_ok = True
print("Head camera: get_rgb_data returned non-empty object")
_print_summary("Head camera data", head_camera_capture_ok)
print("\n======== Summary ========")
_print_summary("Motion verify (blocking)", body_preset_step_passed)
_print_summary("Audio", welcome_pcm_playback_ok)
_print_summary("Head camera", head_camera_capture_ok)
installation_all_checks_passed = (
body_preset_step_passed and welcome_pcm_playback_ok and head_camera_capture_ok
)
print("\nOverall: PASS\n" if installation_all_checks_passed else "\nOverall: FAIL\n")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0 if installation_all_checks_passed else 1
if __name__ == "__main__":
sys.exit(main())
设置底盘位姿（set_base_pose）
适用场景：控制移动底盘在指定参考坐标系下运动至目标位姿。
examples/python/galbot_robot/set_base_pose.py
import time
from galbot_sdk.g1 import ControlStatus, GalbotRobot, Pose
def set_yaw_orientation(pose, yaw):
import math
half_yaw = 0.5 * yaw
pose.orientation.x = 0.0
pose.orientation.y = 0.0
pose.orientation.z = math.sin(half_yaw)
pose.orientation.w = math.cos(half_yaw)
def test1(robot):
# Test 1: set base pose using Pose.
pose = Pose()
pose.position.x = 0.3 # Move to x = 0.3 m in the odom frame.
pose.position.y = 0.0
pose.position.z = 0.0
set_yaw_orientation(pose, 0.0)
status = robot.set_base_pose(pose, True, 15.0)
if status == ControlStatus.SUCCESS:
print("[Test 1] Pose-struct set succeeded.")
else:
print("[Test 1] Pose-struct set failed or timed out.")
time.sleep(2)
def test2(robot):
# Test 2: set base pose using frame ids. "rel(0)" is relative to the current base pose.
x = -0.3 # Move backward by 0.3 m from the current pose.
y = 0.0
yaw = 0.0
frame_id = "rel(0)"
reference_frame_id = "odom"
is_blocking = True
timeout_s = 15.0
status = robot.set_base_pose(x, y, yaw, frame_id, reference_frame_id, is_blocking, timeout_s)
if status == ControlStatus.SUCCESS:
print("[Test 2] Relative set succeeded.")
else:
print("[Test 2] Relative set failed or timed out.")
time.sleep(2)
def test3(robot):
# Test 3: set base pose using explicit interpolation time.
x = 0.0 # Keep the third target at zero relative displacement.
y = 0.0
yaw = 0.0
# frame_id = "base_link"
frame_id = "rel(0)"
# frame_id = "odom"
reference_frame_id = "odom"
time_from_start_s = 5.0
is_blocking = True
timeout_s = 15.0
status = robot.set_base_pose(
x,
y,
yaw,
frame_id,
reference_frame_id,
time_from_start_s,
is_blocking,
timeout_s,
)
if status == ControlStatus.SUCCESS:
print("[Test 3] Full-control set succeeded.")
else:
print("[Test 3] Full-control set failed or timed out.")
time.sleep(2)
if __name__ == "__main__":
robot = GalbotRobot()
robot.init()
time.sleep(2)
print("Initialization succeeded")
# Run all three API forms in sequence so every target is executed.
test1(robot)
test2(robot)
test3(robot)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
设置机器人配置（set_config）
适用场景：通过统一配置接口设置支持的机器人服务或运行参数。
examples/python/galbot_robot/set_config.py
import time
from galbot_sdk.g1 import ConfigService, ConfigItem, ControlStatus, GalbotRobot
# Full list of settable fields (navigation service, camera services, motion
# planning service and control service), their meaning, type and valid range:
# docs/g1/en/set_config_reference.md
def example_navigation(robot):
# Navigation service: adjust replanning thresholds.
status = robot.set_config(
ConfigService.NAVIGATION,
[
ConfigItem("replan_threshold", 30.0),
ConfigItem("no_replan_threshold", 1.0),
],
)
if status == ControlStatus.SUCCESS:
print("[Navigation] set_config succeeded. Restart the device for the new config to take effect.")
else:
print(f"[Navigation] set_config failed: {status}. Check the SDK log for details.")
def example_camera(robot):
# Front head camera: set resolution.
# color_width/color_height must be set together in the same call, and the
# combination must be one of the documented valid resolutions.
status = robot.set_config(
ConfigService.FRONT_HEAD_CAMERA,
[
ConfigItem("color_width", 1280),
ConfigItem("color_height", 992),
],
)
if status == ControlStatus.SUCCESS:
print("[Camera] set_config succeeded. Restart the device for the new config to take effect.")
else:
print(f"[Camera] set_config failed: {status}. Check the SDK log for details.")
def example_motion_plan(robot):
# Motion planning service: set a non-per-chain field (plan_timeout).
status = robot.set_config(
ConfigService.MOTION_PLAN,
[
ConfigItem("plan_timeout", 5.0),
],
)
if status == ControlStatus.SUCCESS:
print("[MotionPlan] set_config succeeded. Restart the device for the new config to take effect.")
else:
print(f"[MotionPlan] set_config failed: {status}. Check the SDK log for details.")
def example_control(robot):
# Control service: report_error_skip is a general parameter shared by
# both G-series and S1 (robot_config.toml).
status = robot.set_config(
ConfigService.CONTROL,
[
ConfigItem("report_error_skip", 1),
],
)
if status == ControlStatus.SUCCESS:
print("[Control] set_config succeeded. Restart the device for the new config to take effect.")
else:
print(f"[Control] set_config failed: {status}. Check the SDK log for details.")
if __name__ == "__main__":
robot = GalbotRobot()
robot.init()
time.sleep(2)
print("Initialization succeeded")
example_navigation(robot)
example_camera(robot)
example_motion_plan(robot)
example_control(robot)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
读取机器人配置（get_config）
适用场景：读取当前场景的持久化配置；也可使用 use_default=True 读取机器人内置默认配置，用于对比当前配置与默认值。
examples/python/galbot_robot/get_config.py
import time
from galbot_sdk.g1 import ConfigService, ControlStatus, GalbotRobot
def print_fields(title, status, fields):
print(f"[{title}] status: {status}")
if status != ControlStatus.SUCCESS:
print("Some fields could not be read. Successful fields are shown below; check the SDK log for details.")
for field in fields:
print(f" {field.key} = {field.value}")
if __name__ == "__main__":
robot = GalbotRobot()
robot.init()
time.sleep(2)
status, fields = robot.get_config(ConfigService.CONTROL, ["report_error_skip", "report_sensor_skip"])
print_fields("Control selected fields", status, fields)
status, fields = robot.get_config(ConfigService.CONTROL, [])
print_fields("Control all fields", status, fields)
status, fields = robot.get_config(ConfigService.CONTROL, ["report_error_skip", "report_sensor_skip"], use_default=True)
print_fields("Control built-in defaults", status, fields)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
设置腿部高度（set_leg_height）
适用场景：通过腿部机构调整机器人机身高度。
examples/python/galbot_robot/set_leg_height_example.py
import time
from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotRobot
def print_status(operation, status):
if status == ControlStatus.SUCCESS:
print(f"{operation} succeeded.")
else:
print(f"{operation} failed, status: {status}")
def get_leg_height(robot, label):
pose, timestamp_ns = robot.get_transform(
"base_link", "head_base_link", timestamp_ns=0, timeout_ms=500
)
height_m = pose[2]
print(
f"{label}: pose=[{', '.join(f'{value:.3f}' for value in pose)}], "
f"tf_timestamp_ns={timestamp_ns}"
)
return height_m >= 0.0, height_m
def confirm_leg_height_motion(current_height_m, target_height_m):
direction = "lower" if target_height_m < current_height_m else "raise"
motion_distance_m = abs(target_height_m - current_height_m)
response = input(
f"The leg mechanism is about to {direction} the body by "
f"{motion_distance_m:.3f} m, from {current_height_m:.3f} m "
f"to {target_height_m:.3f} m. "
"Confirm the surrounding environment is clear and safe to avoid collisions. "
"Enter y to continue; any other input cancels: "
)
return response.strip().lower() == "y"
def main():
# Get and initialize the G1 robot instance.
robot = GalbotRobot()
print("Initializing robot...")
if not robot.init():
print("System initialization failed!")
return
print("System initialized successfully!")
# Wait for WBC communication and state data to become ready.
time.sleep(2)
height_offset_m = 0.20
duration_s = 4.0
height_valid, initial_height_m = get_leg_height(robot, "Initial leg height")
if not height_valid:
print("Failed to get a valid initial leg height.")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
lowered_height_m = initial_height_m - height_offset_m
if not confirm_leg_height_motion(initial_height_m, lowered_height_m):
print("Leg height motion cancelled. Exiting program.")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
print(
f"Lowering leg height by {height_offset_m:.3f} m "
f"to {lowered_height_m:.3f} m."
)
status = robot.set_leg_height(lowered_height_m, duration_s, True)
print_status("set_leg_height(lowered)", status)
lowered_height_valid, actual_lowered_height_m = get_leg_height(
robot, "Height after lowering"
)
if not lowered_height_valid:
print("Failed to get the current leg height; cancelling the restore motion.")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
print(
"Lowered target error: "
f"{abs(actual_lowered_height_m - lowered_height_m):.3f} m."
)
if not confirm_leg_height_motion(actual_lowered_height_m, initial_height_m):
print("Leg height restore motion cancelled. Exiting program.")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
print(f"Restoring initial leg height: {initial_height_m:.3f} m.")
status = robot.set_leg_height(initial_height_m, duration_s, True)
print_status("set_leg_height(initial)", status)
_, final_height_m = get_leg_height(robot, "Final leg height")
print(
"Restored height error: "
f"{abs(final_height_m - initial_height_m):.3f} m."
)
time.sleep(0.5)
print("Switching controller back to leg_pvt_ctrl...")
status = robot.switch_controller(G1ControllerName.LEG_PVT_CTRL)
print_status("switch_controller(leg_pvt_ctrl)", status)
print("Waiting 1 second before shutdown...")
time.sleep(1)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully.")
if __name__ == "__main__":
main()
停止底盘运动（stop_base）
适用场景：在底盘未由导航模块管理时，立即停止当前底盘运动指令。
examples/python/galbot_robot/stop_base.py
import signal
import threading
import time
from galbot_sdk.g1 import ControlStatus, GalbotRobot
motion_done = threading.Event()
shutdown_requested = threading.Event()
def stop_base_on_ctrl_c():
"""Notify the main thread to stop publishing velocity after Ctrl-C."""
while not motion_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is not None:
shutdown_requested.set()
return
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the area is clear.")
if input("Start chassis motion? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
# Block SIGINT before SDK initialization so all SDK threads inherit the mask.
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
robot.init()
time.sleep(1)
print("Initialization succeeded")
print("Chassis is moving. Press Ctrl-C to stop it.")
stop_thread = threading.Thread(target=stop_base_on_ctrl_c)
stop_thread.start()
start_time = time.monotonic()
while not shutdown_requested.is_set() and time.monotonic() - start_time < 5.0:
next_publish_time = time.monotonic() + 0.1
status = robot.set_base_velocity(
[0.1, 0.0, 0.0], [0.0, 0.0, 0.0], duration_s=0.0
)
if status != ControlStatus.SUCCESS:
print(f"Failed to send chassis velocity: {status}")
break
shutdown_requested.wait(
timeout=max(0.0, min(next_publish_time, start_time + 5.0) - time.monotonic())
)
motion_done.set()
stop_thread.join()
print("Sending chassis stop command...")
for _ in range(10):
status = robot.stop_base()
if status == ControlStatus.SUCCESS:
print("Chassis motion stopped successfully")
break
print("Chassis stop failed, retrying...")
time.sleep(0.2)
else:
print("Chassis stop timed out")
# AsyncShutdown raises SIGINT on its calling thread, so unblock it here first.
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")
类：GalbotMotion
获取实例并初始化
适用场景：获取运动规划模块单例并初始化。必须在使用运动规划功能前调用。
examples/python/galbot_motion/get_instance.py
from galbot_sdk.g1 import GalbotMotion
from galbot_sdk.g1 import GalbotRobot
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# You can still manage the robot lifecycle through GalbotRobot
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
正运动学（使用当前状态或指定 RobotStates）
适用场景：根据给定的关节角度计算末端执行器的位姿。用于机械臂任务验证和状态分析。
examples/python/galbot_motion/fk.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, execution timed out")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, a fault occurred and execution cannot continue")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, failed to create internal communication components")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, in motion but not yet in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, data transmission failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, connection failed")
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# Program started, waiting for data
time.sleep(1)
chain_joints = {
"leg": [0.4992, 1.4991, 1.0005, 0.0000, -0.0004],
"head": [0.0000, 0.0],
"left_arm": [1.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000],
"right_arm": [-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000],
}
end_link = "left_arm_end_effector_mount_link"
reference_frame = "base_link"
# Scenario 1: Forward kinematics using the current robot state
try:
status, pose = motion.forward_kinematics(end_link, reference_frame)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Forward kinematics calculation failed"
print(f"✅ Basic forward kinematics successful: pose={pose}")
time.sleep(0.8)
except Exception as e:
print(f"❌ Basic forward kinematics exception: {e}")
# Scenario 2: Forward kinematics with custom arm joints
try:
status, pose = motion.forward_kinematics(
end_link, reference_frame, {"left_arm": chain_joints["left_arm"]}, gm.Parameter()
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Forward kinematics calculation failed"
print(f"✅ Custom-joint forward kinematics successful: pose={pose}")
time.sleep(0.8)
except Exception as e:
print(f"❌ Custom-joint forward kinematics exception: {e}")
# Scenario 3: Forward kinematics based on RobotStates
try:
current_state = motion.get_robot_states()
if not current_state.whole_body_joint:
print("❌ RobotStates-based FK: robot state is empty; ensure sensors/WBC are ready")
else:
status, pose = motion.forward_kinematics_by_state(
end_link, current_state, reference_frame, gm.Parameter()
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Forward kinematics calculation failed"
print(f"✅ RobotStates-based forward kinematics successful: pose={pose}")
except Exception as e:
print(f"❌ RobotStates-based forward kinematics exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
逆运动学（基础与基于 RobotStates）
适用场景：根据期望的末端位姿求解所需的关节角度。是机械臂抓取等操作的核心步骤。
examples/python/galbot_motion/ik.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, execution timed out")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, a fault occurred and execution cannot continue")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, failed to create internal communication components")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, in motion but not yet in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, data transmission failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, connection failed")
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# Program started, waiting for data
time.sleep(1)
chain_joints = {
"leg": [0.4992,1.4991,1.0005,0.0000,-0.0004],
"head": [0.0000,0.0],
"left_arm": [1.9999,-1.6000,-0.5999,-1.6999,0.0000,-0.7999,0.0000],
"right_arm": [-2.0000,1.6001,0.6001,1.7000,0.0000,0.8000,0.0000]
}
chain_pose_baselink = {
"leg": [0.0596,-0.0000,1.0327,0.5000,0.5003,0.4997,0.5000],
"head": [0.0599,0.0002,1.4098,-0.7072,0.0037,0.0037,0.7069],
"left_arm": [0.1267,0.2342,0.7356,0.0220,0.0127,0.0343,0.9991],
"right_arm": [0.1267,-0.2345,0.7358,-0.0225,0.0126,-0.0343,0.9991]
}
whole_body_joint = [
num for key in ["leg", "head", "left_arm", "right_arm"]
for num in chain_joints[key]
]
base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
custom_param = gm.Parameter()
reference_frame = "base_link"
target_frame = "EndEffector"
target_chain = "left_arm"
one_chain = [target_chain]
chain_with_torso = [target_chain, "torso"]
error_chains = [target_chain, "torso", "head"]
# Scenario 1: Single-chain inverse kinematics
try:
status, joint_map = motion.inverse_kinematics(
target_pose=chain_pose_baselink[target_chain],
chain_names=one_chain,
target_frame=target_frame,
reference_frame=reference_frame,
enable_collision_check=False # Disable collision checking for accelerated testing
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Inverse kinematics calculation failed"
print(f"✅ Basic IK succeeded: joint angles={joint_map}")
time.sleep(0.8)
except Exception as e:
print(f"❌ Basic IK exception: {e}")
# Scenario 2: Arm chain + torso inverse kinematics
try:
status, joint_map = motion.inverse_kinematics(
target_pose=chain_pose_baselink[target_chain],
chain_names=chain_with_torso,
target_frame=target_frame,
reference_frame=reference_frame,
enable_collision_check=False # Disable collision checking for accelerated testing
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Inverse kinematics calculation failed"
print(f"✅ IK with custom initial joints succeeded: joint angles={joint_map}")
time.sleep(0.8)
except Exception as e:
print(f"❌ IK with custom initial joints exception: {e}")
# Scenario 3: invalid chain combination
try:
status, joint_map = motion.inverse_kinematics(
target_pose=chain_pose_baselink[target_chain],
chain_names=error_chains,
target_frame=target_frame,
reference_frame=reference_frame,
enable_collision_check=False # Disable collision checking for accelerated testing
)
printStatus(status)
assert status == gm.MotionStatus.INVALID_INPUT, "Inverse kinematics calculation failed"
print(f"✅ Invalid chain-combination input check passed")
time.sleep(0.8)
except Exception as e:
print(f"❌ IK with custom initial joints exception: {e}")
# Scenario 4: Use reference joints
try:
# initial_joint_positions can specify chain joints as IK reference; unspecified chain joints are filled from whole-body joints
status, joint_map = motion.inverse_kinematics(
target_pose=chain_pose_baselink[target_chain],
chain_names=one_chain,
target_frame=target_frame,
reference_frame=reference_frame,
initial_joint_positions=chain_joints,
enable_collision_check=False # Disable collision checking for accelerated testing
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Inverse kinematics calculation failed"
print(f"✅ IK with custom initial joints succeeded: joint angles={joint_map}")
time.sleep(0.8)
except Exception as e:
print(f"❌ IK with custom initial joints exception: {e}")
# Scenario 5: Use RobotStates
try:
ref_robot_state = gm.RobotStates()
ref_robot_state.chain_name = target_chain
ref_robot_state.whole_body_joint = whole_body_joint
ref_robot_state.base_state = base_state
target_frame = "EndEffector"
reference_frame = "base_link"
status, joint_map = motion.inverse_kinematics_by_state(
target_pose=chain_pose_baselink[target_chain],
chain_names=one_chain,
target_frame=target_frame,
reference_frame=reference_frame,
reference_robot_states=ref_robot_state
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Inverse kinematics calculation failed"
print(f"✅ RobotStates-based IK succeeded: joint angles={joint_map}")
except Exception as e:
print(f"❌ RobotStates-based IK exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
获取与设置末端位姿
适用场景：直接获取当前末端位姿，或将末端移动到指定位姿。集成了逆运动学计算和执行。
examples/python/galbot_motion/get_set_end_effector_pose.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, execution timed out")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, a fault occurred and execution cannot continue")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, failed to create internal communication components")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, in motion but not yet in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, data transmission failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, connection failed")
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# Program started, waiting for data
time.sleep(1)
chain_pose_baselink = {
"leg": [0.0596,-0.0000,1.0327,0.5000,0.5003,0.4997,0.5000],
"head": [0.0599,0.0002,1.4098,-0.7072,0.0037,0.0037,0.7069],
"left_arm": [0.1267,0.2342,0.7356,0.0220,0.0127,0.0343,0.9991],
"right_arm": [0.1267,-0.2345,0.7358,-0.0225,0.0126,-0.0343,0.9991]
}
custom_param = gm.Parameter()
target_frame = "EndEffector"
reference_frame = "base_link"
target_chain = "left_arm"
# Scenario 1: Basic version
try:
end_ee_link = "left_arm_end_effector_mount_link"
status, pose = motion.get_end_effector_pose(
end_effector_frame=end_ee_link,
reference_frame=reference_frame
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
print(f"✅ Basic end-effector pose retrieval succeeded: {pose}")
time.sleep(0.8)
except Exception as e:
print(f"❌ Basic end-effector pose retrieval exception: {e}")
# Scenario 2: Specify chain name + custom frame
try:
status, pose = motion.get_end_effector_pose_on_chain(
chain_name=target_chain,
frame_id=target_frame,
reference_frame=reference_frame
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
print(f"✅ End-effector pose retrieval by specified chain succeeded: {pose}")
time.sleep(0.8)
except Exception as e:
print(f"❌ End-effector pose retrieval by specified chain exception: {e}")
end_effector_frame="left_arm"
reference_frame = "base_link"
motion_timeout = 10.0
try:
status = motion.set_end_effector_pose(
target_pose=chain_pose_baselink[end_effector_frame],
end_effector_frame=end_effector_frame,
reference_frame=reference_frame,
enable_collision_check=False,
is_blocking=False,
timeout=motion_timeout,
params=custom_param
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Set end-effector pose failed"
print(f"✅ End-effector pose set succeeded: status={status}")
time.sleep(motion_timeout + 1.0)
except Exception as e:
print(f"❌ End-effector pose setting exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
单点运动规划（关节空间与笛卡尔空间）
适用场景：规划从当前位姿到单个目标位姿的无碰撞运动轨迹。适用于单点目标到达任务。
examples/python/galbot_motion/motion_plan.py
"""G1 motion_plan example aligned with motion_plan_example.cpp."""
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def initialize_interfaces():
motion = GalbotMotion()
print("GalbotMotion::get_instance() status: no return status; call completed.")
robot = GalbotRobot()
print("GalbotRobot::get_instance() status: no return status; call completed.")
if not motion.init():
print("GalbotMotion::init() status: FAILED")
return None, None
print("GalbotMotion::init() status: SUCCESS")
if not robot.init():
print("GalbotRobot::init() status: FAILED")
return None, None
print("GalbotRobot::init() status: SUCCESS")
time.sleep(1.0)
return motion, robot
def shutdown_robot(robot):
robot.request_shutdown()
print("request_shutdown() status: no return status; call completed.")
robot.wait_for_shutdown()
print("wait_for_shutdown() status: no return status; call completed.")
robot.destroy()
print("destroy() status: no return status; call completed.")
def print_control_status(api_name, status):
result = "SUCCESS" if status == gm.ControlStatus.SUCCESS else "FAILED"
print(f"{api_name} status: {str(status).rsplit('.', 1)[-1]} ({result})")
def move_robot_to_home_position(robot):
print("Move robot to the G1 home position.")
status = robot.set_joint_positions(
[
0.5, 1.5, 1.0, 0.0, 0.0,
0.0, 0.0,
2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0,
],
["leg", "head", "left_arm", "right_arm"],
[],
True,
0.1,
30.0,
)
print_control_status("GalbotRobot::set_joint_positions(home)", status)
return status == gm.ControlStatus.SUCCESS
def capture_current_pose(motion, robot, chain, current):
joint_names = list(motion.get_chain_joint_names(chain))
print(f"get_chain_joint_names({chain}) status: {'SUCCESS' if joint_names else 'FAILED'}")
if not joint_names:
return False
joints = list(robot.get_joint_positions([], joint_names))
success = len(joints) == len(joint_names)
print(f"GalbotRobot::get_joint_positions({chain}) status: {'SUCCESS' if success else 'FAILED'}")
if not success:
return False
current["joint_names"][chain] = joint_names
current["joints"][chain] = joints
print(f"Current {chain} joints: {joints}")
status, pose = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link")
result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
print(f"get_end_effector_pose_on_chain({chain}) status: {motion.status_to_string(status)} ({result})")
pose = list(pose)
if status != gm.MotionStatus.SUCCESS or len(pose) != 7:
return False
current["poses"][chain] = pose
print(f"Current {chain} pose [x, y, z, qx, qy, qz, qw]: {pose}")
return True
def make_joint_target(chain, joint_positions, joint_names=None):
target = gm.MotionPlanChainTarget()
target.chain_name = chain
target.mode = gm.MotionPlanTargetMode.kJoint
target.joint.chain_name = chain
target.joint.joint_positions = list(joint_positions)
target.joint.joint_names = list(joint_names or [])
return target
def make_cartesian_target(chain, pose, assist_chains=None):
target = gm.MotionPlanChainTarget()
target.chain_name = chain
target.mode = gm.MotionPlanTargetMode.kCartesian
target.cart.chain_name = chain
target.cart.frame_id = "EndEffector"
target.cart.reference_frame = "base_link"
target.cart.pose = gm.Pose(list(pose))
target.cart.assist_chains = set(assist_chains or [])
return target
def make_planner_config(check_collision):
params = gm.Parameter()
params.set_direct_execute(True)
params.set_blocking(True)
params.set_timeout(120.0)
params.set_check_collision(check_collision)
params.set_reference_frame("base_link")
params.set_actuate("with_torso")
return params
def print_waypoints(label, waypoints):
print(f"{label} waypoints: {len(waypoints)}")
for index, waypoint in enumerate(waypoints):
print(f" waypoint[{index}] targets: {len(waypoint)}")
for target in waypoint:
if target.mode == gm.MotionPlanTargetMode.kJoint:
print(f" {target.chain_name} JOINT q={list(target.joint.joint_positions)}")
else:
print(f" {target.chain_name} CART frame_id={target.cart.frame_id} reference_frame={target.cart.reference_frame}")
def print_traj_result(label, status, traj, motion):
result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
print(f"{label} status: {motion.status_to_string(status)} ({result})")
if status == gm.MotionStatus.SUCCESS:
if not traj:
print("Trajectory map is empty.")
for chain, points in traj.items():
print(f" {chain} trajectory points: {len(points)}")
def make_example_inputs(current):
left_names = current["joint_names"]["left_arm"]
right_names = current["joint_names"]["right_arm"]
torso = lambda: make_joint_target("torso", [0.0], ["leg_joint4"])
cart = lambda chain, pose: make_cartesian_target(chain, pose, ["torso"])
waypoints = [
[
make_joint_target(
"left_arm",
[1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
left_names,
),
make_joint_target(
"right_arm",
[-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0],
right_names,
),
torso(),
],
[
make_joint_target(
"left_arm",
[1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
left_names,
),
torso(),
],
[
cart("left_arm", [0.32666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
cart("right_arm", [0.32666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
],
[
cart("left_arm", [0.32666, 0.43435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
cart("right_arm", [0.32666, -0.43435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
],
[
make_joint_target(
"left_arm",
[1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
left_names,
),
make_joint_target(
"right_arm",
[-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0],
right_names,
),
],
]
return waypoints, make_planner_config(check_collision=False)
def run_example():
motion, robot = initialize_interfaces()
if motion is None or robot is None:
return 1
try:
if not move_robot_to_home_position(robot):
return 1
time.sleep(0.5)
current = {"joint_names": {}, "joints": {}, "poses": {}}
if not capture_current_pose(motion, robot, "left_arm", current):
return 1
if not capture_current_pose(motion, robot, "right_arm", current):
return 1
waypoints, params = make_example_inputs(current)
print_waypoints("motion_plan", waypoints)
print(
"Immediate execution is enabled; waypoints are loaded from "
"motion_plan_targets.json reference values."
)
status, traj = motion.motion_plan(waypoints, params)
print_traj_result("motion_plan", status, traj, motion)
return 0 if status == gm.MotionStatus.SUCCESS else 1
finally:
shutdown_robot(robot)
if __name__ == "__main__":
raise SystemExit(run_example())
多点轨迹规划
适用场景：规划经过多个路径点的连续运动轨迹，适用于需要经过多个中间点的复杂运动。
examples/python/galbot_motion/motion_plan_multi_waypoints.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def confirm_robot_safety():
print("WARNING: The robot will move to the initial joint state. Release the emergency stop and clear nearby obstacles.")
return input("Continue? (y/n): ").strip().lower() == "y"
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, execution timed out")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, a fault occurred and execution cannot continue")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, failed to create internal communication components")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, in motion but not yet in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, data transmission failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, connection failed")
def main():
if not confirm_robot_safety():
print("Example cancelled.")
return
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# Program started, waiting for data
time.sleep(2)
chain_joints = {
"leg": [0.5, 1.5, 1.0, 0.0, 0.0],
"head": [0.0, 0.0],
"left_arm": [2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0],
"right_arm": [-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0]
}
# FK results of chain_joints in base_link, showing their physical correspondence.
chain_pose_baselink = {
"leg": [0.0545, -0.0013, 1.0377, 0.4956, 0.4950, 0.5060, 0.5033],
"head": [0.0517, -0.0016, 1.4155, -0.7046, -0.0042, -0.0056, 0.7096],
"left_arm": [0.1298, 0.2608, 0.7418, 0.0734, 0.0209, -0.0233, 0.9968],
"right_arm": [0.1261, -0.2575, 0.7424, -0.0466, 0.0223, 0.0058, 0.9986]
}
whole_body_joint = [
num for key in ["leg", "head", "left_arm", "right_arm"]
for num in chain_joints[key]
]
move_status = robot.set_joint_positions(
whole_body_joint,
["leg", "head", "left_arm", "right_arm"],
[],
True,
0.1,
30.0,
)
if move_status != gm.ControlStatus.SUCCESS:
print(f"❌ Failed to move to the initial joint state: {move_status}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return
print("✅ Moved to the initial whole-body joint state")
time.sleep(3.0)
base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
custom_param = gm.Parameter()
# NOTE: The two scenarios express the same physical waypoints as Cartesian
# poses and joint positions, so their planning results are consistent. Setting
# custom_param.set_move_line(True) applies Cartesian-space line planning to both.
# 注意：以下两个场景分别使用笛卡尔位姿和关节位置表达相同的物理路点，
# 因此规划结果一致。设置 custom_param.set_move_line(True) 后，二者均按
# 笛卡尔空间直线方式规划。
# Scenario 1: Multi-waypoint planning in Cartesian space (PoseState target)
try:
# Construct target pose
target_pose_state = gm.PoseState()
target_pose_state.chain_name = "left_arm"
# Construct waypoints (3 intermediate poses)
waypoint_poses = [
[0.1297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968],
[0.2297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968],
[0.3297, 0.2608, 0.7417, 0.0734, 0.0209, -0.0233, 0.9968],
[0.3297, 0.2608, 0.8417, 0.0734, 0.0209, -0.0233, 0.9968],
]
status, traj = motion.motion_plan_multi_waypoints(
target=target_pose_state,
waypoint_poses=waypoint_poses,
enable_collision_check=False,
params=custom_param
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Cartesian multi-waypoint single-chain planning failed"
if traj != {}:
print(f"✅ Cartesian waypoint single-chain planning succeeded: trajectory points={len(traj[target_pose_state.chain_name])}")
time.sleep(0.8)
else:
print(f"⚠️ Return status is SUCCESS, but trajectory is empty; possibly already reached, check whether the target matches current state or is within tolerance")
except Exception as e:
print(f"❌ Cartesian multi-point motion planning exception: {e}")
# Scenario 2: Multi-waypoint planning in joint space (JointStates target)
try:
# Construct target pose
target_joint = gm.JointStates()
target_joint.chain_name = "left_arm"
# Construct waypoints (3 intermediate poses)
waypoints = [
[2.0000, -1.5001, -0.6001, -1.7000, 0.0001, -0.7999, 0.0000],
[1.7805, -1.4841, -0.5856, -1.6857, -0.0083, -0.5952, 0.0090],
[1.5313, -1.4746, -0.5672, -1.5923, -0.0109, -0.4406, 0.0185],
[1.6264, -1.4551, -0.5976, -1.9286, -0.0427, -0.1972, 0.0289]
]
status, traj = motion.motion_plan_multi_waypoints(
target=target_joint,
waypoint_poses=waypoints,
enable_collision_check=False,
params=custom_param
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Cartesian multi-waypoint single-chain planning failed"
if traj != {}:
print(f"✅ Joint-waypoint single-chain planning succeeded: trajectory points={len(traj[target_joint.chain_name])}")
else:
print(f"⚠️ Return status is SUCCESS, but trajectory is empty; possibly already reached, check whether the target matches current state or is within tolerance")
except Exception as e:
print(f"❌ Joint-space multi-point motion planning exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
碰撞检测
适用场景：检查给定关节状态是否会发生自碰撞或与环境障碍物碰撞。用于运动规划前的可行性检查。
examples/python/galbot_motion/check_collision.py
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot, GalbotNavigation
import time
# NOTE:
# - GalbotMotion currently does NOT provide real-time obstacle perception / automatic environment updates.
# - Collision checking uses self-collision + the collision world built from objects you load manually via
# add_obstacle()/attach_target_object() (including point clouds if you load them explicitly).
motion = GalbotMotion()
robot = GalbotRobot()
nav = GalbotNavigation()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Result: SUCCESS")
elif(status == gm.MotionStatus.TIMEOUT):
print("Result: TIMEOUT")
elif(status == gm.MotionStatus.FAULT):
print("Result: FAULT")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Result: INVALID_INPUT")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Result: INIT_FAILED")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Result: IN_PROGRESS")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Result: STOPPED_UNREACHED")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Result: DATA_FETCH_FAILED")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Result: PUBLISH_FAIL")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Result: COMM_DISCONNECTED")
if motion.init():
print("GalbotMotion init OK")
else:
print("GalbotMotion init FAILED")
if robot.init():
print("GalbotRobot init OK")
else:
print("GalbotRobot init FAILED")
if nav.init():
print("GalbotNavigation init OK")
else:
print("GalbotNavigation init FAILED")
# Wait for data to be ready.
time.sleep(3)
chain_joints = {
"leg": [0.4992,1.4991,1.0005,0.0000,-0.0004],
"head": [0.0000,0.0],
"left_arm": [1.9999,-1.6000,-0.5999,-1.6999,0.0000,-0.7999,0.0000],
"right_arm": [-2.0000,1.6001,0.6001,1.7000,0.0000,0.8000,0.0000]
}
whole_body_joint = [
num for key in ["leg", "head", "left_arm", "right_arm"]
for num in chain_joints[key]
]
base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
custom_param = gm.Parameter()
try:
# Build RobotStates list to check.
check_states = [gm.RobotStates() for _ in range(2)]
check_states[0].whole_body_joint = whole_body_joint
check_states[0].base_state = base_state
bad_left_arm_joint = [1.99995,-1.60004,0.599905,-1.69994,0,-0.799924,0]
check_left_arm = gm.JointStates()
check_left_arm.chain_name = "left_arm"
check_left_arm.joint_positions = bad_left_arm_joint
check_states[1] = check_left_arm
status, collision_res = motion.check_collision(
start=check_states,
enable_collision_check=True
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "Collision check failed"
assert len(collision_res) == len(check_states), "Result size mismatch"
print(f"OK: collision check finished: {collision_res} (False=no collision)")
except Exception as e:
print(f"ERROR: collision check exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
附加工具
适用场景：在机器人末端添加工具（如夹具、吸盘等），更新运动规划模型，以便考虑工具质量和碰撞体积。
examples/python/galbot_motion/attach_tool.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, execution timed out")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, a fault occurred and execution cannot continue")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, failed to create internal communication components")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, in motion but not yet in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, data transmission failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, connection failed")
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# Program started, waiting for data
time.sleep(2)
try:
chain_name = "left_arm"
tool_name = "suction_cup"
status = motion.attach_tool(
chain=chain_name,
tool=tool_name
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "load failed"
print(f"✅ Tool attached successfully")
# Clean up the attached tool model to avoid affecting subsequent examples.
status = motion.detach_tool(chain=chain_name)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "detach failed"
print(f"✅ Tool detached successfully")
except Exception as e:
print(f"❌ Tool operation exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
卸载工具
适用场景：从机器人末端移除已附加的工具，恢复原始机器人模型。
examples/python/galbot_motion/detach_tool.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, execution timed out")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, a fault occurred and execution cannot continue")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, failed to create internal communication components")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, in motion but not yet in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, stopped but target not reached")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, failed to fetch data")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, data transmission failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, connection failed")
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# Program started, waiting for data
time.sleep(2)
# 1. Detach tool
try:
chain_name = "left_arm"
status = motion.detach_tool(
chain=chain_name
)
printStatus(status)
assert status == gm.MotionStatus.SUCCESS, "detach failed"
print(f"✅ Tool detached successfully")
except Exception as e:
print(f"❌ Tool detachment exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
获取连杆名称列表
适用场景：获取机器人模型中所有连杆的名称列表，用于碰撞检测和运动规划调试。
examples/python/galbot_motion/get_link_names.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
# Get and initialize the GalbotMotion singleton
motion = GalbotMotion()
robot = GalbotRobot()
if motion.init():
print("GalbotMotion initialized successfully")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialized successfully")
else:
print("GalbotRobot initialization failed")
# Program started, waiting for data
time.sleep(2)
try:
# Get all link names
all_link_names = motion.get_link_names(only_end_effector=False)
print(f"\nAll link names (total {len(all_link_names)}):")
for i, link_name in enumerate(all_link_names, 1):
print(f" {i}. {link_name}")
# getend effectorexecute link
ee_link_names = motion.get_link_names(only_end_effector=True)
print(f"\nEnd-effector link names (total {len(ee_link_names)}):")
for i, link_name in enumerate(ee_link_names, 1):
print(f" {i}. {link_name}")
except Exception as e:
print(f"❌ Link-name retrieval exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
加载/移除环境碰撞体
适用场景：向运动规划环境中添加障碍物或移除已添加的障碍物，使运动规划考虑环境障碍。
examples/python/galbot_motion/add_obstacle.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
# NOTE:
# - GalbotMotion currently does NOT provide real-time obstacle perception / automatic environment updates.
# - If you want Motion collision checking to consider obstacles (including point clouds), you must load them
# manually via add_obstacle()/attach_target_object().
motion = GalbotMotion()
robot = GalbotRobot()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Result: SUCCESS")
elif(status == gm.MotionStatus.TIMEOUT):
print("Result: TIMEOUT")
elif(status == gm.MotionStatus.FAULT):
print("Result: FAULT")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Result: INVALID_INPUT")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Result: INIT_FAILED")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Result: IN_PROGRESS")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Result: STOPPED_UNREACHED")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Result: DATA_FETCH_FAILED")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Result: PUBLISH_FAIL")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Result: COMM_DISCONNECTED")
if motion.init():
print("GalbotMotion init OK")
else:
print("GalbotMotion init FAILED")
if robot.init():
print("GalbotRobot init OK")
else:
print("GalbotRobot init FAILED")
# Wait for data to be ready.
time.sleep(2)
# 1) Add a box collision object into Motion environment.
# This affects Motion-side collision checking (e.g., motion_plan/check_collision).
try:
obstacle_id = "box_test_1"
obj_type = "box"
obj_pose = [1.0, 0.0, 1.0, 0,0,0,1]
obj_size = [1.0, 1.0, 1.0]
target_frame = "world"
status = motion.add_obstacle(
obstacle_id=obstacle_id,
obstacle_type=obj_type,
pose=obj_pose,
scale=obj_size,
target_frame=target_frame
)
printStatus(status)
motion.clear_obstacle()
assert status == gm.MotionStatus.SUCCESS, "Failed to add obstacle"
print(f"OK: added obstacle: {obstacle_id}")
except Exception as e:
print(f"ERROR: add obstacle exception: {e}")
# 2) Add a duplicate ID (expected to fail).
try:
obstacle_id = "box_test_1"
obj_type = "box"
obj_pose = [1.0, 0.0, 1.0, 0,0,0,1]
obj_size = [1.0, 1.0, 1.0]
target_frame = "world"
status = motion.add_obstacle(
obstacle_id=obstacle_id,
obstacle_type=obj_type,
pose=obj_pose,
scale=obj_size,
target_frame=target_frame
)
status = motion.add_obstacle(
obstacle_id=obstacle_id,
obstacle_type=obj_type,
pose=obj_pose,
scale=obj_size,
target_frame=target_frame
)
printStatus(status)
motion.clear_obstacle()
assert status == gm.MotionStatus.FAULT, "Expected duplicate obstacle ID to fail"
print("OK: duplicate obstacle ID is rejected")
except Exception as e:
print(f"ERROR: duplicate obstacle exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
批量添加障碍物（add_obstacles）
适用场景：在运动规划前一次添加多个环境碰撞体。
examples/python/galbot_motion/add_multi_obstacle.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
# NOTE:
# - GalbotMotion currently does NOT provide real-time obstacle perception / automatic environment updates.
# - If you want Motion collision checking to consider obstacles (including point clouds), you must load them
# manually via add_obstacle()/attach_target_object().
motion = GalbotMotion()
robot = GalbotRobot()
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Result: SUCCESS")
elif(status == gm.MotionStatus.TIMEOUT):
print("Result: TIMEOUT")
elif(status == gm.MotionStatus.FAULT):
print("Result: FAULT")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Result: INVALID_INPUT")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Result: INIT_FAILED")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Result: IN_PROGRESS")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Result: STOPPED_UNREACHED")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Result: DATA_FETCH_FAILED")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Result: PUBLISH_FAIL")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Result: COMM_DISCONNECTED")
if motion.init():
print("GalbotMotion init OK")
else:
print("GalbotMotion init FAILED")
if robot.init():
print("GalbotRobot init OK")
else:
print("GalbotRobot init FAILED")
# Wait for data to be ready.
time.sleep(2)
# 1) Add a box collision object into Motion environment.
# This affects Motion-side collision checking (e.g., motion_plan/check_collision).
try:
obstacle_id = "box_test_1"
obj_type = "box"
obj_pose = [1.0, 0.0, 1.0, 0,0,0,1]
obj_size = [1.0, 1.0, 1.0]
target_frame = "world"
status = motion.add_obstacle(
obstacle_id=obstacle_id,
obstacle_type=obj_type,
pose=obj_pose,
scale=obj_size,
target_frame=target_frame
)
printStatus(status)
motion.clear_obstacle()
assert status == gm.MotionStatus.SUCCESS, "Failed to add obstacle"
print(f"OK: added obstacle: {obstacle_id}")
except Exception as e:
print(f"ERROR: add obstacle exception: {e}")
# 2) Add a sphere collision object into Motion environment.
# This affects Motion-side collision checking (e.g., motion_plan/check_collision).
try:
obstacle_id = "sphere_1"
obj_type = "sphere"
obj_pose = [2.0, 0.0, 1.0, 0,0,0,1]
obj_size = [1.0,0,0]
target_frame = "world"
status = motion.add_obstacle(
obstacle_id=obstacle_id,
obstacle_type=obj_type,
pose=obj_pose,
scale=obj_size,
target_frame=target_frame
)
printStatus(status)
motion.clear_obstacle()
assert status == gm.MotionStatus.SUCCESS, "Failed to add obstacle"
print(f"OK: added obstacle: {obstacle_id}")
except Exception as e:
print(f"ERROR: add obstacle exception: {e}")
# 3) Add a cylinder collision object into Motion environment.
# This affects Motion-side collision checking (e.g., motion_plan/check_collision).
try:
obstacle_id = "cylinder_1"
obj_type = "cylinder"
obj_pose = [3.0, 0.0, 1.0, 0,0,0,1]
obj_size = [1.0,1.0,0]
target_frame = "world"
status = motion.add_obstacle(
obstacle_id=obstacle_id,
obstacle_type=obj_type,
pose=obj_pose,
scale=obj_size,
target_frame=target_frame
)
printStatus(status)
motion.clear_obstacle()
assert status == gm.MotionStatus.SUCCESS, "Failed to add obstacle"
print(f"OK: added obstacle: {obstacle_id}")
except Exception as e:
print(f"ERROR: add obstacle exception: {e}")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
详细碰撞检测（check_collision_detail）
适用场景：查看详细碰撞结果，确定发生碰撞的机器人连杆或环境物体。
examples/python/galbot_motion/check_collision_detail.py
import sys
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotNavigation, GalbotRobot
def print_collision_details(motion, title, status, collision_infos):
print(f"{title} status: {motion.status_to_string(status)}")
if status != gm.MotionStatus.SUCCESS:
return
if not collision_infos:
print(" no collision pair returned")
return
for i, info in enumerate(collision_infos):
label = "COLLISION" if info.is_collision else "NO COLLISION"
print(
f" - pair [{i}]: {label}, {info.link1} <-> {info.link2}, "
f"distance: {info.distance}, type: {info.collision_type}"
)
motion = GalbotMotion()
robot = GalbotRobot()
navigation = GalbotNavigation()
if not motion.init():
print("GalbotMotion init FAILED", file=sys.stderr)
sys.exit(1)
if not robot.init():
print("GalbotRobot init FAILED", file=sys.stderr)
sys.exit(1)
if not navigation.init():
print("GalbotNavigation init FAILED", file=sys.stderr)
sys.exit(1)
params = gm.Parameter()
params.set_timeout(5.0)
try:
print(">> Detailed collision check: current robot state")
status, collision_infos = motion.check_collision_detail(
robot_states=[],
is_check_once=False,
is_log=False,
params=params,
)
print_collision_details(motion, "current robot state", status, collision_infos)
chain_joints = {
"leg": [0.4992, 1.4991, 1.0005, 0.0000, -0.0004],
"head": [0.0000, 0.0],
"left_arm": [2.0, -1.6, -0.6, -1.7, 0.0, -0.8, 0.0],
"right_arm": [-2.0, 1.6, 0.6, 1.7, 0.0, 0.8, 0.0],
}
whole_body_joint = [num for key in ["leg", "head", "left_arm", "right_arm"] for num in chain_joints[key]]
whole_body_state = gm.RobotStates()
whole_body_state.whole_body_joint = whole_body_joint
whole_body_state.base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
left_arm_state = gm.JointStates()
left_arm_state.chain_name = "left_arm"
left_arm_state.joint_positions = [2.0, -1.6, 0.6, -1.7, 0.0, -0.8, 0.0]
print(">> Detailed collision check: explicit robot states")
status, collision_infos = motion.check_collision_detail(
robot_states=[whole_body_state, left_arm_state],
is_check_once=False,
is_log=True,
params=params,
)
print_collision_details(motion, "explicit robot states", status, collision_infos)
except Exception as e:
print(f"ERROR: check_collision_detail exception: {e}", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
组合运动规划（combine_plan）
适用场景：将多个运动链目标组合为一个协调运动规划请求。
examples/python/galbot_motion/combine_plan.py
"""G1 combine_plan example aligned with combine_plan_example.cpp."""
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
START_JOINT_STATE = {
"leg": [0.499191, 1.49907, 1.00046, 0.0, 0.0],
"head": [0.0, 0.00989756],
"left_arm": [1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
"right_arm": [-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0],
}
START_MOVE_SPEED_RAD_S = 0.12
START_MOVE_TIMEOUT_S = 30.0
UPPER_BODY_GROUPS = ["head", "left_arm", "right_arm"]
def initialize_interfaces():
motion = GalbotMotion()
print("GalbotMotion::get_instance() status: no return status; call completed.")
robot = GalbotRobot()
print("GalbotRobot::get_instance() status: no return status; call completed.")
if not motion.init():
print("GalbotMotion::init() status: FAILED")
return None, None
print("GalbotMotion::init() status: SUCCESS")
if not robot.init():
print("GalbotRobot::init() status: FAILED")
return None, None
print("GalbotRobot::init() status: SUCCESS")
time.sleep(1.0)
return motion, robot
def shutdown_robot(robot):
robot.request_shutdown()
print("request_shutdown() status: no return status; call completed.")
robot.wait_for_shutdown()
print("wait_for_shutdown() status: no return status; call completed.")
robot.destroy()
print("destroy() status: no return status; call completed.")
def print_control_status(api_name, status):
result = "SUCCESS" if status == gm.ControlStatus.SUCCESS else "FAILED"
print(f"{api_name} status: {str(status).rsplit('.', 1)[-1]} ({result})")
def move_robot_to_start_state(robot):
print("Move robot to combine_plan start state.")
print(f" leg start joints: {START_JOINT_STATE['leg']}")
# controller_status = robot.switch_controller(gm.G1ControllerName.LEG_PVT_CTRL)
# print_control_status("GalbotRobot::switch_controller(leg_pvt_ctrl)", controller_status)
# if controller_status != gm.ControlStatus.SUCCESS:
# return False
# time.sleep(0.5)
leg_status = robot.set_joint_positions(
START_JOINT_STATE["leg"], ["leg"], [], True,
START_MOVE_SPEED_RAD_S, START_MOVE_TIMEOUT_S
)
print_control_status("GalbotRobot::set_joint_positions(leg start)", leg_status)
if leg_status != gm.ControlStatus.SUCCESS:
return False
upper_body = (
START_JOINT_STATE["head"]
+ START_JOINT_STATE["left_arm"]
+ START_JOINT_STATE["right_arm"]
)
print(f" upper-body start joints [head, left_arm, right_arm]: {upper_body}")
status = robot.set_joint_positions(
upper_body, UPPER_BODY_GROUPS, [], True,
START_MOVE_SPEED_RAD_S, START_MOVE_TIMEOUT_S
)
print_control_status("GalbotRobot::set_joint_positions(head+arms start)", status)
return status == gm.ControlStatus.SUCCESS
def capture_current_pose(motion, robot, chain, current):
joint_names = list(motion.get_chain_joint_names(chain))
print(f"get_chain_joint_names({chain}) status: {'SUCCESS' if joint_names else 'FAILED'}")
if not joint_names:
return False
joints = list(robot.get_joint_positions([], joint_names))
success = len(joints) == len(joint_names)
print(f"GalbotRobot::get_joint_positions({chain}) status: {'SUCCESS' if success else 'FAILED'}")
if not success:
return False
current["joint_names"][chain] = joint_names
current["joints"][chain] = joints
print(f"Current {chain} joints: {joints}")
status, pose = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link")
result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
print(f"get_end_effector_pose_on_chain({chain}) status: {motion.status_to_string(status)} ({result})")
pose = list(pose)
if status != gm.MotionStatus.SUCCESS or len(pose) != 7:
return False
current["poses"][chain] = pose
print(f"Current {chain} pose [x, y, z, qx, qy, qz, qw]: {pose}")
return True
def make_joint_target(chain, joint_positions, joint_names=None):
target = gm.MotionPlanChainTarget()
target.chain_name = chain
target.mode = gm.MotionPlanTargetMode.kJoint
target.joint.chain_name = chain
target.joint.joint_positions = list(joint_positions)
target.joint.joint_names = list(joint_names or [])
return target
def make_cartesian_target(chain, pose, assist_chains=None):
target = gm.MotionPlanChainTarget()
target.chain_name = chain
target.mode = gm.MotionPlanTargetMode.kCartesian
target.cart.chain_name = chain
target.cart.frame_id = "EndEffector"
target.cart.reference_frame = "base_link"
target.cart.pose = gm.Pose(list(pose))
target.cart.assist_chains = set(assist_chains or [])
return target
def make_planner_config(check_collision):
params = gm.Parameter()
params.set_direct_execute(True)
params.set_blocking(True)
params.set_timeout(120.0)
params.set_check_collision(check_collision)
params.set_reference_frame("base_link")
params.set_actuate("with_torso")
return params
def print_plan_requests(plan_requests):
plan_type_names = {
gm.MotionPlanType.MOTION_PLAN: "MOTION_PLAN",
gm.MotionPlanType.TRAJ_PLAN: "TRAJ_PLAN",
gm.MotionPlanType.MOVE_LINE: "MOVE_LINE",
}
print(f"combine_plan plan requests: {len(plan_requests)}")
for request_index, request in enumerate(plan_requests):
print(
f" request[{request_index}] type={plan_type_names.get(request.plan_type, 'UNKNOWN')} "
f"waypoints={len(request.target)}"
)
for waypoint_index, waypoint in enumerate(request.target):
print(f" waypoint[{waypoint_index}] targets: {len(waypoint)}")
for target in waypoint:
mode = "JOINT" if target.mode == gm.MotionPlanTargetMode.kJoint else "CART"
print(f" {target.chain_name} {mode}")
def print_traj_result(label, status, traj, motion):
result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
print(f"{label} status: {motion.status_to_string(status)} ({result})")
if status == gm.MotionStatus.SUCCESS:
if not traj:
print("Trajectory map is empty.")
for chain, points in traj.items():
print(f" {chain} trajectory points: {len(points)}")
def make_plan_request(plan_type, target):
request = gm.PlanRequest()
request.plan_type = plan_type
request.enforce_pass = True
request.target = target
return request
def make_example_inputs(current):
left_names = current["joint_names"]["left_arm"]
right_names = current["joint_names"]["right_arm"]
cart = lambda chain, pose: make_cartesian_target(chain, pose, ["torso"])
request0 = make_plan_request(
gm.MotionPlanType.MOTION_PLAN,
[
[
make_joint_target(
"left_arm",
[1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
left_names,
),
cart("right_arm", [0.22666, -0.23435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
],
[
make_joint_target(
"left_arm",
[1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
left_names,
)
],
[
make_joint_target(
"left_arm",
[1.99995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
left_names,
),
cart("right_arm", [0.12666, -0.23435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
],
],
)
request1 = make_plan_request(
gm.MotionPlanType.MOVE_LINE,
[
[
cart("left_arm", [0.42666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
cart("right_arm", [0.422666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
],
[cart("left_arm", [0.12666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0])],
[cart("right_arm", [0.12666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0])],
],
)
request2 = make_plan_request(
gm.MotionPlanType.MOTION_PLAN,
[
[
make_joint_target(
"left_arm",
[1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0],
left_names,
)
],
[
make_joint_target(
"right_arm",
[-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0],
right_names,
)
],
[make_joint_target("torso", [0.0, 0.0], ["leg_joint4", "leg_joint5"])],
],
)
plan_requests = [request0, request1, request2]
return plan_requests, make_planner_config(check_collision=True)
def run_example():
motion, robot = initialize_interfaces()
if motion is None or robot is None:
return 1
try:
if not move_robot_to_start_state(robot):
return 1
time.sleep(0.5)
current = {"joint_names": {}, "joints": {}, "poses": {}}
if not capture_current_pose(motion, robot, "left_arm", current):
return 1
if not capture_current_pose(motion, robot, "right_arm", current):
return 1
plan_requests, params = make_example_inputs(current)
print_plan_requests(plan_requests)
print(
"Immediate execution is enabled; plan requests are loaded from "
"combine_file_example_plan_reqs.json reference values."
)
status, traj = motion.combine_plan(plan_requests, params)
print_traj_result("combine_plan", status, traj, motion)
return 0 if status == gm.MotionStatus.SUCCESS else 1
finally:
shutdown_robot(robot)
if __name__ == "__main__":
raise SystemExit(run_example())
获取运动链关节名称（get_chain_joint_names）
适用场景：构造关节状态或目标前，查询运动链中按顺序排列的关节名称。
examples/python/galbot_motion/get_chain_joint_names.py
import time
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def run_example():
motion = GalbotMotion()
robot = GalbotRobot()
# Initialize SDK interfaces directly in the example flow.
if not motion.init():
print("GalbotMotion init FAILED")
return 1
if not robot.init():
print("GalbotRobot init FAILED")
return 1
print("GalbotMotion/GalbotRobot init OK")
time.sleep(1)
# Include common G1 chains and one invalid name to show the empty-result behavior.
for chain in ["left_arm", "right_arm", "leg", "head", "invalid_chain"]:
# This is the API being demonstrated: query joint names by chain name.
joint_names = list(motion.get_chain_joint_names(chain))
print(f"get_chain_joint_names({chain}): {joint_names}")
# Clean shutdown releases SDK resources before the process exits.
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
raise SystemExit(run_example())
获取运动配置（get_config_by_type）
适用场景：读取指定配置类型对应的当前运动控制配置。
examples/python/galbot_motion/get_config_by_type.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def run_example():
motion = GalbotMotion()
robot = GalbotRobot()
# Initialize SDK interfaces directly in the example flow.
if not motion.init():
print("GalbotMotion init FAILED")
return 1
if not robot.init():
print("GalbotRobot init FAILED")
return 1
print("GalbotMotion/GalbotRobot init OK")
time.sleep(1)
# These are common motion-planning service config categories.
config_types = [
"sampler",
"ik_solver",
"trajectory_planner",
"traj_validity_checker",
]
all_success = True
for config_type in config_types:
# This is the API being demonstrated: query one config payload by type.
status, config = motion.get_config_by_type(config_type)
print(
f"get_config_by_type({config_type}) status: {motion.status_to_string(status)}, "
f"payload bytes: {len(config.common_str)}"
)
all_success = all_success and status == gm.MotionStatus.SUCCESS
# Clean shutdown releases SDK resources before the process exits.
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0 if all_success else 1
if __name__ == "__main__":
raise SystemExit(run_example())
获取雅可比矩阵（get_jacobian）
适用场景：计算末端执行器雅可比矩阵，用于微分运动学和速度分析。
examples/python/galbot_motion/get_jacobian.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def print_status(status):
labels = {
gm.MotionStatus.SUCCESS: "SUCCESS",
gm.MotionStatus.TIMEOUT: "TIMEOUT",
gm.MotionStatus.FAULT: "FAULT",
gm.MotionStatus.INVALID_INPUT: "INVALID_INPUT",
gm.MotionStatus.INIT_FAILED: "INIT_FAILED",
gm.MotionStatus.IN_PROGRESS: "IN_PROGRESS",
gm.MotionStatus.STOPPED_UNREACHED: "STOPPED_UNREACHED",
gm.MotionStatus.DATA_FETCH_FAILED: "DATA_FETCH_FAILED",
gm.MotionStatus.PUBLISH_FAIL: "PUBLISH_FAIL",
gm.MotionStatus.COMM_DISCONNECTED: "COMM_DISCONNECTED",
}
return labels.get(status, "UNKNOWN")
def print_vector(values):
print("[", end="")
for index, value in enumerate(values):
if index > 0:
print(", ", end="")
print(value, end="")
print("]", end="")
def print_joint_state(joint_state):
print("{")
for chain_name, joints in joint_state.items():
print(f" {chain_name}: ", end="")
print_vector(joints)
print()
print(" }")
def print_get_jacobian_params(
chain_name,
target_frame,
reference_frame,
joint_state,
):
print("get_jacobian parameters:")
print(f" chain_name: {chain_name}")
print(f" target_frame: {target_frame}")
print(f" reference_frame: {reference_frame}")
print(" joint_state: ", end="")
print_joint_state(joint_state)
def print_jacobian(label, result):
status, matrix = result
print(f"[{label}] Status: {print_status(status)}")
if status != gm.MotionStatus.SUCCESS or not matrix:
print("Jacobian computation failed or returned an empty matrix.")
return
rows = len(matrix)
cols = len(matrix[0])
print(f"Jacobian matrix ({rows}x{cols}):")
for row_index, row in enumerate(matrix):
row_str = f" row {row_index}:"
for value in row:
row_str += f" {value:10.5f}"
print(row_str)
def make_fixed_joint_values(dof):
fixed_values = [-1.2, -0.8, -0.4, 0.0, 0.4, 0.8, 1.2]
return [fixed_values[index % len(fixed_values)] for index in range(dof)]
def run_example():
motion = GalbotMotion()
robot = GalbotRobot()
robot_initialized = False
if not motion.init():
print("GalbotMotion initialization failed")
return 1
print("GalbotMotion initialized successfully")
if not robot.init():
print("GalbotRobot initialization failed")
return 1
robot_initialized = True
print("GalbotRobot initialized successfully")
try:
time.sleep(3)
support_chains = set(motion.get_supported_chains())
print(f"support_chains ({len(support_chains)}): {sorted(support_chains)}")
# support_chains.discard("torso")
if not support_chains:
support_chains = {"head", "left_arm", "right_arm", "torso", "leg"}
try:
current_chain_joint_state = motion.get_chain_joint_state()
except Exception as e:
print(f"get_chain_joint_state exception: {e}")
current_chain_joint_state = {}
target_frame = "EndEffector"
reference_frame = "base_link"
# Known chain DOF used only when the runtime joint state is unavailable.
fallback_dof = {"head": 2, "left_arm": 7, "right_arm": 7, "leg": 5}
for chain_name in sorted(support_chains):
# Prefer the runtime joint count; fall back to a known DOF when it is unavailable.
current_joints = current_chain_joint_state.get(chain_name, [])
if current_joints:
dof = len(current_joints)
else:
dof = fallback_dof.get(chain_name, 0)
if dof == 0:
print(f"Skip chain '{chain_name}': unable to determine chain DOF.")
continue
joint_state = {chain_name: make_fixed_joint_values(dof)}
try:
print(f"=== get_jacobian: {chain_name} ===")
print_get_jacobian_params(
chain_name,
target_frame,
reference_frame,
joint_state,
)
result = motion.get_jacobian(
chain_name,
target_frame,
reference_frame,
joint_state,
)
print_jacobian(chain_name, result)
except Exception as e:
print(f"{chain_name} exception: {e}")
return 0
finally:
if robot_initialized:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
raise SystemExit(run_example())
基于机器人状态获取雅可比矩阵（get_jacobian_by_state）
适用场景：在显式指定的机器人状态下计算雅可比矩阵，而不是使用当前状态。
examples/python/galbot_motion/get_jacobian_by_state.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def print_status(status):
labels = {
gm.MotionStatus.SUCCESS: "SUCCESS",
gm.MotionStatus.TIMEOUT: "TIMEOUT",
gm.MotionStatus.FAULT: "FAULT",
gm.MotionStatus.INVALID_INPUT: "INVALID_INPUT",
gm.MotionStatus.INIT_FAILED: "INIT_FAILED",
gm.MotionStatus.IN_PROGRESS: "IN_PROGRESS",
gm.MotionStatus.STOPPED_UNREACHED: "STOPPED_UNREACHED",
gm.MotionStatus.DATA_FETCH_FAILED: "DATA_FETCH_FAILED",
gm.MotionStatus.PUBLISH_FAIL: "PUBLISH_FAIL",
gm.MotionStatus.COMM_DISCONNECTED: "COMM_DISCONNECTED",
}
return labels.get(status, "UNKNOWN")
def print_vector(values):
print("[", end="")
for index, value in enumerate(values):
if index > 0:
print(", ", end="")
print(value, end="")
print("]", end="")
def print_robot_states(robot_states):
if robot_states is None:
print("None (use current complete robot state)")
return
whole_body_joint = list(getattr(robot_states, "whole_body_joint", []))
base_state = list(getattr(robot_states, "base_state", []))
print("RobotStates")
print(f" whole_body_joint[{len(whole_body_joint)}]: ", end="")
print_vector(whole_body_joint)
print()
print(f" base_state[{len(base_state)}]: ", end="")
print_vector(base_state)
print()
def print_get_jacobian_by_state_params(
chain_name,
target_frame,
reference_frame,
reference_robot_states,
):
print("get_jacobian_by_state parameters:")
print(f" chain_name: {chain_name}")
print(f" target_frame: {target_frame}")
print(f" reference_frame: {reference_frame}")
print(" reference_robot_states: ", end="")
print_robot_states(reference_robot_states)
def print_jacobian(label, result):
status, matrix = result
print(f"[{label}] Status: {print_status(status)}")
if status != gm.MotionStatus.SUCCESS or not matrix:
print("Jacobian computation failed or returned an empty matrix.")
return
rows = len(matrix)
cols = len(matrix[0])
print(f"Jacobian matrix ({rows}x{cols}):")
for row_index, row in enumerate(matrix):
row_str = f" row {row_index}:"
for value in row:
row_str += f" {value:10.5f}"
print(row_str)
def make_fixed_whole_body_joint(dof):
fixed_values = [
0.3,
0.8,
0.5,
0.0,
0.0,
0.1,
-0.2,
1.2,
-0.8,
0.4,
-1.0,
0.2,
-0.6,
0.0,
-1.2,
0.8,
-0.4,
1.0,
-0.2,
0.6,
0.0,
]
return [fixed_values[index % len(fixed_values)] for index in range(dof)]
def run_example():
motion = GalbotMotion()
robot = GalbotRobot()
robot_initialized = False
if not motion.init():
print("GalbotMotion initialization failed")
return 1
print("GalbotMotion initialized successfully")
if not robot.init():
print("GalbotRobot initialization failed")
return 1
robot_initialized = True
print("GalbotRobot initialized successfully")
try:
time.sleep(3)
support_chains = set(motion.get_supported_chains())
print(f"support_chains ({len(support_chains)}): {sorted(support_chains)}")
if not support_chains:
support_chains = {"head", "left_arm", "right_arm", "torso", "leg"}
target_frame = "EndEffector"
reference_frame = "base_link"
whole_body_dof = 21
try:
current_robot_state = motion.get_robot_states()
if getattr(current_robot_state, "whole_body_joint", []):
whole_body_dof = len(current_robot_state.whole_body_joint)
except Exception as e:
print(f"get_robot_states exception: {e}")
reference_robot_states = gm.RobotStates()
reference_robot_states.whole_body_joint = make_fixed_whole_body_joint(
whole_body_dof
)
reference_robot_states.base_state = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# Compute the Jacobian for every chain using an explicit RobotStates
for chain_name in sorted(support_chains):
try:
print(
f"=== get_jacobian_by_state (explicit RobotStates): {chain_name} ==="
)
print_get_jacobian_by_state_params(
chain_name,
target_frame,
reference_frame,
reference_robot_states,
)
result = motion.get_jacobian_by_state(
chain_name,
target_frame,
reference_frame,
reference_robot_states,
)
print_jacobian(f"{chain_name} explicit RobotStates", result)
except Exception as e:
print(f"{chain_name} explicit RobotStates exception: {e}")
# Compute the Jacobian for every chain using None (the current robot state)
for chain_name in sorted(support_chains):
try:
print(f"=== get_jacobian_by_state (None RobotStates): {chain_name} ===")
print_get_jacobian_by_state_params(
chain_name,
target_frame,
reference_frame,
None,
)
result = motion.get_jacobian_by_state(
chain_name,
target_frame,
reference_frame,
None,
)
print_jacobian(f"{chain_name} None RobotStates", result)
except Exception as e:
print(f"{chain_name} None RobotStates exception: {e}")
return 0
finally:
if robot_initialized:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
raise SystemExit(run_example())
通用逆运动学
适用场景：通过通用目标及约束配置求解复杂任务的逆运动学结果。
examples/python/galbot_motion/inverse_kinematics_general.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def make_pose_target_from_current(current_pose, axis=0):
# Pose is [x, y, z, qx, qy, qz, qw]; move one translation axis only.
target = list(current_pose)
if len(target) == 7 and axis < 3:
target[axis] += max(abs(target[axis]) * 0.1, 0.02)
return target
def make_cartesian_target(chain, pose, assist_chains=None):
target = gm.MotionPlanChainTarget()
target.chain_name = chain
target.mode = gm.MotionPlanTargetMode.kCartesian
# frame_id is the end-effector frame; reference_frame is the IK reference frame.
target.cart.chain_name = chain
target.cart.frame_id = "EndEffector"
target.cart.reference_frame = "base_link"
target.cart.pose = gm.Pose(list(pose))
target.cart.assist_chains = set(assist_chains or [])
return target
def run_example():
motion = GalbotMotion()
robot = GalbotRobot()
# Initialize SDK interfaces directly in the example flow.
if not motion.init():
print("GalbotMotion init FAILED")
return 1
if not robot.init():
print("GalbotRobot init FAILED")
return 1
print("GalbotMotion/GalbotRobot init OK")
time.sleep(1)
# Read the current end-effector pose first; the IK target is a small relative move.
pose_status, current_pose = motion.get_end_effector_pose_on_chain(
chain_name="left_arm",
frame_id="EndEffector",
reference_frame="base_link",
)
print(f"get_end_effector_pose_on_chain(left_arm) status: {motion.status_to_string(pose_status)}")
if pose_status != gm.MotionStatus.SUCCESS:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 1
current_pose = list(current_pose)
target_pose = make_pose_target_from_current(current_pose)
print(f"Current left_arm pose: {current_pose}")
print(f"Target left_arm pose (+10% x translation): {target_pose}")
# inverse_kinematics_general accepts the same waypoint target format as planning APIs.
target_waypoint = [
make_cartesian_target("left_arm", target_pose, ["leg"]),
]
# Use the current whole-body state as the IK reference seed when it is available.
reference_state = motion.get_robot_states()
if not getattr(reference_state, "whole_body_joint", []):
reference_state = None
# IK uses Parameter for common options such as blocking, timeout, and reference frame.
params = gm.Parameter()
params.set_direct_execute(False)
params.set_blocking(True)
params.set_timeout(20.0)
params.set_check_collision(False)
params.set_reference_frame("base_link")
# This is the API being demonstrated: solve joint values for the target waypoint.
status, joint_map = motion.inverse_kinematics_general(
target_waypoint=target_waypoint,
reference_robot_states=reference_state,
params=params,
)
print(f"inverse_kinematics_general status: {motion.status_to_string(status)}")
for chain, joints in joint_map.items():
print(f" {chain} joint_names: {list(joints.joint_names)}")
print(f" {chain} joint_positions: {list(joints.joint_positions)}")
# Clean shutdown releases SDK resources before the process exits.
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0 if status == gm.MotionStatus.SUCCESS else 1
if __name__ == "__main__":
raise SystemExit(run_example())
笛卡尔直线运动（move_line）
适用场景：在笛卡尔空间规划并执行末端执行器直线运动。
examples/python/galbot_motion/move_line.py
"""G1 move_line example aligned with move_line_example.cpp."""
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def initialize_interfaces():
motion = GalbotMotion()
print("GalbotMotion::get_instance() status: no return status; call completed.")
robot = GalbotRobot()
print("GalbotRobot::get_instance() status: no return status; call completed.")
if not motion.init():
print("GalbotMotion::init() status: FAILED")
return None, None
print("GalbotMotion::init() status: SUCCESS")
if not robot.init():
print("GalbotRobot::init() status: FAILED")
return None, None
print("GalbotRobot::init() status: SUCCESS")
time.sleep(1.0)
return motion, robot
def shutdown_robot(robot):
robot.request_shutdown()
print("request_shutdown() status: no return status; call completed.")
robot.wait_for_shutdown()
print("wait_for_shutdown() status: no return status; call completed.")
robot.destroy()
print("destroy() status: no return status; call completed.")
def print_control_status(api_name, status):
result = "SUCCESS" if status == gm.ControlStatus.SUCCESS else "FAILED"
print(f"{api_name} status: {str(status).rsplit('.', 1)[-1]} ({result})")
def move_robot_to_home_position(robot):
print("Move robot to the G1 home position.")
status = robot.set_joint_positions(
[
0.5, 1.5, 1.0, 0.0, 0.0,
0.0, 0.0,
2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0,
],
["leg", "head", "left_arm", "right_arm"],
[],
True,
0.1,
30.0,
)
print_control_status("GalbotRobot::set_joint_positions(home)", status)
return status == gm.ControlStatus.SUCCESS
def capture_current_pose(motion, robot, chain, current):
joint_names = list(motion.get_chain_joint_names(chain))
print(f"get_chain_joint_names({chain}) status: {'SUCCESS' if joint_names else 'FAILED'}")
if not joint_names:
return False
joints = list(robot.get_joint_positions([], joint_names))
success = len(joints) == len(joint_names)
print(f"GalbotRobot::get_joint_positions({chain}) status: {'SUCCESS' if success else 'FAILED'}")
if not success:
return False
current["joint_names"][chain] = joint_names
current["joints"][chain] = joints
print(f"Current {chain} joints: {joints}")
status, pose = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link")
result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
print(f"get_end_effector_pose_on_chain({chain}) status: {motion.status_to_string(status)} ({result})")
pose = list(pose)
if status != gm.MotionStatus.SUCCESS or len(pose) != 7:
return False
current["poses"][chain] = pose
print(f"Current {chain} pose [x, y, z, qx, qy, qz, qw]: {pose}")
return True
def make_cartesian_target(chain, pose, assist_chains=None):
target = gm.MotionPlanChainTarget()
target.chain_name = chain
target.mode = gm.MotionPlanTargetMode.kCartesian
target.cart.chain_name = chain
target.cart.frame_id = "EndEffector"
target.cart.reference_frame = "base_link"
target.cart.pose = gm.Pose(list(pose))
target.cart.assist_chains = set(assist_chains or [])
return target
def make_planner_config(check_collision):
params = gm.Parameter()
params.set_direct_execute(True)
params.set_blocking(True)
params.set_timeout(120.0)
params.set_check_collision(check_collision)
params.set_reference_frame("base_link")
params.set_actuate("with_torso")
return params
def print_waypoints(label, waypoints):
print(f"{label} waypoints: {len(waypoints)}")
for index, waypoint in enumerate(waypoints):
print(f" waypoint[{index}] targets: {len(waypoint)}")
for target in waypoint:
print(f" {target.chain_name} CART frame_id={target.cart.frame_id} reference_frame={target.cart.reference_frame}")
def print_traj_result(label, status, traj, motion):
result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
print(f"{label} status: {motion.status_to_string(status)} ({result})")
if status == gm.MotionStatus.SUCCESS:
if not traj:
print("Trajectory map is empty.")
for chain, points in traj.items():
print(f" {chain} trajectory points: {len(points)}")
def make_example_inputs():
cart = lambda chain, pose: make_cartesian_target(chain, pose, ["torso"])
waypoints = [
[
cart("left_arm", [0.42666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
cart("right_arm", [0.422666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
],
[
cart("left_arm", [0.42666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
cart("right_arm", [0.422666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
],
[
cart("left_arm", [0.12666, 0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
cart("right_arm", [0.12666, -0.33435, 1.03569, 0.0, 0.0, 0.0, 1.0]),
],
[
cart("left_arm", [0.12666, 0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
cart("right_arm", [0.12666, -0.33435, 0.73569, 0.0, 0.0, 0.0, 1.0]),
],
]
return waypoints, make_planner_config(check_collision=False)
def run_example():
motion, robot = initialize_interfaces()
if motion is None or robot is None:
return 1
try:
if not move_robot_to_home_position(robot):
return 1
time.sleep(0.5)
current = {"joint_names": {}, "joints": {}, "poses": {}}
if not capture_current_pose(motion, robot, "left_arm", current):
return 1
if not capture_current_pose(motion, robot, "right_arm", current):
return 1
waypoints, params = make_example_inputs()
print_waypoints("move_line", waypoints)
print(
"Immediate execution is enabled; waypoints are loaded from "
"move_line_targets.json reference values."
)
status, traj = motion.move_line(waypoints, params)
print_traj_result("move_line", status, traj, motion)
return 0 if status == gm.MotionStatus.SUCCESS else 1
finally:
shutdown_robot(robot)
if __name__ == "__main__":
raise SystemExit(run_example())
设置运动配置（set_config_by_type）
适用场景：按配置类型更新运动规划参数。
examples/python/galbot_motion/set_config_by_type.py
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def run_example():
motion = GalbotMotion()
robot = GalbotRobot()
# Initialize SDK interfaces directly in the example flow.
if not motion.init():
print("GalbotMotion init FAILED")
return 1
if not robot.init():
print("GalbotRobot init FAILED")
return 1
print("GalbotMotion/GalbotRobot init OK")
time.sleep(1)
# Read the current config first. This keeps the example non-destructive.
get_status, config = motion.get_config_by_type("sampler")
print(f"get_config_by_type(sampler) status: {motion.status_to_string(get_status)}")
if get_status != gm.MotionStatus.SUCCESS:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 1
print(f"Current sampler config payload bytes: {len(config.common_str)}")
# Send the unchanged payload back to cover the API call without changing behavior.
set_status = motion.set_config_by_type(config)
print(f"set_config_by_type(sampler, unchanged payload) status: {motion.status_to_string(set_status)}")
# Clean shutdown releases SDK resources before the process exits.
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0 if set_status == gm.MotionStatus.SUCCESS else 1
if __name__ == "__main__":
raise SystemExit(run_example())
轨迹规划（traj_plan）
适用场景：为已构造的运动规划请求生成带时间参数的轨迹。
examples/python/galbot_motion/traj_plan.py
"""G1 traj_plan example aligned with traj_plan_example.cpp."""
import time
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotMotion, GalbotRobot
def initialize_interfaces():
motion = GalbotMotion()
print("GalbotMotion::get_instance() status: no return status; call completed.")
robot = GalbotRobot()
print("GalbotRobot::get_instance() status: no return status; call completed.")
if not motion.init():
print("GalbotMotion::init() status: FAILED")
return None, None
print("GalbotMotion::init() status: SUCCESS")
if not robot.init():
print("GalbotRobot::init() status: FAILED")
return None, None
print("GalbotRobot::init() status: SUCCESS")
time.sleep(1.0)
return motion, robot
def shutdown_robot(robot):
robot.request_shutdown()
print("request_shutdown() status: no return status; call completed.")
robot.wait_for_shutdown()
print("wait_for_shutdown() status: no return status; call completed.")
robot.destroy()
print("destroy() status: no return status; call completed.")
def print_control_status(api_name, status):
result = "SUCCESS" if status == gm.ControlStatus.SUCCESS else "FAILED"
print(f"{api_name} status: {str(status).rsplit('.', 1)[-1]} ({result})")
def move_robot_to_home_position(robot):
print("Move robot to the G1 home position.")
status = robot.set_joint_positions(
[
0.5, 1.5, 1.0, 0.0, 0.0,
0.0, 0.0,
2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0,
],
["leg", "head", "left_arm", "right_arm"],
[],
True,
0.1,
30.0,
)
print_control_status("GalbotRobot::set_joint_positions(home)", status)
return status == gm.ControlStatus.SUCCESS
def capture_current_joints(motion, robot, chains, current):
for chain in chains:
joint_names = list(motion.get_chain_joint_names(chain))
print(f"get_chain_joint_names({chain}) status: {'SUCCESS' if joint_names else 'FAILED'}")
if not joint_names:
return False
joints = list(robot.get_joint_positions([], joint_names))
success = len(joints) == len(joint_names)
print(f"GalbotRobot::get_joint_positions({chain}) status: {'SUCCESS' if success else 'FAILED'}")
if not success:
return False
current["joint_names"][chain] = joint_names
current["joints"][chain] = joints
print(f"Current {chain} joints: {joints}")
return True
def make_joint_target(chain, joint_positions, joint_names=None):
target = gm.MotionPlanChainTarget()
target.chain_name = chain
target.mode = gm.MotionPlanTargetMode.kJoint
target.joint.chain_name = chain
target.joint.joint_positions = list(joint_positions)
target.joint.joint_names = list(joint_names or [])
return target
def print_waypoints(label, waypoints):
print(f"{label} waypoints: {len(waypoints)}")
for index, waypoint in enumerate(waypoints):
print(f" waypoint[{index}] targets: {len(waypoint)}")
for target in waypoint:
print(f" {target.chain_name} JOINT q={list(target.joint.joint_positions)}")
def print_traj_result(label, status, traj, motion):
result = "SUCCESS" if status == gm.MotionStatus.SUCCESS else "FAILED"
print(f"{label} status: {motion.status_to_string(status)} ({result})")
if status == gm.MotionStatus.SUCCESS:
if not traj:
print("Trajectory map is empty.")
for chain, points in traj.items():
print(f" {chain} trajectory points: {len(points)}")
def make_example_inputs(current):
left_names = current["joint_names"]["left_arm"]
right_names = current["joint_names"]["right_arm"]
waypoints = [
[
make_joint_target(
"left_arm", [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
),
],
[
make_joint_target(
"left_arm", [1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
),
],
[
make_joint_target(
"left_arm", [1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0], right_names
),
],
]
params = gm.Parameter()
params.set_direct_execute(True)
params.set_blocking(True)
params.set_timeout(120.0)
params.set_check_collision(False)
params.set_reference_frame("base_link")
params.set_actuate("with_chain_only")
return waypoints, params
def make_example_inputs_with_leg(current):
left_names = current["joint_names"]["left_arm"]
right_names = current["joint_names"]["right_arm"]
leg_names = current["joint_names"]["leg"]
leg_target = list(current["joints"]["leg"])
leg_target[3] = 0.0
leg_target[4] = 0.0
waypoints = [
[
make_joint_target(
"left_arm", [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
),
make_joint_target("leg", leg_target, leg_names),
],
[
make_joint_target(
"left_arm", [1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
),
make_joint_target("leg", leg_target, leg_names),
],
[
make_joint_target(
"left_arm", [1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0], right_names
),
make_joint_target("leg", leg_target, leg_names),
],
]
params = gm.Parameter()
params.set_direct_execute(True)
params.set_blocking(True)
params.set_timeout(120.0)
params.set_check_collision(False)
params.set_reference_frame("base_link")
params.set_actuate("with_leg")
return waypoints, params
def make_example_inputs_with_torso(current):
left_names = current["joint_names"]["left_arm"]
right_names = current["joint_names"]["right_arm"]
waypoints = [
[
make_joint_target(
"left_arm", [1.99995, -1.4004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-1.99995, 1.4004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
),
make_joint_target("torso", [0.0], ["leg_joint4"]),
],
[
make_joint_target(
"left_arm", [1.7995, -1.6004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-1.7995, 1.6004, 0.599905, 1.69994, 0.0, 0.799924, 0.0], right_names
),
make_joint_target("torso", [0.0], ["leg_joint4"]),
],
[
make_joint_target(
"left_arm", [1.99995, -1.60004, -0.599905, -1.69994, 0.0, -0.799924, 0.0], left_names
),
make_joint_target(
"right_arm", [-2.0, 1.60008, 0.600051, 1.70001, 0.0, 0.799993, 0.0], right_names
),
make_joint_target("torso", [0.0], ["leg_joint4"]),
],
]
params = gm.Parameter()
params.set_direct_execute(True)
params.set_blocking(True)
params.set_timeout(120.0)
params.set_check_collision(False)
params.set_reference_frame("base_link")
params.set_actuate("with_torso")
return waypoints, params
def run_example():
motion, robot = initialize_interfaces()
if motion is None or robot is None:
return 1
try:
if not move_robot_to_home_position(robot):
return 1
time.sleep(0.5)
current = {"joint_names": {}, "joints": {}}
if not capture_current_joints(
motion, robot, ["leg", "left_arm", "right_arm"], current
):
return 1
waypoints, params = make_example_inputs_with_torso(current)
print_waypoints("traj_plan", waypoints)
print(
"Immediate execution is enabled; waypoints are loaded from "
"motion_plan_targets.json reference values."
)
status, traj = motion.traj_plan(waypoints, params)
print_traj_result("traj_plan", status, traj, motion)
return 0 if status == gm.MotionStatus.SUCCESS else 1
finally:
shutdown_robot(robot)
if __name__ == "__main__":
raise SystemExit(run_example())
类：GalbotNavigation
获取实例 / 初始化
适用场景：获取导航模块单例并初始化。必须在使用导航功能前调用。
examples/python/galbot_navigation/get_instance.py
from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
import numpy as np
# Initialize system and navigation module
robot = GalbotRobot()
robot.init()
nav = GalbotNavigation()
nav.init()
print("GalbotNavigation has been initialized:", nav is not None)
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
重定位 / 是否已定位 / 获取当前位姿
适用场景：触发重定位，检查机器人是否已经成功定位，并获取机器人在地图中的当前位姿。
examples/python/galbot_navigation/relocalized.py
from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
import numpy as np
import time
nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
# success
while not nav.is_localized():
nav.relocalize(init_pose)
time.sleep(0.5)
print("Current pose:", nav.get_current_pose())
nav.stop_navigation()
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print('Resources released successfully')
非阻塞导航 + 轮询判断到达
适用场景：启动导航但不阻塞当前线程，可以在导航过程中做其他处理，需要轮询判断是否到达目标。适合需要异步处理的场景。
examples/python/galbot_navigation/navigate_to_goal.py
from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import ControlStatus, G1ControllerName
import numpy as np
import time
import sys
nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()
goal = np.array([0.5, 0.0, 0.0, 0, 0, 0.0, 1.0])
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
sys.exit(1)
else:
print("Controller switched successfully!")
nav.navigate_to_goal(goal, enable_collision_check=True, is_blocking=False, timeout=20)
start_time = time.time()
reached = False
while True:
if nav.check_goal_arrival():
reached = True
break
if time.time() - start_time > 20:
print("Navigation timed out; target not reached within 20s")
break
print("Navigating...")
time.sleep(0.5)
if reached:
print("Target reached")
nav.stop_navigation()
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")
直线移动到目标
适用场景：让机器人沿直线移动到目标点。
examples/python/galbot_navigation/move_straight_to.py
from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import ControlStatus, G1ControllerName
import numpy as np
import time
import sys
nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()
target = np.array([0.2, 0.0, 0.0, 0, 0, 0.0, 1.0])
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
sys.exit(1)
else:
print("Controller switched successfully!")
nav.move_straight_to(target, is_blocking=False, timeout=10)
time.sleep(1.0)
nav.stop_navigation()
# send SIGINT shutdown signal
robot.request_shutdown()
# Wait until entering shutdown state
robot.wait_for_shutdown()
# Perform SDK resource release
robot.destroy()
print("Resources released successfully")
停止导航（stop_navigation）
适用场景：停止正在执行的导航任务。
examples/python/galbot_navigation/stop_navigation.py
"""Stop a blocking navigation request from another Python thread."""
import signal
import threading
import time
import numpy as np
from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
# Set when the navigation call has returned, so the worker stops waiting for a
# Ctrl-C that is never coming.
navigation_done = threading.Event()
def stop_navigation_on_ctrl_c(navigation):
# sigwait() cannot be cancelled, so wait in slices: when the navigation
# finishes on its own no Ctrl-C ever arrives, and the worker still has to
# exit or the join() in the main flow would block forever.
while not navigation_done.is_set():
if signal.sigtimedwait({signal.SIGINT}, 0.2) is None:
continue
print("Worker thread: stopping navigation...")
succeeded, detail = navigation.stop_navigation()
if succeeded:
print("Worker thread: navigation stop succeeded")
else:
print(f"Worker thread: navigation stop FAILED: {detail}")
return
def confirm_motion():
print("⚠️ Ensure the emergency-stop button is released and the path is clear.")
if input("Start straight navigation? (y/n): ").strip().lower() != "y":
raise SystemExit("Motion was not confirmed.")
confirm_motion()
# Block SIGINT before SDK initialization so all SDK threads inherit the mask.
original_signal_mask = signal.pthread_sigmask(signal.SIG_BLOCK, {signal.SIGINT})
robot = GalbotRobot()
navigation = GalbotNavigation()
robot.init()
navigation.init()
time.sleep(3) # Wait for the PNS service to enter its working state.
if robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL) != ControlStatus.SUCCESS:
print("Failed to switch controller!")
else:
# move_straight_to is sent through PNS and is covered by stop_navigation.
# It does not require map localization, so it is a reliable stop example.
print("Robot is moving straight. Press Ctrl-C to stop navigation.")
stop_thread = threading.Thread(
target=stop_navigation_on_ctrl_c, args=(navigation,)
)
stop_thread.start()
result = navigation.move_straight_to(
np.array([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]),
is_blocking=True,
timeout=10,
)
navigation_done.set()
print(f"move_straight_to returned: {result}")
stop_thread.join()
signal.pthread_sigmask(signal.SIG_SETMASK, original_signal_mask)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
获取导航状态 + 轮询 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.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus, ControlStatus, G1ControllerName
import numpy as np
import time
import sys
nav = GalbotNavigation()
nav.init()
robot = GalbotRobot()
robot.init()
goal = np.array([0.5, 0.0, 0.0, 0, 0, 0.0, 1.0])
timeout_s = 20.0
poll_interval_s = 0.5
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
sys.exit(1)
else:
print("Controller switched successfully!")
# Non-blocking navigation
nav.navigate_to_goal(
goal, enable_collision_check=True, is_blocking=False, timeout=timeout_s
)
start = time.time()
while True:
status = nav.get_navigation_status()
elapsed = time.time() - start
if status == NavigationTaskStatus.SUCCESS:
print("Target reached")
break
if status == NavigationTaskStatus.FAILED:
print("Navigation failed; exit error-handling logic promptly")
break
if elapsed >= timeout_s:
print("navigationtimeout, exit")
break
if status == NavigationTaskStatus.RUNNING:
print(f"Navigating... Status: {status.name}, elapsed: {elapsed:.1f}s")
else:
print(f"Status: {status.name}, elapsed: {elapsed:.1f}s")
time.sleep(poll_interval_s)
nav.stop_navigation()
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
完整运行示例（简单流程）
适用场景：展示导航功能的完整使用流程，包括初始化、重定位、导航到目标等步骤，供参考。
examples/python/galbot_navigation/complete_example.py
from galbot_sdk.g1 import GalbotRobot, GalbotNavigation
from galbot_sdk.g1 import ControlStatus, G1ControllerName
import numpy as np
import time
import sys
robot = GalbotRobot()
robot.init()
nav = GalbotNavigation()
nav.init()
init_pose = np.array([0.0, 0.0, 0.0, 0, 0, 0.0, 1.0])
goal_pose = np.array([1.0, 0.0, 0.0, 0, 0, 0.0, 1.0])
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
sys.exit(1)
else:
print("Controller switched successfully!")
while not nav.is_localized():
nav.relocalize(init_pose)
time.sleep(0.5)
if nav.check_path_reachability(goal_pose, nav.get_current_pose()):
nav.navigate_to_goal(
goal_pose, enable_collision_check=True, is_blocking=True, timeout=30
)
print("Whether reached:", nav.check_goal_arrival())
nav.stop_navigation()
# Shutdown system
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
携带物体时的导航包围盒过滤
适用场景：机器人携带附着物体导航时，过滤对应的导航碰撞几何体。
examples/python/galbot_navigation/bounding_box_filter.py
from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
def print_boxes(boxes):
print(f"Current bounding boxes: {len(boxes)}")
for index, box in enumerate(boxes):
print(f"--- Box [{index}] ---")
print("Tag:", box["box_tag"])
print("Parent:", box["parent_link_name"])
print("Size:", box["box_size"])
print("Pose:", box["box_pose"])
robot = GalbotRobot()
robot.init()
nav = GalbotNavigation()
nav.init()
# Add a box so navigation does not treat the corresponding fused points as obstacles.
box_info = {
"box_size": [0.4, 0.3, 0.28],
"box_pose": [0.45, 0.0, 0.25, 0.0, 0.0, 0.0, 1.0],
"box_tag": 1001,
"parent_link_name": "base_link",
}
attached_box_info = {
"box_size": [0.4, 0.3, 0.28],
"box_pose": [0.2, 0.0, -0.2, 0.0, 0.0, 0.0, 1.0],
"box_tag": 2001,
"parent_link_name": "left_arm_end_effector_mount_link",
}
success, status = nav.add_bounding_box(box_info)
print("add_bounding_box:", success, status)
print_boxes(nav.get_bounding_box())
success, status = nav.attach_box_to_link(attached_box_info, [])
print("attach_box_to_link:", success, status)
success, status = nav.detach_box_from_link(2001)
print("detach_box_from_link:", success, status)
success, status = nav.remove_bounding_box(1001)
print("remove_bounding_box:", success, status)
print_boxes(nav.get_bounding_box())
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("Resources released successfully")
沿轨迹导航（navigate_along_trajectory）
适用场景：执行由一组有序底盘位姿构成的导航轨迹。
examples/python/galbot_navigation/navigate_along_trajectory.py
from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus, Pose
import numpy as np
import sys
import time
kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kTaskTimeoutSec = 100.0
def navigation_target_status_to_string(status):
if status == NavigationTaskStatus.UNKNOWN:
return "UNKNOWN"
if status == NavigationTaskStatus.RUNNING:
return "RUNNING"
if status == NavigationTaskStatus.SUCCESS:
return "SUCCESS"
if status == NavigationTaskStatus.FAILED:
return "FAILED"
if status == NavigationTaskStatus.INTERRUPTED:
return "INTERRUPTED"
if status == NavigationTaskStatus.OCCUPIED:
return "OCCUPIED"
if status == NavigationTaskStatus.COLLISION:
return "COLLISION"
if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
return "CLOSE_TO_OBSTACLE"
return "UNKNOWN"
def format_pose(pose):
values = [
pose.position.x,
pose.position.y,
pose.position.z,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
]
return "[" + ", ".join(f"{value:g}" for value in values) + "]"
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def wait_for_localization(navigation, init_pose):
retry = 0
while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
navigation.relocalize(init_pose)
time.sleep(kLocalizationRetrySleepMs / 1000.0)
retry += 1
return navigation.is_localized()
def print_task_snapshot(navigation, handle, snapshot, include_pose=True):
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
if include_pose:
print_current_pose(navigation, indent=" ")
def monitor_task_until_terminal(navigation, handle, timeout_s):
start_time = time.time()
while True:
snapshot = navigation.get_navigation_target_status(handle.task_id)
print_task_snapshot(navigation, handle, snapshot)
if snapshot.status in {
NavigationTaskStatus.SUCCESS,
NavigationTaskStatus.FAILED,
NavigationTaskStatus.INTERRUPTED,
NavigationTaskStatus.OCCUPIED,
NavigationTaskStatus.COLLISION,
NavigationTaskStatus.CLOSE_TO_OBSTACLE,
}:
return
if (time.time() - start_time) >= timeout_s:
print(" timeout reached while waiting for task completion.")
return
time.sleep(0.5)
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!")
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!")
return -1
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
return -1
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
waypoints = [
Pose([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]),
Pose([0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]),
Pose([1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]),
]
time.sleep(3)
if not wait_for_localization(navigation, init_pose):
print("localization failed", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return -1
time.sleep(2)
print("set target: waypoints")
for i, waypoint in enumerate(waypoints):
print(f" waypoint_{i}: {format_pose(waypoint)}")
frame_id = "map"
speed_ratio = 1.0
enable_collision_check = False
handle = navigation.navigate_along_trajectory(waypoints, frame_id, speed_ratio, enable_collision_check)
if handle.request_sent:
monitor_task_until_terminal(navigation, handle, kTaskTimeoutSec)
else:
print("trajectory task submission failed")
navigation.stop_navigation()
print("\nfinal task summary")
snapshot = navigation.get_navigation_target_status(handle.task_id)
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
sys.exit(main())
导航至目标 V2（navigate_to_goal_v2）
适用场景：使用扩展目标接口及其附加选项导航至指定目标位姿。
examples/python/galbot_navigation/navigate_to_goal_v2.py
from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
import numpy as np
import sys
import time
kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def wait_for_localization(navigation, init_pose):
retry = 0
while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
navigation.relocalize(init_pose)
time.sleep(kLocalizationRetrySleepMs / 1000.0)
retry += 1
return navigation.is_localized()
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!", file=sys.stderr)
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!", file=sys.stderr)
return -1
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!", file=sys.stderr)
return -1
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
time.sleep(3)
if not wait_for_localization(navigation, init_pose):
print("localization failed", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return -1
print_current_pose(navigation)
relative_goal = np.array([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
max_vel = np.array([0.5, 0.5, 0.5], dtype=np.float64)
pose_frame = "base_link"
enable_collision_check = True
is_blocking = True
timeout_s = 20.0
omni_plan = False
print("navigate_to_goal_v2 relative target in base_link frame")
success, status = navigation.navigate_to_goal_v2(
relative_goal,
max_vel,
pose_frame=pose_frame,
enable_collision_check=enable_collision_check,
is_blocking=is_blocking,
timeout=timeout_s,
omni_plan=omni_plan,
)
print(f"navigate_to_goal_v2 result: success={success}, status={status}")
print_current_pose(navigation)
navigation.stop_navigation()
print("navigation stopped")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("example completed ...")
return 0
if __name__ == "__main__":
sys.exit(main())
速度导航（navigate_with_velocity）
适用场景：通过速度指令连续控制底盘平移和旋转。
examples/python/galbot_navigation/navigate_with_velocity.py
from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus
import sys
import time
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def init_sdk():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!", file=sys.stderr)
return None, None
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!", file=sys.stderr)
return None, None
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!", file=sys.stderr)
return None, None
time.sleep(3)
print_current_pose(navigation)
return navigation, robot
def shutdown_sdk(navigation, robot):
navigation.stop_navigation()
print_current_pose(navigation)
print("navigation stopped")
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("example completed ...")
def monitor_navigation(navigation, timeout_s, poll_interval_s=0.5):
start = time.time()
while True:
status = navigation.get_navigation_status()
elapsed = time.time() - start
print(f" status: {status.name}, elapsed: {elapsed:.1f}s")
print_current_pose(navigation, indent=" ")
if status in (
NavigationTaskStatus.SUCCESS,
NavigationTaskStatus.FAILED,
NavigationTaskStatus.INTERRUPTED,
NavigationTaskStatus.OCCUPIED,
NavigationTaskStatus.COLLISION,
NavigationTaskStatus.CLOSE_TO_OBSTACLE,
):
break
if elapsed >= timeout_s:
print(" monitor timeout reached")
break
time.sleep(poll_interval_s)
def test_base_vel_cmd():
navigation, robot = init_sdk()
if navigation is None or robot is None:
return -1
vx = 0.3
vy = 0.0
vyaw = 0.0
duration_s = 6.0
enable_collision_check = True
print(
f"navigate_with_velocity: vx={vx}, vy={vy}, "
f"vyaw={vyaw}, duration_s={duration_s}"
)
success, status = navigation.navigate_with_velocity(
vx, vy, vyaw, duration_s, enable_collision_check
)
print(f"navigate_with_velocity result: success={success}, status={status}")
monitor_navigation(navigation, timeout_s=duration_s + 2.0)
shutdown_sdk(navigation, robot)
return 0
def test_change_vel_cmd():
navigation, robot = init_sdk()
if navigation is None or robot is None:
return -1
print("send first velocity command")
success, status = navigation.navigate_with_velocity(0.3, 0.0, 0.0, 6.0, True)
print(f"navigate_with_velocity result 1: success={success}, status={status}")
monitor_navigation(navigation, timeout_s=4.0)
print("send second velocity command with y direction")
success, status = navigation.navigate_with_velocity(0.1, 0.2, 0.0, 6.0, True)
print(f"navigate_with_velocity result 2: success={success}, status={status}")
monitor_navigation(navigation, timeout_s=8.0)
shutdown_sdk(navigation, robot)
return 0
def main():
return test_base_vel_cmd()
# return test_change_vel_cmd()
if __name__ == "__main__":
sys.exit(main())
导航配置
适用场景：读取和更新导航模块使用的运行配置。
examples/python/galbot_navigation/navigation_set_get_config.py
from galbot_sdk.g1 import GalbotNavigation, GalbotRobot
import numpy as np
import sys
def print_result(name, result):
success, status = result
print(f"{name}: success={success}, status={status}")
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!", file=sys.stderr)
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!", file=sys.stderr)
return -1
print("dump navigation configs before setting parameters")
print_result("dump_navigation_configs", navigation.dump_navigation_configs())
# valid range: [0.05, 1.5]
vel_limit = np.array([0.5, 0.5, 0.5], dtype=np.float64)
# valid range: [0.05, 6.0]
acc_limit = np.array([0.5, 0.5, 0.5], dtype=np.float64)
# valid range: [0.05, 12.0]
jerk_limit = np.array([5.0, 5.0, 5.0], dtype=np.float64)
# valid range: [0.03, 2.0]
arrival_threshold = np.array([0.05, 0.05, 0.05], dtype=np.float64)
timeout_s = 30.0
print_result(
"set_navigation_velocity_limit",
navigation.set_navigation_velocity_limit(vel_limit),
)
print_result(
"set_navigation_kinematics_limits",
navigation.set_navigation_kinematics_limits(vel_limit, acc_limit, jerk_limit),
)
print_result("set_navigation_timeout", navigation.set_navigation_timeout(timeout_s))
print_result(
"set_navigation_arrival_threshold",
navigation.set_navigation_arrival_threshold(arrival_threshold),
)
print("dump navigation configs after setting parameters")
print_result("dump_navigation_configs", navigation.dump_navigation_configs())
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
sys.exit(main())
多路点导航（navigation_through_waypoints）
适用场景：让机器人按照指定顺序依次访问多个导航路点。
examples/python/galbot_navigation/navigation_through_waypoints.py
from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus, Pose, Waypoint, WaypointParams
import numpy as np
import sys
import time
kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kTaskTimeoutSec = 100.0
def navigation_target_status_to_string(status):
if status == NavigationTaskStatus.UNKNOWN:
return "UNKNOWN"
if status == NavigationTaskStatus.RUNNING:
return "RUNNING"
if status == NavigationTaskStatus.SUCCESS:
return "SUCCESS"
if status == NavigationTaskStatus.FAILED:
return "FAILED"
if status == NavigationTaskStatus.INTERRUPTED:
return "INTERRUPTED"
if status == NavigationTaskStatus.OCCUPIED:
return "OCCUPIED"
if status == NavigationTaskStatus.COLLISION:
return "COLLISION"
if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
return "CLOSE_TO_OBSTACLE"
return "UNKNOWN"
def format_pose(pose):
values = [
pose.position.x,
pose.position.y,
pose.position.z,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
]
return "[" + ", ".join(f"{value:g}" for value in values) + "]"
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def wait_for_localization(navigation, init_pose):
retry = 0
while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
navigation.relocalize(init_pose)
time.sleep(kLocalizationRetrySleepMs / 1000.0)
retry += 1
return navigation.is_localized()
def print_task_snapshot(navigation, handle, snapshot):
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
print_current_pose(navigation, indent=" ")
def monitor_task_until_terminal(navigation, handle, timeout_s):
start_time = time.time()
while True:
snapshot = navigation.get_navigation_target_status(handle.task_id)
print_task_snapshot(navigation, handle, snapshot)
now = time.time()
if snapshot.status in {
NavigationTaskStatus.SUCCESS,
NavigationTaskStatus.FAILED,
NavigationTaskStatus.INTERRUPTED,
NavigationTaskStatus.OCCUPIED,
NavigationTaskStatus.COLLISION,
NavigationTaskStatus.CLOSE_TO_OBSTACLE,
}:
return
if (now - start_time) >= timeout_s:
print(" timeout reached while waiting for task completion.")
return
time.sleep(1.0)
def make_waypoint(pose, params=None):
if params is None:
params = WaypointParams()
return Waypoint(pose, params)
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!")
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!")
return -1
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
return -1
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
waypoint_2_params = WaypointParams()
waypoint_2_params.arrival_position_threshold_x = 0.02
waypoint_2_params.arrival_position_threshold_y = 0.02
waypoint_2_params.arrival_orientation_threshold = 0.08
waypoint_2_params.velocity_scale = 0.2
waypoint_3_params = WaypointParams()
waypoint_3_params.arrival_position_threshold_x = 0.04
waypoint_3_params.arrival_position_threshold_y = 0.04
waypoint_3_params.arrival_orientation_threshold = 0.05
waypoint_3_params.velocity_scale = 0.8
waypoint_3_params.acceleration_scale = 0.8
waypoint_3_params.jerk_scale = 0.8
waypoints = [
make_waypoint(Pose([0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])),
make_waypoint(Pose([0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]), waypoint_2_params),
make_waypoint(Pose([1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0]), waypoint_3_params),
]
time.sleep(3)
if not wait_for_localization(navigation, init_pose):
print("localization failed", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return -1
time.sleep(2)
print("set target: waypoints")
for i, waypoint in enumerate(waypoints):
print(f" waypoint_{i}: {format_pose(waypoint.pose)}")
frame_id = "map"
enable_collision_check = True
handle = navigation.navigate_through_waypoints(waypoints, frame_id, enable_collision_check)
if handle.request_sent:
monitor_task_until_terminal(navigation, handle, kTaskTimeoutSec)
else:
print("navigation failed to submit task")
navigation.stop_navigation()
print("\nfinal task summary")
snapshot = navigation.get_navigation_target_status(handle.task_id)
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
sys.exit(main())
设置导航目标（set_navigation_target）
适用场景：在启动或管理导航任务前，单独设置导航目标。
examples/python/galbot_navigation/set_navigation_target.py
from galbot_sdk.g1 import ControlStatus, G1ControllerName, GalbotNavigation, GalbotRobot
from galbot_sdk.g1 import NavigationTaskStatus, Pose
import numpy as np
import sys
import time
kLocalizationRetryLimit = 20
kLocalizationRetrySleepMs = 500
kPollIntervalMs = 500
kMonitorDurationMs = 2000
def navigation_target_status_to_string(status):
if status == NavigationTaskStatus.UNKNOWN:
return "UNKNOWN"
if status == NavigationTaskStatus.RUNNING:
return "RUNNING"
if status == NavigationTaskStatus.SUCCESS:
return "SUCCESS"
if status == NavigationTaskStatus.FAILED:
return "FAILED"
if status == NavigationTaskStatus.INTERRUPTED:
return "INTERRUPTED"
if status == NavigationTaskStatus.OCCUPIED:
return "OCCUPIED"
if status == NavigationTaskStatus.COLLISION:
return "COLLISION"
if status == NavigationTaskStatus.CLOSE_TO_OBSTACLE:
return "CLOSE_TO_OBSTACLE"
return "UNKNOWN"
def format_pose(pose):
values = [
pose.position.x,
pose.position.y,
pose.position.z,
pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w,
]
return "[" + ", ".join(f"{value:g}" for value in values) + "]"
def print_current_pose(navigation, indent=" "):
current_pose = navigation.get_current_pose()
print(
f"{indent}current_pose: ["
f"{current_pose[0]:g}, {current_pose[1]:g}, {current_pose[2]:g}, "
f"{current_pose[3]:g}, {current_pose[4]:g}, {current_pose[5]:g}, {current_pose[6]:g}]"
)
def print_task_snapshot(navigation, handle, snapshot, include_pose=True):
print(
f" task_id: {handle.task_id}, request_sent: {'true' if handle.request_sent else 'false'}, "
f"status: {navigation_target_status_to_string(snapshot.status)}"
)
if include_pose:
print_current_pose(navigation, indent=" ")
def wait_for_localization(navigation, init_pose):
retry = 0
while (not navigation.is_localized()) and retry < kLocalizationRetryLimit:
navigation.relocalize(init_pose)
time.sleep(kLocalizationRetrySleepMs / 1000.0)
retry += 1
return navigation.is_localized()
def print_task_summary(navigation, handles):
print("\nfinal task summary")
for handle in handles:
snapshot = navigation.get_navigation_target_status(handle.task_id)
print_task_snapshot(navigation, handle, snapshot, include_pose=False)
def main():
navigation = GalbotNavigation()
robot = GalbotRobot()
if robot.init():
print("Base instance initialized successfully!")
else:
print("Base instance initialization failed!")
return -1
if navigation.init():
print("Navigation instance initialized successfully!")
else:
print("Navigation instance initialization failed!")
return -1
res = robot.switch_controller(G1ControllerName.CHASSIS_POSE_CTRL)
if res != ControlStatus.SUCCESS:
print("Failed to switch controller!")
return -1
init_pose = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], dtype=np.float64)
time.sleep(3)
if not wait_for_localization(navigation, init_pose):
print("localization failed", file=sys.stderr)
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return -1
frame_id = "map"
speed_ratio = 0.9
enable_collision_check = True
target_points = [
Pose([1.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
Pose([0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
Pose([0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00]),
]
handles = []
for i, target in enumerate(target_points):
print(f"set target: dynamic_goal_{i}, target_pose: {format_pose(target)}")
handle = navigation.set_navigation_target(target, frame_id, speed_ratio, enable_collision_check)
handles.append(handle)
start_time = time.time()
while True:
snapshot = navigation.get_navigation_target_status(handle.task_id)
print_task_snapshot(navigation, handle, snapshot)
now = time.time()
elapsed_ms = (now - start_time) * 1000.0
if elapsed_ms >= kMonitorDurationMs:
break
time.sleep(kPollIntervalMs / 1000.0)
print_task_summary(navigation, handles)
print("\nstop navigation and shutdown")
navigation.stop_navigation()
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
return 0
if __name__ == "__main__":
sys.exit(main())
类：GalbotPerception
高精度双目深度估计，单次推理并保存伪彩色深度图
适用场景：触发一次双目深度估计推理，并获取推理结果
examples/python/galbot_perception/foundation_stereo_example.py
"""Foundation stereo depth example (G1): single run_once, save a pseudo-color depth image."""
import time
import cv2
import numpy as np
try:
from galbot_sdk.g1 import (
GalbotPerception,
GalbotRobot,
PerceptionModule,
)
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
raise
OUTPUT_IMAGE_PATH = "foundation_stereo_depth.png"
def main():
robot = GalbotRobot()
try:
if not robot.init():
print("Robot init failed")
return
print("Robot init OK")
perception = GalbotPerception()
if not perception.init({PerceptionModule.FOUNDATION_STEREO, PerceptionModule.LIGHT_STEREO}):
print("Perception init failed")
return
print("Perception init OK")
time.sleep(12) # Wait for perception models to load
print("Triggering single inference...")
if not perception.run_once(PerceptionModule.FOUNDATION_STEREO):
print("run_once failed to send command")
return
print("Waiting for inference result...")
if not perception.wait_for_new_result(
PerceptionModule.FOUNDATION_STEREO, timeout_s=6.0
):
print("Timed out waiting for inference result")
return
ok, result = perception.get_latest_result(PerceptionModule.FOUNDATION_STEREO)
if not ok:
print("get_latest_result failed")
return
print(result.get_result_info())
depth_map = result.instance_mask
if depth_map is None:
print("No depth map (instance_mask is empty)")
return
print(f"Depth map shape: {depth_map.shape}, dtype: {depth_map.dtype}")
print(f"Depth value range: [{np.nanmin(depth_map)}, {np.nanmax(depth_map)}]")
depth_f = depth_map.astype(np.float32)
valid = depth_f[depth_f > 0]
if valid.size > 0:
vmin, vmax = np.percentile(valid, [1, 99])
normalized = np.clip((depth_f - vmin) / (vmax - vmin + 1e-6), 0, 1)
else:
normalized = np.zeros_like(depth_f)
colored = cv2.applyColorMap(
(normalized * 255).astype(np.uint8), cv2.COLORMAP_TURBO
)
if cv2.imwrite(OUTPUT_IMAGE_PATH, colored):
print(f"Saved: {OUTPUT_IMAGE_PATH}")
else:
print(f"Failed to save: {OUTPUT_IMAGE_PATH}")
finally:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
类：Parameter
适用场景：创建运动规划参数对象，用于配置运动规划的各种参数如速度限制、加速度限制、规划时间等。
examples/python/galbot_motion/create_parameter.py
from galbot_sdk.g1 import Parameter, create_parameter, G1JointGroup
# Create Parameter via constructor and set options
p = Parameter()
p.set_blocking(True)
p.set_check_collision(False)
p.set_timeout(5.0)
p.set_actuate('with_chain_only')
p.set_tool_pose(False)
p.set_reference_frame('base_link')
p.joint_state = {
G1JointGroup.left_arm: [0.0] * 7,
# Can add others if needed:
# G1JointGroup.right_arm: [0.0] * 7,
# G1JointGroup.leg: [0.0] * 4,
}
print('blocking:', p.get_blocking())
print('collision check:', p.get_check_collision())
print('timeout:', p.get_timeout())
# Or use factory function to quickly create Parameter
p2 = create_parameter(direct_execute=False, blocking=True, timeout=3.0, actuate='with_chain_only', tool_pose=False, check_collision=True)
print('Factory-created timeout:', p2.get_timeout())
辅助函数
check_motion_status
适用场景：检查运动规划执行结果状态，判断规划是否成功，打印错误信息。
examples/python/galbot_motion/check_motion_status.py
from galbot_sdk.g1 import MotionStatus, check_motion_status
status_str = check_motion_status(MotionStatus.SUCCESS)
print('MotionStatus string:', status_str)
create_joint_state / create_pose_state
适用场景：快速创建关节状态和位姿状态对象，方便运动规划接口调用。
examples/python/galbot_motion/create_pose_state.py
from galbot_sdk.g1 import create_joint_state, create_pose_state, JointStates, PoseState, Pose
# Create helper objects using the factory function
js = create_joint_state()
ps = create_pose_state()
# Fill example fields
js.chain_name = 'left_arm'
js.joint_positions = [0.0] * 7
ps.chain_name = 'left_arm'
# 7D vector: x, y, z, qx, qy, qz, qw
ps.pose = Pose([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
print(type(js), js.chain_name)
print(type(ps), ps.chain_name)
教程源码示例
以下可运行教程维护在 examples/g1/python/tutorials 中；使用步骤与安全说明请参阅入门教程。
示例 1：Galbot 控制入门
适用场景：G1 最基础的关节控制：执行双臂比心动作并恢复初始姿态。运行前请确认机器人处于工作模式且急停按钮已旋开；夹爪控制及更多控制案例请参阅本页 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.g1 import GalbotRobot
from galbot_sdk.g1 import ControlStatus
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
exit(1)
def demo_heart_pose(robot: GalbotRobot,
joint_group_names: list,
position_seq: list,
is_blocking: bool,
max_speed: float,
timeout_s: float,
retry_count: int = 3
):
"""
Robot heart gesture demonstration function
Parameters:
robot (GalbotRobot): GalbotRobot instance
joint_group_names (list): List of joint group names to control
position_seq (list): List of joint group angle sequences to set
is_blocking (bool): Whether to set angles in blocking mode
max_speed (float): Maximum speed
timeout_s (float): Timeout time (seconds)
retry_count (int, optional): Number of retries, default is 3
Returns:
None
"""
# Get current joint group angles for subsequent restoration
original_pos = robot.get_joint_positions(joint_group_names, [])
print(f"Current angles of joint group {joint_group_names}: {original_pos}")
# Start heart gesture
pos_idx = 0
print("Starting heart gesture...")
while True:
time.sleep(1)
pos = position_seq[pos_idx]
control_status = robot.set_joint_positions(
pos, joint_group_names, [], is_blocking, max_speed, timeout_s
)
# If setting fails, retry 3 times
retry_cnt = retry_count
while control_status != ControlStatus.SUCCESS and retry_cnt > 0:
print(f"Setting angles for joint group {joint_group_names} failed, retrying {retry_cnt}...")
retry_cnt = retry_cnt - 1
time.sleep(1)
control_status = robot.set_joint_positions(
pos, joint_group_names, [], is_blocking, max_speed, timeout_s
)
# If successful, switch to next pose sequence
if control_status == ControlStatus.SUCCESS:
print(f"Setting angles for joint group {joint_group_names} successful")
pos_idx = pos_idx + 1
# If failed, break the loop
else:
print(f"Setting angles for joint group {joint_group_names} failed")
break
# If all pose sequences are completed, break the loop
if pos_idx > len(position_seq) - 1:
break
# Get current joint group angles
print("Showing heart gesture for 15 seconds, then restoring original pose...")
time.sleep(5)
# Restore original pose joint group angles
control_status = robot.set_joint_positions(
original_pos, joint_group_names, [], is_blocking, max_speed, timeout_s
)
# If setting fails, retry 5 times
retry_cnt = retry_count
while control_status != ControlStatus.SUCCESS and retry_cnt > 0:
print(f"Setting angles for joint group {joint_group_names} failed, retrying {retry_cnt}...")
retry_cnt = retry_cnt - 1
time.sleep(2)
control_status = robot.set_joint_positions(
original_pos, joint_group_names, [], is_blocking, max_speed, timeout_s
)
# If successful, restore original pose
if control_status == ControlStatus.SUCCESS:
print(f"Restoring angles for joint group {joint_group_names} successful")
else:
print(f"Restoring angles for joint group {joint_group_names} failed")
def check_robot_safety():
"""Check if the robot is safe"""
# Prompt for precautions
print("⚠️ Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing execution...")
break
elif key == 'n':
print("User not confirmed, program exiting...")
exit(1)
else:
print("Input error, please enter 'y' or 'n'")
def main():
check_robot_safety()
try:
# Get robot instance
robot = GalbotRobot()
# Initialize robot
state = robot.init()
if not state:
print(f"Initialization failed")
exit(1)
else:
print(f"Initialization successful")
print(f"Is robot running: {robot.is_running()}")
# Wait for data preparation
time.sleep(3)
# Get list of joint names
joint_names = robot.get_joint_names()
if len(joint_names) > 0:
print(f"List of joint names: {joint_names}")
else:
print(f"Failed to get list of joint names")
# Get joint positions using joint group names, empty returns all joints by default
joint_group_names = ["left_arm", "right_arm"]
# Left and right arm heart gesture sequence
position_seq = [
[1.53, 0.36, -2.54, -1.80, 0.12, -0.82, 0.09, # left_arm
-1.53, -0.36, 2.54, 1.80, -0.12, 0.82, -0.09] # right_arm
]
# Whether to block and wait for joints to reach position
is_blocking = True
# Limit maximum joint speed to 0.1rad/s
max_speed = 0.1
# Maximum blocking wait time
timeout_s = 20
# Perform heartbeat gesture
demo_heart_pose(robot, joint_group_names, position_seq,
is_blocking, max_speed, timeout_s)
except Exception as e:
print(f"An exception occurred: {e}")
finally:
# Actively send SIGINT shutdown signal
robot.request_shutdown()
# Wait to enter shutdown state
robot.wait_for_shutdown()
# Release SDK resources
robot.destroy()
print('Resource release successful')
if __name__ == "__main__":
main()
示例 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.g1 as gm
from galbot_sdk.g1 import GalbotMotion
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import ControlStatus
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it is in PYTHONPATH")
exit(1)
import os
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Sequence, Dict
def printStatus(status):
if(status == gm.MotionStatus.SUCCESS):
print("Execution result: SUCCESS, Execution successful")
elif(status == gm.MotionStatus.TIMEOUT):
print("Execution result: TIMEOUT, Execution timeout")
elif(status == gm.MotionStatus.FAULT):
print("Execution result: FAULT, Fault occurred, unable to continue execution")
elif(status == gm.MotionStatus.INVALID_INPUT):
print("Execution result: INVALID_INPUT, Input parameters do not meet requirements")
elif(status == gm.MotionStatus.INIT_FAILED):
print("Execution result: INIT_FAILED, Internal communication component creation failed")
elif(status == gm.MotionStatus.IN_PROGRESS):
print("Execution result: IN_PROGRESS, Moving but not in position")
elif(status == gm.MotionStatus.STOPPED_UNREACHED):
print("Execution result: STOPPED_UNREACHED, Stopped but did not reach target")
elif(status == gm.MotionStatus.DATA_FETCH_FAILED):
print("Execution result: DATA_FETCH_FAILED, Data acquisition failed")
elif(status == gm.MotionStatus.PUBLISH_FAIL):
print("Execution result: PUBLISH_FAIL, Data sending failed")
elif(status == gm.MotionStatus.COMM_DISCONNECTED):
print("Execution result: COMM_DISCONNECTED, Connection failed")
def quat_normalize(q: np.ndarray) -> np.ndarray:
q = np.array(q, dtype=np.float64)
return q / np.linalg.norm(q)
def quat_conjugate(q: np.ndarray) -> np.ndarray:
"""
Calculate the conjugate of a quaternion
Parameters:
q (np.ndarray): Input quaternion [x, y, z, w]
Returns:
np.ndarray: Conjugate of the quaternion [x, y, z, w]
"""
qx, qy, qz, qw = q
return np.array([-qx, -qy, -qz, qw])
def quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
"""
Calculate the product of two quaternions
Parameters:
q1 (np.ndarray): First quaternion [x, y, z, w]
q2 (np.ndarray): Second quaternion [x, y, z, w]
Returns:
np.ndarray: Product of the two quaternions [x, y, z, w]
"""
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
return np.array([
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2,
w1*w2 - x1*x2 - y1*y2 - z1*z2
])
def orientation_error_angle(A: np.ndarray, B: np.ndarray) -> float:
"""
Calculate the rotation angle error between two quaternions (radians)
Parameters:
A (np.ndarray): First quaternion [x, y, z, w]
B (np.ndarray): Second quaternion [x, y, z, w]
Returns:
float: Rotation angle error (radians)
"""
qA = quat_normalize(A[3:7])
qB = quat_normalize(B[3:7])
q_err = quat_multiply(qB, quat_conjugate(qA))
q_err = quat_normalize(q_err)
# Numerical stability
qw = np.clip(q_err[3], -1.0, 1.0)
angle = 2 * np.arccos(qw)
return angle # Unit: radians
def calculate_error(pose1: Sequence[float], pose2: Sequence[float]) -> Dict[str, float]:
"""
Calculate position error and rotation error between two poses (radians)
Parameters:
pose1 (Sequence[float]): First pose [x, y, z, qx, qy, qz, qw]
pose2 (Sequence[float]): Second pose [x, y, z, qx, qy, qz, qw]
Returns:
dict: Dictionary containing position error (meters) and rotation error (radians)
"""
A, B = np.array(pose1), np.array(pose2)
pos_err = np.linalg.norm(A[:3] - B[:3])
rot_err = orientation_error_angle(A, B)
return {
"position_error_norm": pos_err,
"orientation_error_rad": rot_err,
"orientation_error_deg": np.degrees(rot_err)
}
def check_robot_safety():
"""Check if the robot is safe"""
# Prompt for precautions
print("⚠️ Note: 1. Please ensure the robot's emergency stop button is released; 2. Please ensure there are no obstacles in front, back, left, and right of the robot to avoid unexpected situations.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing execution...")
break
elif key == 'n':
print("User not confirmed, program exiting...")
exit(1)
else:
print("Input error, please enter 'y' or 'n'")
def main():
check_robot_safety()
try:
# Get GalbotMotion singleton and initialize
robot = GalbotRobot()
motion = GalbotMotion()
if motion.init():
print("GalbotMotion initialization successful")
else:
print("GalbotMotion initialization failed")
if robot.init():
print("GalbotRobot initialization successful")
else:
print("GalbotRobot initialization failed")
# Program starts immediately, wait for data readiness time
time.sleep(3)
# Define target pose
chain_pose_baselink = {
"leg": [0.0596,-0.0000,1.0327,0.5000,0.5003,0.4997,0.5000],
"head": [0.0599,0.0002,1.4098,-0.7072,0.0037,0.0037,0.7069],
"left_arm": [0.1267,0.2342,0.7356,0.0220,0.0127,0.0343,0.9991],
"right_arm": [0.1267,-0.2345,0.7358,-0.0225,0.0126,-0.0343,0.9991]
}
# set initial joint positions
joint_pos = [0.5, 1.5, 1.0, 0.0, 0.0,
0.0, 0.0,
2.0, -1.5, -0.6, -1.7, 0.0, -0.8, 0.0,
-2.0, 1.5, 0.6, 1.7, 0.0, 0.8, 0.0]
joint_groups_names = ["leg", "head", "left_arm", "right_arm"]
joint_names = []
is_blocking = True
max_speed_rad_s = 0.1
timeout_s = 30.0
status = robot.set_joint_positions(
joint_pos, joint_groups_names, joint_names, is_blocking, max_speed_rad_s, timeout_s
)
if status != ControlStatus.SUCCESS:
print("set join position failed")
else:
print("set join position successful")
# Define target chain name, target pose, reference pose, end link
target_frame = "EndEffector"
reference_frame = "base_link"
target_chain = "left_arm"
end_link = "left_arm_end_effector_mount_link"
# 1. Get current left_arm end-effector pose
try:
status, original_pose = motion.get_end_effector_pose_on_chain(
chain_name=target_chain,
frame_id=target_frame,
reference_frame=reference_frame
)
assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
print(f"✅ Current {target_chain} end-effector pose: {original_pose}")
time.sleep(0.8)
except Exception as e:
print(f"❌ Exception getting end-effector pose by chain name: {e}")
# 2. Solve joint angles based on target pose IK and verify the solution
# 2.1 Solve joint angles joint_angles_ik for target pose through IK
try:
status, joint_angles_ik = motion.inverse_kinematics(
target_pose=chain_pose_baselink[target_chain],
chain_names=[target_chain],
target_frame=target_frame,
reference_frame=reference_frame,
enable_collision_check=False # Disable collision detection
)
assert status == gm.MotionStatus.SUCCESS, "IK solving failed"
print(f"✅ Target {target_chain} IK solving successful joint_angles_ik: {joint_angles_ik}")
time.sleep(1)
except Exception as e:
print(f"❌ IK solving exception: {e}")
# 2.2 Set end-effector pose to target pose tgt_pose_ik by setting joint group angles joint_angles_ik
try:
status = robot.set_joint_positions(
joint_angles_ik[target_chain],
[target_chain],
[],
True,
0.1,
20.0,
)
assert status == ControlStatus.SUCCESS, "Setting joint group angles failed"
print(f"✅ Setting {target_chain} joint group angles successful.")
time.sleep(1)
except Exception as e:
print(f"❌ Setting {target_chain} joint group angles exception: {e}")
# 2.3 Verify whether the set joint group angles are consistent with the solved angles
try:
status, tgt_pose_ik = motion.get_end_effector_pose_on_chain(
chain_name=target_chain,
frame_id=target_frame,
reference_frame=reference_frame
)
assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
print(f"✅ Getting {target_chain} end-effector pose successful: {tgt_pose_ik}")
time.sleep(1)
error = calculate_error(tgt_pose_ik, chain_pose_baselink[target_chain])
print(f"End-effector pose error: {error}")
except Exception as e:
print(f"❌ Getting {target_chain} end-effector pose exception: {e}")
# 2.4 Verify whether the end-effector pose tgt_pose_fk corresponding to joint group angles joint_angles_ik solved by FK is consistent with target pose tgt_pose_ik
try:
status, tgt_pose_fk = motion.forward_kinematics(
target_frame=end_link,
reference_frame=reference_frame,
joint_state=joint_angles_ik,
params=gm.Parameter()
)
assert status == gm.MotionStatus.SUCCESS, "FK solving failed"
print(f"✅ Target {target_chain} FK solving successful: {tgt_pose_fk}")
time.sleep(1)
error = calculate_error(tgt_pose_fk, chain_pose_baselink[target_chain])
print(f"FK solving error: {error}")
except Exception as e:
print(f"❌ FK solving exception: {e}")
time.sleep(3)
print()
# 3. Restore to original pose by setting end-effector pose
# 3.1 Set end-effector pose to restore to original pose
try:
status = motion.set_end_effector_pose(
target_pose=original_pose,
end_effector_frame=target_chain,
reference_frame=reference_frame,
enable_collision_check=False,
is_blocking=True,
timeout=5.0,
params=gm.Parameter()
)
assert status == gm.MotionStatus.SUCCESS, "Setting end-effector pose failed"
print(f"✅ Setting end-effector pose successful: status={status}")
time.sleep(1)
except Exception as e:
print(f"❌ Setting {target_chain} end-effector pose exception: {e}")
# 3.2 Get end-effector pose and verify whether it has been restored to original pose
try:
status, original_pose_rec = motion.get_end_effector_pose_on_chain(
chain_name=target_chain,
frame_id=target_frame,
reference_frame=reference_frame
)
assert status == gm.MotionStatus.SUCCESS, "Failed to get end-effector pose"
print(f"✅ Getting {target_chain} end-effector pose successful: {original_pose_rec}")
time.sleep(0.8)
error = calculate_error(original_pose_rec, original_pose)
print(f"Restore end-effector pose error: {error}")
except Exception as e:
print(f"❌ Setting end-effector pose exception: {e}")
except Exception as e:
print(f"❌ Main program exception: {e}")
finally:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__=="__main__":
main()
示例 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.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
except ImportError:
print("Failed to import galbot_sdk, please install it first or check if it's in PYTHONPATH")
exit(1)
import os
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Sequence, Dict
def quat_normalize(q: np.ndarray) -> np.ndarray:
q = np.array(q, dtype=np.float64)
return q / np.linalg.norm(q)
def quat_conjugate(q: np.ndarray) -> np.ndarray:
"""
Calculate the conjugate of a quaternion
Parameters:
q (np.ndarray): Input quaternion [x, y, z, w]
Returns:
np.ndarray: Conjugate of the quaternion [x, y, z, w]
"""
qx, qy, qz, qw = q
return np.array([-qx, -qy, -qz, qw])
def quat_multiply(q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
"""
Calculate the product of two quaternions
Parameters:
q1 (np.ndarray): First quaternion [x, y, z, w]
q2 (np.ndarray): Second quaternion [x, y, z, w]
Returns:
np.ndarray: Product of the two quaternions [x, y, z, w]
"""
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
return np.array([
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2,
w1*w2 - x1*x2 - y1*y2 - z1*z2
])
def quat_to_matrix(q: Sequence[float]) -> np.ndarray:
"""
Convert quaternion [x, y, z, w] to a 3x3 rotation matrix.
"""
x, y, z, w = quat_normalize(q)
xx, yy, zz = x * x, y * y, z * z
xy, xz, yz = x * y, x * z, y * z
wx, wy, wz = w * x, w * y, w * z
return np.array([
[1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
[2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
[2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)],
], dtype=np.float64)
def matrix_to_quat(m: np.ndarray) -> np.ndarray:
"""
Convert a 3x3 rotation matrix to quaternion [x, y, z, w].
"""
m = np.asarray(m, dtype=np.float64)
trace = float(np.trace(m))
if trace > 0.0:
s = np.sqrt(trace + 1.0) * 2.0
w = 0.25 * s
x = (m[2, 1] - m[1, 2]) / s
y = (m[0, 2] - m[2, 0]) / s
z = (m[1, 0] - m[0, 1]) / s
elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2.0
w = (m[2, 1] - m[1, 2]) / s
x = 0.25 * s
y = (m[0, 1] + m[1, 0]) / s
z = (m[0, 2] + m[2, 0]) / s
elif m[1, 1] > m[2, 2]:
s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2.0
w = (m[0, 2] - m[2, 0]) / s
x = (m[0, 1] + m[1, 0]) / s
y = 0.25 * s
z = (m[1, 2] + m[2, 1]) / s
else:
s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2.0
w = (m[1, 0] - m[0, 1]) / s
x = (m[0, 2] + m[2, 0]) / s
y = (m[1, 2] + m[2, 1]) / s
z = 0.25 * s
return quat_normalize([x, y, z, w])
def orientation_error_angle(A: np.ndarray, B: np.ndarray) -> float:
"""
Calculate the rotation angle error between two quaternions (in radians)
Parameters:
A (np.ndarray): First quaternion [x, y, z, w]
B (np.ndarray): Second quaternion [x, y, z, w]
Returns:
float: Rotation angle error (in radians)
"""
qA = quat_normalize(A[3:7])
qB = quat_normalize(B[3:7])
q_err = quat_multiply(qB, quat_conjugate(qA))
q_err = quat_normalize(q_err)
# Numerically stable
qw = np.clip(q_err[3], -1.0, 1.0)
angle = 2 * np.arccos(qw)
return angle # Unit: radians
def calculate_error(pose1: Sequence[float], pose2: Sequence[float]) -> Dict[str, float]:
"""
Calculate the position error and rotation error between two poses (in radians)
Parameters:
pose1 (Sequence[float]): First pose, [x, y, z, qx, qy, qz, qw]
pose2 (Sequence[float]): Second pose, [x, y, z, qx, qy, qz, qw]
Returns:
dict: Dictionary containing position error (in meters) and rotation error (in radians)
"""
A, B = np.array(pose1), np.array(pose2)
pos_err = np.linalg.norm(A[:3] - B[:3])
rot_err = orientation_error_angle(A, B)
return {
"position_error_norm": pos_err,
"orientation_error_rad": rot_err,
"orientation_error_deg": np.degrees(rot_err)
}
def local_pose_to_global(start_pose: Sequence[float], local_pose: Sequence[float]):
"""
Convert local pose to global pose
Parameters:
start_pose (Sequence[float]): Start pose, [x, y, z, qx, qy, qz, qw]
local_pose (Sequence[float]): Local pose, [x, y, z, qx, qy, qz, qw]
Returns:
Sequence[float]: Global pose, [x, y, z, qx, qy, qz, qw]
"""
start_mat = np.eye(4)
start_mat[:3, :3] = quat_to_matrix(start_pose[3:7])
start_mat[:3, 3] = [start_pose[0], start_pose[1], start_pose[2]]
local_mat = np.eye(4)
local_mat[:3, :3] = quat_to_matrix(local_pose[3:7])
local_mat[:3, 3] = [local_pose[0], local_pose[1], local_pose[2]]
global_mat = start_mat @ local_mat
return global_mat[:3, 3].tolist() + matrix_to_quat(global_mat[:3, :3]).tolist()
def demo_square_move(robot: GalbotRobot, nav: GalbotNavigation):
"""
Demonstrate robot moving in a square pattern in navigation environment
Parameters:
robot (GalbotRobot): Robot instance
nav (GalbotNavigation): Navigation instance
"""
try:
start_pose = nav.get_current_pose()
except Exception as e:
print(f"Failed to get current pose: {e}")
return
# Move forward 0.5m, turn left 90 degrees
local_pose = [0.5, 0.0, 0.0, 0.0, 0.0, 0.707, 0.707]
try:
# Move forward 0.5m each time, turn left 90 degrees, repeat 4 times to form a square
for _ in range(4):
# Calculate target pose
cur_pose = nav.get_current_pose()
goal_pose = local_pose_to_global(cur_pose, local_pose)
# Check if path is reachable
if nav.check_path_reachability(goal_pose, cur_pose):
# Navigate to target pose
retry_cnt = 3
while True:
status = nav.navigate_to_goal(goal_pose, enable_collision_check=True, is_blocking=True, timeout=30)
time.sleep(0.5)
retry_cnt -= 1
if nav.check_goal_arrival() or retry_cnt < 0:
break
else:
print(f"Navigation failed, retrying...{retry_cnt}")
print("navigate_to_goal return status:", status)
print("Has arrived:", nav.check_goal_arrival())
else:
print("Path unreachable or unsafe")
cur_pose = nav.get_current_pose()
print(f"Current pose: {cur_pose}, Error from start pose: {calculate_error(cur_pose, start_pose)}")
except Exception as e:
print(f"Exception occurred during navigation: {e}")
def move_to_original(robot: GalbotRobot, nav: GalbotNavigation):
"""
Demonstrate robot returning to start pose in navigation environment
Parameters:
robot (GalbotRobot): Robot instance
nav (GalbotNavigation): Navigation instance
"""
cur_pose = nav.get_current_pose()
goal_pose = [0, 0, 0, 0, 0, 0, 1]
try:
if nav.check_path_reachability(goal_pose, cur_pose):
retry_cnt = 3
while True:
status = nav.navigate_to_goal(goal_pose, enable_collision_check=True, is_blocking=True, timeout=30)
time.sleep(0.5)
retry_cnt -= 1
if nav.check_goal_arrival() or retry_cnt < 0:
break
else:
print(f"Navigation failed, retrying...{retry_cnt}")
print("navigate_to_goal return status:", status)
print("Has arrived:", nav.check_goal_arrival())
else:
print("Path unreachable or unsafe")
except Exception as e:
print(f"Exception occurred during navigation: {e}")
def check_robot_safety():
"""Check if robot is safe"""
# Prompt important notes
print("⚠️ Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations; 3. Please ensure the area around the robot is clear of obstacles.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing...")
break
elif key == 'n':
print("User did not confirm, exiting program...")
exit(1)
else:
print("Invalid input, please enter 'y' or 'n'")
def main():
check_robot_safety()
try:
# Get robot instance
robot = GalbotRobot()
# Get navigation instance
nav = GalbotNavigation()
# Initialize robot
if robot.init():
print("Robot initialization successful")
else:
print("Robot initialization failed")
# Initialize navigation
if nav.init():
print("Navigation initialization successful")
else:
print("Navigation initialization failed")
# Wait for data preparation
time.sleep(1)
# Check initial localization status
is_localized = nav.is_localized()
if not is_localized:
print("Localization failed, attempting to re-localize: Please move the robot to the origin of the map!")
time.sleep(3)
while not is_localized:
nav.relocalize([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
time.sleep(0.5)
is_localized = nav.is_localized()
# square_move
demo_square_move(robot, nav)
except Exception as e:
print(f"Exception occurred: {e}")
finally:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
if __name__ == "__main__":
main()
示例 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.g1 import GalbotRobot
from galbot_sdk.g1 import RgbOutputFormat, SensorType
except ImportError:
print("import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH")
exit(1)
import os
try:
import open3d as o3d
except ImportError:
os.system("pip install open3d")
import open3d as o3d
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
import time
from typing import Dict
def convert_pointcloud(cloud):
"""
Convert cloud dict to NumPy array dictionary
Parameters:
cloud (dict): PointCloud2 protobuf message object
Returns:
Dictionary: {field_name: NumPy array}
- Single-element fields: shape (N,)
- Multi-element fields: shape (N, count) or (N,)
- N = width * height (total number of points)
"""
if not cloud:
return {}
num_points = cloud["height"] * cloud["width"]
if num_points == 0:
return {}
DTYPE_MAP = {
1: np.int8,
2: np.uint8,
3: np.int16,
4: np.uint16,
5: np.int32,
6: np.uint32,
7: np.float32,
8: np.float64
}
dtype_list = []
for field in cloud["fields"]:
# Get base data type
np_dtype_class = DTYPE_MAP.get(field["datatype"])
if np_dtype_class is None:
raise ValueError(f"Unsupported data type: {field['datatype']}")
dtype_inst = np.dtype(np_dtype_class)
# Handle byte order (endianness)
if dtype_inst.itemsize > 1:
byteorder = '>' if cloud["is_bigendian"] else '<'
dtype_inst = dtype_inst.newbyteorder(byteorder)
# Add to dtype list
if field["count"] == 1:
dtype_list.append((field["name"], dtype_inst))
else:
# Multi-element fields (e.g., rgb)
dtype_list.append((field["name"], dtype_inst, field["count"]))
# Create structured dtype
dtype = np.dtype(dtype_list)
# Data integrity check
expected_size = num_points * cloud["point_step"]
if len(cloud["data"]) < expected_size:
raise ValueError(
f"Insufficient data length: expected {expected_size} bytes, "
f"actual {len(cloud['data'])} bytes"
)
# Create NumPy structured array from binary data
# count parameter ensures only expected number of points are read
arr = np.frombuffer(cloud["data"], dtype=dtype, count=num_points)
# Convert to regular dictionary (copy data to avoid modifying original)
result = {}
for field in cloud["fields"]:
field_data = arr[field["name"]]
# Handle shape of multi-element fields
if field["count"] == 1:
result[field["name"]] = field_data.copy()
else:
# Keep original shape or flatten, choose according to needs
result[field["name"]] = field_data.copy()
return result
def get_xyz_array(pointcloud_dict: Dict[str, np.ndarray],
remove_nan: bool = False) -> np.ndarray:
"""
Extract XYZ coordinate array from converted point cloud dictionary
Parameters:
pointcloud_dict (Dict[str, np.ndarray]): Dictionary returned by pointcloud2_to_numpy()
remove_nan (bool, optional): Whether to remove points containing NaN (for FLOAT32/FLOAT64 types). Defaults to False.
Returns:
Nx3 point coordinate array
"""
required = ['x', 'y', 'z']
if not all(k in pointcloud_dict for k in required):
raise ValueError("Point cloud data missing required xyz fields")
points = np.stack([pointcloud_dict['x'],
pointcloud_dict['y'],
pointcloud_dict['z']], axis=1)
if remove_nan:
mask = ~np.isnan(points).any(axis=1)
points = points[mask]
return points
def save_xyz_to_pcd(xyz_array: np.ndarray, filename: str, binary: bool = False) -> None:
"""
Save XYZ coordinates to PCD file format (simplest option for coordinate-only data)
Parameters:
xyz_array (np.ndarray): Nx3 array of XYZ coordinates
filename (str): Output PCD file path
binary (bool, optional): If True, saves in binary format; otherwise ASCII. Defaults to False.
"""
if xyz_array.ndim != 2 or xyz_array.shape[1] != 3:
raise ValueError(f"xyz_array must have shape (N, 3), got {xyz_array.shape}")
num_points = xyz_array.shape[0]
header = [
"# .PCD v0.7 - Point Cloud Data file format",
"VERSION 0.7",
"FIELDS x y z",
"SIZE 4 4 4",
"TYPE F F F", # F = float32
"COUNT 1 1 1",
f"WIDTH {num_points}",
"HEIGHT 1",
"VIEWPOINT 0 0 0 1 0 0 0",
f"POINTS {num_points}",
f"DATA {'binary' if binary else 'ascii'}"
]
if binary:
with open(filename, 'wb') as f:
f.write(('\n'.join(header) + '\n').encode('ascii'))
f.write(xyz_array.astype(np.float32).tobytes())
else:
with open(filename, 'w') as f:
f.write('\n'.join(header) + '\n')
np.savetxt(f, xyz_array, fmt='%f')
def decode_compressed_image(compressed_image):
"""
decode CompressedImage image
Parameters:
compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"] == "jpeg":
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(compressed_image)
else:
raise ValueError(f"Unsupport data format: {compressed_image['format']}")
def decode_rgb_image(image_data):
"""decode rgb image"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Fail to Decode RGB Image")
return img
def decode_depth_image(image_data):
"""decode depth image"""
depth_img = np.frombuffer(image_data["data"], dtype=np.uint16).copy()
# Check if height and width exist
if "height" not in image_data or "width" not in image_data:
raise ValueError("Missing 'height' or 'width' in depth image metadata.")
if image_data["height"] == 0 or image_data["width"] == 0:
raise ValueError(f"Invalid 'height' ({image_data['height']}) or 'width' ({image_data['width']}) in depth image metadata.")
# Parse depth image
depth_img = depth_img.reshape((image_data["height"], image_data["width"]))
depth_img = depth_img.astype(np.float32) / image_data["depth_scale"]
return depth_img
def depth_rgb_to_pointcloud(depth, rgb, fx, fy, cx, cy, depth_scale=1.0):
"""
Convert depth map and RGB image to point cloud
Parameters:
depth: (H, W) depth map
rgb: (H, W, 3) RGB image
depth_scale: Depth unit scaling (use 0.001 for mm->m conversion)
Returns:
points: (N, 3) point cloud coordinate array
colors: (N, 3) point cloud color array (0-1 range)
"""
assert depth.shape[:2] == rgb.shape[:2]
H, W = depth.shape
# Pixel coordinates
u, v = np.meshgrid(np.arange(W), np.arange(H))
# Depth (converted to meters)
Z = depth.astype(np.float32) * depth_scale
# Filter invalid depths
valid = Z > 0
X = (u - cx) * Z / fx
Y = (v - cy) * Z / fy
points = np.stack((X, Y, Z), axis=-1)
colors = rgb.astype(np.float32) / 255.0
# Keep only valid points
points = points[valid]
colors = colors[valid]
return points, colors
def check_robot_safety():
"""Check if robot is safe"""
# Prompt important notes
print("⚠️ Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations.")
while True:
key = input("Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)...")
if key == 'y':
print("User confirmed, continuing...")
break
elif key == 'n':
print("User did not confirm, exiting program...")
exit(1)
else:
print("Invalid input, please enter 'y' or 'n'")
def main():
SHOW_IMAGE = False
check_robot_safety()
try:
# Get and initialize the GalbotRobot singleton
robot = GalbotRobot()
# Get RGB and depth images from the left arm, depth images from the right arm,
# base LiDAR data, and torso IMU data
enable_sensor_set = {SensorType.LEFT_ARM_CAMERA, # Left arm depth camera
SensorType.LEFT_ARM_DEPTH_CAMERA, # Left arm RGB camera
SensorType.BASE_LIDAR,} # Base LiDAR
# To save resource overhead, only cameras and radar sensors passed during initialization can acquire data
robot.init(enable_sensor_set)
print("Initialization successful")
# Program starts immediately, wait for data readiness
time.sleep(5)
# Get RGB image from the left arm
rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA, RgbOutputFormat.JPEG, True)
if not rgb_image_data:
print("No rgb image data!")
else:
print("get rgb image suceess")
print(rgb_image_data['header'])
img = decode_compressed_image(rgb_image_data)
# Save RGB image
cv2.imwrite("rgb_image_data.jpg", img)
# Visualize RGB image
if SHOW_IMAGE:
cv2.namedWindow("rgb image", cv2.WINDOW_NORMAL)
cv2.imshow("rgb image", img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Get depth image from the left arm
depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not depth_data or "data" not in depth_data:
print("Depth camera not ready")
else:
print("get depth data suceess")
print(depth_data['header'])
depth_img_raw = decode_compressed_image(depth_data)
depth_img = cv2.normalize(depth_img_raw, None, 0, 255, cv2.NORM_MINMAX) # Normalize, mapping depth values to 0-1 range
depth_img = depth_img.astype(np.uint8)
# Save depth image
cv2.imwrite("depth_data.jpg", depth_img)
# Visualize depth image
if SHOW_IMAGE:
cv2.namedWindow("depth image", cv2.WINDOW_NORMAL)
cv2.imshow("depth image", depth_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
# Get base LiDAR data
lidar_data = robot.get_lidar_data(SensorType.BASE_LIDAR)
if not lidar_data:
print("No lidar data!")
else:
pointcloud_dict = convert_pointcloud(lidar_data)
xyz_points = get_xyz_array(pointcloud_dict)
save_xyz_to_pcd(xyz_points, "output_xyz.pcd")
print(pointcloud_dict)
print("get lidar data success")
# Visualize LiDAR point cloud
if SHOW_IMAGE:
vis = o3d.visualization.Visualizer()
vis.create_window()
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz_points)
vis.add_geometry(pcd)
vis.run()
vis.destroy_window()
# Get torso IMU data
imu_data = robot.get_imu_data(SensorType.TORSO_IMU)
if not imu_data:
print("No imu data!")
else:
print("get imu data suceess")
try:
camera_info = robot.get_camera_intrinsic(SensorType.LEFT_ARM_DEPTH_CAMERA)
if not camera_info:
print("No camera info!")
else:
print(camera_info)
except Exception as e:
camera_info = {
"width": 1280,
"height": 720,
"distortion_model": "plumb_bob",
"camera_type": "D405",
"K": [653.4349365234375, 0.0, 639.95159912109375,
0.0, 652.48858642578125, 365.29425048828125,
0.0, 0.0, 1.0],
}
# Convert depth map and RGB image to point cloud and save
if depth_data and rgb_image_data:
points, colors = depth_rgb_to_pointcloud(
depth_img_raw,
img,
fx=camera_info['K'][0],
fy=camera_info['K'][4],
cx=camera_info['K'][2],
cy=camera_info['K'][5],
depth_scale=0.1 # If depth is in mm, set to 0.001
)
save_xyz_to_pcd(points, "left_arm_camera_pointcloud.pcd", binary=True)
print(f"RGB fused depth map point cloud saved to left_arm_camera_pointcloud.pcd, number of points: {points.shape[0]}")
except Exception as e:
print(f"❌ Exception occurred: {e}")
finally:
# Actively send SIGINT exit signal
robot.request_shutdown()
# Wait to enter shutdown state
robot.wait_for_shutdown()
# Release SDK resources
robot.destroy()
print('Resource release successful')
if __name__=="__main__":
main()
示例 5：执行 VLA
适用场景：展示 VLA 客户端如何通过 get_synced_observation 采集同步观测、接收包含头部的 23 维绝对关节 action chunk，并使用高级实时关节控制接口 set_joint_commands 按控制频率逐帧快速执行。Galbot 官方支持通过主从臂采集示教数据，具体方法请参考 TM01 使用手册。采集生成的 MCAP 数据可通过 galbot-mcap2lerobot 转换为 LeRobot 数据集，用于后续模型训练。完成模型训练并部署推理服务后，可参考本示例完成同步观测获取和 action chunk 执行。若模型输出末端 EE pose，请参考 set_end_effector_command 接口。
examples/python/tutorials/example5_execute_vla.py
"""Execute absolute-joint VLA action chunks with Galbot SDK interfaces.
VLA control requires observations and actions to stay aligned in time. This
example uses ``get_synced_observation`` to acquire a synchronized camera image
and robot joint state for each model request. It then uses
``set_joint_commands`` to stream the model's 23-dimensional absolute-joint
action chunk frame by frame at the configured control frequency.
For the training-data workflow, Galbot officially supports demonstration data
collection with leader-follower arms. See the TM01 user manual:
https://developer.galbot.com/docs/tm01/1.4.0/zh/tm01
The collected MCAP data can be converted to the LeRobot dataset format with
galbot-mcap2lerobot for subsequent model training:
https://github.com/GalaxyGeneralRobotics/galbot-mcap2lerobot
After deploying the trained model as an inference service, use this example as
a reference for acquiring synchronized observations and executing its action
chunks on the robot.
This example demonstrates absolute joint-position model output. If a model
instead outputs end-effector poses (EE poses such as
``[x, y, z, qx, qy, qz, qw]``), use ``set_end_effector_command`` for real-time
Cartesian control. See
``examples/g1/python/galbot_robot/set_end_effector_commands.py`` for that API.
Before running, release the emergency-stop button and clear the area around the
robot. Keep a hand near the emergency-stop button during execution.
"""
import json
import time
from pathlib import Path
from typing import Dict, List, Sequence, Tuple
import numpy as np
try:
from galbot_sdk.g1 import (
ControlStatus,
G1JointGroup,
GalbotRobot,
JointCommand,
SensorType,
)
except ImportError:
print(
"Failed to import galbot_sdk. Install the SDK or add it to PYTHONPATH "
"before running this example."
)
raise SystemExit(1)
# The order of these groups defines the meaning of every action vector:
# 5 + 2 + 7 + 1 + 7 + 1 = 23 dimensions.
ACTION_GROUPS = [
"leg",
"head",
"left_arm",
"left_gripper",
"right_arm",
"right_gripper",
]
GROUP_SLICES = {
"leg": slice(0, 5),
"head": slice(5, 7),
"left_arm": slice(7, 14),
"left_gripper": slice(14, 15),
"right_arm": slice(15, 22),
"right_gripper": slice(22, 23),
}
GROUP_JOINT_NAMES = {
"leg": [f"leg_joint{i}" for i in range(1, 6)],
"head": [f"head_joint{i}" for i in range(1, 3)],
"left_arm": [f"left_arm_joint{i}" for i in range(1, 8)],
"left_gripper": ["left_gripper_joint1"],
"right_arm": [f"right_arm_joint{i}" for i in range(1, 8)],
"right_gripper": ["right_gripper_joint1"],
}
CONTROL_FREQUENCY_HZ = 20.0
# Number of model action frames returned and executed in one chunk.
ACTION_CHUNK_SIZE = 30
GRIPPER_VELOCITY = 0.1
GRIPPER_EFFORT = 10.0
TASK_PROMPT = "Based on the current view, predict the next robot action."
# Shared by the G1 Python and C++ tutorial examples.
ACTION_JSON_PATH = (
Path(__file__).resolve().parents[2] / "assets" / "tutorials" / "example5_vla.json"
)
def confirm_safe_environment() -> None:
"""Require an explicit confirmation before sending robot commands."""
print(
"WARNING: Release the emergency-stop button, keep the robot in working "
"mode, and clear people and obstacles from its motion range."
)
answer = input("Continue with the VLA execution example? [y/N]: ").strip().lower()
if answer not in {"y", "yes"}:
raise SystemExit("Execution cancelled by the user.")
def action_joint_names() -> List[str]:
"""Return joint names in exactly the same order as an action vector."""
return [
joint_name for group in ACTION_GROUPS for joint_name in GROUP_JOINT_NAMES[group]
]
def collect_observation(robot: GalbotRobot) -> Dict[str, object]:
"""Collect the synchronized image and state normally sent to a VLA server."""
observation = robot.get_synced_observation(
[SensorType.HEAD_LEFT_CAMERA],
True,
)
if not observation:
raise RuntimeError("get_synced_observation failed")
image_message = observation.rgb_data_map.get(SensorType.HEAD_LEFT_CAMERA)
if image_message is None:
raise RuntimeError("Synchronized head-left image is missing")
if observation.joint_state is None:
raise RuntimeError("Synchronized joint state is missing")
state_by_name = {
joint_state.joint_name: joint_state
for joint_state in observation.joint_state.joint_state_vec
}
missing_names = [name for name in action_joint_names() if name not in state_by_name]
if missing_names:
raise RuntimeError(f"Joint state is missing joints: {missing_names}")
state = np.asarray(
[state_by_name[name].position for name in action_joint_names()],
dtype=np.float32,
)
return {
"state": state,
"image": bytes(image_message.data),
"prompt": TASK_PROMPT,
}
def load_mock_actions(json_path: Path) -> np.ndarray:
"""Load the action frames returned by the simulated VLA service."""
if not json_path.is_file():
raise FileNotFoundError(f"Mock VLA action file does not exist: {json_path}")
with json_path.open("r", encoding="utf-8") as file:
payload = json.load(file)
actions = np.asarray(payload.get("frames"), dtype=np.float32)
if payload.get("num_frames") != len(actions):
raise ValueError(
"Mock num_frames does not match the number of frames in the JSON: "
f"{payload.get('num_frames')} != {len(actions)}"
)
return validate_action_chunk(actions)
def mock_vla_server(
observation: Dict[str, object],
all_actions: np.ndarray,
cursor: int,
chunk_size: int = ACTION_CHUNK_SIZE,
) -> Tuple[Dict[str, np.ndarray], int]:
"""Simulate one model request and return the next JSON action chunk.
A real VLA service would replace this function and infer actions from the
observation. This local version directly returns consecutive absolute joint
frames loaded from the copied client-demo JSON file.
"""
state = np.asarray(observation["state"], dtype=np.float32)
expected_dim = len(action_joint_names())
if state.shape != (expected_dim,):
raise ValueError(
f"Observation state has shape {state.shape}; expected ({expected_dim},)"
)
start = int(cursor)
end = min(start + int(chunk_size), len(all_actions))
actions = all_actions[start:end]
print(
"Mock VLA server received one request: "
f"prompt={observation['prompt']!r}, "
f"image_bytes={len(observation['image'])}, frames={start}:{end}"
)
print(f"Mock VLA server returned action chunk: shape={actions.shape}")
return {"actions": actions}, end
def move_to_first_action(robot: GalbotRobot, first_action: np.ndarray) -> None:
"""Move safely to the absolute pose used by the copied JSON trajectory."""
# Stabilize the lower body first, then move both arms and the head together.
for position_groups in (["leg"], ["head", "left_arm", "right_arm"]):
joint_positions = np.concatenate(
[first_action[GROUP_SLICES[group]] for group in position_groups]
).tolist()
status = robot.set_joint_positions(
joint_positions=joint_positions,
joint_groups=position_groups,
joint_names=[],
is_blocking=True,
speed_rad_s=0.2,
timeout_s=10.0,
)
if status != ControlStatus.SUCCESS:
raise RuntimeError(
f"Failed to move {position_groups} to the first JSON action: {status}"
)
gripper_targets = (
(G1JointGroup.left_gripper, first_action[GROUP_SLICES["left_gripper"]][0]),
(
G1JointGroup.right_gripper,
first_action[GROUP_SLICES["right_gripper"]][0],
),
)
for joint_group, position in gripper_targets:
status = robot.set_gripper_command(
end_effector=joint_group,
width_m=float(position),
velocity_mps=0.1,
effort=GRIPPER_EFFORT,
is_blocking=True,
)
if status != ControlStatus.SUCCESS:
raise RuntimeError(
f"Failed to move {joint_group} to the first JSON action: {status}"
)
print("Robot reached the first pose in the mock VLA action file.")
def validate_action_chunk(actions: np.ndarray) -> np.ndarray:
"""Validate the model result before converting it to SDK commands."""
actions = np.asarray(actions, dtype=np.float32)
expected_dim = len(action_joint_names())
if actions.ndim != 2 or actions.shape[1] != expected_dim:
raise ValueError(
"VLA actions must have shape (T, D), where "
f"D={expected_dim}; got {actions.shape}"
)
if not np.all(np.isfinite(actions)):
raise ValueError("VLA actions contain NaN or infinity")
return actions
def build_joint_commands(action: Sequence[float]) -> List[JointCommand]:
"""Convert one model action frame to SDK ``JointCommand`` objects."""
values = list(float(value) for value in action)
expected_dim = len(action_joint_names())
if len(values) != expected_dim:
raise ValueError(f"Action dimension is {len(values)}; expected {expected_dim}")
gripper_indices = {
action_joint_names().index("left_gripper_joint1"),
action_joint_names().index("right_gripper_joint1"),
}
commands = []
for index, value in enumerate(values):
command = JointCommand()
command.position = value
if index in gripper_indices:
command.velocity = GRIPPER_VELOCITY
command.effort = GRIPPER_EFFORT
commands.append(command)
return commands
def execute_action_chunk(robot: GalbotRobot, actions: np.ndarray) -> None:
"""Stream one VLA action chunk with the high-frequency SDK interface."""
actions = validate_action_chunk(actions)
if actions.shape[0] == 0:
print("The VLA model returned an empty chunk; execution is complete.")
return
period_s = 1.0 / CONTROL_FREQUENCY_HZ
next_tick = time.monotonic()
print(
f"Streaming {actions.shape[0]} frames at {CONTROL_FREQUENCY_HZ:.1f} Hz "
"with set_joint_commands..."
)
for frame_index, action in enumerate(actions, start=1):
status = robot.set_joint_commands(
joint_commands=build_joint_commands(action),
joint_groups=ACTION_GROUPS,
joint_names=[],
time_from_start_s=0.0,
)
if status != ControlStatus.SUCCESS:
raise RuntimeError(
f"set_joint_commands failed at frame {frame_index}: {status}"
)
next_tick += period_s
remaining_s = next_tick - time.monotonic()
if remaining_s > 0.0:
time.sleep(remaining_s)
print("Action chunk execution completed.")
def main() -> None:
"""Request and execute JSON action chunks until the mock service is empty."""
confirm_safe_environment()
robot = GalbotRobot()
initialized = False
try:
if not robot.init({SensorType.HEAD_LEFT_CAMERA}, True):
raise RuntimeError("GalbotRobot initialization failed")
initialized = True
# Allow the first synchronized image and joint state to become ready.
time.sleep(5.0)
all_actions = load_mock_actions(ACTION_JSON_PATH)
print(f"Loaded mock VLA actions from: {ACTION_JSON_PATH}")
print(f"Action data shape: {all_actions.shape} (full23, including head)")
move_to_first_action(robot, all_actions[0])
cursor = 0
while True:
observation = collect_observation(robot)
response, cursor = mock_vla_server(
observation,
all_actions,
cursor,
)
if response["actions"].shape[0] == 0:
print("Mock VLA server has no more actions.")
break
execute_action_chunk(robot, response["actions"])
print("VLA execution example finished successfully.")
finally:
if initialized:
robot.request_shutdown()
robot.wait_for_shutdown()
robot.destroy()
print("SDK resources released.")
if __name__ == "__main__":
main()
示例 6：综合任务：导航+感知+抓取+放置
适用场景：完整抓取放置应用的参考示例，串联感知、运动规划与控制和导航，展示预抓取、抬升、回撤、抓取与放置之间的导航，以及使用 move_line 完成直线抓取和放置。
examples/python/tutorials/example6_pick_and_place.py
"""
This tutorial demonstrates a complete pick-and-place task for a standard application scenario.
It integrates navigation, perception, motion planning, and robot control, and can be used as a
reference implementation for standard applications.
Before the task starts, the robot moves to the known 21-joint initial pose defined by this example.
After picking, the robot navigates to its current map pose with the X coordinate offset by 0.1 m,
then performs placement.
The final grasp approach and place descent use `move_line`; other end-effector motions use
`set_end_effector_pose`.
"""
try:
import galbot_sdk.g1 as gm
from galbot_sdk.g1 import GalbotNavigation
from galbot_sdk.g1 import GalbotRobot
from galbot_sdk.g1 import GalbotMotion
from galbot_sdk.g1 import G1JointGroup, SensorType
except ImportError:
print(
"Import galbot_sdk failed, please install it first or check if it is in the PYTHONPATH"
)
exit(1)
import os
try:
import numpy as np
except ImportError:
os.system("pip install numpy")
import numpy as np
try:
import cv2
except ImportError:
os.system("pip install opencv-python")
import cv2
import time
from typing import Sequence
PLACE_NAVIGATION_X_OFFSET_M = 0.1
GRASP_TRANSITION_OFFSET_M = 0.05
PLACE_POSE_BASE = [0.4, 0.3, 0.7, 0.0, 0.0, 0.0, 1.0]
# Placeholder grasp pose for this runnable tutorial. Replace it with the
# base-frame pose produced by the application's perception module.
GRASP_POSE_BASE_PLACEHOLDER = [0.4, 0.3, 0.65, 0.0, 0.0, 0.0, 1.0]
INITIAL_LEG_JOINT_NAMES = [
"leg_joint1",
"leg_joint2",
"leg_joint3",
"leg_joint4",
"leg_joint5",
]
INITIAL_LEG_JOINT_POSITIONS = [0.4, 1.2, 0.8, 0.0, 0.0]
INITIAL_UPPER_BODY_JOINT_NAMES = [
"head_joint1",
"head_joint2",
"left_arm_joint1",
"left_arm_joint2",
"left_arm_joint3",
"left_arm_joint4",
"left_arm_joint5",
"left_arm_joint6",
"left_arm_joint7",
"right_arm_joint1",
"right_arm_joint2",
"right_arm_joint3",
"right_arm_joint4",
"right_arm_joint5",
"right_arm_joint6",
"right_arm_joint7",
]
INITIAL_UPPER_BODY_JOINT_POSITIONS = [
0.0,
0.0,
1.90,
-1.46,
-0.54,
-1.96,
0.0,
-0.4,
0.0,
-1.90,
1.46,
0.54,
1.96,
0.0,
0.4,
0.0,
]
def move_to_initial_pose(robot: GalbotRobot) -> bool:
"""Move the legs first, then move the head and both arms together."""
leg_status = robot.set_joint_positions(
joint_positions=INITIAL_LEG_JOINT_POSITIONS,
joint_names=INITIAL_LEG_JOINT_NAMES,
is_blocking=True,
speed_rad_s=0.2,
timeout_s=20.0,
)
if leg_status != gm.ControlStatus.SUCCESS:
print(f"❌ Failed to move the legs to the initial pose: status={leg_status}")
return False
print(f"✅ Successfully moved the legs to the initial pose: status={leg_status}")
upper_body_status = robot.set_joint_positions(
joint_positions=INITIAL_UPPER_BODY_JOINT_POSITIONS,
joint_names=INITIAL_UPPER_BODY_JOINT_NAMES,
is_blocking=True,
speed_rad_s=0.2,
timeout_s=20.0,
)
if upper_body_status != gm.ControlStatus.SUCCESS:
print(
"❌ Failed to move the head and arms to the initial pose: "
f"status={upper_body_status}"
)
return False
print(
"✅ Successfully moved the head and arms to the initial pose: "
f"status={upper_body_status}"
)
return True
def decode_compressed_image(compressed_image, camera_info={}):
"""
decode CompressedImage image
Parameters:
compressed_image (dict): image dict, keys:[header, format, data, "depth_scale"]
Returns:
numpy.ndarray: decoded image
"""
image_data = compressed_image["data"]
if compressed_image["format"] == "jpeg":
return decode_rgb_image(image_data)
elif compressed_image["format"] == "16UC1":
return decode_depth_image(
image_data, compressed_image["depth_scale"], camera_info
)
else:
raise ValueError(f"Unsupport data format: {compressed_image['format']}")
def decode_rgb_image(image_data):
"""decode rgb image"""
nparr = np.frombuffer(image_data, np.uint8)
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
if img is None:
raise ValueError("Fail to Decode RGB Image")
return img
def decode_depth_image(image_data, depth_scale, camera_info):
"""decode depth image"""
depth_img = np.frombuffer(image_data, dtype=np.uint16).copy()
if not camera_info:
depth_img = depth_img.reshape((720, 1280))
else:
depth_img = depth_img.reshape((camera_info["height"], camera_info["width"]))
depth_img = depth_img.astype(np.float32) / depth_scale
return depth_img
def navigation_to_goal(
nav: GalbotNavigation, goal_pose: Sequence[float], retry_cnt: int = 3
):
"""
Navigate to target pose
Parameters:
nav (GalbotNavigation): Navigation instance
goal_pose (Sequence[float]): Target pose [x, y, z, qx, qy, qz, qw]
retry_cnt (int, optional): Number of retries. Defaults to 3.
"""
try:
cur_pose = nav.get_current_pose()
print(f"Current pose: {cur_pose}")
if nav.check_path_reachability(goal_pose, cur_pose):
retry_cnt = 3
while True:
status = nav.navigate_to_goal(
goal_pose, enable_collision_check=True, is_blocking=True, timeout=20
)
time.sleep(0.5)
retry_cnt -= 1
if nav.check_goal_arrival() or retry_cnt < 0:
break
else:
print(f"Navigation failed: status={status}, retrying: {retry_cnt}")
print("navigate_to_goal return status:", status)
print("Has arrived:", nav.check_goal_arrival())
else:
print("Path unreachable or unsafe")
except Exception as e:
print(f"Exception occurred during navigation: {e}")
def quaternion_to_rotation_matrix(quaternion: Sequence[float]) -> np.ndarray:
"""Convert an [x, y, z, w] quaternion to a 3x3 rotation matrix."""
quaternion_array = np.asarray(quaternion, dtype=np.float64)
if quaternion_array.shape != (4,):
raise ValueError("Quaternion must contain exactly four values")
norm = np.linalg.norm(quaternion_array)
if norm < np.finfo(np.float64).eps:
raise ValueError("Quaternion norm must be greater than zero")
x, y, z, w = quaternion_array / norm
return np.array(
[
[
1.0 - 2.0 * (y * y + z * z),
2.0 * (x * y - z * w),
2.0 * (x * z + y * w),
],
[
2.0 * (x * y + z * w),
1.0 - 2.0 * (x * x + z * z),
2.0 * (y * z - x * w),
],
[
2.0 * (x * z - y * w),
2.0 * (y * z + x * w),
1.0 - 2.0 * (x * x + y * y),
],
],
dtype=np.float64,
)
def pose_camera_to_base(
robot: GalbotRobot, pose_camera: Sequence[float]
) -> Sequence[float]:
"""
Transform camera pose to chassis coordinate system
Parameters:
robot (GalbotRobot): Robot instance
pose_camera (Sequence[float]): Camera pose [x, y, z, qx, qy, qz, qw]
Returns:
Sequence[float]: Chassis pose [x, y, z, qx, qy, qz, qw]
"""
source_frame = "left_arm_camera_color_optical_frame"
target_frame = "base_link"
base_to_cam = robot.get_transform(target_frame, source_frame)[0]
if base_to_cam is None:
print("Failed to get transform from camera to chassis")
return None
else:
print("base_to_cam: ", base_to_cam)
base_to_cam_mat = np.eye(4)
base_to_cam_mat[:3, :3] = quaternion_to_rotation_matrix(base_to_cam[3:])
base_to_cam_mat[:3, 3] = np.array(base_to_cam[:3])
pose_base_mat = (
base_to_cam_mat[:3, :3] @ np.array(pose_camera[:3]).reshape(3, 1)
+ base_to_cam_mat[:3, 3:]
)
return pose_base_mat.flatten()[:3].tolist() + [0, 0, 0, 1]
def detect_object(robot: GalbotRobot):
try:
# This example uses the left-arm RGB and depth cameras.
rgb_image_data = robot.get_rgb_data(SensorType.LEFT_ARM_CAMERA)
depth_data = robot.get_depth_data(SensorType.LEFT_ARM_DEPTH_CAMERA)
# Decode image data
if not rgb_image_data:
print("No rgb image data!")
else:
print("get rgb image suceess")
img = decode_compressed_image(rgb_image_data)
if not depth_data:
print("No depth_data!")
else:
depth_img = decode_compressed_image(depth_data)
print("get depth data suceess")
# Placeholder perception result in the OpenCV camera frame (x right, y down, z forward).
# Replace this pose with the output of an application-specific RGB-D detector.
object_pose_camera = [0.0, 0.20, 0.29, 0.0, 0.71, 0.0, 0.71]
print(f"object_pose_camera: {object_pose_camera}")
# Calculate target pose in chassis coordinate system
object_pose_base = pose_camera_to_base(robot, object_pose_camera)
print(f"Target pose in chassis coordinate system: {object_pose_base}")
except Exception as e:
object_pose_base = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
print(f"Target detection exception: {e}")
return object_pose_base
def check_robot_safety():
"""Check if robot is safe"""
# Prompt important notes
print(
"⚠️ Note: 1. Please ensure the emergency stop button of the robot is released; 2. Please ensure there are no obstructions around the robot to avoid unexpected situations. 3. Please ensure the area around the robot is clear of obstacles."
)
while True:
key = input(
"Please confirm that the robot's emergency stop button is released and there are no obstructions, continue? (y/n)..."
)
if key == "y":
print("User confirmed, continuing...")
break
elif key == "n":
print("User did not confirm, exiting program...")
exit(1)
else:
print("Invalid input, please enter 'y' or 'n'")
def pick_and_place(
robot: GalbotRobot,
nav: GalbotNavigation,
motion: GalbotMotion,
object_pose_base: Sequence[float],
navigation_enabled: bool,
):
try:
# Attach tool to end effector
status = motion.attach_tool(chain="left_arm", tool="galbot_gripper")
time.sleep(0.5)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to attach tool: status={status}")
return False
else:
print(f"✅ Successfully attached tool: status={status}")
# The target poses represent the attached gripper TCP, not the bare arm flange.
# Tool pose mode makes set_end_effector_pose control the attached tool TCP.
motion_params = gm.Parameter()
motion_params.set_direct_execute(True)
motion_params.set_blocking(True)
motion_params.set_timeout(20.0)
motion_params.set_tool_pose(True)
motion_params.set_check_collision(True)
motion_params.set_reference_frame("base_link")
motion_params.set_actuate("with_chain_only")
# Open left gripper
# Set left gripper width to 0.1m, speed to 0.05m, force to 10N, will block until gripper reaches position
status = robot.set_gripper_command(
G1JointGroup.left_gripper, 0.1, 0.05, 10, False
)
time.sleep(0.5)
print(f"✅ Successfully set left gripper width to 0.1m: status={status}")
# This fixed grasp pose is only a placeholder for the runnable tutorial.
# A real application should use object_pose_base from perception instead.
print(f"Perception pose: {object_pose_base}")
grasp_pose = list(GRASP_POSE_BASE_PLACEHOLDER)
pre_grasp_pose = list(grasp_pose)
pre_grasp_pose[0] -= GRASP_TRANSITION_OFFSET_M
lift_grasp_pose = list(grasp_pose)
lift_grasp_pose[2] += GRASP_TRANSITION_OFFSET_M
retreat_pose = list(lift_grasp_pose)
retreat_pose[0] -= GRASP_TRANSITION_OFFSET_M
print(f"pre_grasp_pose: {pre_grasp_pose}")
status = motion.set_end_effector_pose(
target_pose=pre_grasp_pose,
end_effector_frame="left_arm",
reference_frame="base_link",
enable_collision_check=True,
is_blocking=True,
timeout=20.0,
params=motion_params,
)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to execute pre_grasp pose command: status={status}")
return False
print(f"✅ Successfully executed pre_grasp pose command: {pre_grasp_pose}")
# Use move_line for the final 5 cm straight-line approach to the object.
grasp_target = gm.MotionPlanChainTarget()
grasp_target.chain_name = "left_arm"
grasp_target.mode = gm.MotionPlanTargetMode.kCartesian
grasp_target.cart.chain_name = "left_arm"
grasp_target.cart.frame_id = "EndEffector"
grasp_target.cart.reference_frame = "base_link"
grasp_target.cart.pose = gm.Pose(grasp_pose)
print(f"grasp_pose: {grasp_pose}")
status, _ = motion.move_line([[grasp_target]], motion_params)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to execute grasp move_line: status={status}")
return False
print(f"✅ Successfully executed grasp move_line: {grasp_pose}")
# Close gripper to grasp object
status = robot.set_gripper_command(
G1JointGroup.left_gripper, 0.02, 0.05, 10, False
)
time.sleep(0.5)
print(f"✅ Successfully closed left gripper: status={status}")
print(f"lift_grasp_pose: {lift_grasp_pose}")
status = motion.set_end_effector_pose(
target_pose=lift_grasp_pose,
end_effector_frame="left_arm",
reference_frame="base_link",
enable_collision_check=True,
is_blocking=True,
timeout=20.0,
params=motion_params,
)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to execute lift_grasp pose command: status={status}")
return False
print(f"✅ Successfully executed lift_grasp pose command: {lift_grasp_pose}")
print(f"retreat_pose: {retreat_pose}")
status = motion.set_end_effector_pose(
target_pose=retreat_pose,
end_effector_frame="left_arm",
reference_frame="base_link",
enable_collision_check=True,
is_blocking=True,
timeout=20.0,
params=motion_params,
)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to execute retreat pose command: status={status}")
return False
print(f"✅ Successfully executed retreat pose command: {retreat_pose}")
# Return to the initial left-arm joint pose before navigation.
left_arm_status = robot.set_joint_positions(
joint_positions=INITIAL_UPPER_BODY_JOINT_POSITIONS[2:9],
joint_names=INITIAL_UPPER_BODY_JOINT_NAMES[2:9],
is_blocking=True,
speed_rad_s=0.2,
timeout_s=20.0,
)
if left_arm_status != gm.ControlStatus.SUCCESS:
print(
f"❌ Failed to restore the left-arm initial pose: status={left_arm_status}"
)
return False
print(
f"✅ Successfully restored the left-arm initial pose: status={left_arm_status}"
)
if navigation_enabled:
place_navigation_goal = list(nav.get_current_pose())
place_navigation_goal[0] += PLACE_NAVIGATION_X_OFFSET_M
print(f"Place navigation target pose: {place_navigation_goal}")
navigation_to_goal(nav, place_navigation_goal)
time.sleep(2)
print("✅ Navigation stage completed; starting Place")
else:
print(
"⚠️ Navigation is unavailable; skipping navigation and continuing "
"with Place at the current position."
)
# Move above the place point, then descend 5 cm in a straight line.
place_pose = list(PLACE_POSE_BASE)
pre_place_pose = list(place_pose)
pre_place_pose[2] += GRASP_TRANSITION_OFFSET_M
print(f"pre_place_pose: {pre_place_pose}")
status = motion.set_end_effector_pose(
target_pose=pre_place_pose,
end_effector_frame="left_arm",
reference_frame="base_link",
enable_collision_check=True,
is_blocking=True,
timeout=20.0,
)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to execute pre_place pose command: status={status}")
return False
print(f"✅ Successfully executed pre_place pose command: {pre_place_pose}")
place_target = gm.MotionPlanChainTarget()
place_target.chain_name = "left_arm"
place_target.mode = gm.MotionPlanTargetMode.kCartesian
place_target.cart.chain_name = "left_arm"
place_target.cart.frame_id = "EndEffector"
place_target.cart.reference_frame = "base_link"
place_target.cart.pose = gm.Pose(place_pose)
print(f"place_pose: {place_pose}")
status, _ = motion.move_line([[place_target]], motion_params)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to execute place move_line: status={status}")
return False
print(f"✅ Successfully executed place move_line: {place_pose}")
# Release target
status = robot.set_gripper_command(
G1JointGroup.left_gripper, 0.1, 0.05, 10, False
)
time.sleep(0.5)
print(f"✅ Successfully released left gripper: status={status}")
# Return the left arm to its initial joint pose after placing the object.
left_arm_status = robot.set_joint_positions(
joint_positions=INITIAL_UPPER_BODY_JOINT_POSITIONS[2:9],
joint_names=INITIAL_UPPER_BODY_JOINT_NAMES[2:9],
is_blocking=True,
speed_rad_s=0.2,
timeout_s=20.0,
)
if left_arm_status != gm.ControlStatus.SUCCESS:
print(
"❌ Failed to restore the left-arm pose after placing: "
f"status={left_arm_status}"
)
return False
print(
"✅ Successfully restored the left-arm pose after placing: "
f"status={left_arm_status}"
)
# Detach tool from end effector
status = motion.detach_tool(chain="left_arm")
time.sleep(0.5)
if status != gm.MotionStatus.SUCCESS:
print(f"❌ Failed to detach tool: status={status}")
else:
print(f"✅ Successfully detached tool: status={status}")
return True
except Exception as e:
print(f"Exception occurred during pick_and_place: {e}")
return False
def main():
check_robot_safety()
try:
# Get robot instance
robot = GalbotRobot()
# Get GalbotMotion instance
motion = GalbotMotion()
# Get navigation instance
nav = GalbotNavigation()
# Enable only the RGB and depth cameras used for target detection.
enable_sensor_set = {
SensorType.LEFT_ARM_CAMERA,
SensorType.LEFT_ARM_DEPTH_CAMERA,
}
# Initialize robot
if robot.init(enable_sensor_set):
print("GalbotRobot initialization successful")
else:
print("GalbotRobot initialization failed")
if motion.init():
print("GalbotMotion initialization successful")
else:
print("GalbotMotion initialization failed")
if nav.init():
print("GalbotNavigation initialization successful")
else:
print("GalbotNavigation initialization failed")
# Program starts immediately, wait for data readiness
time.sleep(1)
if not move_to_initial_pose(robot):
raise RuntimeError("Failed to move to the task initial pose")
# Check localization once. If unavailable, Pick still runs and navigation
# between Pick and Place is skipped.
try:
navigation_enabled = nav.is_localized()
except Exception as e:
navigation_enabled = False
print(f"⚠️ Navigation status check failed: {e}")
if navigation_enabled:
print("✅ Robot is localized; Place navigation is enabled.")
else:
print(
"⚠️ Robot is not localized; navigation will be skipped. "
"This run demonstrates Pick and Place at the current position."
)
print()
# Detect the target before Pick.
object_pose_base = detect_object(robot)
print()
# Execute Pick, navigate, and then Place.
if not pick_and_place(robot, nav, motion, object_pose_base, navigation_enabled):
raise RuntimeError("Pick-and-place execution failed")
print()
except Exception as e:
print(f"Exception occurred: {e}")
finally:
# Actively send SIGINT exit signal
robot.request_shutdown()
# Wait to enter shutdown state
robot.wait_for_shutdown()
# Release SDK resources
robot.destroy()
print("Resource release successful")
if __name__ == "__main__":
main()