Getting Started Tutorial
The following simple examples will help you quickly understand the basic development and applications of the Galbot robot. To run all the examples below, you need to first download and install the Galbot SDK and configure it according to the Installation and Configuration instructions.
Example1. Basic Robot Control
This example demonstrates the basic control of the GALBOT robot. After executing this program, the robot's two arms will slowly raise to the top of its head, forming a heart-like gesture. After initializing the robot instance and checking the safety status, the robot's left and right arms execute a preset heart gesture sequence. The arms first move to the heart gesture position and hold for a period of time, then return to the original posture. The entire process includes basic control flows such as joint position acquisition, joint angle setting, blocking wait, and exception handling.
Note: 1. Keep the emergency stop button in the open state; 2. Ensure there are no obstructions within 2 meters around the robot to avoid dangerous contact between the arms and obstacles.
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. Arm Manipulation
This example demonstrates the arm manipulation of the GALBOT robot. After executing this program, the robot's left arm will slowly raise to a certain height and then slowly return to its original posture. The main functionalities include forward and inverse kinematics (IK/FK) solving, end-effector pose control, and joint angle setting. By obtaining the current left arm end-effector pose, using inverse kinematics to calculate the joint angles corresponding to the target pose, setting the joint angles to move the robot to the target position, verifying the consistency between the final pose and the target pose, and finally restoring the robot to its original pose.
Note: 1. Keep the emergency stop button in the open state; 2. Ensure there are no obstructions within 2 meters around the robot to avoid dangerous contact between the arms and obstacles.
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. Robot Navigation
Before running this example, please make sure you have completed the Map Engine Mapping & Localization process:
Complete the map engine section before running navigation for the first time
Navigation depends on the map engine. You must complete mapping and localization before running this example.
For detailed instructions, please refer to: Routine Operations - Map Engine Mapping & Localization
This example demonstrates the navigation functionality of the GALBOT robot. After executing this program, the robot will navigate a square path in front and to the left of its starting position. After initializing the navigation and robot instances and checking the safety status, the robot executes a square trajectory motion, moving forward 0.5 meters and turning left 90 degrees each time, repeating 4 times to form a square path, and returning to the starting position after completion. The entire process includes navigation control functions such as pose acquisition, path reachability checking, navigation to target points, target arrival detection, and exception handling.
Note: 1. Keep the emergency stop button in the open state; 2. Ensure there are no obstructions within 2 meters around the robot to avoid dangerous contact between the arms and obstacles.
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. Sensor Data Acquisition
This example demonstrates the sensor data acquisition functionality of the GALBOT robot. By initializing the robot and enabling sensors such as the left arm camera, depth camera, base lidar, and torso IMU, the program acquires and processes RGB images, depth images, lidar point cloud data, and IMU data. It decodes and visualizes the images, converts and saves point cloud data in PCD format, and implements the fusion of depth images with RGB images to generate colored point clouds.
Note: 1. Keep the emergency stop button in the open state; 2. Ensure there are no obstructions within 2 meters around the robot to avoid dangerous contact between the arms and obstacles.
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. Execute simulated VLA results
This example uses
get_synced_observationto collect a synchronized image and joint state, receives a 23-dimensional absolute-joint action chunk including the head, and rapidly streams it frame by frame through the high-level real-time joint control APIset_joint_commands. Galbot supports demonstration data collection with leader-follower arms; see the TM01 user manual. The collected MCAP data can be converted to the LeRobot dataset format with galbot-mcap2lerobot for subsequent model training. After deploying the trained model as an inference service, refer to this example for synchronized observation acquisition and action-chunk execution. The example action data is stored inexamples/g1/assets/tutorials/example5_vla.jsonfor reuse by both Python and C++ tutorials. This example covers absolute-joint control only; for model output expressed as end-effector EE poses, refer toset_end_effector_command.
Note: 1. Keep the emergency stop button in the open state; 2. Ensure there are no obstructions within 2 meters around the robot to avoid dangerous contact between the arms and obstacles.
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. Integrated Task: Navigation + Perception + Grasping + Placement
This example provides a complete pick-and-place application reference integrating perception, motion planning and control, and navigation. It includes pre-grasp, lift, retreat, navigation between picking and placement, and linear grasp/place motions with
move_line.
Note: 1. Keep the emergency stop button in the open state; 2. Ensure there are no obstructions within 2 meters around the robot to avoid dangerous contact between the arms and obstacles.
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()