入门教程
接下来将会通过几个简单示例的介绍，来快速了解Galbot机器人的基础开发应用。运行接下来的所有示例，您需要首先下载安装Galbot SDK，并参考安装与配置说明进行配置。
Example1. 机器人的基础控制
本示例展示了GALBOT机器人的基础控制，执行本程序后，机器人的两臂将缓慢举起到头顶，形成一个类似比心的姿势。通过初始化机器人实例、检查安全状态后，让机器人的左右手臂执行预设的比心动作序列，先将手臂移动到比心姿势保持一段时间，然后恢复到原始姿态，整个过程包含了关节位置获取、关节角度设置、阻塞等待以及异常处理等基本控制流程。
注意：1. 保持急停按钮为打开状态；2. 确保机器人附近2米范围无遮挡，避免手臂触碰障碍物出现危险。
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()
Example2. 手臂操控
本示例展示了GALBOT机器人的手臂操控，执行本程序后，机器人左臂将缓缓抬起一定高度，然后在缓缓恢复原始姿态。主要实现了正逆运动学（IK/FK）求解、末端执行器位姿控制以及关节角度设置等功能，通过获取当前左臂末端位姿、使用逆运动学计算目标位姿对应的关节角度、设置关节角度使机器人移动到目标位置，并验证最终位姿与目标位姿的一致性，最后将机器人恢复到原始位姿。
注意：1. 保持急停按钮为打开状态；2. 确保机器人附近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()
Example3. 机器人导航
在运行本示例前，请确认已完成**地图引擎建图与定位**流程：
第一次运行导航前请先完成地图引擎内容
导航功能依赖地图引擎，必须先完成建图和定位才能运行本示例。
详细操作步骤请参考：常规操作 - 地图引擎建图与定位
本示例展示了GALBOT机器人的导航功能，执行本程序后，机器人在其左前方绕行一个正方形路径。通过初始化导航和机器人实例、检查安全状态后，让机器人执行正方形轨迹运动，每次向前移动0.5米并左转90度，重复4次形成一个正方形路径，并在完成后返回起始位置，整个过程包含了位姿获取、路径可达性检查、导航到目标点、目标到达检测以及异常处理等导航控制功能。
注意：1. 保持急停按钮为打开状态；2. 确保机器人附近2米范围无遮挡，避免手臂触碰障碍物出现危险。
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()
Example4. 传感器数据获取
本示例展示了GALBOT机器人的传感器数据采集功能。通过初始化机器人并启用左臂相机、深度相机、底座激光雷达和躯干IMU等传感器，程序获取并处理RGB图像、深度图像、激光雷达点云数据和IMU数据，对图像进行解码和可视化，将点云数据转换并保存为PCD格式文件，同时实现深度图与RGB图像融合生成彩色点云的功能。
注意：1. 保持急停按钮为打开状态；2. 确保机器人附近2米范围无遮挡，避免手臂触碰障碍物出现危险。
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()
Example5. 执行模拟VLA结果
本示例使用
get_synced_observation采集同步图像和关节状态，接收包含头部的 23 维绝对关节 action chunk，并通过高级实时关节控制接口set_joint_commands按控制频率逐帧快速执行。Galbot 官方支持通过主从臂采集示教数据，具体方法请参考 TM01 使用手册。采集生成的 MCAP 数据可通过 galbot-mcap2lerobot 转换为 LeRobot 数据集，用于后续模型训练。完成模型训练并部署推理服务后，可参考本示例完成同步观测获取和 action chunk 执行。示例动作数据保存在examples/g1/assets/tutorials/example5_vla.json，可由 Python 和 C++ 教程共用。当前示例只展示绝对关节控制；若模型输出末端 EE pose，请参考set_end_effector_command接口。
注意：1. 保持急停按钮为打开状态；2. 确保机器人附近2米范围无遮挡，避免手臂触碰障碍物出现危险。
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()
Example6. 综合任务：导航+感知+抓取+放置
本示例提供完整抓取放置应用的参考实现，串联感知、运动规划与控制和导航，展示预抓取、抬升、回撤、抓取与放置之间的导航，以及使用
move_line完成直线抓取和放置。
注意：1. 保持急停按钮为打开状态；2. 确保机器人附近2米范围无遮挡，避免手臂触碰障碍物出现危险。
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()