C++ Examples

This file provides brief examples for the publicly available functions and types in the API, demonstrating how to use these interfaces.

Robot Joint Names (G3)

Joint Group List

Robot joint group names include: ["head", "left_arm", "right_arm", "leg", "left_gripper", "right_gripper", "left_suction_cup", "right_suction_cup"]

Detailed Information for Each Joint Group

Joint Group	English Name	Number of Joints	Joint Name List
Head	head	2	head_joint1, head_joint2
Legs	leg	5	leg_joint1, leg_joint2, leg_joint3, leg_joint4, leg_joint5
Left Arm	left_arm	7	left_arm_joint1, left_arm_joint2, left_arm_joint3, left_arm_joint4, left_arm_joint5, left_arm_joint6, left_arm_joint7
Right Arm	right_arm	7	right_arm_joint1, right_arm_joint2, right_arm_joint3, right_arm_joint4, right_arm_joint5, right_arm_joint6, right_arm_joint7
Left Gripper	left_gripper	1	left_gripper_joint1
Right Gripper	right_gripper	1	right_gripper_joint1
Left Suction Cup	left_suction_cup	1	left_suction_cup_joint1
Right Suction Cup	right_suction_cup	1	right_suction_cup_joint1

How To Use joint_groups Correctly

A "joint group" is the SDK control unit instead of a single joint. This grouping is used to ensure:

• Kinematic-chain consistency: arm/head/leg joints are validated and controlled as one chain.
• Deterministic command ordering: joint_groups are expanded to concrete joints in group order.
• Group-level validation: active/passive group constraints are checked before execution.

Usage rules:

1. Prefer joint_groups when controlling full chains (head/arm/leg/gripper/suction cup).
2. If joint_names is provided, it takes precedence over joint_groups.
3. If both joint_groups and joint_names are empty, SDK defaults to all active body joint groups.
4. To avoid hard-coded mistakes, call get_joint_group_names() and get_joint_names(true, groups) first.

Typical Group Scenarios (G3)

Joint Group	Typical Scenario
head	Head orientation / camera aiming
left_arm / right_arm	Arm reaching and manipulation
leg	Lower-body posture adjustment
left_gripper / right_gripper	Grasp width control
left_suction_cup / right_suction_cup	Vacuum pick and place

Sensor Types and Frames

For sensor data access (get_rgb_data, get_imu_data, get_lidar_data), use the SensorType enum to specify which sensor to query. Available SensorType enums are documented in: C++ API Reference > SensorType.

For sensor extrinsic calibration, there are two methods:

• get_sensor_extrinsic(): SDK internally maps frame IDs, pass SensorType directly
• get_transform(): Requires explicit frame names. The second column below lists frame IDs for each sensor.

SensorType	Frame ID
HEAD_LEFT_CAMERA	head_left_camera_color_optical_frame
HEAD_RIGHT_CAMERA	head_right_camera_color_optical_frame
LEFT_ARM_CAMERA	left_arm_camera_color_optical_frame
RIGHT_ARM_CAMERA	right_arm_camera_color_optical_frame
BASE_LIDAR	lidar_base_link
TORSO_IMU	imu_base_link
BASE_ULTRASONIC	— (no TF frame)
LEFT_FRONT_SURROUND_CAMERA	left_front_surround_color_optical_frame
RIGHT_FRONT_SURROUND_CAMERA	right_front_surround_color_optical_frame
LEFT_REAR_SURROUND_CAMERA	left_rear_surround_color_optical_frame
RIGHT_REAR_SURROUND_CAMERA	right_rear_surround_color_optical_frame

Note: Call get_frame_names() to get all available coordinate frames.

Class: GalbotRobot

Tips: If you get data immediately after program startup, the data may not be ready right away. You may sleep for a few seconds as appropriate.

Get Instance and Initialize (get_instance && init)

Applicable Scenarios: During program startup, get the robot singleton and complete SDK initialization. Must be called before using other APIs.

examples/cpp/galbot_robot/src/get_instance_init_example.cpp

#include <iostream>
#include <vector>
#include <array>
#include <memory>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);
    
    // Initialize singleton instance
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Check whether it is in the running state
    while (robot.is_running()) {
        // do something
        std::cout << "System is running." << std::endl;
        break;
    }

    // Register an exit callback (optional; automatically triggered when an exit signal is received)
    robot.register_exit_callback([]() {
        std::cout << "System is exiting..." << std::endl;
    });
    std::cout << "System exit callback registered successfully" << std::endl;

    // Send exit signal
    robot.request_shutdown();
    // Wait until entering shutdown state
    robot.wait_for_shutdown();
    // Release SDK related resources
    robot.destroy();
    std::cout << "Program finished" << std::endl;

    return 0;
}

Release SDK Resources and Exit Program

Applicable Scenarios: Release all resources occupied by the SDK before program exit, normally shut down the system.

examples/cpp/galbot_robot/src/get_instance_init_example.cpp

#include <iostream>
#include <vector>
#include <array>
#include <memory>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);
    
    // Initialize singleton instance
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Check whether it is in the running state
    while (robot.is_running()) {
        // do something
        std::cout << "System is running." << std::endl;
        break;
    }

    // Register an exit callback (optional; automatically triggered when an exit signal is received)
    robot.register_exit_callback([]() {
        std::cout << "System is exiting..." << std::endl;
    });
    std::cout << "System exit callback registered successfully" << std::endl;

    // Send exit signal
    robot.request_shutdown();
    // Wait until entering shutdown state
    robot.wait_for_shutdown();
    // Release SDK related resources
    robot.destroy();
    std::cout << "Program finished" << std::endl;

    return 0;
}

Log interface

Applicable Scenarios: Output logs using the SDK's built-in logging system, unifying log levels and formats.

examples/cpp/galbot_robot/src/logger_example.cpp

#include <iostream>

#include "galbot_robot.hpp"
#include "galbot_sdk_logger.hpp"

using namespace galbot::sdk;

int main() {
  // Get and initialize the GalbotRobot singleton
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  robot.init();
  // ==============================
  // 1. Initialize SDK logging
  // ==============================
  LoggerConfig logger_config;
  // Log generation path; if not filled, defaults to ~/galbot_sdk_log/user_log
  logger_config.path = "";
  // Log file name; if not filled, defaults to <process_name>_<current_time>_<pid>_<thread_id>.log
  logger_config.file_name = "";
  // Maximum bytes per single log file
  logger_config.file_max_size = 1024 * 1024 * 10; // 10MB
  // Maximum number of rotating log files; creates new file when full, retaining up to file_max_num files
  logger_config.file_max_num = 5;
  // Minimum log output level
  logger_config.level = LogLevel::DEBUG;
  // Whether to output to terminal; default is false
  logger_config.console_output = false;

  if (!init_galbot_sdk_logger(logger_config)) {
    std::cerr << "failed to init galbot sdk logger" << std::endl;
    return -1;
  } else {
    std::cout << "galbot sdk logger initialized successfully" << std::endl;
  }

  // ==============================
  // 2. Print logs at different levels
  // ==============================
  GALBOT_SDK_LOG_TRACE << "this is trace log";
  GALBOT_SDK_LOG_DEBUG << "this is debug log";
  GALBOT_SDK_LOG_INFO << "this is info log";
  GALBOT_SDK_LOG_WARN << "this is warning log";
  GALBOT_SDK_LOG_ERROR << "this is error log";
  GALBOT_SDK_LOG_CRITICAL << "this is critical log";

  // ==============================
  // 3. Supports chained << style
  // ==============================
  int robot_id = 3;
  GALBOT_SDK_LOG_INFO << "robot_id = " << robot_id;

  GALBOT_SDK_LOG_INFO << "example program finished";

  // Exit the system and release SDK and logger resources
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

Set Joint Positions (set_joint_positions)

Applicable Scenarios: Single-point movement, low-frequency position control tasks. This interface performs velocity-limited trajectory interpolation internally, making it suitable for one-time movement to target joint angles.

WARNING: This interface is NOT suitable for high-frequency joint control scenarios with model inference output! Each call to this interface produces a new trajectory interpolation, continuous calls will result in discontinuous motion and delay.

If you are working on a model inference scenario, please use set_joint_commands or set_joint_commands_batch to issue joint commands directly.

examples/cpp/galbot_robot/src/set_joint_positions_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get specified joint names; joint groups include ["leg", "head", "left_arm", "right_arm"]
    std::vector<std::string> joint_groups = {"head"};
    bool only_active_joint = true;  // Get active joints
    auto head_joint_names_vec =
        robot.get_joint_names(only_active_joint, joint_groups);
    std::cout << "Head joint names:" << std::endl;
    for (size_t i = 0; i < head_joint_names_vec.size(); ++i) {
        std::cout << i << ": " << head_joint_names_vec[i] << std::endl;
    }
    // Passing an empty array returns all joint group information by default
    std::vector<std::string> null_vec = {};
    auto all_joint_names_vec =
        robot.get_joint_names(only_active_joint, null_vec);
    std::cout << "All joint names:" << std::endl;
    for (size_t i = 0; i < all_joint_names_vec.size(); ++i) {
        std::cout << i << ": " << all_joint_names_vec[i] << std::endl;
    }

    // Joint groups to control; passing an empty array controls leg, head, left_arm, and right_arm by default
    joint_groups = {"head"};
    // Specific joints to control; if provided, this overrides the joint_groups parameter
    std::vector<std::string> joint_names = {};
    // Joint positions; head joint group contains two joints
    std::vector<double> joint_pos = {0.2, 0.2};
    // Whether to block and wait for joint angles to reach position or timeout
    bool is_block = true;
    // Maximum joint speed (rad/s)
    double speed_rad_s = 0.1;
    // Maximum wait time (seconds)
    double timeout_s = 10.0;

    // Set joint positions
    ControlStatus joint_execution_status =
        robot.set_joint_positions(joint_pos, joint_groups,joint_names, 
            is_block, speed_rad_s,timeout_s);

    if (joint_execution_status == ControlStatus::SUCCESS) {
        std::cout << "Joint command set successfully!" << std::endl;
    } else {
        std::cerr << "Failed to set joint command!" << std::endl;
    }

    // Query joint positions by group; empty array defaults to leg, head, dual-arm groups. Second parameter specifies joint names, which overrides joint_groups if provided.
    auto ret_positions = robot.get_joint_positions(joint_groups, {});
    for (auto position : ret_positions) {
        std::cout << "joint positions is " << position << std::endl;
    }

    // Use specific joint names for control; this parameter overrides joint_groups
    joint_names = {"head_joint1", "head_joint2"};
    joint_pos = {0.0, 0.0};

    // Set joint positions
    joint_execution_status = robot.set_joint_positions(joint_pos, joint_groups,joint_names, 
            is_block, speed_rad_s,timeout_s);

    if (joint_execution_status == ControlStatus::SUCCESS) {
        std::cout << "Joint command set successfully!" << std::endl;
    } else {
        std::cerr << "Failed to set joint command!" << std::endl;
    }

    // Query joint positions by group; empty array defaults to leg, head, dual-arm groups. Second parameter specifies joint names, which overrides joint_groups if provided.
    ret_positions = robot.get_joint_positions(joint_groups, {});
    for (auto position : ret_positions) {
        std::cout << "joint positions is " << position << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Set Gripper Command (set_gripper_command)

Applicable Scenarios: Control gripper opening and closing. Supports both position control and torque control modes.

examples/cpp/galbot_robot/src/set_gripper_command_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_gripper_state(std::shared_ptr<GripperState> gripper_state) {
    std::cout << "Timestamp (ns): " << gripper_state->timestamp_ns << std::endl;

    std::cout << " width "  << gripper_state->width << " velocity " << gripper_state->velocity
                << " effort " << gripper_state->effort << " is moving "
                << gripper_state->is_moving << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Gripper width (m)
    double width_m = 0.02;
    // Gripper speed (m/s)
    double velocity_mps = 0.05;
    // Gripper torque (N·m)
    double effort = 10;
    // Whether to block until execution completes
    bool is_blocking = true;
    // Set left gripper width to 0.02m, speed 0.05m/s, torque 10, block until execution completes
    ControlStatus gripper_execution_status =
        robot.set_gripper_command("left_gripper", width_m, velocity_mps,
                                        effort, is_blocking);

    if (gripper_execution_status == ControlStatus::SUCCESS) {
        std::cout << "Gripper command set successfully!" << std::endl;
    } else {
        std::cerr << "Failed to set gripper command!" << std::endl;
    }

    // Get gripper state
    JointStateMessage joint_state;
    auto gripper_state_ptr = robot.get_gripper_state("left_gripper");

    if (gripper_state_ptr == nullptr) {
        std::cerr << "get gripper state error" << std::endl;
    } else {
        print_gripper_state(gripper_state_ptr);
    }

    // Gripper width (m)
    width_m = 0.1;
    // Gripper speed (m/s)
    velocity_mps = 0.05;
    // Gripper torque (N·m)
    effort = 10;
    // Whether to block until execution completes
    is_blocking = false;
    // Set left gripper width to 0.1m, speed 0.05m/s, torque 10, block until execution completes
    gripper_execution_status =
        robot.set_gripper_command("left_gripper", width_m, velocity_mps,
                                        effort, is_blocking);

    if (gripper_execution_status == ControlStatus::SUCCESS) {
        std::cout << "Gripper command set successfully!" << std::endl;
    } else {
        std::cerr << "Failed to set gripper command!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Set Suction Cup Command (set_suction_cup_command)

Applicable Scenarios: Control suction cup to suck/release objects, get current suction cup status.

examples/cpp/galbot_robot/src/set_suction_cup_command_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Activate suction cup
    if (robot.set_suction_cup_command("right_suction_cup", true) == ControlStatus::SUCCESS) {
        std::cout << "Suction cup activation command sent successfully" << std::endl;
        
    } else {
        std::cerr << "Suction cup activation command failed to send!" << std::endl;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    // Deactivate suction cup
    if (robot.set_suction_cup_command("right_suction_cup", false) == ControlStatus::SUCCESS) {
        std::cout << "Suction cup deactivation command sent successfully" << std::endl;
        
    } else {
        std::cerr << "Suction cup deactivation command failed to send" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Stop Trajectory Execution (stop_trajectory_execution)

Applicable Scenarios: Stop currently executing joint trajectory and interrupt ongoing motion.

examples/cpp/galbot_robot/src/stop_trajectory_execution_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Stop trajectory execution
    while(true) {
        ControlStatus joint_execution_status =
            robot.stop_trajectory_execution();
        
        // Check execution results
        if (joint_execution_status == ControlStatus::SUCCESS) {
            std::cout << "Trajectory stop command sent successfully" << std::endl;
            break;
        } else {
            std::cerr << "Failed to send trajectory stop command, retrying..." << std::endl;
        }
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Set Trajectory (execute_joint_trajectory)

Applicable Scenarios: Execute a predefined joint-space trajectory with multiple waypoints. The SDK executes the entire trajectory according to time nodes.

examples/cpp/galbot_robot/src/execute_joint_trajectory_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

double g_target_time = 10;
double g_start_time = 10;

std::string trajectory_status_to_string(TrajectoryControlStatus status) {
  switch (status) {
  case TrajectoryControlStatus::INVALID_INPUT:
    return "INVALID_INPUT";
  case TrajectoryControlStatus::RUNNING:
    return "RUNNING";
  case TrajectoryControlStatus::COMPLETED:
    return "COMPLETED";
  case TrajectoryControlStatus::STOPPED_UNREACHED:
    return "STOPPED_UNREACHED";
  case TrajectoryControlStatus::ERROR:
    return "ERROR";
  case TrajectoryControlStatus::DATA_FETCH_FAILED:
    return "DATA_FETCH_FAILED";
  case TrajectoryControlStatus::STATUS_NUM:
    return "STATUS_NUM";
  default:
    return "UNKNOWN_STATUS";
  }
}

void wait_for_traj_reached(const std::vector<std::string> &joint_groups) {
    std::vector<TrajectoryControlStatus> traj_exec_states;
    int count = 0;
    bool all_reached = false;
    while (count++ < 150) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
        all_reached = true;
        traj_exec_states = GalbotRobot::get_instance(MachineType::G3)
                            .check_trajectory_execution_status(joint_groups);
        if (traj_exec_states.size() != joint_groups.size()) {
        std::cout << "traj_exec_states size != joint_groups size" << std::endl;
        }
        for (int i = 0; i < joint_groups.size(); ++i) {
        std::cout << joint_groups[i] << " exec state is "
                    << trajectory_status_to_string(traj_exec_states[i])
                    << std::endl;
        if (traj_exec_states[i] != TrajectoryControlStatus::COMPLETED) {
            all_reached = false;
        }
        }

        if (all_reached) {
            std::cout << "all reached" << std::endl;
            break;
        }
    }
    for (const auto &status : traj_exec_states) {
        std::cout << "done reached state is " << trajectory_status_to_string(status)
                << std::endl;
    }
}

std::vector<TrajectoryPoint>
generate_target_trajectory(int32_t joint_size, double ampl = 0.2,
                           double cycle = 10) {
  double amplitude = -ampl;
  double frequency = 1.0 / cycle;
  double phase = -M_PI / 2;
  double offset = amplitude;
  double dt = 0.004;
  int step = g_target_time / dt;

  std::vector<TrajectoryPoint> trajectory_data_vec;
  trajectory_data_vec.resize(step + 1);
  // Create a RobotCommand trajectory
  for (int i = 0; i <= step; ++i) {
    double t = i * dt;
    trajectory_data_vec[i].time_from_start_second = g_start_time + t;
    trajectory_data_vec[i].joint_command_vec.resize(joint_size);
    // Joint command
    for (int j = 0; j < joint_size; ++j) {
      trajectory_data_vec[i].joint_command_vec[j].position =
          offset + amplitude * std::sin(2 * M_PI * frequency * t + phase);
      trajectory_data_vec[i].joint_command_vec[j].velocity =
          amplitude * 2 * M_PI * frequency *
          std::cos(2 * M_PI * frequency * t + phase);
    }
  }

  return trajectory_data_vec;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Execute joint trajectory
    Trajectory trajectory;
    // Enter joint group name to control, including ["leg", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]
    trajectory.joint_groups = {"head"};
    // Fill this field to control specific joint angles, which will override joint_groups if provided
    trajectory.joint_names = {};
    // Generate trajectory
    trajectory.points = generate_target_trajectory(2);
    // Whether to block until trajectory execution completes; when false, you can use
    bool is_traj_block = false;

    // Wait for trajectory execution to complete; this function wraps check_trajectory_execution_status to check trajectory execution status
    wait_for_traj_reached(trajectory.joint_groups);

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

    return 0;
}

Set Joint Commands (set_joint_commands)

Applicable Scenarios: High-frequency joint control, such as model inference output (each inference step outputs one frame of joint commands), custom trajectory tracking. Issues joint commands directly to the low-level controller without extra interpolation.

examples/cpp/galbot_robot/src/set_joint_command_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_joint_positions(const std::vector<double>& positions) {
    std::cout << "Current joint positions:" << std::endl;
    for (size_t i = 0; i < positions.size(); ++i) {
        std::cout << "  joint " << i << ": " << positions[i] << " rad" << std::endl;
    }
    std::cout << std::endl;
}

std::string execution_status_to_string(ControlStatus status) {
    switch (status) {
        case ControlStatus::SUCCESS:
            return "SUCCESS";
        case ControlStatus::TIMEOUT:
            return "TIMEOUT";
        case ControlStatus::FAULT:
            return "FAULT";
        case ControlStatus::INVALID_INPUT:
            return "INVALID_INPUT";
        case ControlStatus::INIT_FAILED:
            return "INIT_FAILED";
        case ControlStatus::IN_PROGRESS:
            return "IN_PROGRESS";
        case ControlStatus::STOPPED_UNREACHED:
            return "STOPPED_UNREACHED";
        case ControlStatus::DATA_FETCH_FAILED:
            return "DATA_FETCH_FAILED";
        case ControlStatus::PUBLISH_FAIL:
            return "PUBLISH_FAIL";
        case ControlStatus::COMM_DISCONNECTED:
            return "COMM_DISCONNECTED";
        default:
            return "UNKNOWN_STATUS";
    }
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    std::vector<std::string> joint_groups = {"head"};
    std::vector<std::string> joint_names = {};

    std::vector<JointCommand> joint_commands(2);
    joint_commands[0].position = 0.2;
    joint_commands[1].position = 0.2;
    // Set head joint angles to 0.3 0.3
    ControlStatus execution_status =
        robot.set_joint_commands(joint_commands, joint_groups, joint_names);
    if (execution_status != ControlStatus::SUCCESS) {
        std::cout << "Joint angle command sending failed" << std::endl;
    } else {
        std::cout << "Joint angle command sent successfully" << std::endl;
    }

    // , waitexecute
    std::this_thread::sleep_for(std::chrono::milliseconds(10000));

    // Query joint positions
    auto ret_positions = robot.get_joint_positions(joint_groups, {});
    print_joint_positions(ret_positions);

    // Step 2: Return to initial position —— set both head joints to 0.0 rad
    joint_commands[0].position = 0.0;
    joint_commands[1].position = 0.0;

    // Set joint commands by joint names; setting joint_names overrides joint_groups
    joint_groups = {""};
    joint_names = {"head_joint1", "head_joint2"};
    execution_status =
        robot.set_joint_commands(joint_commands, joint_groups, joint_names);
    if (execution_status != ControlStatus::SUCCESS) {
        std::cout << "Joint angle command sending failed" << std::endl;
    } else {
        std::cout << "Joint angle command sent successfully" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    std::cout << "\nProgram exited" << std::endl;
    return 0;
}

Set Joint Commands in Batch Mode (set_joint_commands_batch)

Applicable Scenarios: Batch issue multiple future frames of joint commands, suitable for scenarios where motion prediction models output multiple steps at once, improving control frequency and continuity.

examples/cpp/galbot_robot/src/set_joint_commands_batch_example.cpp

#include <chrono>
#include <cmath>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    default:
      return "UNKNOWN_STATUS";
  }
}

std::vector<TrajectoryPoint> generate_batch_trajectory(int32_t joint_size, double ampl = 0.2, double cycle = 10,
                                                       int num_points = 10) {
  double amplitude = -ampl;
  double frequency = 1.0 / cycle;
  double phase = -M_PI / 2;
  double offset = amplitude;
  double dt = cycle / num_points;

  std::vector<TrajectoryPoint> trajectory_data_vec;
  trajectory_data_vec.resize(num_points);
  // Create batch trajectory points
  for (int i = 0; i < num_points; ++i) {
    double t = i * dt;
    trajectory_data_vec[i].time_from_start_second = t;
    trajectory_data_vec[i].joint_command_vec.resize(joint_size);
    // Joint command
    for (int j = 0; j < joint_size; ++j) {
      trajectory_data_vec[i].joint_command_vec[j].position =
          offset + amplitude * std::sin(2 * M_PI * frequency * t + phase);
      trajectory_data_vec[i].joint_command_vec[j].velocity =
          amplitude * 2 * M_PI * frequency * std::cos(2 * M_PI * frequency * t + phase);
      // trajectory_data_vec[i].joint_command_vec[j].acceleration = 0.0;
      // trajectory_data_vec[i].joint_command_vec[j].effort = 0.0;
    }
  }

  return trajectory_data_vec;
}

int main() {
  // Get object instance
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  // Initialize system
  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  // Program started, waiting for data
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));

  // Batch set joint commands
  Trajectory trajectory;
  // Enter joint group names to control, including ["leg", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]
  trajectory.joint_groups = {"head"};
  // Fill this field to control specific joint angles, which will override joint_groups if provided
  trajectory.joint_names = {};
  // Generate batched trajectory points (joint commands at multiple time points)
  trajectory.points = generate_batch_trajectory(2, 0.2, 10.0, 10);

  // Batch set joint commands (non-blocking, returns immediately)
  ControlStatus status = robot.set_joint_commands_batch(trajectory);
  std::cout << "Batch joint command status: " << control_status_to_string(status) << std::endl;

  if (status == ControlStatus::SUCCESS) {
    std::cout << "Batch commands submitted, executing in background (non-blocking)" << std::endl;
  } else {
    std::cerr << "Failed to submit batch commands!" << std::endl;
  }

  // Wait for a while to let the command execute
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // Exit system and release SDK resources
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

Publish Raw Target Directly (PublishTarget)

Applicable Scenarios: Advanced users construct a SingoriXTarget directly and publish it through the WBCS publish channel. Suitable for high-frequency raw target streaming and one-shot dispatch of mixed joint/task targets.

examples/cpp/galbot_robot/src/publish_target_example.cpp

/**
 * @file publish_target_example.cpp
 * @brief G3 PublishTarget menu example for SDK mirror SingoriXTarget.
 *
 * This example shows how to construct `SingoriXTarget` directly at the SDK layer
 * and send it to the low-level WBCS through `PublishTarget()`.
 *
 * 1. Joint-space commands are written into `target_group_trajectory_map`
 *    - Typical for head / arm style joint-space control
 *    - Each group corresponds to one `TargetGroupTrajectory`
 *    - Each trajectory point is described by `GroupCommand + JointCommand`
 *
 * 2. Chassis pose / twist style task-space commands are written into
 *    `target_task_trajectory_map`
 *    - Typical for chassis pose / chassis twist control
 *    - Each task corresponds to one `TargetTaskTrajectory`
 *    - Each trajectory point is described by `TaskCommand + FrameTriad`
 *
 * 3. One `SingoriXTarget` can contain both joint trajectory and task trajectory
 *    - This supports one-shot dispatch for whole-body / mixed control
 *
 * 4. The current SDK does not automatically switch the chassis controller when
 *    calling `PublishTarget()` / `RequestTarget()`
 *    - Therefore this example explicitly calls
 *      `switch_controller(G3ControllerName::CHASSIS_POSE_CTRL)` or
 *      `switch_controller(G3ControllerName::CHASSIS_TWIST_CTRL)` before
 *      chassis pose / twist / mixed scenes
 *
 * 5. For the base twist scene, this example automatically sends a zero-twist
 *    target after the configured duration to stop the chassis.
 */

#include <chrono>
#include <cstdint>
#include <iostream>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr const char* kChassisTaskName = "chassis";
constexpr const char* kChassisSubtaskPose = "chassis_pose";
constexpr const char* kChassisSubtaskTwist = "chassis_twist";

int64_t now_ns() {
  return std::chrono::duration_cast<std::chrono::nanoseconds>(
             std::chrono::system_clock::now().time_since_epoch())
      .count();
}

const char* to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

Quaternion yaw_to_quaternion(double yaw) {
  Quaternion q;
  q.z = std::sin(yaw * 0.5);
  q.w = std::cos(yaw * 0.5);
  return q;
}

SingoriXTarget make_empty_target() {
  SingoriXTarget target;
  target.header.timestamp_ns = now_ns();
  target.header.frame_id = "base_link";
  return target;
}

TargetConfig make_group_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_DEFAULT;
  config.target_type = TARGET_TYPE_PROVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_DEFAULT;
  config.target_priority = 1;
  return config;
}

TargetConfig make_pose_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_FRAME_POSE;
  // config.target_type = TARGET_TYPE_PROVERRIDE;
  config.target_type = TARGET_TYPE_OVERRIDE; // single point, override
  config.target_sampling = TargetSampling::TARGET_SAMPLING_LINEAR_INTERPOLATE;
  config.target_priority = 1;
  return config;
}

TargetConfig make_twist_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_FRAME_TWIST;
  config.target_type = TARGET_TYPE_OVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_DIRECT_PASS;
  config.target_priority = 1;
  return config;
}

SingoriXTarget build_joint_target(const std::string& group_name,
                                  const std::vector<std::string>& joint_names,
                                  const std::vector<double>& positions,
                                  double time_from_start_s) {
  SingoriXTarget target = make_empty_target();
  auto& group_traj = target.target_group_trajectory_map[group_name];
  group_traj.target_config = make_group_target_config();
  group_traj.joint_names = joint_names;

  GroupCommand command;
  command.time_from_start_s = time_from_start_s;
  command.joint_commands.resize(joint_names.size());
  for (size_t i = 0; i < joint_names.size(); ++i) {
    command.joint_commands[i].position = positions[i];
    command.joint_commands[i].velocity = 0.0;
    command.joint_commands[i].acceleration = 0.0;
    command.joint_commands[i].effort = 0.0;
  }
  group_traj.group_commands.push_back(command);
  return target;
}

SingoriXTarget build_chassis_pose_target(double x,
                                         double y,
                                         double yaw,
                                         double time_from_start_s,
                                         const std::string& frame_id = "rel(0)", // "rel(0)" means relative to the current base_link pose
                                         const std::string& reference_frame_id = "odom") {
  SingoriXTarget target = make_empty_target();
  auto& task_traj = target.target_task_trajectory_map[kChassisTaskName];
  task_traj.target_config = make_pose_target_config();
  task_traj.group_names = {G3JointGroup::CHASSIS};
  task_traj.subtask_names = {std::string(kChassisSubtaskPose) + "_" + std::to_string(now_ns())};

  TaskCommand command;
  command.time_from_start_s = time_from_start_s;

  FrameTriad triad;
  triad.header.timestamp_ns = now_ns();
  triad.header.frame_id = frame_id;
  triad.body_frame_id = "base_link";
  triad.reference_frame_id = reference_frame_id;
  triad.pose = Pose();
  triad.pose->position.x = x;
  triad.pose->position.y = y;
  triad.pose->position.z = 0.0;
  triad.pose->orientation = yaw_to_quaternion(yaw);
  command.subtask_commands.push_back(triad);

  task_traj.task_commands.push_back(command);
  return target;
}

SingoriXTarget build_chassis_twist_target(double vx,
                                          double vy,
                                          double wz,
                                          double time_from_start_s) {
  SingoriXTarget target = make_empty_target();
  auto& task_traj = target.target_task_trajectory_map[kChassisTaskName];
  task_traj.target_config = make_twist_target_config();
  task_traj.group_names = {G3JointGroup::CHASSIS};
  task_traj.subtask_names = {std::string(kChassisSubtaskTwist) + "_" + std::to_string(now_ns())};

  TaskCommand command;
  command.time_from_start_s = time_from_start_s;

  FrameTriad triad;
  triad.header.timestamp_ns = now_ns();
  triad.header.frame_id = "base_link";
  triad.body_frame_id = "base_link";
  triad.reference_frame_id = "base_link";
  triad.twist = Twist();
  triad.twist->linear = Vector3{vx, vy, 0.0};
  triad.twist->angular = Vector3{0.0, 0.0, wz};
  command.subtask_commands.push_back(triad);

  task_traj.task_commands.push_back(command);
  return target;
}

SingoriXTarget build_stop_twist_target() {
  return build_chassis_twist_target(0.0, 0.0, 0.0, 0.1);
}

SingoriXTarget merge_targets(const std::vector<SingoriXTarget>& targets) {
  SingoriXTarget merged = make_empty_target();
  for (const auto& target : targets) {
    for (const auto& [group_name, group_traj] : target.target_group_trajectory_map) {
      merged.target_group_trajectory_map[group_name] = group_traj;
    }
    for (const auto& [task_name, task_traj] : target.target_task_trajectory_map) {
      merged.target_task_trajectory_map[task_name] = task_traj;
    }
  }
  return merged;
}

void print_menu() {
  std::cout << "\nAvailable commands:\n"
            << "  joint        - publish a joint-only head target\n"
            << "  base_pose    - publish a chassis pose target\n"
            << "  base_twist   - publish a chassis twist target with auto stop\n"
            << "  mixed_pose   - publish head + left_arm + chassis pose in one target\n"
            << "  mixed_twist  - publish head + left_arm + chassis twist in one target\n"
            << "  quit         - exit example\n"
            << std::endl;
}

ControlStatus ensure_controller(GalbotRobot& robot, const std::string& controller_name) {
  const auto status = robot.switch_controller(controller_name);
  std::cout << "switch_controller(" << controller_name << "): " << to_string(status) << std::endl;
  return status;
}

void print_result(const std::string& scene_name, ControlStatus status) {
  std::cout << scene_name << " PublishTarget status: " << to_string(status) << std::endl;
}

ControlStatus run_twist_scene(GalbotRobot& robot,
                              const std::string& scene_name,
                              const SingoriXTarget& target,
                              double twist_duration_s) {
  std::cout << scene_name << ": start moving for " << twist_duration_s << " seconds" << std::endl;
  const auto motion_status = robot.PublishTarget(target);
  print_result(scene_name, motion_status);
  if (motion_status != ControlStatus::SUCCESS) {
    return motion_status;
  }

  std::this_thread::sleep_for(std::chrono::duration<double>(twist_duration_s));
  std::cout << scene_name << ": send stop twist target" << std::endl;
  const auto stop_status = robot.PublishTarget(build_stop_twist_target());
  std::cout << scene_name << " stop status: " << to_string(stop_status) << std::endl;
  return stop_status;
}

}  // namespace

int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  if (!robot.init()) {
    std::cerr << "robot init failed" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::seconds(2));

  const auto head_joint_names = robot.get_joint_names(true, {G3JointGroup::HEAD});
  const auto left_arm_joint_names = robot.get_joint_names(true, {G3JointGroup::LEFT_ARM});
  if (head_joint_names.empty() || left_arm_joint_names.empty()) {
    std::cerr << "failed to fetch active head/left_arm joints" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  const std::vector<std::string> head_single_joint = {head_joint_names.front()};
  const std::vector<std::string> arm_single_joint = {left_arm_joint_names.front()};
  constexpr double kJointTimeS = 3.0;
  constexpr double kPoseTimeS = 4.0;
  constexpr double kTwistCommandTimeS = 0.2;
  constexpr double kTwistDurationS = 2.0;

  print_menu();

  std::string command;
  while (true) {
    std::cout << "Enter command: ";
    if (!(std::cin >> command)) {
      break;
    }
    if (command == "quit") {
      break;
    }

    if (command == "joint") {
      const auto target = build_joint_target(G3JointGroup::HEAD, head_single_joint, {0.2}, kJointTimeS);
      print_result("joint", robot.PublishTarget(target));
      continue;
    }

    if (command == "base_pose") {
      if (ensure_controller(robot, G3ControllerName::CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_chassis_pose_target(0.2, 0.0, 0.0, kPoseTimeS);
      print_result("base_pose", robot.PublishTarget(target));
      continue;
    }

    if (command == "base_twist") {
      if (ensure_controller(robot, G3ControllerName::CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_chassis_twist_target(0.05, 0.0, 0.0, kTwistCommandTimeS);
      run_twist_scene(robot, "base_twist", target, kTwistDurationS);
      continue;
    }

    if (command == "mixed_pose") {
      if (ensure_controller(robot, G3ControllerName::CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(G3JointGroup::HEAD, head_single_joint, {0.15}, kJointTimeS),
          build_chassis_pose_target(0.1, 0.0, 0.0, kPoseTimeS),
      });
      print_result("mixed_pose", robot.PublishTarget(target));
      continue;
    }

    if (command == "mixed_twist") {
      if (ensure_controller(robot, G3ControllerName::CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(G3JointGroup::HEAD, head_single_joint, {-0.15}, kJointTimeS),
          build_chassis_twist_target(0.05, 0.0, 0.0, kTwistCommandTimeS),
      });
      run_twist_scene(robot, "mixed_twist", target, kTwistDurationS);
      continue;
    }

    std::cout << "Unknown command: " << command << std::endl;
    print_menu();
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

Request Raw Target Directly (RequestTarget)

Applicable Scenarios: Advanced users construct a SingoriXTarget directly and send it through the WBCS request channel. Suitable when the caller needs the service-side error response for a raw target dispatch.

examples/cpp/galbot_robot/src/request_target_example.cpp

/**
 * @file request_target_example.cpp
 * @brief G3 RequestTarget menu example for SDK mirror SingoriXTarget.
 *
 * This example shows how to construct `SingoriXTarget` directly at the SDK layer
 * and send it to the low-level WBCS through `RequestTarget()`.
 *
 * 1. Joint-space commands are written into `target_group_trajectory_map`
 *    - Typical for head / arm style joint-space control
 *    - Each group corresponds to one `TargetGroupTrajectory`
 *    - Each trajectory point is described by `GroupCommand + JointCommand`
 *
 * 2. Chassis pose / twist style task-space commands are written into
 *    `target_task_trajectory_map`
 *    - Typical for chassis pose / chassis twist control
 *    - Each task corresponds to one `TargetTaskTrajectory`
 *    - Each trajectory point is described by `TaskCommand + FrameTriad`
 *
 * 3. One `SingoriXTarget` can contain both joint trajectory and task trajectory
 *    - This supports one-shot dispatch for whole-body / mixed control
 *
 * 4. The current SDK does not automatically switch the chassis controller when
 *    calling `PublishTarget()` / `RequestTarget()`
 *    - Therefore this example explicitly calls
 *      `switch_controller(G3ControllerName::CHASSIS_POSE_CTRL)` or
 *      `switch_controller(G3ControllerName::CHASSIS_TWIST_CTRL)` before
 *      chassis pose / twist / mixed scenes
 *
 * 5. For the base twist scene, this example automatically sends a zero-twist
 *    target after the configured duration to stop the chassis.
 */

#include <chrono>
#include <cstdint>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr const char* kChassisTaskName = "chassis";
constexpr const char* kChassisSubtaskPose = "chassis_pose";
constexpr const char* kChassisSubtaskTwist = "chassis_twist";

int64_t now_ns() {
  return std::chrono::duration_cast<std::chrono::nanoseconds>(
             std::chrono::system_clock::now().time_since_epoch())
      .count();
}

Quaternion yaw_to_quaternion(double yaw) {
  Quaternion q;
  q.z = std::sin(yaw * 0.5);
  q.w = std::cos(yaw * 0.5);
  return q;
}

SingoriXTarget make_empty_target() {
  SingoriXTarget target;
  target.header.timestamp_ns = now_ns();
  target.header.frame_id = "base_link";
  return target;
}

TargetConfig make_group_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_DEFAULT;
  config.target_type = TARGET_TYPE_PROVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_DEFAULT;
  config.target_priority = 1;
  return config;
}

TargetConfig make_pose_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_FRAME_POSE;
  config.target_type = TARGET_TYPE_OVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_LINEAR_INTERPOLATE;
  config.target_priority = 1;
  return config;
}

TargetConfig make_twist_target_config() {
  TargetConfig config;
  config.target_data = TARGET_DATA_FRAME_TWIST;
  config.target_type = TARGET_TYPE_OVERRIDE;
  config.target_sampling = TargetSampling::TARGET_SAMPLING_DIRECT_PASS;
  config.target_priority = 1;
  return config;
}

SingoriXTarget build_joint_target(const std::string& group_name,
                                  const std::vector<std::string>& joint_names,
                                  const std::vector<double>& positions,
                                  double time_from_start_s) {
  SingoriXTarget target = make_empty_target();
  auto& group_traj = target.target_group_trajectory_map[group_name];
  group_traj.target_config = make_group_target_config();
  group_traj.joint_names = joint_names;

  GroupCommand command;
  command.time_from_start_s = time_from_start_s;
  command.joint_commands.resize(joint_names.size());
  for (size_t i = 0; i < joint_names.size(); ++i) {
    command.joint_commands[i].position = positions[i];
    command.joint_commands[i].velocity = 0.0;
    command.joint_commands[i].acceleration = 0.0;
    command.joint_commands[i].effort = 0.0;
  }
  group_traj.group_commands.push_back(command);
  return target;
}

SingoriXTarget build_chassis_pose_target(double x,
                                         double y,
                                         double yaw,
                                         double time_from_start_s,
                                         const std::string& frame_id = "rel(0)",
                                         const std::string& reference_frame_id = "odom") {
  SingoriXTarget target = make_empty_target();
  auto& task_traj = target.target_task_trajectory_map[kChassisTaskName];
  task_traj.target_config = make_pose_target_config();
  task_traj.group_names = {G3JointGroup::CHASSIS};
  task_traj.subtask_names = {std::string(kChassisSubtaskPose) + "_" + std::to_string(now_ns())};

  TaskCommand command;
  command.time_from_start_s = time_from_start_s;

  FrameTriad triad;
  triad.header.timestamp_ns = now_ns();
  triad.header.frame_id = frame_id;
  triad.body_frame_id = "base_link";
  triad.reference_frame_id = reference_frame_id;
  triad.pose = Pose();
  triad.pose->position.x = x;
  triad.pose->position.y = y;
  triad.pose->position.z = 0.0;
  triad.pose->orientation = yaw_to_quaternion(yaw);
  command.subtask_commands.push_back(triad);

  task_traj.task_commands.push_back(command);
  return target;
}

SingoriXTarget build_chassis_twist_target(double vx,
                                          double vy,
                                          double wz,
                                          double time_from_start_s) {
  SingoriXTarget target = make_empty_target();
  auto& task_traj = target.target_task_trajectory_map[kChassisTaskName];
  task_traj.target_config = make_twist_target_config();
  task_traj.group_names = {G3JointGroup::CHASSIS};
  task_traj.subtask_names = {std::string(kChassisSubtaskTwist) + "_" + std::to_string(now_ns())};

  TaskCommand command;
  command.time_from_start_s = time_from_start_s;

  FrameTriad triad;
  triad.header.timestamp_ns = now_ns();
  triad.header.frame_id = "base_link";
  triad.body_frame_id = "base_link";
  triad.reference_frame_id = "base_link";
  triad.twist = Twist();
  triad.twist->linear = Vector3{vx, vy, 0.0};
  triad.twist->angular = Vector3{0.0, 0.0, wz};
  command.subtask_commands.push_back(triad);

  task_traj.task_commands.push_back(command);
  return target;
}

SingoriXTarget build_stop_twist_target() {
  return build_chassis_twist_target(0.0, 0.0, 0.0, 0.1);
}

SingoriXTarget merge_targets(const std::vector<SingoriXTarget>& targets) {
  SingoriXTarget merged = make_empty_target();
  for (const auto& target : targets) {
    for (const auto& [group_name, group_traj] : target.target_group_trajectory_map) {
      merged.target_group_trajectory_map[group_name] = group_traj;
    }
    for (const auto& [task_name, task_traj] : target.target_task_trajectory_map) {
      merged.target_task_trajectory_map[task_name] = task_traj;
    }
  }
  return merged;
}

void print_menu() {
  std::cout << "\nAvailable commands:\n"
            << "  joint        - request a joint-only head target\n"
            << "  base_pose    - request a chassis pose target\n"
            << "  base_twist   - request a chassis twist target with auto stop\n"
            << "  mixed_pose   - request head + left_arm + chassis pose in one target\n"
            << "  mixed_twist  - request head + left_arm + chassis twist in one target\n"
            << "  quit         - exit example\n"
            << std::endl;
}

void print_error_info(const std::string& scene_name, const std::shared_ptr<ErrorInfo>& error_info) {
  if (error_info == nullptr) {
    std::cout << scene_name << ": RequestTarget returned nullptr" << std::endl;
    return;
  }
  if (error_info->error_vec.empty()) {
    std::cout << scene_name << ": RequestTarget success, service returned no errors" << std::endl;
    return;
  }

  std::cout << scene_name << ": RequestTarget returned " << error_info->error_vec.size() << " error entries:"
            << std::endl;
  for (const auto& error : error_info->error_vec) {
    std::cout << "  component=" << error.commpent << ", code=" << error.error_code
              << ", description=" << error.description << std::endl;
  }
}

ControlStatus ensure_controller(GalbotRobot& robot, const std::string& controller_name) {
  const auto status = robot.switch_controller(controller_name);
  std::cout << "switch_controller(" << controller_name << "): ";
  std::cout << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << std::endl;
  return status;
}

void run_twist_scene(GalbotRobot& robot,
                     const std::string& scene_name,
                     const SingoriXTarget& target,
                     double twist_duration_s) {
  std::cout << scene_name << ": start moving for " << twist_duration_s << " seconds" << std::endl;
  print_error_info(scene_name, robot.RequestTarget(target));
  std::this_thread::sleep_for(std::chrono::duration<double>(twist_duration_s));
  std::cout << scene_name << ": send stop twist target" << std::endl;
  print_error_info(scene_name + "_stop", robot.RequestTarget(build_stop_twist_target()));
}

}  // namespace

int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  if (!robot.init()) {
    std::cerr << "robot init failed" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::seconds(2));

  const auto head_joint_names = robot.get_joint_names(true, {G3JointGroup::HEAD});
  if (head_joint_names.empty()) {
    std::cerr << "failed to fetch active head joints" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  const std::vector<std::string> head_single_joint = {head_joint_names.front()};
  constexpr double kJointTimeS = 3.0;
  constexpr double kPoseTimeS = 4.0;
  constexpr double kTwistCommandTimeS = 0.2;
  constexpr double kTwistDurationS = 2.0;

  print_menu();

  std::string command;
  while (true) {
    std::cout << "Enter command: ";
    if (!(std::cin >> command)) {
      break;
    }
    if (command == "quit") {
      break;
    }

    if (command == "joint") {
      const auto target = build_joint_target(G3JointGroup::HEAD, head_single_joint, {0.2}, kJointTimeS);
      print_error_info("joint", robot.RequestTarget(target));
      continue;
    }

    if (command == "base_pose") {
      if (ensure_controller(robot, G3ControllerName::CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_chassis_pose_target(0.2, 0.0, 0.0, kPoseTimeS);
      print_error_info("base_pose", robot.RequestTarget(target));
      continue;
    }

    if (command == "base_twist") {
      if (ensure_controller(robot, G3ControllerName::CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_chassis_twist_target(0.05, 0.0, 0.0, kTwistCommandTimeS);
      run_twist_scene(robot, "base_twist", target, kTwistDurationS);
      continue;
    }

    if (command == "mixed_pose") {
      if (ensure_controller(robot, G3ControllerName::CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(G3JointGroup::HEAD, head_single_joint, {0.15}, kJointTimeS),
          build_chassis_pose_target(0.1, 0.0, 0.0, kPoseTimeS),
      });
      print_error_info("mixed_pose", robot.RequestTarget(target));
      continue;
    }

    if (command == "mixed_twist") {
      if (ensure_controller(robot, G3ControllerName::CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(G3JointGroup::HEAD, head_single_joint, {-0.15}, kJointTimeS),
          build_chassis_twist_target(0.05, 0.0, 0.0, kTwistCommandTimeS),
      });
      run_twist_scene(robot, "mixed_twist", target, kTwistDurationS);
      continue;
    }

    std::cout << "Unknown command: " << command << std::endl;
    print_menu();
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

Stop Base Motion (stop_base)

Applicable Scenarios: Immediately stop all motion of the base, used for emergency stop.

examples/cpp/galbot_robot/src/stop_base_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Stop chassis motion
    while (true) {
        ControlStatus status = robot.stop_base();
        if (status == ControlStatus::SUCCESS) {
            std::cout << "Chassis motion has been stopped successfully!" << std::endl;
            break;
        } else {
            std::cerr << "Chassis stop motion failed, retrying..." << std::endl;
        }
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get And Set Base Velocity (get_base_velocity && set_base_velocity)

duration_s=0 publishes once; a positive value blocks the calling thread and publishes at 10 Hz for the window measured after the first successful send. Expiry only ends publishing; it does not call stop_base(). Negative, non-finite, or unrepresentable durations return INVALID_INPUT. Actual stopping depends on the watchdog; SUCCESS does not confirm a stopped base. Do not issue concurrent base commands during publishing: they do not cancel the loop. Use single-publish mode for caller-controlled stopping.

Applicable Scenarios: Control and get linear and angular velocity of the mobile base, used for navigation or remote control.

examples/cpp/galbot_robot/src/get_set_base_velocity_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_base_velocity(const std::shared_ptr<BaseVelocityInfo>& base_velocity_info) {
    if (!base_velocity_info) {
        std::cerr << "Base velocity data is empty" << std::endl;
        return;
    }

    // Print linear and angular velocity in the same format as the standalone example.
    const auto& linear = base_velocity_info->linear_velocity;
    std::cout << "Linear velocity (m/s): "
              << "vx=" << linear[0] << ", vy=" << linear[1] << ", vz=" << linear[2] << std::endl;

    const auto& angular = base_velocity_info->angular_velocity;
    std::cout << "Angular velocity (rad/s): "
              << "wx=" << angular[0] << ", wy=" << angular[1] << ", wz=" << angular[2] << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Read base velocity before issuing the motion command.
    auto base_velocity_before = robot.get_base_velocity();
    if (base_velocity_before) {
        std::cout << "Base velocity before command:" << std::endl;
        print_base_velocity(base_velocity_before);
    } else {
        std::cerr << "Failed to get base velocity before command." << std::endl;
    }

    // Please confirm the surrounding environment before chassis testing
    // Set chassis speed, linear_velocity first two fields are x and y velocities, angular_velocity third field is z rotation speed
    std::array<double, 3> linear_velocity = {0.2, 0.0, 0.0};    // 0.2 m/s
    std::array<double, 3> angular_velocity = {0.0, 0.0, 0.0};    // 0.0 rad/s
    double duration_s = 3.0;  // Block while publishing at 10 Hz; no stop is sent on expiry.

    if (robot.set_base_velocity(linear_velocity, angular_velocity, duration_s) == ControlStatus::SUCCESS) {
        std::cout << "Velocity publishing completed after " << duration_s
                  << " seconds; stopping depends on the watchdog." << std::endl;
    } else {
        std::cerr << "Set chassis speed failed." << std::endl;
    }

    // Observe immediately: publishing completion does not imply a stopped base.
    auto base_velocity_after = robot.get_base_velocity();
    if (base_velocity_after) {
        std::cout << "Base velocity after command:" << std::endl;
        print_base_velocity(base_velocity_after);
    } else {
        std::cerr << "Failed to get base velocity after command." << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Joint States (get_joint_states)

Applicable Scenarios: Get complete state information of all joints, including position, velocity, current, etc. Suitable for algorithms requiring full joint information (such as dynamics calculation, state estimation). The following example uses the left arm.

examples/cpp/galbot_robot/src/get_joint_states_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_joint_states(const std::vector<JointState>& joint_states) {
    for (const auto& states : joint_states) {
        std::cout << "--- Joint State ---" << std::endl;
        std::cout << "Joint Name:   " << states.joint_name   << std::endl;
        std::cout << "Position:     " << states.position     << " rad" << std::endl;
        std::cout << "Velocity:     " << states.velocity     << " rad/s" << std::endl;
        std::cout << "Acceleration: " << states.acceleration << " rad/s^2" << std::endl;
        std::cout << "Effort:       " << states.effort       << " Nm" << std::endl;
        std::cout << "Current:      " << states.current      << " A" << std::endl;
        std::cout << "Timestamp:    " << states.timestamp_ns << " ns" << std::endl;
        std::cout << "------------------" << std::endl;
    }
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get joint states by joint group names; returns all joints if empty
    std::vector<std::string> joint_groups = {"left_arm"};
    auto ret_states = robot.get_joint_states(joint_groups, {});
    if (ret_states.empty()) {
        std::cout << "No joint states returned for the specified joint groups" << std::endl;
    } else {
        print_joint_states(ret_states);
    }

    // Get specified joint states; if provided, overrides joint group input
    std::vector<std::string> joint_names = {"left_arm_joint1", "left_arm_joint2"};
    ret_states = robot.get_joint_states(joint_groups, joint_names);
    if (ret_states.empty()) {
        std::cout << "No joint states returned for the specified joint names" << std::endl;
    } else {
        print_joint_states(ret_states);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Joint Positions (get_joint_positions)

Applicable Scenarios: Only get current joint positions. Use this interface when you only need joint position information, it's more lightweight than get_joint_states.

examples/cpp/galbot_robot/src/get_joint_positions_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_joint_positions(const std::vector<double>& positions) {
    for (const auto& pos : positions) {
        std::cout << "joint positions is " << pos << std::endl;
    }
    std::cout << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get joint positions by joint group names; returns all joints if empty
    std::vector<std::string> joint_groups = {"left_arm"};
    auto ret_positions = robot.get_joint_positions(joint_groups, {});
    std::cout << "Left arm joint positions:" << std::endl;
    print_joint_positions(ret_positions);

    // Get specified joint positions; if provided, overrides joint group input
    std::vector<std::string> joint_names = {"left_arm_joint1", "left_arm_joint2"};
    ret_positions = robot.get_joint_positions({joint_groups}, joint_names);
    std::cout << "Positions of left arm joint1 and joint2:" << std::endl;
    print_joint_positions(ret_positions);

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Joint Names (get_joint_names)

Applicable Scenarios: Get a list of all joint names of the robot, used for iterating through joints or generating configuration files.

examples/cpp/galbot_robot/src/get_joint_names_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get specified joint names; joint groups include ["leg", "head", "left_arm", "right_arm"]
    std::vector<std::string> joint_groups = {"head"};
    bool only_active_joint = true;  // Get active joints
    auto head_joint_names_vec =
        robot.get_joint_names(only_active_joint, joint_groups);
    std::cout << "Head joint names:" << std::endl;
    for (size_t i = 0; i < head_joint_names_vec.size(); ++i) {
        std::cout << i << ": " << head_joint_names_vec[i] << std::endl;
    }

    // Passing an empty array returns all joint group information by default
    std::vector<std::string> null_vec = {};
    auto all_joint_names_vec =
        robot.get_joint_names(only_active_joint, null_vec);
    std::cout << "All joint names:" << std::endl;
    for (size_t i = 0; i < all_joint_names_vec.size(); ++i) {
        std::cout << i << ": " << all_joint_names_vec[i] << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Gripper State (get_gripper_state)

Applicable Scenarios: Get current gripper position and status, determine if gripper is open or closed.

examples/cpp/galbot_robot/src/get_gripper_state_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_gripper_state(std::shared_ptr<GripperState> gripper_state) {
    std::cout << "Timestamp (ns): " << gripper_state->timestamp_ns << std::endl;

    std::cout << " width "  << gripper_state->width << " velocity " << gripper_state->velocity
                << " effort " << gripper_state->effort << " is moving "
                << gripper_state->is_moving << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get gripper state
    auto gripper_state_ptr = robot.get_gripper_state("left_gripper");

    if (gripper_state_ptr == nullptr) {
        std::cerr << "get gripper state error" << std::endl;
    } else {
        std::cout << "Left gripper state:" << std::endl;
        print_gripper_state(gripper_state_ptr);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Suction Cup State (get_suction_cup_state)

Applicable Scenarios: Get whether the suction cup is currently suctioned and whether it successfully detected an object.

examples/cpp/galbot_robot/src/get_suction_cup_state_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_suction_cup_state(std::shared_ptr<SuctionCupState> suction_cup_state) {
    std::cout << "Timestamp (ns): " << suction_cup_state->timestamp_ns << std::endl;
    std::cout << "Activation: " << suction_cup_state->activation << std::endl;
    std::cout << "Pressure: " << suction_cup_state->pressure << " Pa" << std::endl;
    std::cout << "Action State: " << int(suction_cup_state->action_state) << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get suction cup state
    auto suction_cup_state_ptr = robot.get_suction_cup_state("right_suction_cup");

    if (suction_cup_state_ptr == nullptr) {
        std::cerr << "get suction cup state error" << std::endl;
    } else {
        std::cout << "Right suction cup status:" << std::endl;
        print_suction_cup_state(suction_cup_state_ptr);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Transform (get_transform)

Applicable Scenarios: Get the transformation (pose) between two coordinate frames, used in robotic arm grasping, visual localization and other scenarios.

examples/cpp/galbot_robot/src/get_transform_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_pose_vec(const std::vector<double> &pose_vec) {
    // Output pose_vec
    std::cout << "pose_vec = [";
    for (size_t i = 0; i < pose_vec.size(); ++i) {
        std::cout << pose_vec[i];
        if (i + 1 < pose_vec.size())
        std::cout << ", ";
    }
    std::cout << "]" << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get coordinate transform
    std::pair<std::vector<double>, int64_t> tf_ret = robot.get_transform("left_arm_link1", "left_arm_link7", 0);

    if (tf_ret.first.empty()) {
        std::cout << "get_transform error" << std::endl;
    } else {
        std::cout << "tf_timestamp_ns: " << tf_ret.second << std::endl;
        print_pose_vec(tf_ret.first);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get IMU Data (get_imu_data)

Applicable Scenarios: Get acceleration, angular velocity and pose information from the base IMU, used for state estimation and motion analysis.

examples/cpp/galbot_robot/src/get_imu_data_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_imu_data(const std::shared_ptr<ImuData>& imu_data) {
    if (!imu_data) {
        std::cerr << "IMU data is empty" << std::endl;
        return;
    }

    std::cout << "Timestamp (ns): " << imu_data->timestamp_ns << std::endl;

    std::cout << "Accelerometer: "
              << "x=" << imu_data->accel.x << ", "
              << "y=" << imu_data->accel.y << ", "
              << "z=" << imu_data->accel.z << std::endl;

    std::cout << "Gyroscope: "
              << "x=" << imu_data->gyro.x << ", "
              << "y=" << imu_data->gyro.y << ", "
              << "z=" << imu_data->gyro.z << std::endl;

    std::cout << "Magnetometer: "
              << "x=" << imu_data->magnet.x << ", "
              << "y=" << imu_data->magnet.y << ", "
              << "z=" << imu_data->magnet.z << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    std::unordered_set<SensorType> sensor_types = {
        SensorType::CHASSIS_IMU
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get IMU data
    // - SensorType::LIDAR_IMU: lidar IMU
    // - SensorType::CHASSIS_IMU: Chassis lidar IMU
    std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::CHASSIS_IMU);
    if (imu_data) {
        std::cout << "IMU data retrieved successfully!" << std::endl;
        print_imu_data(imu_data);
    } else {
        std::cerr << "Failed to get IMU data!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Battery Information (get_bms_information)

Applicable Scenarios: Get battery voltage, charge level and other information, used for low battery detection and status monitoring.

examples/cpp/galbot_robot/src/get_bms_information_example.cpp

#include <iostream>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_bms_information(const std::shared_ptr<BmsInfo>& bms_info) {
    if (!bms_info) {
        std::cerr << "BMS info is empty" << std::endl;
        return;
    }

    std::cout << "Voltage (V): " << bms_info->voltage << std::endl;
    std::cout << "Current (A): " << bms_info->current << std::endl;
    std::cout << "Battery level (%): " << bms_info->battery_level << std::endl;
    std::cout << "Temperature (C): " << bms_info->temperature << std::endl;
    std::cout << "Charging status: " << std::boolalpha << bms_info->charging_status
              << std::noboolalpha << std::endl;
    std::cout << "Health status: " << std::boolalpha << bms_info->health_status
              << std::noboolalpha << std::endl;
    std::cout << "Capacity (Ah): " << bms_info->capacity << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    // Get BMS information
    auto bms_info = robot.get_bms_information();
    if (bms_info) {
        std::cout << "BMS info retrieved successfully!" << std::endl;
        print_bms_information(bms_info);
    } else {
        std::cerr << "Failed to get BMS info!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Device Information (get_device_information)

Applicable Scenarios: Get device information such as hardware version and firmware version, used for debugging and compatibility checking.

examples/cpp/galbot_robot/src/get_device_information_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_device_info(const std::shared_ptr<DeviceInfo>& device_info) {
  if (!device_info) {
    std::cerr << "Device information is empty" << std::endl;
    return;
  }

  std::cout << "Device information:" << std::endl;
  std::cout << "  Model: " << (device_info->model.empty() ? "N/A" : device_info->model) << std::endl;
  std::cout << "  Serial number: " << (device_info->serial_number.empty() ? "N/A" : device_info->serial_number) << std::endl;
  std::cout << "  Firmware version: " << (device_info->firmware_version.empty() ? "N/A" : device_info->firmware_version)
            << std::endl;
  std::cout << "  Hardware version: " << (device_info->hardware_version.empty() ? "N/A" : device_info->hardware_version)
            << std::endl;
  std::cout << "  : " << (device_info->manufacturer.empty() ? "N/A" : device_info->manufacturer) << std::endl;
}

int main() {
  // Get object instance
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  // Initialize system
  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  // Program started, waiting for data
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // Get device information
  std::shared_ptr<DeviceInfo> device_info = robot.get_device_information();
  if (device_info) {
    std::cout << "Device information retrieved successfully!" << std::endl;
    print_device_info(device_info);
  } else {
    std::cerr << "Failed to get device information!" << std::endl;
  }

  // Exit system and release SDK resources
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

Get Camera Image Data (get_rgb_data && get_ir_data) (get_camera_data)

Applicable Scenarios: Get RGB and infrared image data for visual perception and object detection. G3 does not provide arm-mounted depth cameras.

examples/cpp/galbot_robot/src/get_camera_data_example.cpp

#include <iostream>
#include <vector>
#include <memory>
#include <unordered_set>
#include <chrono>
#include <thread>

#include "galbot_robot.hpp"
#include "opencv2/opencv.hpp"

using namespace galbot::sdk;

void print_rgb_data(const std::shared_ptr<RgbData> &rgb_data) {
  if (rgb_data == nullptr) {
    std::cout << "rgb_data is nullptr" << std::endl;
    return;
  }

  std::cout << "Camera image timestamp: "
            << rgb_data->header.timestamp_ns
            << std::endl;
  std::cout << "format is " << rgb_data->format << std::endl;
  std::cout << "frame_id is " << rgb_data->header.frame_id << std::endl;
  std::cout << "data size is " << rgb_data->data.size() << std::endl;

  std::cout << "show image:";

  std::shared_ptr<cv::Mat> img = rgb_data->convert_to_cv2_mat();

  cv::imwrite("result_image.jpg", *img);

  std::cout << "Image saved to result_image.jpg" << std::endl;
}

void print_ir_data(const std::string& name, const std::shared_ptr<IrData>& ir_data) {
  if (ir_data == nullptr) {
    std::cout << name << " is nullptr" << std::endl;
    return;
  }

  std::cout << name << " timestamp: " << ir_data->header.timestamp_ns << std::endl;
  std::cout << "format is " << ir_data->format << std::endl;
  std::cout << "frame_id is " << ir_data->header.frame_id << std::endl;
  std::cout << "size is " << ir_data->width << "x" << ir_data->height << std::endl;
  std::cout << "data size is " << ir_data->data.size() << std::endl;

  std::shared_ptr<cv::Mat> img = ir_data->convert_to_cv2_mat();
  if (img && !img->empty()) {
    std::string filename = name + ".png";
    cv::imwrite(filename, *img);
    std::cout << "Image saved to " << filename << std::endl;
  }
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // G3 does not have arm-mounted depth cameras.
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::HEAD_LEFT_CAMERA,       // Head left camera
        SensorType::LEFT_ARM_INFRA_CAMERA_1,    // Left arm IR camera 1
        SensorType::LEFT_ARM_INFRA_CAMERA_2,    // Left arm IR camera 2
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }
    // Wait for camera data ready
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get RGB image data
    std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(SensorType::HEAD_LEFT_CAMERA, RgbOutputFormat::JPEG, true);
    if (rgb_data) {
        std::cout << "RGB image data retrieved successfully!" << std::endl;
        print_rgb_data(rgb_data);
    } else {
        std::cerr << "Failed to get RGB image data!" << std::endl;
    }

    // Get left arm IR camera 1 image data
    std::shared_ptr<IrData> ir1_data = robot.get_ir_data(SensorType::LEFT_ARM_INFRA_CAMERA_1);
    if (ir1_data) {
        std::cout << "IR camera 1 data retrieved successfully!" << std::endl;
        print_ir_data("left_arm_infra1", ir1_data);
    } else {
        std::cerr << "Failed to get IR camera 1 data!" << std::endl;
    }

    // Get left arm IR camera 2 image data
    std::shared_ptr<IrData> ir2_data = robot.get_ir_data(SensorType::LEFT_ARM_INFRA_CAMERA_2);
    if (ir2_data) {
        std::cout << "IR camera 2 data retrieved successfully!" << std::endl;
        print_ir_data("left_arm_infra2", ir2_data);
    } else {
        std::cerr << "Failed to get IR camera 2 data!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Subscribe Video Data (subscribe_video_data)

Applicable Scenarios: Subscribe to a sensor's H.264 video stream and receive encoded frames via a callback, used when you need push-based delivery of compressed video for recording, real-time processing, or network streaming.

examples/cpp/galbot_robot/src/subscribe_video_data_example.cpp

#include <chrono>
#include <fstream>
#include <iostream>
#include <memory>
#include <mutex>
#include <thread>
#include <unordered_set>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {
constexpr const char* kOutputPath = "head_left.h264";
constexpr int kDurationSeconds = 10;
constexpr int kLogEveryNFrames = 30;

std::mutex g_state_mutex;
std::ofstream g_output_file;
int g_frame_count = 0;

// Important:
// The video callback is executed by the SDK internal dispatch thread.
// Do not perform time-consuming operations in this callback; keep it
// short and non-blocking. Long-running work may delay video frame delivery
// for this subscription.
void video_data_callback(const std::shared_ptr<EncodedVideoData>& video_data) {
  if (video_data == nullptr) {
    return;
  }

  std::lock_guard<std::mutex> lock(g_state_mutex);
  if (g_output_file.is_open() && !video_data->data.empty()) {
    g_output_file.write(reinterpret_cast<const char*>(video_data->data.data()),
                        static_cast<std::streamsize>(video_data->data.size()));
  }

  ++g_frame_count;
  if (g_frame_count == 1 || g_frame_count % kLogEveryNFrames == 0) {
    std::cout << "frame_count=" << g_frame_count
              << ", timestamp_ns=" << video_data->header.timestamp_ns
              << ", format=" << video_data->format
              << ", bytes=" << video_data->data.size() << std::endl;
  }
}
}  // namespace

int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  std::unordered_set<SensorType> sensor_types = {SensorType::HEAD_LEFT_CAMERA};
  if (!robot.init(sensor_types)) {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }
  std::cout << "System initialized successfully!" << std::endl;

  g_output_file.open(kOutputPath, std::ios::binary | std::ios::trunc);
  if (!g_output_file.is_open()) {
    std::cerr << "Failed to open output file: " << kOutputPath << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }
  std::cout << "writing H.264 stream to " << kOutputPath << std::endl;

  SensorStatus status = robot.subscribe_video_data(SensorType::HEAD_LEFT_CAMERA, video_data_callback);
  if (status != SensorStatus::SUCCESS) {
    std::cerr << "subscribe_video_data failed, status=" << static_cast<int>(status) << std::endl;
    g_output_file.close();
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }
  std::cout << "subscribe_video_data success" << std::endl;

  std::this_thread::sleep_for(std::chrono::seconds(kDurationSeconds));

  SensorStatus unsubscribe_status = robot.unsubscribe_video_data(SensorType::HEAD_LEFT_CAMERA);
  std::cout << "unsubscribe_video_data status=" << static_cast<int>(unsubscribe_status) << std::endl;

  {
    std::lock_guard<std::mutex> lock(g_state_mutex);
    if (g_output_file.is_open()) {
      g_output_file.close();
    }
    std::cout << "received " << g_frame_count << " frames" << std::endl;
  }
  std::cout << "saved H.264 stream to " << kOutputPath << std::endl;

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resources released successfully" << std::endl;
  return 0;
}

Get Sensor Parameters (get_camera_intrinsic && get_sensor_extrinsic)

Applicable Scenarios: Get camera intrinsics and sensor extrinsics relative to the robot base, used for computer vision calculations and 3D reconstruction.

examples/cpp/galbot_robot/src/get_camera_params.cpp

#include <iostream>
#include <vector>
#include <memory>
#include <unordered_set>
#include <chrono>
#include <thread>

#include "galbot_robot.hpp"
#include "opencv2/opencv.hpp"

using namespace galbot::sdk;

void print_rgb_data(
  const std::shared_ptr<RgbData> &rgb_data) {
  if (rgb_data == nullptr) {
    std::cout << "rgb_data is nullptr" << std::endl;
    return;
  }

  std::cout << "Camera image timestamp: "
            << rgb_data->header.timestamp_ns
            << std::endl;
  std::cout << "format is " << rgb_data->format << std::endl;
  std::cout << "frame_id is " << rgb_data->header.frame_id << std::endl;
  std::cout << "data size is " << rgb_data->data.size() << std::endl;

  std::cout << "show image:";

  std::shared_ptr<cv::Mat> img = rgb_data->convert_to_cv2_mat();

  cv::imwrite("result_image.jpg", *img);

  std::cout << "Image saved to result_image.jpg" << std::endl;
}

void print_camera_info(const std::shared_ptr<CameraInfo>& camerainfo){
    if (camerainfo == nullptr) {
        std::cout << "camerainfo is nullptr" << std::endl;
        return;
    }

    std::cout << "camera info:" << std::endl;
    std::cout << "header {" << std::endl;
    std::cout << "  timestamp_ns: " << camerainfo->header.timestamp_ns << std::endl;
    std::cout << "  frame_id: " << camerainfo->header.frame_id << std::endl;
    std::cout << "}" << std::endl;
    std::cout << "width: " << camerainfo->width << std::endl;
    std::cout << "height: " << camerainfo->height << std::endl;
    std::cout << "distortion_model: " << camerainfo->distortion_model << std::endl;
    std::cout << "d: ";
    for (auto& d_i : camerainfo->d) {
        std::cout << d_i << " ";
    }
    std::cout << std::endl;
    std::cout << "k: ";
    for (auto& k_i : camerainfo->k) {
        std::cout << k_i << " ";
    }
    std::cout << std::endl;
    std::cout << "r: ";
    for (auto& r_i : camerainfo->r) {
        std::cout << r_i << " ";
    }
    std::cout << std::endl;
    std::cout << "p: ";
    for (auto& p_i : camerainfo->p) {
        std::cout << p_i << " ";
    }
    std::cout << std::endl;
    std::cout << "T: ";
    for (auto& t_i : camerainfo->T) {
        std::cout << t_i << " ";
    }
    std::cout << std::endl;
    std::cout << "roi {" << std::endl;
    std::cout << "  width: " << camerainfo->roi.width << std::endl;
    std::cout << "  height: " << camerainfo->roi.height << std::endl;
    std::cout << "}" << std::endl;
    std::cout << "camera_type: " << camerainfo->camera_type << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // G3 provides RGB cameras but no arm-mounted depth cameras.
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::HEAD_LEFT_CAMERA,       // Head left camera
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }
    // Wait for camera data ready
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get RGB image data
    std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(SensorType::HEAD_LEFT_CAMERA, RgbOutputFormat::JPEG, true);
    if (rgb_data) {
        std::cout << "RGB image data retrieved successfully!" << std::endl;
        print_rgb_data(rgb_data);
    } else {
        std::cerr << "Failed to get RGB image data!" << std::endl;
    }

    // Get camera parameters
    std::shared_ptr<CameraInfo> rgb_camerainfo = robot.get_camera_intrinsic(SensorType::HEAD_LEFT_CAMERA);
    if (rgb_camerainfo) {
        std::cout << "Camera parameters retrieved successfully!" << std::endl;
        print_camera_info(rgb_camerainfo);
    } else {
        std::cerr << "Failed to get camera parameters!" << std::endl;
    }

    // Get sensor extrinsics
    std::pair<std::vector<double>, int64_t> sensor_extrinsic = robot.get_sensor_extrinsic(SensorType::HEAD_LEFT_CAMERA);
    if (!sensor_extrinsic.first.empty()) {
        std::cout << "Sensor extrinsics retrieved successfully!" << std::endl;
        std::cout << "transform: ";
        for (auto& t_i : sensor_extrinsic.first) {
            std::cout << t_i << " ";
        }
        std::cout << std::endl;
        std::cout << "timestamp: " << sensor_extrinsic.second << std::endl;
    } else {
        std::cerr << "Failed to get sensor extrinsics!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Lidar Data (get_lidar_data)

Applicable Scenarios: Get LiDAR point cloud data, used for navigation, obstacle avoidance, and environment modeling.

examples/cpp/galbot_robot/src/get_lidar_data_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <cmath>
#include <chrono>
#include <thread>
#include <memory>
#include <unordered_set>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Define point structure
struct Point3D {
    float x, y, z;
};

/**
 * Directly extract the XYZ array from LidarData via pointer operations
 */
std::vector<Point3D> get_xyz_points(const std::shared_ptr<LidarData>& cloud, bool remove_nan = false) {
    std::vector<Point3D> points;
    if (!cloud || cloud->data.empty()) return points;

    // 1. Find the offsets of x, y, z fields (offset)
    // In the Python version, fields are indexed by field name; here we precompute offsets for better performance
    int32_t off_x = -1, off_y = -1, off_z = -1;
    for (const auto& f : cloud->fields) {
        if (f.name == "x") off_x = f.offset;
        else if (f.name == "y") off_y = f.offset;
        else if (f.name == "z") off_z = f.offset;
    }

    if (off_x == -1 || off_y == -1 || off_z == -1) {
        std::cerr << "Error: point cloud data is missing required xyz fields" << std::endl;
        return points;
    }

    uint32_t num_points = cloud->width * cloud->height;
    points.reserve(num_points);

    const uint8_t* raw_data = cloud->data.data();
    uint32_t point_step = cloud->point_step;

    // 2. Read directly via pointer (core zero-copy logic)
    for (uint32_t i = 0; i < num_points; ++i) {
        // Calculate starting pointer for current point
        const uint8_t* pt_ptr = raw_data + (i * point_step);

        // Use reinterpret_cast to directly cast pointer types and read memory
        // Assume LiDAR data is float32 (F), which is the most common format
        float x = *reinterpret_cast<const float*>(pt_ptr + off_x);
        float y = *reinterpret_cast<const float*>(pt_ptr + off_y);
        float z = *reinterpret_cast<const float*>(pt_ptr + off_z);

        // Handle NaN values (corresponds to Python remove_nan logic)
        if (remove_nan) {
            if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
                continue;
            }
        }

        points.push_back({x, y, z});
    }

    return points;
}

/**
 * Save to a PCD file
 */
void save_xyz_to_pcd(const std::vector<Point3D>& points, const std::string& filename) {
    std::ofstream fs(filename);
    if (!fs.is_open()) {
        std::cerr << "Unable to open file for writing: " << filename << std::endl;
        return;
    }

    // PCD 0.7 Header
    fs << "# .PCD v0.7 - Point Cloud Data file format\n"
       << "VERSION 0.7\n"
       << "FIELDS x y z\n"
       << "SIZE 4 4 4\n"
       << "TYPE F F F\n"
       << "COUNT 1 1 1\n"
       << "WIDTH " << points.size() << "\n"
       << "HEIGHT 1\n"
       << "VIEWPOINT 0 0 0 1 0 0 0\n"
       << "POINTS " << points.size() << "\n"
       << "DATA ascii\n";

    // Data
    for (const auto& p : points) {
        fs << p.x << " " << p.y << " " << p.z << "\n";
    }

    fs.close();
    std::cout << "Saved " << points.size() << " points to " << filename << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize sensors; only cameras and LiDAR sensors passed during initialization can retrieve data
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::BASE_LIDAR              // Chassis lidar
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get LiDAR data
    std::shared_ptr<LidarData> lidar_data = robot.get_lidar_data(SensorType::BASE_LIDAR);
    if (lidar_data) {
        std::cout << "Lidar data retrieved successfully!" << std::endl;
        std::vector<Point3D> xyz_points = get_xyz_points(lidar_data, false);
        if (!xyz_points.empty()) {
            save_xyz_to_pcd(xyz_points, "output_xyz.pcd");
        }
    } else {
        std::cerr << "Failed to get lidar data!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Synchronized Observation Data (get_synced_observation)

Applicable Scenarios: Get synchronized observation data, mainly for obtaining time-aligned RGB camera and joint data in model inference scenarios.

examples/cpp/galbot_robot/src/get_synced_observation.cpp

#include <algorithm>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <unordered_set>
#include <vector>

#include "galbot_robot.hpp"

namespace {

std::string sensor_name(galbot::sdk::SensorType sensor) {
  using galbot::sdk::SensorType;
  switch (sensor) {
    case SensorType::HEAD_LEFT_CAMERA:
      return "HEAD_LEFT_CAMERA";
    case SensorType::HEAD_RIGHT_CAMERA:
      return "HEAD_RIGHT_CAMERA";
    case SensorType::LEFT_ARM_CAMERA:
      return "LEFT_ARM_CAMERA";
    case SensorType::RIGHT_ARM_CAMERA:
      return "RIGHT_ARM_CAMERA";
    default:
      return "UNKNOWN_CAMERA";
  }
}

}  // namespace

int main() {
  using namespace galbot::sdk;

  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  std::unordered_set<SensorType> enable_sensor_set = {
      SensorType::HEAD_LEFT_CAMERA,
      SensorType::LEFT_ARM_CAMERA,
      SensorType::RIGHT_ARM_CAMERA,
  };
  if (!robot.init(enable_sensor_set, true)) {
    std::cout << "init failed" << std::endl;
    robot.destroy();
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(300));

  const std::vector<SensorType> cameras = {
      SensorType::LEFT_ARM_CAMERA,   // anchor camera
      SensorType::RIGHT_ARM_CAMERA,  // nearest-neighbor aligned
      SensorType::HEAD_LEFT_CAMERA,  // nearest-neighbor aligned
  };
  auto obs = robot.get_synced_observation(cameras, true);
  if (!obs) {
    std::cout << "get_synced_observation failed" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  // Sync-mode RGB payloads are tightly packed CPU-owned NV12, not JPEG.
  std::cout << "[Synced RGB]" << std::endl;
  const auto anchor_it = obs->rgb_data_map.find(cameras[0]);
  if (anchor_it == obs->rgb_data_map.end() || !anchor_it->second) {
    std::cout << "  anchor missing" << std::endl;
  } else {
    const int64_t anchor_ts_ns = anchor_it->second->header.timestamp_ns;
    std::cout << "  anchor=" << sensor_name(cameras[0]) << " timestamp_ns=" << anchor_ts_ns << std::endl;
    for (const auto& cam : cameras) {
      const auto it = obs->rgb_data_map.find(cam);
      if (it == obs->rgb_data_map.end() || !it->second) {
        std::cout << "  " << sensor_name(cam) << ": missing" << std::endl;
        continue;
      }
      const int64_t cam_ts_ns = it->second->header.timestamp_ns;
      std::cout << "  " << sensor_name(cam) << " timestamp_ns=" << cam_ts_ns
                << " delta_to_anchor_ms=" << (cam_ts_ns - anchor_ts_ns) / 1e6
                << " format=" << it->second->format
                << " dimensions=" << it->second->width << "x" << it->second->height
                << " bytes=" << it->second->data.size() << std::endl;
    }
  }

  if (obs->joint_state) {
    std::cout << "[Joint] timestamp_ns=" << obs->joint_state->timestamp_ns
              << " joint_count=" << obs->joint_state->joint_state_vec.size() << std::endl;
    const size_t print_n = std::min<size_t>(obs->joint_state->joint_state_vec.size(), 5);
    for (size_t i = 0; i < print_n; ++i) {
      const auto& js = obs->joint_state->joint_state_vec[i];
      std::cout << "  [" << i << "] joint_name=" << js.joint_name
                << " position=" << js.position
                << " velocity=" << js.velocity
                << " effort=" << js.effort << std::endl;
    }
  } else {
    std::cout << "[Joint] missing" << std::endl;
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

One-Click Reset to Zero – Odometry Frame (whole_body_reset_zero_odom)

Applicable Scenarios: Quickly move all joints to zero position (initial pose) based on odometry frame. Typically used for initialization after program startup.

examples/cpp/galbot_robot/src/whole_body_reset_zero_odom_example.cpp

#include <iostream>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {
const char* control_status_to_string(ControlStatus status) {
    switch (status) {
        case ControlStatus::SUCCESS:
            return "SUCCESS";
        case ControlStatus::TIMEOUT:
            return "TIMEOUT";
        case ControlStatus::FAULT:
            return "FAULT";
        case ControlStatus::INVALID_INPUT:
            return "INVALID_INPUT";
        case ControlStatus::INIT_FAILED:
            return "INIT_FAILED";
        case ControlStatus::IN_PROGRESS:
            return "IN_PROGRESS";
        case ControlStatus::STOPPED_UNREACHED:
            return "STOPPED_UNREACHED";
        case ControlStatus::DATA_FETCH_FAILED:
            return "DATA_FETCH_FAILED";
        case ControlStatus::PUBLISH_FAIL:
            return "PUBLISH_FAIL";
        case ControlStatus::COMM_DISCONNECTED:
            return "COMM_DISCONNECTED";
        default:
            return "UNKNOWN_STATUS";
    }
}
}  // namespace

int main() {
    auto& robot = GalbotRobot::get_instance(MachineType::G3);
    auto& motion = GalbotMotion::get_instance(MachineType::G3);

    if (!robot.init()) {
        std::cerr << "GalbotRobot init failed." << std::endl;
        return -1;
    }
    if (!motion.init()) {
        std::cerr << "GalbotMotion init failed." << std::endl;
        return -1;
    }

    // Optional frames (frame_id: base_link/odom/map, reference_frame_id: odom/map)
    const std::string frame_id = "base_link";
    const std::string reference_frame_id = "odom";

    // reset to zero
    auto result = robot.zero_whole_body_and_base(
        frame_id,
        reference_frame_id,
        /*is_blocking=*/true,
        /*leg_head_speed_rad_s=*/0.2,
        /*leg_head_timeout_s=*/15.0,
        /*params=*/default_param);
    
    std::cout << "Zero joint status: " << motion.status_to_string(result.first) << std::endl;
    std::cout << "Zero base status: " << control_status_to_string(result.second) << std::endl;

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

    return 0;
}

One-Click Reset to Zero – Map Frame (whole_body_reset_zero_map)

Applicable Scenarios: Quickly move all joints to zero position (initial pose) based on map frame.

examples/cpp/galbot_robot/src/whole_body_reset_zero_map_example.cpp

#include <iostream>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {
const char* control_status_to_string(ControlStatus status) {
    switch (status) {
        case ControlStatus::SUCCESS:
            return "SUCCESS";
        case ControlStatus::TIMEOUT:
            return "TIMEOUT";
        case ControlStatus::FAULT:
            return "FAULT";
        case ControlStatus::INVALID_INPUT:
            return "INVALID_INPUT";
        case ControlStatus::INIT_FAILED:
            return "INIT_FAILED";
        case ControlStatus::IN_PROGRESS:
            return "IN_PROGRESS";
        case ControlStatus::STOPPED_UNREACHED:
            return "STOPPED_UNREACHED";
        case ControlStatus::DATA_FETCH_FAILED:
            return "DATA_FETCH_FAILED";
        case ControlStatus::PUBLISH_FAIL:
            return "PUBLISH_FAIL";
        case ControlStatus::COMM_DISCONNECTED:
            return "COMM_DISCONNECTED";
        default:
            return "UNKNOWN_STATUS";
    }
}
}  // namespace

int main() {
    auto& robot = GalbotRobot::get_instance(MachineType::G3);
    auto& motion = GalbotMotion::get_instance(MachineType::G3);

    if (!robot.init()) {
        std::cerr << "GalbotRobot init failed." << std::endl;
        return -1;
    }
    if (!motion.init()) {
        std::cerr << "GalbotMotion init failed." << std::endl;
        return -1;
    }

    // Optional frames (frame_id: base_link/odom/map, reference_frame_id: odom/map)
    const std::string frame_id = "base_link";
    const std::string reference_frame_id = "map";

    // reset to zero
    auto result = robot.zero_whole_body_and_base(
        frame_id,
        reference_frame_id,
        /*is_blocking=*/true,
        /*leg_head_speed_rad_s=*/0.2,
        /*leg_head_timeout_s=*/15.0,
        /*params=*/default_param);
    
    std::cout << "Zero joint status: " << motion.status_to_string(result.first) << std::endl;
    std::cout << "Zero base status: " << control_status_to_string(result.second) << std::endl;

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

    return 0;
}

Emergency Stop and Resume (emergency_stop / resume_from_emergency_stop)

Applicable Scenarios: Immediately halt all robot motor power for safety-critical stops, then re-enable all robot motors and restore normal operation after the emergency stop has been triggered.

examples/cpp/galbot_robot/src/emergency_stop_and_resume_example.cpp

#include <iostream>
#include <chrono>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "Initialization succeeded" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    // Trigger software emergency stop
    ControlStatus status = robot.emergency_stop();
    if (status == ControlStatus::SUCCESS) {
        std::cout << "Emergency stop successfully." << std::endl;
    } else {
        std::cout << "Emergency stop failed." << std::endl;
    }

    std::cout << "Waiting 5 seconds before resuming from emergency stop..." << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(5));

    // Resume from software emergency stop
    status = robot.resume_from_emergency_stop();
    if (status == ControlStatus::SUCCESS) {
        std::cout << "Resume from emergency stop successfully." << std::endl;
    } else {
        std::cout << "Resume from emergency stop failed." << std::endl;
    }

    std::cout << "Waiting 15 seconds before exiting..." << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(15));

    // Send exit signal
    robot.request_shutdown();
    // Wait until entering shutdown state
    robot.wait_for_shutdown();
    // Release SDK resources
    robot.destroy();
    std::cout << "Resources released successfully" << std::endl;

    return 0;
}

Audio Playback (play_audio / stop_audio)

Applicable Scenarios: Play an audio file through the robot speaker and stop the current playback task.

examples/cpp/galbot_robot/src/audio_example.cpp

#include <chrono>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <limits>
#include <sstream>
#include <string>
#include <unordered_set>
#include <vector>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Added command help table structure
struct CommandInfo {
  std::string cmd;
  std::string description;
  std::string parameters;
  std::string example;
};

// Command help table
const std::vector<CommandInfo> COMMAND_TABLE = {
    {"start_microphone_stream_input", "Start microphone streaming audio input feature", "No parameters", "start_microphone_stream_input"},
    {"stop_microphone_stream_input", "Stop the specified microphone streaming audio input", "No parameters", "stop_microphone_stream_input"},
    {"write_audio_stream_output", "PCM audiodata audio output", "Audio file name to play (16K 16bit Mono PCM audio file path)", "write_audio_stream_output"},
    {"stop_audio_stream_output", "Stop the specified audio output stream or all active audio output streams", "No parameters", "stop_audio_stream_output"},
    {"set_volume", "Set system global volume", "Volume setting parameter (0-100)", "set_volume"},
    {"get_volume", "Get current system global volume", "No parameters", "get_volume"},
    {"help", "Show help", "No parameters", "help"},
    {"q", "Exit program", "No parameters", "q"}};

// command
void printCommandHelp() {
  std::cout << "\n================ Command Help Table =================" << std::endl;
  std::cout << std::left << std::setw(30) << "Command" << std::setw(30) << "Description" << std::setw(30) << "Parameters"
            << std::setw(30) << "example" << std::endl;
  std::cout << std::string(110, '-') << std::endl;

  for (const auto& cmd : COMMAND_TABLE) {
    std::cout << std::left << std::setw(40) << cmd.cmd << std::setw(60) << cmd.description << std::setw(40)
              << cmd.parameters << std::setw(30) << cmd.example << std::endl;
  }
  std::cout << "=============================================\n" << std::endl;
}


void save_audio_to_file(const std::string& filename, const std::vector<uint8_t>& audio_data) {
  if (audio_data.empty()) {
    return;
  }
  std::ofstream outfile(filename, std::ios::app | std::ios::binary);
  if (!outfile) {
    std::cout << "Failed to open file for writing: " << filename << std::endl;
    return;
  }
  outfile.write(reinterpret_cast<const char*>(audio_data.data()),
                static_cast<std::streamsize>(audio_data.size()));
}

void handle_audio_data(const std::shared_ptr<AudioData>& audio_data) {
  if (audio_data == nullptr) {
    return;
  }

  const int64_t timestamp_ns = audio_data->header.timestamp_ns;
  (void)timestamp_ns;

  const std::string& audio_type = audio_data->type;
  const auto data_size = audio_data->data.size();
  (void)audio_data->format;

  if (audio_type == "vad_begin") {
    std::cout << "\n[VAD_BEGIN] Voice activity detection started" << std::endl;
  } else if (audio_type == "vad_end") {
    std::cout << "[VAD_END] Voice activity detection ended" << std::endl;
  } else if (audio_type == "vad_chunk") {
    // std::cout << "[AUDIO_CHUNK] Received " << data_size << " bytes of " << audio_format
    //           << " audio data (timestamp_ns: " << timestamp_ns << ")" << std::endl;
    save_audio_to_file("mic_vad.pcm", audio_data->data);
  } else if (audio_type == "denoise_chunk") {
    // std::cout << "[DENOISE_AUDIO] Received " << data_size << " bytes of denoised audio (timestamp_ns: "
    //           << timestamp_ns << ")" << std::endl;
    save_audio_to_file("mic_denoise.pcm", audio_data->data);
  } else if (audio_type == "waken_up") {
    std::string payload(audio_data->data.begin(), audio_data->data.end());
    std::cout << "\n[WAKEN_UP] Wake-up event triggered: " << payload << std::endl;
  } else {
    std::cout << "[" << audio_type << "] Received audio data, size: " << data_size << " bytes" << std::endl;
  }
}


// Safely read numeric inputs of multiple types and handle input errors
template<typename T>
bool safe_read_number(T& value, const std::string& prompt = "") {
  if (!prompt.empty()) {
    std::cout << prompt;
  }

  std::string input;
  std::getline(std::cin, input);

  // Check input stream status
  if (std::cin.fail() && !std::cin.eof()) {
    std::cin.clear();
    std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
    std::cout << "Input error, please enter a valid number." << std::endl;
    return false;
  }

  // Trim leading and trailing spaces
  input.erase(0, input.find_first_not_of(" \t"));
  input.erase(input.find_last_not_of(" \t") + 1);

  if (input.empty()) {
    std::cout << "Input error, please enter a valid number." << std::endl;
    return false;
  }

  try {
    size_t pos;
    T result;

    if (std::is_integral<T>()) {
      result = static_cast<T>(std::stoll(input, &pos));
    } else if (std::is_floating_point<T>()) {
      result = static_cast<T>(std::stold(input, &pos));
    }

    // Check whether the entire string has been converted
    if (pos != input.length()) {
      std::cout << "Input error, please enter a valid number." << std::endl;
      return false;
    }

    value = result;
    return true;
  } catch (const std::exception&) {
    std::cout << "Input error, please enter a valid number." << std::endl;
    return false;
  }
}


// Safely read filename input and handle input errors
bool safe_read_file_name(std::string& file_name, const std::string& prompt = "") {
  if (!prompt.empty()) {
    std::cout << prompt;
  }

  std::getline(std::cin, file_name);

  // Check input stream status
  if (std::cin.fail() && !std::cin.eof()) {
    std::cin.clear();
    std::cin.ignore();
  }
  return true;
}

int main(int argc, char* argv[]) {
  std::cout << "start" << std::endl;
  // Get the base_instance instance
  auto& base_instance = GalbotRobot::get_instance(MachineType::G3);

  if (!base_instance.init()) {
    std::cout << "base_instance init fail" << std::endl;
    return -1;
  }

  // Waiting for init done
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));

  printCommandHelp();

  std::string cmd, stream_id;
  while (base_instance.is_running()) {
    std::cout << "Enter interface name to test:";
    std::string input_line;
    std::getline(std::cin, input_line);

    // Extract the first word as the command (ignore subsequent arguments)
    std::istringstream iss(input_line);
    iss >> cmd;

    // If input is empty, continue the loop
    if (cmd.empty()) {
      continue;
    }
    if (cmd == "help") {
      printCommandHelp();
      continue;
    } else if (cmd == "start_microphone_stream_input") {
      stream_id = base_instance.start_microphone_stream_input([](const std::shared_ptr<AudioData> audio_data){
        handle_audio_data(audio_data);
      });

      std::cout << "Start microphone stream input, stream_id: "<< stream_id << std::endl;
    } else if (cmd == "stop_microphone_stream_input") {
      base_instance.stop_microphone_stream_input(stream_id);
      std::cout << "Stop microphone stream input, stream_id : " << stream_id << std::endl;
    } else if (cmd == "write_audio_stream_output") {
      std::string play_file;
      safe_read_file_name(play_file, "Please enter audio file name to play (16K 16bit Mono PCM audio file path): ");

      const auto chunk_size = 2560; // Data block size of 2560 bytes
      std::vector<char> audio_chunk(chunk_size, 0);
      std::cout << "Write audio stream data..." << std::endl;
      std::ifstream infile(play_file, std::ios::binary);
      if (!infile) {
        std::cout << "Failed to open for reading: " << play_file << std::endl;
        continue;
      } else {
        stream_id = "spk_stream_output_test";
        while (infile) {
          infile.read(audio_chunk.data(), static_cast<std::streamsize>(chunk_size));
          std::streamsize bytes_read = infile.gcount();
          if (bytes_read <= 0) break;
          std::string out_chunk(audio_chunk.data(), bytes_read);
          base_instance.write_audio_stream_output(out_chunk, stream_id);
          std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }
      }
      std::cout << "Write audio stream output, stream id : " << stream_id << std::endl;
    } else if (cmd == "stop_audio_stream_output") {
      base_instance.stop_audio_stream_output(stream_id);
      std::cout << "Stop audio stream output, stream id: " << stream_id << std::endl;
    } else if (cmd == "set_volume") {
      float volume = 60;
      if (!safe_read_number(volume, "Please enter volume setting parameter (0-100): ")) {
        continue;
      }

      if (!base_instance.set_volume(volume)) {
        std::cout << "Failed to set volume: " << volume << std::endl;
        continue;
      }
      std::cout << "Set current volume: " << volume << std::endl;
    } else if (cmd == "get_volume") {
      auto volume = base_instance.get_volume();
      std::cout << "Get current volume: " << volume << std::endl;
    } else if (cmd == "q") {
      break;
    } else {
      std::cout << "Invalid input" << std::endl;
    }
  }

  base_instance.request_shutdown();
  base_instance.wait_for_shutdown();
  base_instance.destroy();

  std::cout << "proccess end" << std::endl;
  return 0;
}

Controller Management

Applicable Scenarios: Query controller states and switch or manage the controllers used by robot components.

examples/cpp/galbot_robot/src/controller_management_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_active_controller(const std::string& group_name, const std::string& controller_name) {
  std::cout << "Active controller for " << group_name << ": " << controller_name << std::endl;
}

// Helper function to print control status
void print_status(const std::string& operation, ControlStatus status) {
  std::cout << operation << ": ";
  if (status == ControlStatus::SUCCESS) {
    std::cout << "SUCCESS";
  } else {
    std::cout << "FAILED (Code: " << static_cast<int>(status) << ")";
  }
  std::cout << std::endl;
}

int main() {
  // Get robot instance
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  // Initialize system
  std::cout << "Initializing robot..." << std::endl;
  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  // Wait for data readiness
  std::this_thread::sleep_for(std::chrono::seconds(1));

  // Define the controller we want to test
  // Using LEFT_ARM_PVT_CTRL as the test subject
  const std::string test_controller = G3ControllerName::LEFT_ARM_PVT_CTRL;
  const std::string controller_str = G3ControllerName::LEFT_ARM_PVT_CTRL;
  const std::string group_str = "left_arm";

  std::cout << "\n--- Starting Controller Management Example ---\n" << std::endl;

  // 1. Get active controller before switching
  std::string active_controller = robot.get_active_controller(group_str);
  print_active_controller(group_str, active_controller);

  // 2. Switch controller
  // Demonstrates switch_controller interface
  // This ensures the controller is acquired and started
  std::cout << "\n[Test 1] Switching to " << controller_str << "..." << std::endl;
  ControlStatus status = robot.switch_controller(test_controller);
  print_status("switch_controller", status);
  
  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  active_controller = robot.get_active_controller(group_str);
  print_active_controller(group_str, active_controller);

  // 3. Stop controller
  // Demonstrates stop_controller interface
  std::cout << "\n[Test 2] Stopping controller for group " << group_str << "..." << std::endl;
  status = robot.stop_controller(group_str);
  print_status("stop_controller", status);

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  // 4. Release controller
  // Demonstrates release_controller interface
  std::cout << "\n[Test 3] Releasing controller for group " << group_str << "..." << std::endl;
  status = robot.release_controller(group_str);
  print_status("release_controller", status);

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  // 5. Acquire controller
  // Demonstrates acquire_controller interface
  std::cout << "\n[Test 4] Acquiring " << controller_str << "..." << std::endl;
  status = robot.acquire_controller(test_controller);
  print_status("acquire_controller", status);

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  // 6. Start controller
  // Demonstrates start_controller interface
  std::cout << "\n[Test 5] Starting controller for group " << group_str << "..." << std::endl;
  status = robot.start_controller(group_str);
  print_status("start_controller", status);

  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  std::cout << "\n--- Controller Management Example Finished ---\n" << std::endl;

  // Shutdown
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

Get And Set Dexterous Hand State (get_dexhand_state && set_dexhand_command)

Applicable Scenarios: Read joint and operating states from a configured dexterous hand and send target joint commands to a configured dexterous hand.

examples/cpp/galbot_robot/src/get_set_dexhand_state_example.cpp

#include <algorithm>
#include <chrono>
#include <cctype>
#include <iostream>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr int kCycleCount = 3;
constexpr int kStartupDelayMs = 2000;
constexpr int kCommandIntervalMs = 1000;

// These demo positions match the standalone set_dexhand_command example.
// Each dexterous hand model has its own joint count and position unit.
const std::vector<double> INSPIRE_RH56DFX_POSITION_OPEN = {
    1000.0,  // finger1
    1000.0,  // finger2
    1000.0,  // finger3
    1000.0,  // finger4
    1000.0,  // finger5
    1000.0,  // finger6
};

const std::vector<double> INSPIRE_RH56DFX_POSITION_CLOSE = {
    850.0,  // finger1
    350.0,  // finger2
    60.0,   // finger3
    60.0,   // finger4
    60.0,   // finger5
    60.0,   // finger6
};

const std::vector<double> INSPIRE_RH56F2_POSITION_OPEN = {
    1750.0,  // thumb_bend
    1550.0,  // thumb_rotate
    1740.0,  // index
    1740.0,  // middle
    1740.0,  // ring
    1740.0,  // little
};

const std::vector<double> INSPIRE_RH56F2_POSITION_CLOSE = {
    1600.0,  // thumb_bend
    1250.0,  // thumb_rotate
    950.0,   // index
    950.0,   // middle
    950.0,   // ring
    950.0,   // little
};

const std::vector<double> BRAINCO_POSITION_OPEN = {
    100.0,  // finger1
    100.0,  // finger2
    100.0,  // finger3
    100.0,  // finger4
    100.0,  // finger5
    100.0,  // finger6
};

const std::vector<double> BRAINCO_POSITION_CLOSE = {
    33.3,   // finger1
    100.0,  // finger2
    86.7,   // finger3
    86.7,   // finger4
    86.7,   // finger5
    86.7,   // finger6
};

const std::vector<double> LINKER_L20_POSITION_OPEN = {
    1.3543754995475996,   // thumb_roll
    1.562069680534925,    // thumb_yaw
    0.23561944901923448,  // index_yaw
    0.23561944901923448,  // middle_yaw
    0.23561944901923448,  // ring_yaw
    0.23561944901923448,  // little_yaw
    0.82030474843733492,  // thumb_root
    1.2217304763960306,   // index_root
    1.2217304763960306,   // middle_root
    1.2217304763960306,   // ring_root
    1.2217304763960306,   // little_root
    1.2042771838760873,   // thumb_tip
    1.7453292519943295,   // index_tip
    1.7453292519943295,   // middle_tip
    1.7453292519943295,   // ring_tip
    1.7453292519943295,   // little_tip
};

const std::vector<double> LINKER_L20_POSITION_CLOSE = {
    0.85,  // thumb_roll
    1.55,  // thumb_yaw
    0.00,  // index_yaw
    0.00,  // middle_yaw
    0.00,  // ring_yaw
    0.00,  // little_yaw
    0.40,  // thumb_root
    0.00,  // index_root
    0.00,  // middle_root
    0.00,  // ring_root
    0.00,  // little_root
    0.35,  // thumb_tip
    0.00,  // index_tip
    0.00,  // middle_tip
    0.00,  // ring_tip
    0.00,  // little_tip
};

const std::vector<double> SHARPA_POSITION_OPEN = {
    0.0,  // thumb CMC_FE
    0.0,  // thumb CMC_AA
    0.0,  // thumb MCP_FE
    0.0,  // thumb MCP_AA
    0.0,  // thumb IP
    0.0,  // index MCP_FE
    0.0,  // index MCP_AA
    0.0,  // index PIP
    0.0,  // index DIP
    0.0,  // middle MCP_FE
    0.0,  // middle MCP_AA
    0.0,  // middle PIP
    0.0,  // middle DIP
    0.0,  // ring MCP_FE
    0.0,  // ring MCP_AA
    0.0,  // ring PIP
    0.0,  // ring DIP
    0.0,  // pinky CMC
    0.0,  // pinky MCP_FE
    0.0,  // pinky MCP_AA
    0.0,  // pinky PIP
    0.0,  // pinky DIP
};

const std::vector<double> SHARPA_POSITION_CLOSE = {
    0.87,  // thumb CMC_FE
    0.0,   // thumb CMC_AA
    0.44,  // thumb MCP_FE
    0.0,   // thumb MCP_AA
    0.87,  // thumb IP
    0.78,  // index MCP_FE
    0.0,   // index MCP_AA
    0.87,  // index PIP
    0.70,  // index DIP
    0.78,  // middle MCP_FE
    0.0,   // middle MCP_AA
    0.87,  // middle PIP
    0.70,  // middle DIP
    0.78,  // ring MCP_FE
    0.0,   // ring MCP_AA
    0.87,  // ring PIP
    0.70,  // ring DIP
    0.13,  // pinky CMC
    0.78,  // pinky MCP_FE
    0.0,   // pinky MCP_AA
    0.87,  // pinky PIP
    0.70,  // pinky DIP
};

std::string to_lower(std::string value) {
  std::transform(value.begin(), value.end(), value.begin(), [](unsigned char ch) {
    return static_cast<char>(std::tolower(ch));
  });
  return value;
}

bool parse_dexhand_type(const std::string& type_name, DexHandType& dexhand_type) {
  const std::string key = to_lower(type_name);
  if (key == "inspire") {
    dexhand_type = DexHandType::INSPIRE;
    return true;
  }
  if (key == "inspire_rh56dfx") {
    dexhand_type = DexHandType::INSPIRE_RH56DFX;
    return true;
  }
  if (key == "inspire_rh56f2") {
    dexhand_type = DexHandType::INSPIRE_RH56F2;
    return true;
  }
  if (key == "brainco" || key == "revo") {
    dexhand_type = DexHandType::BRAINCO;
    return true;
  }
  if (key == "sharpa") {
    dexhand_type = DexHandType::SHARPA;
    return true;
  }
  if (key == "linker_l20") {
    dexhand_type = DexHandType::LINKER_L20;
    return true;
  }
  return false;
}

bool is_exit_input(const std::string& value) {
  const std::string key = to_lower(value);
  return key == "q" || key == "quit" || key == "exit";
}

void print_supported_types() {
  std::cout << "Supported dexterous hand types:" << std::endl;
  std::cout << "  inspire          Compatibility alias for inspire_rh56f2" << std::endl;
  std::cout << "  inspire_rh56dfx  Inspire RH56DFX dexterous hand" << std::endl;
  std::cout << "  inspire_rh56f2   Inspire RH56F2 dexterous hand" << std::endl;
  std::cout << "  brainco          BrainCo dexterous hand" << std::endl;
  std::cout << "  revo             Alias for brainco" << std::endl;
  std::cout << "  sharpa           Sharpa dexterous hand" << std::endl;
  std::cout << "  linker_l20       Linker Hand L20 dexterous hand" << std::endl;
}

bool read_dexhand_type(const std::string& side_name, DexHandType& dexhand_type, std::string& type_label) {
  while (true) {
    std::cout << "Enter " << side_name << " dexterous hand type (or q to quit): ";
    std::string raw_value;
    if (!std::getline(std::cin, raw_value)) {
      return false;
    }

    type_label = to_lower(raw_value);
    if (is_exit_input(type_label)) {
      return false;
    }
    if (parse_dexhand_type(type_label, dexhand_type)) {
      return true;
    }

    std::cout << "Unsupported dexterous hand type: " << raw_value << std::endl;
    std::cout << "Please choose from: inspire, inspire_rh56dfx, inspire_rh56f2, brainco, revo, sharpa, linker_l20"
              << std::endl;
  }
}

std::vector<JointCommand> make_dexhand_command(const std::vector<double>& positions) {
  std::vector<JointCommand> commands(positions.size());
  for (size_t i = 0; i < positions.size(); ++i) {
    commands[i].position = positions[i];
  }
  return commands;
}

const std::vector<double>& dexhand_motion_positions(DexHandType dexhand_type, const std::string& action) {
  const bool is_open = action == "open";
  if (dexhand_type == DexHandType::INSPIRE_RH56DFX) {
    return is_open ? INSPIRE_RH56DFX_POSITION_OPEN : INSPIRE_RH56DFX_POSITION_CLOSE;
  }
  if (dexhand_type == DexHandType::INSPIRE || dexhand_type == DexHandType::INSPIRE_RH56F2) {
    return is_open ? INSPIRE_RH56F2_POSITION_OPEN : INSPIRE_RH56F2_POSITION_CLOSE;
  }
  if (dexhand_type == DexHandType::BRAINCO) {
    return is_open ? BRAINCO_POSITION_OPEN : BRAINCO_POSITION_CLOSE;
  }
  if (dexhand_type == DexHandType::SHARPA) {
    return is_open ? SHARPA_POSITION_OPEN : SHARPA_POSITION_CLOSE;
  }
  return is_open ? LINKER_L20_POSITION_OPEN : LINKER_L20_POSITION_CLOSE;
}

void print_dexhand_state(const std::string& hand_name, const DexhandState& dexhand_state,
                         DexHandType dexhand_type) {
  const char* type_label = dexhand_type == DexHandType::SHARPA ? "sharpa" : "dexterous";
  std::cout << hand_name << " " << type_label << " hand state:" << std::endl;
  std::cout << "Timestamp (ns): " << dexhand_state.timestamp_ns << std::endl;

  const auto& joint_state_vec = dexhand_state.joint_state.joint_state_vec;
  std::cout << "  Joint states (" << joint_state_vec.size() << " joints):" << std::endl;
  for (size_t i = 0; i < joint_state_vec.size(); ++i) {
    const auto& js = joint_state_vec[i];
    if (dexhand_type == DexHandType::SHARPA) {
      // Sharpa state does not use the generic acceleration field in the same way.
      std::cout << "    joint" << (i + 1) << ": position=" << js.position << ", velocity=" << js.velocity
                << ", effort=" << js.effort << ", current=" << js.current << std::endl;
    } else {
      // Some SDK backends fill joint_name; otherwise keep a stable fallback label.
      const std::string joint_label = !js.joint_name.empty()
                                          ? js.joint_name
                                          : hand_name + "_dexhand_joint" + std::to_string(i + 1);
      std::cout << "    " << joint_label << ": position=" << js.position
                << ", velocity=" << js.velocity << ", acceleration=" << js.acceleration
                << ", effort=" << js.effort << ", current=" << js.current << std::endl;
    }
  }

  if (dexhand_type != DexHandType::SHARPA) {
    return;
  }

  const std::unordered_map<std::string, EffortInfo>& force_sensor_map = dexhand_state.force_sensor_map;
  if (force_sensor_map.empty()) {
    std::cout << "  (no force sensor data)" << std::endl;
    return;
  }

  std::cout << "  Force sensors (" << force_sensor_map.size() << " sensors):" << std::endl;
  for (const auto& sensor_pair : force_sensor_map) {
    const auto& effort = sensor_pair.second;
    std::cout << "    " << sensor_pair.first << " @ " << effort.timestamp_ns
              << ": Fx=" << effort.force.x << ", Fy=" << effort.force.y << ", Fz=" << effort.force.z
              << ", Mx=" << effort.torque.x << ", My=" << effort.torque.y << ", Mz=" << effort.torque.z
              << std::endl;
  }
}

}  // namespace

int main() {
  std::cout << "Starting get_set_dexhand_state example" << std::endl;
  print_supported_types();

  // Ask the user to select the actual dexterous hand model installed on each side.
  DexHandType left_type;
  std::string left_label;
  if (!read_dexhand_type("left", left_type, left_label)) {
    std::cout << "Example canceled before robot initialization" << std::endl;
    return 0;
  }

  DexHandType right_type;
  std::string right_label;
  if (!read_dexhand_type("right", right_type, right_label)) {
    std::cout << "Example canceled before robot initialization" << std::endl;
    return 0;
  }

  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  // Initialize the robot SDK before sending commands or reading states.
  if (!robot.init()) {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  std::cout << "Initialization succeeded" << std::endl;
  std::this_thread::sleep_for(std::chrono::milliseconds(kStartupDelayMs));

  bool completed = true;

  // Run one initial open command, then run three close/open demo cycles.
  for (int action_index = 0; action_index < 1 + kCycleCount * 2; ++action_index) {
    const std::string action = (action_index == 0 || action_index % 2 == 0) ? "open" : "close";
    if (action_index > 0 && action == "close") {
      const int cycle_index = (action_index + 1) / 2;
      std::cout << "Running dexterous hand cycle " << cycle_index << "/" << kCycleCount << std::endl;
    }

    // Send the target position command to the left dexterous hand.
    const auto left_positions = dexhand_motion_positions(left_type, action);
    const auto left_command = make_dexhand_command(left_positions);
    const ControlStatus left_status = robot.set_dexhand_command(
        "left_dexhand",
        left_command,
        left_type,
        false);
    if (left_status != ControlStatus::SUCCESS) {
      std::cerr << "Failed to " << action << " left " << left_label
                << " dexterous hand (" << left_command.size() << " joints), status="
                << static_cast<int>(left_status) << std::endl;
      completed = false;
    } else {
      std::cout << "Left " << left_label << " dexterous hand " << action
                << " command sent (" << left_command.size() << " joints)" << std::endl;
    }

    // Send the same action to the right dexterous hand with its own model type.
    const auto right_positions = dexhand_motion_positions(right_type, action);
    const auto right_command = make_dexhand_command(right_positions);
    const ControlStatus right_status = robot.set_dexhand_command(
        "right_dexhand",
        right_command,
        right_type,
        false);
    if (right_status != ControlStatus::SUCCESS) {
      std::cerr << "Failed to " << action << " right " << right_label
                << " dexterous hand (" << right_command.size() << " joints), status="
                << static_cast<int>(right_status) << std::endl;
      completed = false;
    } else {
      std::cout << "Right " << right_label << " dexterous hand " << action
                << " command sent (" << right_command.size() << " joints)" << std::endl;
    }

    // Give the hardware or simulator time to execute the command before reading state.
    std::this_thread::sleep_for(std::chrono::milliseconds(kCommandIntervalMs));

    // Read back the left hand state after the command has been applied.
    DexhandState left_state;
    const ControlStatus left_state_status = robot.get_dexhand_state("left_dexhand", left_state, left_type);
    if (left_state_status != ControlStatus::SUCCESS) {
      std::cerr << "Failed to get left dexterous hand state!" << std::endl;
      completed = false;
    } else {
      print_dexhand_state("Left", left_state, left_type);
    }

    // Read back the right hand state after the command has been applied.
    DexhandState right_state;
    const ControlStatus right_state_status = robot.get_dexhand_state("right_dexhand", right_state, right_type);
    if (right_state_status != ControlStatus::SUCCESS) {
      std::cerr << "Failed to get right dexterous hand state!" << std::endl;
      completed = false;
    } else {
      print_dexhand_state("Right", right_state, right_type);
    }
  }

  if (completed) {
    std::cout << "get_set_dexhand_state example completed" << std::endl;
  } else {
    std::cout << "get_set_dexhand_state example completed with one or more errors" << std::endl;
  }

  // Release SDK resources after the initialized example finishes or fails.
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resources released successfully" << std::endl;

  return completed ? 0 : 1;
}

Get Force Sensor Data (get_force_sensor_data)

Applicable Scenarios: Obtain force and torque measurements for contact detection and force-aware manipulation.

examples/cpp/galbot_robot/src/get_force_sensor_data_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

std::string force_sensor_type_to_string(GalbotOneFoxtrotSensor sensor_type) {
    switch (sensor_type) {
        case GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE:
            return "LEFT_WRIST_FORCE";
        case GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE:
            return "RIGHT_WRIST_FORCE";
        default:
            return "UNKNOWN_FORCE_SENSOR";
    }
}

void print_force_data(GalbotOneFoxtrotSensor sensor_type, const std::shared_ptr<ForceData>& force_data) {
    std::cout << "--- " << force_sensor_type_to_string(sensor_type) << " ---" << std::endl;
    if (!force_data) {
        std::cerr << "  Force data is empty" << std::endl;
        return;
    }

    std::cout << "  Timestamp (ns): " << force_data->timestamp_ns << std::endl;
    std::cout << "  Force (N):  "
              << "fx=" << force_data->force.x << ", "
              << "fy=" << force_data->force.y << ", "
              << "fz=" << force_data->force.z << std::endl;
    std::cout << "  Torque (Nm): "
              << "tx=" << force_data->torque.x << ", "
              << "ty=" << force_data->torque.y << ", "
              << "tz=" << force_data->torque.z << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get left wrist force sensor data
    std::cout << "\n===== Get left wrist force sensor data =====" << std::endl;
    auto left_force_data = robot.get_force_sensor_data(GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE);
    print_force_data(GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE, left_force_data);

    // Get calibrated left-arm wrench expressed in base_link
    std::cout << "\n===== Get calibrated left wrist force data in base_link =====" << std::endl;
    auto calibrated_left_force_data =
        robot.get_force_sensor_data(GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE, true, "base_link");
    print_force_data(GalbotOneFoxtrotSensor::LEFT_WRIST_FORCE, calibrated_left_force_data);

    // Get right wrist force sensor data
    std::cout << "\n===== Get right wrist force sensor data =====" << std::endl;
    auto right_force_data = robot.get_force_sensor_data(GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE);
    print_force_data(GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE, right_force_data);

    // Keep calibrated right-arm wrench in its native end-effector mount frame
    std::cout << "\n===== Get calibrated right wrist force data in mount frame =====" << std::endl;
    auto calibrated_right_force_data = robot.get_force_sensor_data(
        GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE, true, "right_arm_end_effector_mount_link");
    print_force_data(GalbotOneFoxtrotSensor::RIGHT_WRIST_FORCE, calibrated_right_force_data);

    // Iterate and get all force sensor data
    std::cout << "\n===== Get all force sensor data =====" << std::endl;
    for (int i = 0; i < static_cast<int>(GalbotOneFoxtrotSensor::FORCE_NUM); ++i) {
        auto sensor_type = static_cast<GalbotOneFoxtrotSensor>(i);
        auto force_data = robot.get_force_sensor_data(sensor_type);
        print_force_data(sensor_type, force_data);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Odometry Data (get_odom)

Applicable Scenarios: Read the robot base pose and motion state reported by odometry.

examples/cpp/galbot_robot/src/get_odom_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_odom_data(const std::shared_ptr<OdomData>& odom_data) {
    if (!odom_data) {
        std::cerr << "Odom data is empty" << std::endl;
        return;
    }

    std::cout << "Timestamp (ns): " << odom_data->timestamp_ns << std::endl;

    std::cout << "Position (m): "
              << "x=" << odom_data->position[0] << ", "
              << "y=" << odom_data->position[1] << ", "
              << "z=" << odom_data->position[2] << std::endl;

    std::cout << "Orientation (quaternion): "
              << "qx=" << odom_data->orientation[0] << ", "
              << "qy=" << odom_data->orientation[1] << ", "
              << "qz=" << odom_data->orientation[2] << ", "
              << "qw=" << odom_data->orientation[3] << std::endl;

    std::cout << "Linear velocity (m/s): "
              << "vx=" << odom_data->linear_velocity[0] << ", "
              << "vy=" << odom_data->linear_velocity[1] << ", "
              << "vz=" << odom_data->linear_velocity[2] << std::endl;

    std::cout << "Angular velocity (rad/s): "
              << "wx=" << odom_data->angular_velocity[0] << ", "
              << "wy=" << odom_data->angular_velocity[1] << ", "
              << "wz=" << odom_data->angular_velocity[2] << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get odometry data
    std::shared_ptr<OdomData> odom_data = robot.get_odom();
    if (odom_data) {
        std::cout << "Odometry data retrieved successfully!" << std::endl;
        print_odom_data(odom_data);
    } else {
        std::cerr << "Failed to get odometry data!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Ultrasonic Data (get_ultrasonic_data)

Applicable Scenarios: Read ultrasonic distance measurements for short-range obstacle awareness.

examples/cpp/galbot_robot/src/get_ultrasonic_data_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>
#include <unordered_set>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

std::string ultrasonic_type_to_string(UltrasonicType ultrasonic_type) {
    switch (ultrasonic_type) {
        case UltrasonicType::FRONT_LEFT:
            return "FRONT_LEFT";
        case UltrasonicType::FRONT_RIGHT:
            return "FRONT_RIGHT";
        case UltrasonicType::RIGHT_LEFT:
            return "RIGHT_LEFT";
        case UltrasonicType::RIGHT_RIGHT:
            return "RIGHT_RIGHT";
        case UltrasonicType::BACK_LEFT:
            return "BACK_LEFT";
        case UltrasonicType::BACK_RIGHT:
            return "BACK_RIGHT";
        case UltrasonicType::LEFT_LEFT:
            return "LEFT_LEFT";
        case UltrasonicType::LEFT_RIGHT:
            return "LEFT_RIGHT";
        default:
            return "UNKNOWN_ULTRASONIC";
    }
}

void print_ultrasonic_data(UltrasonicType ultrasonic_type, const std::shared_ptr<UltrasonicData>& ultrasonic_data) {
    std::cout << "--- " << ultrasonic_type_to_string(ultrasonic_type) << " ---" << std::endl;
    if (!ultrasonic_data) {
        std::cerr << "  Ultrasonic data is empty" << std::endl;
        return;
    }

    std::cout << "  Timestamp (ns): " << ultrasonic_data->timestamp_ns << std::endl;
    std::cout << "  Distance (m): " << ultrasonic_data->distance << std::endl;
}

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize sensors; only sensors passed during initialization can retrieve data to save resources
    std::unordered_set<SensorType> sensor_types = {
        SensorType::BASE_ULTRASONIC  // Chassis ultrasonic sensor
    };

    // Initialize system
    if (robot.init(sensor_types)) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get single ultrasonic sensor data
    std::cout << "\n===== Get single ultrasonic sensor data =====" << std::endl;
    auto ultrasonic_data = robot.get_ultrasonic_data(UltrasonicType::FRONT_LEFT);
    print_ultrasonic_data(UltrasonicType::FRONT_LEFT, ultrasonic_data);

    // Iterate to get all 8 ultrasonic sensor data
    std::cout << "\n===== Get all ultrasonic sensor data =====" << std::endl;
    for (int i = 0; i < static_cast<int>(UltrasonicType::ULTRASONIC_NUM); ++i) {
        auto ultrasonic_type = static_cast<UltrasonicType>(i);
        auto data = robot.get_ultrasonic_data(ultrasonic_type);
        print_ultrasonic_data(ultrasonic_type, data);
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Installation Verification

Applicable Scenarios: Verify SDK installation, robot connection, initialization, and basic interface availability.

examples/cpp/galbot_robot/src/installation_welcome_verify.cpp

#include <chrono>
#include <condition_variable>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <unordered_set>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

constexpr double kSpeedRadS = 0.12;
constexpr double kTimeoutS = 20.0;
constexpr auto kVideoStreamTimeout = std::chrono::seconds(10);
const std::string kWelcomePcmRel{};

constexpr double kHeadArmsDemoDtS = 0.06;
constexpr int kHeadArmsDemoSeg1 = 28;
constexpr int kHeadArmsDemoSeg2 = 32;
constexpr int kHeadArmsDemoSeg3 = 28;
constexpr double kHeadSwayYawPosRad = 0.22;
constexpr double kHeadSwayYawNegRad = -0.22;

const std::vector<double> kPresetLeg = {0.5, 1.5, 1.0, 0.0, 0.0};
const std::vector<double> kPresetHead = {0.0, 0.0};
const std::vector<double> kPresetLeftArm = {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0};
const std::vector<double> kPresetRightArm = {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0};

const std::vector<std::string> kHeadArmsDemoGroups = {"head", "left_arm", "right_arm"};

TrajectoryPoint make_point(const std::vector<double>& joint_positions_rad, double time_from_start_sec) {
  TrajectoryPoint point;
  point.time_from_start_second = time_from_start_sec;
  for (double q : joint_positions_rad) {
    JointCommand jc;
    jc.position = q;
    point.joint_command_vec.push_back(jc);
  }
  return point;
}

Trajectory build_trajectory(const std::vector<std::vector<double>>& joint_waypoints_rad,
                            const std::vector<std::string>& joint_groups, double time_step_sec) {
  Trajectory trajectory;
  trajectory.joint_groups = joint_groups;
  trajectory.joint_names = {};
  double time_from_start_sec = 0.0;
  for (const std::vector<double>& waypoint : joint_waypoints_rad) {
    time_from_start_sec += time_step_sec;
    trajectory.points.push_back(make_point(waypoint, time_from_start_sec));
  }
  return trajectory;
}

std::vector<std::vector<double>> linspace_rows(const std::vector<double>& start_positions_rad,
                                               const std::vector<double>& end_positions_rad, int num_samples) {
  std::vector<std::vector<double>> out;
  if (num_samples < 2) {
    out.push_back(start_positions_rad);
    return out;
  }
  out.reserve(static_cast<size_t>(num_samples));
  const int n = num_samples - 1;
  for (int sample_idx = 0; sample_idx < num_samples; ++sample_idx) {
    const double blend = static_cast<double>(sample_idx) / static_cast<double>(n);
    std::vector<double> row(start_positions_rad.size());
    for (size_t j = 0; j < start_positions_rad.size(); ++j) {
      row[j] = start_positions_rad[j] + blend * (end_positions_rad[j] - start_positions_rad[j]);
    }
    out.push_back(std::move(row));
  }
  return out;
}

void append_rows(std::vector<std::vector<double>>& dest, const std::vector<std::vector<double>>& src) {
  dest.insert(dest.end(), src.begin(), src.end());
}

std::vector<double> upper_body_preset_pose_rad() {
  std::vector<double> pose;
  pose.reserve(kPresetHead.size() + kPresetLeftArm.size() + kPresetRightArm.size());
  pose.insert(pose.end(), kPresetHead.begin(), kPresetHead.end());
  pose.insert(pose.end(), kPresetLeftArm.begin(), kPresetLeftArm.end());
  pose.insert(pose.end(), kPresetRightArm.begin(), kPresetRightArm.end());
  return pose;
}

// Slow head yaw left-right (joint2 fixed); arms shift slightly with the sway. Legs not in trajectory.
std::vector<std::vector<double>> build_slow_head_arms_demo_waypoints_rad() {
  const std::vector<double> home = upper_body_preset_pose_rad();
  const std::vector<double> pose_yaw_pos = {
    kHeadSwayYawPosRad, 0.0,
    1.5 + 0.07, -1.36 + 0.04, -0.45, 1.53, -0.1, -0.42 + 0.04, 0.0,
    -1.5 - 0.07, 1.36 - 0.04, 0.45, -1.53, 0.1, 0.42 - 0.04, 0.0
  };
  const std::vector<double> pose_yaw_neg = {
    kHeadSwayYawNegRad, 0.0,
    1.5 - 0.05, -1.36 - 0.03, -0.45, 1.53, -0.1, -0.42 - 0.03, 0.0,
    -1.5 + 0.05, 1.36 + 0.03, 0.45, -1.53, 0.1, 0.42 + 0.03, 0.0
  };

  std::vector<std::vector<double>> rows;
  append_rows(rows, linspace_rows(home, pose_yaw_pos, kHeadArmsDemoSeg1));
  append_rows(rows, linspace_rows(pose_yaw_pos, pose_yaw_neg, kHeadArmsDemoSeg2));
  append_rows(rows, linspace_rows(pose_yaw_neg, home, kHeadArmsDemoSeg3));
  return rows;
}

std::string getenv_string(const std::string& key) {
  const char* v = std::getenv(key.c_str());
  return (v != nullptr && v[0] != '\0') ? std::string(v) : std::string();
}

std::string resolve_welcome_pcm_path() {
  const std::string env_pcm = getenv_string("GALBOT_WELCOME_PCM");
  if (!env_pcm.empty()) {
    return env_pcm;
  }
  return kWelcomePcmRel;
}

bool play_pcm(GalbotRobot& robot, const std::string& pcm_file_path) {
  constexpr std::size_t kPcmReadChunkBytes = 2560;
  const std::string audio_stream_id = "install_welcome_pcm";
  std::ifstream pcm_input(pcm_file_path, std::ios::binary);
  if (!pcm_input) {
    std::cerr << "[audio] Failed to open/read PCM: " << pcm_file_path << std::endl;
    return false;
  }
  std::vector<char> buf(kPcmReadChunkBytes);
  while (pcm_input) {
    pcm_input.read(buf.data(), static_cast<std::streamsize>(kPcmReadChunkBytes));
    const std::streamsize n = pcm_input.gcount();
    if (n <= 0) {
      break;
    }
    const std::string chunk(buf.data(), static_cast<std::size_t>(n));
    if (!robot.write_audio_stream_output(chunk, audio_stream_id)) {
      std::cerr << "[audio] write_audio_stream_output failed" << std::endl;
      return false;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
  }
  return true;
}

void print_summary(const std::string& step_description, bool step_passed) {
  std::cout << (step_passed ? "[PASS] " : "[FAIL] ") << step_description << std::endl;
}

// Verify that the head camera publishes a non-empty H.264 frame without writing
// installation-test data to disk. Shared state keeps callback data valid even if
// the SDK dispatch thread finishes the callback while unsubscription is running.
bool verify_head_h264_stream(GalbotRobot& robot) {
  struct VideoCheckState {
    std::mutex mutex;
    std::condition_variable frame_received;
    bool has_valid_frame{false};
    std::string format;
    std::size_t frame_size_bytes{0};
  };

  const auto state = std::make_shared<VideoCheckState>();
  const SensorStatus subscribe_status = robot.subscribe_video_data(
      SensorType::HEAD_LEFT_CAMERA, [state](const std::shared_ptr<EncodedVideoData>& video_data) {
        // Accept both the documented lowercase spelling and the uppercase middleware payload.
        if (video_data == nullptr || video_data->data.empty() ||
            (video_data->format != "h264" && video_data->format != "H264")) {
          return;
        }

        {
          std::lock_guard<std::mutex> lock(state->mutex);
          state->has_valid_frame = true;
          state->format = video_data->format;
          state->frame_size_bytes = video_data->data.size();
        }
        state->frame_received.notify_one();
      });
  if (subscribe_status != SensorStatus::SUCCESS) {
    std::cerr << "[FAIL] subscribe_video_data failed, status=" << static_cast<int>(subscribe_status) << std::endl;
    return false;
  }

  bool received_valid_frame = false;
  std::string received_format;
  std::size_t received_frame_size_bytes = 0;
  {
    std::unique_lock<std::mutex> lock(state->mutex);
    received_valid_frame =
        state->frame_received.wait_for(lock, kVideoStreamTimeout, [&state]() { return state->has_valid_frame; });
    if (received_valid_frame) {
      received_format = state->format;
      received_frame_size_bytes = state->frame_size_bytes;
    }
  }

  // Always unsubscribe after a successful subscription, including timeout paths.
  const SensorStatus unsubscribe_status = robot.unsubscribe_video_data(SensorType::HEAD_LEFT_CAMERA);
  if (unsubscribe_status != SensorStatus::SUCCESS) {
    std::cerr << "[FAIL] unsubscribe_video_data failed, status=" << static_cast<int>(unsubscribe_status) << std::endl;
    return false;
  }
  if (!received_valid_frame) {
    std::cerr << "[FAIL] Timed out waiting for a non-empty H.264 frame" << std::endl;
    return false;
  }

  std::cout << "Head camera H.264 frame: " << received_frame_size_bytes << " bytes format=" << received_format
            << std::endl;
  return true;
}

int main() {
  bool body_preset_step_passed = false;
  bool welcome_pcm_playback_ok = false;
  bool head_video_stream_ok = false;

  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  const std::unordered_set<SensorType> required_sensors = {SensorType::HEAD_LEFT_CAMERA};
  if (!robot.init(required_sensors)) {
    std::cerr << "GalbotRobot::init failed" << std::endl;
    return 1;
  }
  std::cout << "Robot initialized (HEAD_LEFT_CAMERA enabled)" << std::endl;
  std::this_thread::sleep_for(std::chrono::duration<double>(5.0));

  if (!robot.set_volume(100.0f)) {
    std::cerr << "[WARN] set_volume(100) failed; welcome PCM may play quietly" << std::endl;
  }

  bool leg_step_ok = false;
  bool upper_step_ok = false;
  bool demo_step_ok = false;

  const ControlStatus leg_set_status =
      robot.set_joint_positions(kPresetLeg, {"leg"}, {}, true, kSpeedRadS, kTimeoutS);
  leg_step_ok = (leg_set_status == ControlStatus::SUCCESS);
  if (!leg_step_ok) {
    std::cerr << "[FAIL] Leg set_joint_positions not SUCCESS (blocking)" << std::endl;
  }
  print_summary("Leg set_joint_positions (blocking)", leg_step_ok);

  if (leg_step_ok) {
    std::vector<double> upper_body_joint_targets_rad;
    upper_body_joint_targets_rad.insert(upper_body_joint_targets_rad.end(), kPresetHead.begin(), kPresetHead.end());
    upper_body_joint_targets_rad.insert(upper_body_joint_targets_rad.end(), kPresetLeftArm.begin(),
                                        kPresetLeftArm.end());
    upper_body_joint_targets_rad.insert(upper_body_joint_targets_rad.end(), kPresetRightArm.begin(),
                                        kPresetRightArm.end());

    const ControlStatus upper_body_set_status =
        robot.set_joint_positions(upper_body_joint_targets_rad, kHeadArmsDemoGroups, {}, true, kSpeedRadS, kTimeoutS);
    upper_step_ok = (upper_body_set_status == ControlStatus::SUCCESS);
    if (!upper_step_ok) {
      std::cerr << "[FAIL] head/left_arm/right_arm set_joint_positions not SUCCESS (blocking)" << std::endl;
    }
    print_summary("Upper set_joint_positions head+arms (blocking)", upper_step_ok);

    if (upper_step_ok) {
      const std::vector<std::vector<double>> demo_waypoints_rad = build_slow_head_arms_demo_waypoints_rad();
      const Trajectory demo_trajectory = build_trajectory(demo_waypoints_rad, kHeadArmsDemoGroups, kHeadArmsDemoDtS);
      const ControlStatus demo_exec_status = robot.execute_joint_trajectory(demo_trajectory, true);
      demo_step_ok = (demo_exec_status == ControlStatus::SUCCESS);
      if (!demo_step_ok) {
        std::cerr << "[FAIL] Head/arms execute_joint_trajectory not SUCCESS (blocking)" << std::endl;
      }
      print_summary("Head/arms demo trajectory (blocking)", demo_step_ok);
    } else {
      std::cerr << "[INFO] Skipping head/arms demo: upper set_joint_positions did not return SUCCESS." << std::endl;
      print_summary("Head/arms demo trajectory (blocking)", false);
    }
  } else {
    std::cerr << "[INFO] Skipping upper-body preset and head/arms demo: leg set_joint_positions not SUCCESS."
              << std::endl;
    print_summary("Upper set_joint_positions head+arms (blocking)", false);
    print_summary("Head/arms demo trajectory (blocking)", false);
  }

  body_preset_step_passed = leg_step_ok && upper_step_ok && demo_step_ok;

  const std::string resolved_welcome_pcm_path = resolve_welcome_pcm_path();
  std::cout << "PCM path: " << resolved_welcome_pcm_path << std::endl;
  if (resolved_welcome_pcm_path.empty()) {
    std::cerr << "[FAIL] Welcome PCM path is empty: set GALBOT_WELCOME_PCM (no built-in default path)."
              << std::endl;
    welcome_pcm_playback_ok = false;
  } else {
    std::ifstream pcm_probe(resolved_welcome_pcm_path, std::ios::binary);
    if (!pcm_probe) {
      std::cerr << "[FAIL] PCM file not found or not readable: " << resolved_welcome_pcm_path << std::endl;
      welcome_pcm_playback_ok = false;
    } else {
      welcome_pcm_playback_ok = play_pcm(robot, resolved_welcome_pcm_path);
    }
  }
  print_summary("Welcome PCM playback", welcome_pcm_playback_ok);

  head_video_stream_ok = verify_head_h264_stream(robot);
  print_summary("Head camera H.264 video stream", head_video_stream_ok);

  std::cout << "\n======== Summary ========" << std::endl;
  print_summary("Motion verify (blocking)", body_preset_step_passed);
  print_summary("Audio", welcome_pcm_playback_ok);
  print_summary("Head camera H.264 video stream", head_video_stream_ok);

  const bool installation_all_checks_passed =
      body_preset_step_passed && welcome_pcm_playback_ok && head_video_stream_ok;
  std::cout << (installation_all_checks_passed ? "\nOverall: PASS\n" : "\nOverall: FAIL\n");

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

  return installation_all_checks_passed ? 0 : 1;
}

Set Robot Configuration (set_config)

Applicable Scenarios: Configure supported robot services or runtime parameters through the unified configuration interface.

examples/cpp/galbot_robot/src/set_config_example.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <string>

#include "galbot_robot.hpp"

// Full list of settable fields (navigation service, camera services, motion
// planning service and control service), their meaning, type and valid range:
// docs/g3/en/set_config_reference.md

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Navigation service: adjust replanning thresholds.
    std::vector<ConfigItem> navigation_fields = {
        ConfigItem{"replan_threshold", 30.0},
        ConfigItem{"no_replan_threshold", 1.0},
    };
    ControlStatus navigation_status = robot.set_config(ConfigService::NAVIGATION, navigation_fields);
    if (navigation_status == ControlStatus::SUCCESS) {
        std::cout << "[Navigation] set_config succeeded. Restart the device for the new config to take effect."
                  << std::endl;
    } else {
        std::cerr << "[Navigation] set_config failed. Check the SDK log for details." << std::endl;
    }

    // Front head camera: set resolution.
    // color_width/color_height must be set together in the same call, and the
    // combination must be one of the documented valid resolutions.
    std::vector<ConfigItem> camera_fields = {
        ConfigItem{"color_width", static_cast<int64_t>(1280)},
        ConfigItem{"color_height", static_cast<int64_t>(992)},
    };
    ControlStatus camera_status = robot.set_config(ConfigService::FRONT_HEAD_CAMERA, camera_fields);
    if (camera_status == ControlStatus::SUCCESS) {
        std::cout << "[Camera] set_config succeeded. Restart the device for the new config to take effect."
                  << std::endl;
    } else {
        std::cerr << "[Camera] set_config failed. Check the SDK log for details." << std::endl;
    }

    // Motion planning service: set a non-per-chain field (plan_timeout).
    std::vector<ConfigItem> motion_plan_fields = {
        ConfigItem{"plan_timeout", 5.0},
    };
    ControlStatus motion_plan_status = robot.set_config(ConfigService::MOTION_PLAN, motion_plan_fields);
    if (motion_plan_status == ControlStatus::SUCCESS) {
        std::cout << "[MotionPlan] set_config succeeded. Restart the device for the new config to take effect."
                  << std::endl;
    } else {
        std::cerr << "[MotionPlan] set_config failed. Check the SDK log for details." << std::endl;
    }

    // Control service: report_error_skip is a general parameter shared by
    // both G-series and S1 (robot_config.toml).
    std::vector<ConfigItem> control_fields = {
        ConfigItem{"report_error_skip", static_cast<int64_t>(1)},
    };
    ControlStatus control_status = robot.set_config(ConfigService::CONTROL, control_fields);
    if (control_status == ControlStatus::SUCCESS) {
        std::cout << "[Control] set_config succeeded. Restart the device for the new config to take effect."
                  << std::endl;
    } else {
        std::cerr << "[Control] set_config failed. Check the SDK log for details." << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Read Robot Configuration (get_config)

Use case: Read the active scene's persisted configuration, or read built-in robot defaults to compare the two.

examples/cpp/galbot_robot/src/get_config_example.cpp

#include <iostream>
#include <string>
#include <type_traits>
#include <variant>
#include <vector>
#include "galbot_robot.hpp"
using namespace galbot::sdk;
int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  if (!robot.init()) return -1;
  const std::vector<std::string> keys = {"report_error_skip", "report_sensor_skip"};
  std::vector<ConfigItem> fields;
  auto status = robot.get_config(ConfigService::CONTROL, keys, &fields);
  for (const auto& field : fields) {
    std::cout << field.key << " = ";
    std::visit([](const auto& value) {
      using T = std::decay_t<decltype(value)>;
      if constexpr (std::is_arithmetic_v<T> || std::is_same_v<T, std::string>) std::cout << value;
      else std::cout << "[array, " << value.size() << " elements]";
    }, field.value);
    std::cout << std::endl;
  }
  fields.clear(); status = robot.get_config(ConfigService::CONTROL, keys, &fields, true);
  std::cout << "default status: " << static_cast<int>(status) << std::endl;
  robot.request_shutdown(); robot.wait_for_shutdown(); robot.destroy(); return 0;
}

Set Leg Height (set_leg_height)

Applicable Scenarios: Adjust the robot body height through the leg mechanism.

examples/cpp/galbot_robot/src/set_leg_height_example.cpp

#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_status(const std::string& operation, ControlStatus status) {
  if (status == ControlStatus::SUCCESS) {
    std::cout << operation << " succeeded." << std::endl;
  } else {
    std::cerr << operation << " failed, status: " << static_cast<int>(status) << std::endl;
  }
}

bool get_leg_height(GalbotRobot& robot, const std::string& label, double& height_m) {
  const auto [pose, timestamp_ns] = robot.get_transform("base_link", "head_base_link", 0, 500);
  std::cout << std::fixed << std::setprecision(3) << label << ": pose=[";
  for (size_t i = 0; i < 7; ++i) {
    std::cout << pose[i] << (i + 1 < 7 ? ", " : "");
  }
  std::cout << "], tf_timestamp_ns=" << timestamp_ns << std::endl;

  height_m = pose[2];
  return height_m >= 0.0;
}

bool confirm_leg_height_motion(double current_height_m, double target_height_m) {
  const char* direction = target_height_m < current_height_m ? "lower" : "raise";
  const double motion_distance_m = std::abs(target_height_m - current_height_m);
  std::cout << "The leg mechanism is about to " << direction << " the body by "
            << std::fixed << std::setprecision(3) << motion_distance_m
            << " m, from " << current_height_m << " m to " << target_height_m
            << " m. Confirm the surrounding environment is clear and safe to avoid collisions. "
            << "Enter y to continue; any other input cancels: ";
  std::string response;
  std::getline(std::cin, response);
  return response == "y" || response == "Y";
}

int main() {
  // Get and initialize the G3 robot instance.
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  std::cout << "Initializing robot..." << std::endl;
  if (!robot.init()) {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }
  std::cout << "System initialized successfully!" << std::endl;

  // Wait for WBC communication and state data to become ready.
  std::this_thread::sleep_for(std::chrono::seconds(2));

  constexpr double kHeightOffsetM = 0.20;
  constexpr double kDurationS = 4.0;

  double initial_height_m = 0.0;
  if (!get_leg_height(robot, "Initial leg height", initial_height_m)) {
    std::cerr << "Failed to get a valid initial leg height." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  const double lowered_height_m = initial_height_m - kHeightOffsetM;
  if (!confirm_leg_height_motion(initial_height_m, lowered_height_m)) {
    std::cout << "Leg height motion cancelled. Exiting program." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
  }

  std::cout << "Lowering leg height by " << kHeightOffsetM
            << " m to " << lowered_height_m << " m." << std::endl;
  auto status = robot.set_leg_height(lowered_height_m, kDurationS, true);
  print_status("set_leg_height(lowered)", status);

  double actual_lowered_height_m = 0.0;
  if (!get_leg_height(robot, "Height after lowering", actual_lowered_height_m)) {
    std::cerr << "Failed to get the current leg height; cancelling the restore motion." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }
  std::cout << "Lowered target error: " << std::abs(actual_lowered_height_m - lowered_height_m)
            << " m." << std::endl;

  if (!confirm_leg_height_motion(actual_lowered_height_m, initial_height_m)) {
    std::cout << "Leg height restore motion cancelled. Exiting program." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
  }

  std::cout << "Restoring initial leg height: " << initial_height_m << " m." << std::endl;
  status = robot.set_leg_height(initial_height_m, kDurationS, true);
  print_status("set_leg_height(initial)", status);

  double final_height_m = 0.0;
  get_leg_height(robot, "Final leg height", final_height_m);
  std::cout << "Restored height error: " << std::abs(final_height_m - initial_height_m)
            << " m." << std::endl;

  std::this_thread::sleep_for(std::chrono::milliseconds(500));
  std::cout << "switch_controller back to leg_pvt_ctrl" << std::endl;
  robot.switch_controller(G3ControllerName::LEG_PVT_CTRL);

  std::cout << "Waiting 1 second before shutdown..." << std::endl;
  std::this_thread::sleep_for(std::chrono::seconds(1));

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resources released successfully." << std::endl;
  return 0;
}

Whole-body End-effector Commands (set_end_effector_command / clear_end_effector_command)

Applicable Scenarios: High-frequency model-inference scenarios that require real-time Cartesian end-effector control. Use clear_end_effector_command to stop and clear the active real-time end-effector command.

examples/cpp/galbot_robot/src/set_end_effector_commands_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {
    // Get object instance
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // Wait for data to become available
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    // Get current WBC end-effector poses
    // Keys: "ree_pose", "lee_pose", "head_pose" — each is [x, y, z, qx, qy, qz, qw]
    auto ee_info = robot.get_wbc_end_effector_poses(); // Before using this function, ensure that `using_wbc` in the configuration file is set to `true`.
    std::cout << "\nCurrent WBC end-effector poses:" << std::endl;
    for (const auto& [frame, pose] : ee_info) {
        std::cout << "  Frame: " << frame << ", Pose: [";
        for (size_t i = 0; i < pose.size(); ++i) {
            std::cout << pose[i];
            if (i + 1 < pose.size()) std::cout << ", ";
        }
        std::cout << "]" << std::endl;
    }

    auto it = ee_info.find("ree_pose");
    if (it != ee_info.end() && it->second.size() >= 7) {
        std::vector<double> target_pose(it->second.begin(), it->second.begin() + 7);
        target_pose[0] -= 0.1;  // Move 10 cm in -X direction

        // Example: absolute pose — omit reference_frames to default every pose to "world"
        ControlStatus status = robot.set_end_effector_command(
            {target_pose},
            {"right_arm_end_effector_mount_link"}
        );

        if (status == ControlStatus::SUCCESS) {
            std::cout << "\nPublished end-effector command (default reference_frames -> world)" << std::endl;
        } else {
            std::cerr << "Failed to publish end-effector command!" << std::endl;
        }
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    // Clear end-effector commands
    ControlStatus clear_status = robot.clear_end_effector_command();
    if (clear_status == ControlStatus::SUCCESS) {
        std::cout << "End-effector commands cleared successfully!" << std::endl;
    } else {
        std::cerr << "Failed to clear end-effector commands!" << std::endl;
    }

    // Exit system and release SDK resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    std::cout << "Resource cleanup successful" << std::endl;

    return 0;
}

Class: GalbotMotion

Get Instance and Initialize (get_instance && init)

Applicable Scenarios: Get the motion planning module singleton and initialize. Must be called before using motion planning functions.

examples/cpp/galbot_motion/src/get_instance_init_example.cpp

#include <iostream>
#include <set>
#include <string>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (planner.init()) {
        std::cout << "Planner initialized successfully!" << std::endl;
    } else {
        std::cerr << "Planner initialization failed!" << std::endl;
        return -1;
    }
    
    if (robot.init()) {
        std::cout << "System initialized successfully!" << std::endl;
    } else {
        std::cerr << "System initialization failed!" << std::endl;
        return -1;
    }

    // You can still manage the robot lifecycle through GalbotRobot
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Forward Kinematics (Using Current State or Specified RobotStates) (forward_kinematics)

Applicable Scenarios: Calculate end effector pose based on given joint angles. Used for robotic arm task verification and state analysis.

examples/cpp/galbot_motion/src/forward_kinematics_example.cpp

#include <algorithm>
#include <chrono>
#include <iostream>
#include <map>
#include <string>
#include <thread>
#include <tuple>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Helper print function
void print_pose(const std::string& label, const std::tuple<MotionStatus, std::vector<double>>& res,
                GalbotMotion& planner) {
  std::cout << "[" << label << "] Status: " << planner.status_to_string(std::get<0>(res)) << std::endl;

  if (std::get<0>(res) == MotionStatus::SUCCESS) {
    std::cout << "End-effector pose: ";
    for (double v : std::get<1>(res)) {
      std::cout << v << " ";
    }
    std::cout << "\n" << std::endl;
  } else {
    std::cout << "Calculation failed!" << std::endl;
  }
}

int main() {
  auto& planner = GalbotMotion::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  if (planner.init()) {
    std::cout << "Planner initialized successfully!" << std::endl;
  } else {
    std::cerr << "Planner initialization failed!" << std::endl;
    return -1;
  }

  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
  bool navigation_ready = navigation.init();
  if (navigation_ready) {
    std::cout << "Navigation initialized successfully!" << std::endl;
  } else {
    std::cerr << "Navigation initialization failed! World/map-frame FK calls below will be skipped." << std::endl;
  }

  // Program started, waiting for data
  std::this_thread::sleep_for(std::chrono::milliseconds(3000));

  std::map<std::string, std::vector<double>> chain_joints = {
      {"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
      {"head", {0.0000, 0.0}},
      {"left_arm", {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0}},
      {"right_arm", {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0}}};

  std::vector<double> whole_body_joint;
  std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
  for (const auto& key : keys) {
    whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
  }

  std::vector<double> base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
  std::string end_link = "left_arm_end_effector_mount_link";
  std::string reference_frame = "base_link";

  // --- test 1: defaultparameters (current status) ---
  try {
    std::cout << ">> Executing: Basic forward kinematics..." << std::endl;
    auto res1 = planner.forward_kinematics(end_link, reference_frame);
    print_pose("Basic version", res1, planner);
  } catch (const std::exception& e) {
    std::cerr << "❌ Basic version exception: " << e.what() << std::endl;
  }

  // --- test 2: jointstatus + parameters ---
  try {
    std::cout << ">> Executing: Forward kinematics with custom joints..." << std::endl;

    std::unordered_map<std::string, std::vector<double>> custom_joint_state = {{"left_arm", chain_joints["left_arm"]}};
    auto custom_param_ptr = std::make_shared<Parameter>();
    auto res2 = planner.forward_kinematics(end_link, reference_frame, custom_joint_state, custom_param_ptr);

    print_pose("Custom parameters", res2, planner);

    if (navigation_ready) {
      auto res2_world = planner.forward_kinematics(end_link, "world", custom_joint_state, custom_param_ptr);
      print_pose("Custom joints in world frame", res2_world, planner);
    } else {
      std::cout << "[Custom joints in world frame] skipped: navigation not initialized." << std::endl;
    }
  } catch (const std::exception& e) {
    std::cerr << "❌ Custom-parameter exception: " << e.what() << std::endl;
  }

  // --- test 3: RobotStates forward kinematics(current status, planning)---
  try {
    std::cout << ">> Executing: Forward kinematics based on RobotStates..." << std::endl;

    RobotStates current_state = planner.get_robot_states();
    if (current_state.whole_body_joint.empty()) {
      std::cerr << "❌ RobotStates-based: Unable to get current body joint states; ensure WBC/sensors are ready." << std::endl;
    } else {
      auto ref_robot_state_ptr = std::make_shared<RobotStates>(std::move(current_state));
      auto res3 = planner.forward_kinematics_by_state(end_link, ref_robot_state_ptr, reference_frame,
                                                      std::make_shared<Parameter>());
      print_pose("Based on RobotStates", res3, planner);
    }
  } catch (const std::exception& e) {
    std::cerr << "❌ RobotStates-based exception: " << e.what() << std::endl;
  }

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

  return 0;
}

Get Link Names (get_link_names)

Applicable Scenarios: Get a list of names of all links in the robot model, used for collision detection and motion planning debugging.

examples/cpp/galbot_motion/src/get_link_names_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_link_names(const std::vector<std::string>& link_names, const std::string& title) {
  std::cout << title << " (total " << link_names.size() << " items):" << std::endl;
  for (size_t i = 0; i < link_names.size(); ++i) {
    std::cout << "  " << (i + 1) << ". " << link_names[i] << std::endl;
  }
}

int main() {
  // Get object instance
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  // Initialize system
  if (!motion.init()) {
    std::cerr << "GalbotMotion initialization failed!" << std::endl;
    return -1;
  }
  if (!robot.init()) {
    std::cerr << "GalbotRobot initialization failed!" << std::endl;
    return -1;
  }

  std::cout << "System initialized successfully!" << std::endl;

  // Program started, waiting for data
  std::this_thread::sleep_for(std::chrono::milliseconds(2000));

  try {
    // Get all link names
    std::vector<std::string> all_link_names = motion.get_link_names(false);
    print_link_names(all_link_names, "\nAll link names");

    // getend effectorexecute link
    std::vector<std::string> ee_link_names = motion.get_link_names(true);
    print_link_names(ee_link_names, "\nEnd-effector link names");
  } catch (const std::exception& e) {
    std::cerr << "Get link name exception: " << e.what() << std::endl;
  }

  // Exit system and release SDK resources
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();

  return 0;
}

Inverse Kinematics (Basic and RobotStates-based) (inverse_kinematics)

Applicable Scenarios: Solve for the required joint angles based on the desired end effector pose. This is a core step for operations like robotic arm grasping.

examples/cpp/galbot_motion/src/inverse_kinematics_example.cpp

#include <algorithm>
#include <chrono>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Helper function: print inverse kinematics result
void print_ik_result(const std::string& label,
                     const std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<double>>>& res,
                     GalbotMotion& planner) {
  auto status = std::get<0>(res);
  auto joint_map = std::get<1>(res);

  std::cout << "[" << label << "] Status feedback: " << planner.status_to_string(status) << std::endl;

  if (status == MotionStatus::SUCCESS) {
    std::cout << "✅ IK computation succeeded! Joint angles obtained:" << std::endl;
    for (const auto& [name, joints] : joint_map) {
      std::cout << "  - Chain [" << name << "]: ";
      for (double v : joints)
        std::cout << v << " ";
      std::cout << std::endl;
    }
  } else {
    std::cout << "❌ inverse kinematics failed, checkinput targetpose." << std::endl;
  }
  std::cout << "---------------------------------------------------" << std::endl;
}

int main() {
  auto& planner = GalbotMotion::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  if (planner.init()) {
    std::cout << "Planner initialized successfully!" << std::endl;
  } else {
    std::cerr << "Planner initialization failed!" << std::endl;
    return -1;
  }

  if (robot.init()) {
    std::cout << "System initialized successfully!" << std::endl;
  } else {
    std::cerr << "System initialization failed!" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // Joint state definition
  std::unordered_map<std::string, std::vector<double>> chain_joints = {
      {"leg", {0.4992, 1.4991, 1.0005, 0.0000, -0.0004}},
      {"head", {0.0000, 0.0}},
      {"left_arm", {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0}},
      {"right_arm", {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0}}};

  // Target pose definition (x, y, z, qx, qy, qz, qw)
  std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
      {"left_arm",  { 0.4827, 0.2329, 0.8883, 0.0405, 0.0396, -0.0604, 0.9966 }},
      {"right_arm", { 0.2818, -0.2430, 0.8389, -0.0456, 0.0479, 0.0573, 0.9962 }}}; // pose from fk

  // Whole-body joint vector concatenation (Leg -> Head -> Left Arm -> Right Arm)
  std::vector<double> whole_body_joint;
  std::vector<std::string> key_order = {"leg", "head", "left_arm", "right_arm"};
  for (const auto& key : key_order) {
    whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
  }

  // General configuration
  std::string reference_frame = "base_link";
  std::string target_frame = "EndEffector";
  std::string target_chain = "left_arm";
  auto params = std::make_shared<Parameter>();

  // Scenario 1: Single-chain inverse kinematics
  try {
    std::cout << ">> Running Scenario 1: Single-chain IK test..." << std::endl;
    std::vector<std::string> one_chain = {target_chain};

    auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain],  // 1. target_pose
                                          one_chain                           // 2. chain_names
                                          // target_frame,                      // 3. target_frame
                                          // reference_frame,                   // 4. reference_frame
                                          // {},                                // 5. initial_joint_positions (empty)
                                          // false,                             // 6. enable_collision_check (bool)
                                          // params                             // 7. params (shared_ptr)
    );
    print_ik_result("Single-chain inverse kinematics", res, planner);
    std::this_thread::sleep_for(std::chrono::milliseconds(800));
  } catch (const std::exception& e) {
    std::cerr << "Scenario 1 exception: " << e.what() << std::endl;
  }

  // Scenario 2: Arm chain + torso inverse kinematics
  try {
    std::cout << ">> Running Scenario 2: Arm chain + torso IK test..." << std::endl;
    std::vector<std::string> chain_with_torso = {target_chain, "torso"};

    auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], chain_with_torso, target_frame,
                                          reference_frame, {}, false, params);
    print_ik_result("Arm + torso inverse kinematics", res, planner);
    std::this_thread::sleep_for(std::chrono::milliseconds(800));
  } catch (const std::exception& e) {
    std::cerr << "Scenario 2 exception: " << e.what() << std::endl;
  }

  // Scenario 3: invalid chain combination
  try {
    std::cout << ">> Running Scenario 3: Invalid chain-combination test..." << std::endl;
    std::vector<std::string> error_chains = {target_chain, "torso", "head"};

    auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], error_chains, target_frame,
                                          reference_frame, {}, false, params);
    print_ik_result("Invalid chain combination detection", res, planner);
  } catch (const std::exception& e) {
    std::cerr << "Scenario 3 exception: " << e.what() << std::endl;
  }

  // Scenario 4: use reference joints (initial_joint_positions can specify chain joints as IK references; unspecified chain joints are filled with whole-body joints)
  try {
    std::cout << ">> Running Scenario 4: IK test with initial reference values..." << std::endl;
    std::vector<std::string> one_chain = {target_chain};

    auto res = planner.inverse_kinematics(chain_pose_baselink[target_chain], one_chain, target_frame, reference_frame,
                                          chain_joints, false, params);
    print_ik_result("Inverse kinematics with reference values", res, planner);
    std::this_thread::sleep_for(std::chrono::milliseconds(800));
  } catch (const std::exception& e) {
    std::cerr << "Scenario 4 exception: " << e.what() << std::endl;
  }

  // scenario 5: RobotStates inverse kinematics
  try {
    std::cout << ">> Running Scenario 5: IK test based on RobotStates..." << std::endl;

    // Construct RobotStates smart pointer
    auto ref_state = std::make_shared<RobotStates>();
    ref_state->chain_name = target_chain;
    ref_state->whole_body_joint = whole_body_joint;
    ref_state->base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};

    std::vector<std::string> one_chain = {target_chain};

    auto res = planner.inverse_kinematics_by_state(chain_pose_baselink[target_chain],  // 1. target_pose
                                                   one_chain,                          // 2. chain_names
                                                   target_frame,                       // 3. target_frame
                                                   reference_frame,                    // 4. reference_frame
                                                   ref_state,  // 5. reference_robot_states (shared_ptr)
                                                   false,      // 6. enable_collision_check (bool)
                                                   params      // 7. params (shared_ptr)
    );
    print_ik_result("RobotStates inverse kinematics", res, planner);
  } catch (const std::exception& e) {
    std::cerr << "Scenario 5 exception: " << e.what() << std::endl;
  }

  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resources released." << std::endl;

  return 0;
}

Get and Set End Effector Pose (get_set_end_effort_pos)

Applicable Scenarios: Directly get current end effector pose, or move the end effector to a specified pose. Integrates inverse kinematics calculation and execution.

examples/cpp/galbot_motion/src/get_set_end_effort_pos_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Helper function: print pose information
void print_pose_info(const std::string& label, const std::vector<double>& pose) {
    if (pose.size() == 7) {
        std::cout << "[" << label << "] Pose: "
                  << "pos(" << pose[0] << ", " << pose[1] << ", " << pose[2] << "), "
                  << "ori(" << pose[3] << ", " << pose[4] << ", " << pose[5] << ", " << pose[6] << ")" 
                  << std::endl;
    }
}

bool confirm_robot_safety() {
    std::cout << "WARNING: Release the emergency stop and clear obstacles around the robot." << std::endl;
    std::cout << "Continue? (y/n): " << std::flush;

    std::string response;
    if (!std::getline(std::cin, response)) {
        return false;
    }
    return response == "y" || response == "Y";
}

int main() {

    if (!confirm_robot_safety()) {
        std::cout << "Example cancelled." << std::endl;
        return 0;
    }

    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!planner.init()) {
        std::cerr << "GalbotMotion initialization failed" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot initialization failed" << std::endl;
        return -1;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    const std::vector<double> joint_pos = {
        0.5, 1.5, 1.0, 0.0, 0.0,
        0.0, 0.0,
        1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0,
        -1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0
    };
    const std::vector<std::string> joint_groups = {"leg", "head", "left_arm", "right_arm"};
    const ControlStatus move_status =
        robot.set_joint_positions(joint_pos, joint_groups, {}, true, 0.1, 30.0);
    if (move_status != ControlStatus::SUCCESS) {
        std::cerr << "Failed to move to the initial whole-body joint state: "
                  << static_cast<int>(move_status) << std::endl;
        robot.request_shutdown();
        robot.wait_for_shutdown();
        robot.destroy();
        return -1;
    }
    std::cout << "Moved to the initial whole-body joint state" << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
        {"leg",       {0.0541, -0.0013, 1.0364, 0.4970, 0.4964, 0.5037, 0.5028}},
        {"head",      {0.0519, -0.0011, 1.4145, -0.7061, -0.0029, -0.0056, 0.7081}},
        {"left_arm",  {0.4827, 0.2329, 0.8383, 0.0405, 0.0396, -0.0604, 0.9966}},
        {"right_arm", {0.2818, -0.2430, 0.8389, -0.0456, 0.0479, 0.0573, 0.9962}}
    };

    std::string reference_frame = "base_link";
    std::string target_frame = "EndEffector";
    std::string target_chain = "left_arm";
    auto custom_param = std::make_shared<Parameter>();

    // --- Scenario 1: Get end-effector pose (basic version) ---
    try {
        std::cout << ">> Scenario 1: Getting the basic end-effector pose..." << std::endl;
        std::string end_ee_link = "left_arm_end_effector_mount_link";

        auto res = planner.get_end_effector_pose(end_ee_link, reference_frame);
        
        MotionStatus status = std::get<0>(res);
        std::vector<double> pose = std::get<1>(res);

        std::cout << "Execution status: " << planner.status_to_string(status) << std::endl;
        if (status == MotionStatus::SUCCESS) {
            print_pose_info("Basic version", pose);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) {
        std::cerr << "❌ Scenario 1 exception: " << e.what() << std::endl;
    }

    // --- Scenario 2: Get end-effector pose by specified chain name + custom frame ---
    try {
        std::cout << ">> Scenario 2: Getting pose by specified chain name..." << std::endl;

        auto res = planner.get_end_effector_pose_on_chain(target_chain, target_frame, reference_frame);
        
        MotionStatus status = std::get<0>(res);
        std::vector<double> pose = std::get<1>(res);

        std::cout << "Execution status: " << planner.status_to_string(status) << std::endl;
        if (status == MotionStatus::SUCCESS) {
            print_pose_info("Specified chain name version", pose);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) {
        std::cerr << "❌ Scenario 2 exception: " << e.what() << std::endl;
    }

    // --- Scenario 3: Set end-effector pose ---
    try {
        std::cout << ">> Scenario 3: Setting end-effector pose..." << std::endl;
        
        std::string ee_frame = "left_arm";
        std::vector<double> target_pose = chain_pose_baselink[ee_frame];

        MotionStatus status = planner.set_end_effector_pose(
            target_pose,        // 1
            ee_frame,           // 2
            reference_frame,    // 3
            nullptr,            // 4. Important: pass nullptr if no specific reference state is used
            false,              // 5. enable_collision_check
            true,               // 6. is_blocking
            5.0,                // 7. timeout
            custom_param        // 8. params
        );

        std::cout << "Set status: " << planner.status_to_string(status) << std::endl;
        if (status == MotionStatus::SUCCESS) {
            std::cout << "✅ Command sent successfully (blocking wait mode)" << std::endl;
        }
    } catch (const std::exception& e) {
        std::cerr << "❌ Pose-setting exception: " << e.what() << std::endl;
    }

    // --- Scenario 4: Get end-effector pose again after execution ---
    try {
        std::cout << ">> Scenario 4: Getting the basic end-effector pose..." << std::endl;
        std::string end_ee_link = "left_arm_end_effector_mount_link";

        auto res = planner.get_end_effector_pose(end_ee_link, reference_frame);
        
        MotionStatus status = std::get<0>(res);
        std::vector<double> pose = std::get<1>(res);

        std::cout << "Execution status: " << planner.status_to_string(status) << std::endl;
        if (status == MotionStatus::SUCCESS) {
            print_pose_info("Basic version", pose);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) {
        std::cerr << "❌ Scenario 4 exception: " << e.what() << std::endl;
    }

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

    return 0;
}

Single Point Motion Planning (Joint Space and Cartesian Space) (single_point_planning)

Applicable Scenarios: Plan a collision-free motion trajectory from the current pose to a single target pose. Suitable for single-point target arrival tasks.

examples/cpp/galbot_motion/src/single_point_planning_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Define a trajectory return type to simplify the code
using TrajResult = std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<std::vector<double>>>>;

/**
 * Helper function: print planning result
 */
void print_plan_info(const std::string& label, const TrajResult& res, const std::string& chain_name, GalbotMotion& planner) {
    auto status = std::get<0>(res);
    auto traj_map = std::get<1>(res);

    std::cout << "[" << label << "] Status: " << planner.status_to_string(status) << std::endl;
    if (status == MotionStatus::SUCCESS) {
        if (traj_map.count(chain_name) && !traj_map[chain_name].empty()) {
            std::cout << "✅ Planning succeeded: trajectory points = " << traj_map[chain_name].size() << std::endl;
        } else {
            std::cout << "⚠️ Status is SUCCESS but trajectory is empty (possibly already at target)" << std::endl;
        }
    } else {
        std::cout << "❌ Planning failed" << std::endl;
    }
    std::cout << "---------------------------------------" << std::endl;
}

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!planner.init()) {
        std::cerr << "GalbotMotion init FAILED" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot init FAILED" << std::endl;
        return -1;
    }
    
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));

    std::unordered_map<std::string, std::vector<double>> chain_joints = {
        {"leg",       { 0.5, 1.5, 1.0, 0.0, 0.0}},
        {"head",      {0.0, 0.0}},
        {"left_arm",  {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0}},
        {"right_arm", {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0}}
    };

    // 此为 chain_joints 关节状态下对应的笛卡尔空间位姿 (left_arm 的 x方向移动0.2m 以做区别)
    std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
        {"leg",       {0.0541, -0.0013, 1.0364, 0.4970, 0.4964, 0.5037, 0.5028}},
        {"head",      {0.0519, -0.0011, 1.4145, -0.7061, -0.0029, -0.0056, 0.7081}},
        {"left_arm",  {0.4827, 0.2329, 0.8383, 0.0405, 0.0396, -0.0604, 0.9966}},
        {"right_arm", {0.2818, -0.2430, 0.8389, -0.0456, 0.0479, 0.0573, 0.9962}}
    };

    auto params = std::make_shared<Parameter>();
    std::string target_chain = "left_arm";

    // NOTE:
    // - GalbotMotion does NOT provide real-time obstacle perception / automatic environment updates today.
    // - When enable_collision_check=true, collision checking uses self-collision + objects you load manually via
    //   add_obstacle/attach_target_object (including point clouds if you load them explicitly).

    // Scenario 1: joint-space planning, target type = joint state
    try {
        std::cout << ">> Scenario 1: joint-space planning (joint target)..." << std::endl;

        // Construct target joint state
        auto target_joint = std::make_shared<JointStates>();
        target_joint->chain_name = target_chain;
        target_joint->joint_positions = chain_joints[target_chain];

        auto res = planner.motion_plan(
            target_joint,   // 1. target
            nullptr,        // 2. start (nullptr means start from current state)
            nullptr,        // 3. reference_robot_states
            false,          // 4. enable_collision_check
            params          // 5. params
        );

        print_plan_info("Joint-space planning (joint target)", res, target_chain, planner);
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    // Scenario 2: joint-space planning, target type = end-effector pose (Cartesian)
    try {
        std::cout << ">> Scenario 2: joint-space planning (pose target)..." << std::endl;

        // Construct target pose state
        auto target_pose = std::make_shared<PoseState>();
        target_pose->chain_name = target_chain;
        target_pose->frame_id = "EndEffector";
        target_pose->reference_frame = "base_link";
        target_pose->pose = chain_pose_baselink[target_chain];

        auto res = planner.motion_plan(
            target_pose,    // 1. target
            nullptr,        // 2. start
            nullptr,        // 3. reference_robot_states
            false,          // 4. enable_collision_check
            params          // 5. params
        );

        print_plan_info("Joint-space planning (pose target)", res, target_chain, planner);
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    // Scenario 3: joint-space planning with an explicit start state
    try {
        std::cout << ">> Scenario 3: joint-space planning (explicit start)..." << std::endl;

        auto target_joint = std::make_shared<JointStates>();
        target_joint->chain_name = target_chain;
        target_joint->joint_positions = chain_joints[target_chain];

        auto start_joint = std::make_shared<JointStates>();
        start_joint->chain_name = target_chain;
        start_joint->joint_positions = std::vector<double>(7, 0.0); // Use seven zeros as the starting point

        auto res = planner.motion_plan(
            target_joint, 
            start_joint,    // 2. Specify the start point
            nullptr, 
            false, 
            params
        );

        print_plan_info("Joint-space planning (explicit start)", res, target_chain, planner);
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

Multi-Waypoint Trajectory Planning (multi_point_planning)

Applicable Scenarios: Plan a continuous motion trajectory through multiple waypoints, suitable for complex motions that need to pass through multiple intermediate points.

examples/cpp/galbot_motion/src/multi_point_planning_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>
#include <algorithm>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Trajectory return type definition
using TrajResult = std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<std::vector<double>>>>;

/**
 * Helper function: print planning result
 */
void print_multi_plan_result(const std::string& label, const TrajResult& res, const std::string& chain_name, GalbotMotion& planner) {
    auto status = std::get<0>(res);
    auto traj_map = std::get<1>(res);

    std::cout << "[" << label << "] Status feedback: " << planner.status_to_string(status) << std::endl;
    if (status == MotionStatus::SUCCESS) {
        if (traj_map.count(chain_name) && !traj_map[chain_name].empty()) {
            std::cout << "✅ Multi-point planning succeeded: total trajectory points = " << traj_map[chain_name].size() << std::endl;
        } else {
            std::cout << "⚠️ Status is SUCCESS but trajectory is empty; target may overlap current pose." << std::endl;
        }
    } else {
        std::cout << "❌ Multi-point planning failed." << std::endl;
    }
    std::cout << "---------------------------------------------------" << std::endl;
}

int main() {
    std::cout << "WARNING: The robot will move to the initial joint state. "
              << "Release the emergency stop and clear nearby obstacles." << std::endl;
    std::cout << "Continue? (y/n): ";
    std::string response;
    std::getline(std::cin, response);
    if (response != "y" && response != "Y") {
        std::cout << "Example cancelled." << std::endl;
        return 0;
    }

    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!planner.init()) {
        std::cerr << "GalbotMotion initialization failed" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot initialization failed" << std::endl;
        return -1;
    }
    
    std::this_thread::sleep_for(std::chrono::seconds(2));

    std::unordered_map<std::string, std::vector<double>> chain_joints = {
        {"leg",       {0.5, 1.5, 1.0, 0.0, 0.0}},
        {"head",      {0.0, 0.0}},
        {"left_arm",  {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0}},
        {"right_arm", {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
    for (const auto& key : keys) {
        whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
    }

    const ControlStatus move_status =
        robot.set_joint_positions(whole_body_joint, keys, {}, true, 0.1, 30.0);
    if (move_status != ControlStatus::SUCCESS) {
        std::cerr << "❌ Failed to move to the initial joint state." << std::endl;
        robot.request_shutdown();
        robot.wait_for_shutdown();
        robot.destroy();
        return -1;
    }
    std::cout << "✅ Moved to the initial whole-body joint state." << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(3));

    auto params = std::make_shared<Parameter>();
    std::string target_chain = "left_arm";

    // NOTE: The two scenarios express the same physical waypoints as Cartesian
    // poses and joint positions, so their planning results are consistent. Calling
    // params->set_move_line(true) applies Cartesian-space line planning to both.
    // 注意：以下两个场景分别使用笛卡尔位姿和关节位置表达相同的物理路点，
    // 因此规划结果一致。调用 params->set_move_line(true) 后，二者均按
    // 笛卡尔空间直线方式规划。

    // --- Scenario 1: Multi-waypoint planning in Cartesian space (PoseState target) ---
    try {
        std::cout << ">> Running Scenario 1: Multi-waypoint planning in Cartesian space..." << std::endl;

        auto target_pose_state = std::make_shared<PoseState>();
        target_pose_state->chain_name = target_chain;

        // Construct waypoints (3 intermediate poses)
        std::vector<std::vector<double>> waypoint_poses = {
            {0.2826, 0.2329, 0.8382, 0.0405, 0.0397, -0.0604, 0.9966},
            {0.3826, 0.2329, 0.8382, 0.0405, 0.0397, -0.0604, 0.9966},
            {0.4826, 0.2329, 0.8382, 0.0405, 0.0397, -0.0604, 0.9966},
            {0.4826, 0.2329, 0.9382, 0.0405, 0.0397, -0.0604, 0.9966}
        };

        auto res = planner.motion_plan_multi_waypoints(
            target_pose_state,
            waypoint_poses,
            nullptr,  // start
            nullptr,  // reference_robot_states
            false,    // enable_collision_check
            params    // params
        );

        print_multi_plan_result("Cartesian multi-waypoint single-chain planning", res, target_chain, planner);
        std::this_thread::sleep_for(std::chrono::milliseconds(800));
    } catch (const std::exception& e) { std::cerr << "Scenario 1 exception: " << e.what() << std::endl; }

    // --- Scenario 2: Multi-waypoint planning in joint space (JointStates target) ---
    try {
        std::cout << ">> Running Scenario 2: Multi-waypoint planning in joint space..." << std::endl;

        auto target_joint = std::make_shared<JointStates>();
        target_joint->chain_name = target_chain;

        // Construct waypoints (3 intermediate poses)
        std::vector<std::vector<double>> waypoints = {
            {1.5001, -1.3601, -0.4500, 1.5298, -0.1000, -0.4201, 0.0001},
            {1.1402, -1.3833, -0.4217, 1.2422, -0.1168, -0.3496, 0.0364},
            {0.6887, -1.4364, -0.3930, 0.6654, -0.1178, -0.4724, 0.0895},
            {0.8356, -1.3735, -0.4206, 1.2988, -0.1444, 0.0090, 0.0550}
        };

        auto res = planner.motion_plan_multi_waypoints(
            target_joint,
            waypoints,
            nullptr,
            nullptr,
            false,
            params
        );

        print_multi_plan_result("Joint-space multi-waypoint", res, target_chain, planner);
    } catch (const std::exception& e) { std::cerr << "Scenario 2 exception: " << e.what() << std::endl; }

    // 4. Clean up resources
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

Collision Detection (collision_detection)

Applicable Scenarios: Check whether a given joint configuration will cause self-collision or collision with environmental obstacles. Used for feasibility checking before motion planning.

examples/cpp/galbot_motion/src/collision_detection_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>
#include <thread>
#include <chrono>
#include <tuple>
#include <memory>
#include <stdexcept>
#include <algorithm>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"
#include "galbot_navigation.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);

    // NOTE:
    // - GalbotNavigation (galbotNav) may use real-time obstacle perception/avoidance during navigation (deployment dependent).
    // - GalbotMotion does NOT provide real-time obstacle perception today; Motion collision uses self-collision +
    //   manually loaded obstacles (add_obstacle/attach_target_object).

    if (!planner.init()) {
        std::cerr << "GalbotMotion init FAILED" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot init FAILED" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "GalbotNavigation init FAILED" << std::endl;
        return -1;
    }

    std::unordered_map<std::string, std::vector<double>> chain_joints = {
        {"leg",       {0.5, 1.5, 1.0, 0.0, 0.0}},
        {"head",      {0.0, 0.0}},
        {"left_arm",  {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0}},
        {"right_arm", {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"leg", "head", "left_arm", "right_arm"};
    for (const auto& key : keys) {
        whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
    }

    std::vector<double> base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
    std::vector<double> bad_left_arm_joint = {1.00217,-1.60012,0.450993,1.19433,0.450993,-0.387987,0.076236};

    auto custom_param = std::make_shared<Parameter>();

    try {
        std::cout << ">> Running collision check..." << std::endl;

        // Construct a RobotStates list for collision checking
        std::vector<std::shared_ptr<RobotStates>> check_states;

        // status 0: status (RobotStates)
        auto state0 = std::make_shared<RobotStates>();
        state0->whole_body_joint = whole_body_joint;
        state0->base_state = base_state;
        check_states.push_back(state0);

        // status 1: status (JointStates RobotStates)
        auto state1 = std::make_shared<JointStates>();
        state1->chain_name = "left_arm";
        state1->joint_positions = bad_left_arm_joint;
        check_states.push_back(state1);

        // Call collision detection interface
        auto res = planner.check_collision(check_states, true, custom_param);

        MotionStatus status = std::get<0>(res);
        std::vector<bool> collision_results = std::get<1>(res);

        std::cout << "Status: " << planner.status_to_string(status) << std::endl;

        if (status == MotionStatus::SUCCESS) {
            std::cout << "OK: collision check finished (false=no collision, true=collision):" << std::endl;
            for (size_t i = 0; i < collision_results.size(); ++i) {
                std::cout << "  - status [" << i << "]: " 
                          << (collision_results[i] ? "COLLISION" : "NO COLLISION") 
                          << std::endl;
            }
        } else {
            std::cerr << "ERROR: collision check returned failure." << std::endl;
        }

    } catch (const std::exception& e) {
        std::cerr << "ERROR: collision check exception: " << e.what() << std::endl;
    }

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

    return 0;
}

Attach Tool (attach_tool)

Applicable Scenarios: Attach a tool (such as a gripper, suction cup, etc.) to the robot end effector, updating the motion planning model to account for tool mass and collision volume.

examples/cpp/galbot_motion/src/attach_tool_example.cpp

#include <iostream>
#include <string>
#include <thread>
#include <chrono>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!planner.init()) {
        std::cerr << "GalbotMotion initialization failed" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot initialization failed" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // --- Execute tool-attach operation ---
    try {
        std::string chain_name = "left_arm";
        std::string tool_name = "suction_cup";

        MotionStatus status = planner.attach_tool(chain_name, tool_name);

        std::cout << "Execution status feedback: " << planner.status_to_string(status) << std::endl;

        if (status == MotionStatus::SUCCESS) {
            std::cout << "✅ Tool attached successfully: " << tool_name << std::endl;

            // Clean up the attached tool model to avoid affecting subsequent examples.
            status = planner.detach_tool(chain_name);
            std::cout << "Cleanup status feedback: " << planner.status_to_string(status) << std::endl;

            if (status == MotionStatus::SUCCESS) {
                std::cout << "✅ Tool detached successfully." << std::endl;
            } else {
                std::cerr << "❌ Tool cleanup failed." << std::endl;
            }
        } else {
            std::cerr << "❌ Tool attachment failed. Please check whether the tool name is defined in the configuration file." << std::endl;
        }

    } catch (const std::exception& e) {
        std::cerr << "❌ Exception occurred during runtime: " << e.what() << std::endl;
    }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

Detach Tool (detach_tool)

Applicable Scenarios: Remove an attached tool from the robot end effector, restoring the original robot model.

examples/cpp/galbot_motion/src/detach_tool_example.cpp

#include <iostream>
#include <string>
#include <thread>
#include <chrono>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!planner.init()) {
        std::cerr << "GalbotMotion initialization failed" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot initialization failed" << std::endl;
        return -1;
    }

    // Program started, waiting for data
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // --- Execute tool-detach operation ---
    try {
        std::string chain_name = "left_arm";
        MotionStatus status = planner.detach_tool(chain_name);

        std::cout << "Execution status feedback: " << planner.status_to_string(status) << std::endl;

        if (status == MotionStatus::SUCCESS) {
            std::cout << "✅ Tool detached successfully." << std::endl;
        } else {
            std::cerr << "❌ Tool detachment failed." << std::endl;
        }

    } catch (const std::exception& e) {
        std::cerr << "❌ Exception occurred during runtime: " << e.what() << std::endl;
    }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

Add/Remove Environment Collision Objects (env_collider_operation)

Applicable Scenarios: Add obstacles to the motion planning environment or remove added obstacles, enabling motion planning to account for environmental obstacles.

examples/cpp/galbot_motion/src/env_collider_operation_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <array>
#include <thread>
#include <chrono>
#include <memory>
#include <stdexcept>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

/*
    NOTE:
    - GalbotMotion does NOT provide real-time obstacle perception / automatic environment updates today.
    - To make Motion collision checking consider environmental obstacles, you must load them manually
        (e.g., box/mesh/point_cloud via add_obstacle/attach_target_object).
    - Real-time perception / navigation-style obstacle updates in Motion is a planned future feature.
*/

int main() {
    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!planner.init()) {
        std::cerr << "GalbotMotion init FAILED" << std::endl;
        return -1;
    }
    if (!robot.init()) {
        std::cerr << "GalbotRobot init FAILED" << std::endl;
        return -1;
    }
    
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // Scenario 1: add a Box collision object into Motion environment.
    try {
        std::cout << ">> Scenario 1: add obstacle..." << std::endl;

        std::string obstacle_id = "box_test_1";
        std::string obj_type = "box";
        std::vector<double> obj_pose = {1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        std::array<double, 3> obj_scale = {1.0, 1.0, 1.0};
        std::string target_frame = "world";

        MotionStatus status = planner.add_obstacle(
            obstacle_id,      // 1. ID
            obj_type,         // 2. Type
            obj_pose,         // 3. Pose
            obj_scale,        // 4. Scale (array)
            "",               // 5. key
            target_frame,     // 6. target_frame
            "",               // 7. ee_frame
            {},               // 8. ref_joints
            {},               // 9. ref_base
            {},               // 10. ignore_links
            0.0,              // 11. safe_margin
            0.0               // 12. resolution
        );

        std::cout << "Status: " << planner.status_to_string(status) << std::endl;
        
        planner.clear_obstacle();
        
        if (status == MotionStatus::SUCCESS) {
            std::cout << "OK: obstacle added" << std::endl;
        }
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    // Scenario 2: add a duplicate ID (expected to fail).
    try {
        std::cout << "\n>> Scenario 2: test duplicate ID..." << std::endl;

        std::string obstacle_id = "box_test_2";
        std::string obj_type = "box";
        std::vector<double> obj_pose = {1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
        std::array<double, 3> obj_scale = {1.0, 1.0, 1.0};
        std::string target_frame = "world";

        // First addition
        planner.add_obstacle(obstacle_id, obj_type, obj_pose, obj_scale, "", target_frame, "", {}, {}, {}, 0.0, 0.0);

        // Second addition with same ID
        MotionStatus status = planner.add_obstacle(obstacle_id, obj_type, obj_pose, obj_scale, "", target_frame, "", {}, {}, {}, 0.0, 0.0);

        std::cout << "Status: " << planner.status_to_string(status) << std::endl;
        
        planner.clear_obstacle();

        if (status == MotionStatus::FAULT) {
            std::cout << "OK: duplicate ID rejected (expected)" << std::endl;
        }
    } catch (const std::exception& e) { std::cerr << e.what() << std::endl; }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return 0;
}

Detailed Collision Check (check_collision_detail)

Applicable Scenarios: Inspect detailed collision results and identify the robot links or environment objects involved.

examples/cpp/galbot_motion/src/check_collision_detail_example.cpp

#include <iostream>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_collision_details(GalbotMotion& motion, const std::string& title, MotionStatus status,
                             const std::vector<CollisionInfo>& collision_infos) {
  std::cout << title << " status: " << motion.status_to_string(status) << std::endl;
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (collision_infos.empty()) {
    std::cout << "  no collision pair returned" << std::endl;
    return;
  }
  for (size_t i = 0; i < collision_infos.size(); ++i) {
    const auto& info = collision_infos[i];
    std::cout << "  - pair [" << i << "]: " << (info.is_collision ? "COLLISION" : "NO COLLISION") << ", "
              << info.link1 << " <-> " << info.link2 << ", distance: " << info.distance
              << ", type: " << info.collision_type << std::endl;
  }
}

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  auto& navigation = GalbotNavigation::get_instance(MachineType::G3);

  if (!motion.init()) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  if (!robot.init()) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "GalbotNavigation init FAILED" << std::endl;
    return -1;
  }

  auto params = std::make_shared<Parameter>();
  params->set_timeout(5.0);

  try {
    std::cout << ">> Detailed collision check: current robot state" << std::endl;
    auto current_result = motion.check_collision_detail(std::vector<std::shared_ptr<RobotStates>>{}, false, false, params);
    print_collision_details(motion, "current robot state", std::get<0>(current_result), std::get<1>(current_result));

    std::unordered_map<std::string, std::vector<double>> chain_joints = {
        {"leg",       {0.5, 1.5, 1.0, 0.0, 0.0}},
        {"head",      {0.0, 0.0}},
        {"left_arm",  {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0}},
        {"right_arm", {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0}}
    };
    std::vector<double> whole_body_joint;
    for (const auto& key : {"leg", "head", "left_arm", "right_arm"}) {
      whole_body_joint.insert(whole_body_joint.end(), chain_joints[key].begin(), chain_joints[key].end());
    }

    std::vector<std::shared_ptr<RobotStates>> robot_states;
    auto whole_body_state = std::make_shared<RobotStates>();
    whole_body_state->whole_body_joint = whole_body_joint;
    whole_body_state->base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
    robot_states.push_back(whole_body_state);

    auto left_arm_state = std::make_shared<JointStates>();
    left_arm_state->chain_name = "left_arm";
    left_arm_state->joint_positions = {1.00217, -1.60012,0.385718,1.19433,0.450993,-0.452389,0.241414};
    robot_states.push_back(left_arm_state);

    std::cout << ">> Detailed collision check: explicit robot states" << std::endl;
    auto explicit_result = motion.check_collision_detail(robot_states, false, true, params);
    print_collision_details(motion, "explicit robot states", std::get<0>(explicit_result), std::get<1>(explicit_result));
  } catch (const std::exception& e) {
    std::cerr << "ERROR: check_collision_detail exception: " << e.what() << std::endl;
  }

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

  return 0;
}

Combined Motion Planning (combine_plan)

Applicable Scenarios: Combine multiple chain targets into one coordinated planning request.

examples/cpp/galbot_motion/src/combine_plan_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

using TrajMap = std::unordered_map<std::string, std::vector<std::vector<double>>>;
using TrajResult = std::tuple<MotionStatus, TrajMap>;

struct CurrentRobotState {
  std::unordered_map<std::string, std::vector<std::string>> joint_names;
  std::unordered_map<std::string, std::vector<double>> joints;
  std::unordered_map<std::string, std::vector<double>> poses;
};

struct ExampleInputs {
  std::vector<PlanRequest> plan_reqs;
  Parameter params;
};

struct StartJointState {
  std::vector<double> leg;
  std::vector<double> head;
  std::vector<double> left_arm;
  std::vector<double> right_arm;
};

const StartJointState kCombinePlanStartState = {
    {0.4992, 1.4991, 1.0005, 0.0, -0.0004},
    {0.0, 0.0},
    {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0},
    {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0},
};

constexpr double kStartMoveSpeedRadS = 0.12;
constexpr double kStartMoveTimeoutS = 30.0;
const std::vector<std::string> kUpperBodyGroups = {"head", "left_arm", "right_arm"};

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

std::vector<double> pose_to_vector(const Pose& pose) {
  return {pose.position.x,    pose.position.y,    pose.position.z,   pose.orientation.x,
          pose.orientation.y, pose.orientation.z, pose.orientation.w};
}

void print_api_status(const std::string& api_name, bool success) {
  std::cout << api_name << " status: " << (success ? "SUCCESS" : "FAILED") << std::endl;
}

void print_api_status(const std::string& api_name, MotionStatus status, GalbotMotion& motion) {
  std::cout << api_name << " status: " << motion.status_to_string(status) << " ("
            << (status == MotionStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_api_status(const std::string& api_name, ControlStatus status) {
  std::cout << api_name << " status: " << control_status_to_string(status) << " ("
            << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

void print_no_status_api_completed(const std::string& api_name) {
  std::cout << api_name << " status: no return status; call completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  print_no_status_api_completed("request_shutdown()");
  robot.wait_for_shutdown();
  print_no_status_api_completed("wait_for_shutdown()");
  robot.destroy();
  print_no_status_api_completed("destroy()");
}

std::vector<double> make_upper_body_start_positions(const StartJointState& start_state) {
  std::vector<double> positions;
  positions.reserve(start_state.head.size() + start_state.left_arm.size() + start_state.right_arm.size());
  positions.insert(positions.end(), start_state.head.begin(), start_state.head.end());
  positions.insert(positions.end(), start_state.left_arm.begin(), start_state.left_arm.end());
  positions.insert(positions.end(), start_state.right_arm.begin(), start_state.right_arm.end());
  return positions;
}

bool move_robot_to_start_state(GalbotRobot& robot) {
  std::cout << "Move robot to combine_plan start state." << std::endl;
  std::cout << "  leg start joints: ";
  print_vector(kCombinePlanStartState.leg);
  std::cout << std::endl;

  // const ControlStatus controller_status = robot.switch_controller(G3ControllerName::LEG_PVT_CTRL);
  // print_api_status("GalbotRobot::switch_controller(leg_pvt_ctrl)", controller_status);
  // if (controller_status != ControlStatus::SUCCESS) {
  //   return false;
  // }
  // std::this_thread::sleep_for(std::chrono::milliseconds(500));
  const ControlStatus leg_status =
      robot.set_joint_positions(kCombinePlanStartState.leg, {"leg"}, {}, true, kStartMoveSpeedRadS, kStartMoveTimeoutS);
  print_api_status("GalbotRobot::set_joint_positions(leg start)", leg_status);
  if (leg_status != ControlStatus::SUCCESS) {
    return false;
  }

  const std::vector<double> upper_body_start = make_upper_body_start_positions(kCombinePlanStartState);
  std::cout << "  upper-body start joints [head, left_arm, right_arm]: ";
  print_vector(upper_body_start);
  std::cout << std::endl;

  const ControlStatus upper_body_status =
      robot.set_joint_positions(upper_body_start, kUpperBodyGroups, {}, true, kStartMoveSpeedRadS, kStartMoveTimeoutS);
  print_api_status("GalbotRobot::set_joint_positions(head+arms start)", upper_body_status);
  return upper_body_status == ControlStatus::SUCCESS;
}

MotionPlanChainTarget make_joint_target(const std::string& chain, const std::vector<double>& joint_positions,
                                        const std::vector<std::string>& joint_names = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kJoint;
  target.joint.chain_name = chain;
  target.joint.joint_positions = joint_positions;
  target.joint.joint_names = joint_names;
  return target;
}

MotionPlanChainTarget make_cartesian_target(const std::string& chain, const std::vector<double>& pose,
                                            const std::vector<std::string>& assist_chains = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kCartesian;
  target.cart.chain_name = chain;
  target.cart.frame_id = "EndEffector";
  target.cart.reference_frame = "base_link";
  target.cart.pose = Pose(pose);
  target.cart.assist_chains.insert(assist_chains.begin(), assist_chains.end());
  return target;
}


bool capture_current_joints(GalbotMotion& motion, GalbotRobot& robot, const std::vector<std::string>& chains,
                            CurrentRobotState& current) {
  for (const auto& chain : chains) {
    if (current.joints.find(chain) != current.joints.end()) {
      continue;
    }

    std::vector<std::string> joint_names = motion.get_chain_joint_names(chain);
    print_api_status("get_chain_joint_names(" + chain + ")", !joint_names.empty());
    if (joint_names.empty()) {
      std::cerr << "Failed to get joint names for chain: " << chain << std::endl;
      return false;
    }

    std::vector<double> joint_positions = robot.get_joint_positions({}, joint_names);
    const bool joint_positions_success = joint_positions.size() == joint_names.size();
    print_api_status("GalbotRobot::get_joint_positions(" + chain + ")", joint_positions_success);
    if (!joint_positions_success) {
      std::cerr << "Failed to get current joint positions for chain: " << chain << std::endl;
      return false;
    }

    current.joint_names[chain] = joint_names;
    current.joints[chain] = joint_positions;

    std::cout << "Current " << chain << " joints: ";
    print_vector(joint_positions);
    std::cout << std::endl;
  }
  return true;
}

bool capture_current_pose(GalbotMotion& motion, GalbotRobot& robot, const std::string& chain,
                          CurrentRobotState& current) {
  if (!capture_current_joints(motion, robot, {chain}, current)) {
    return false;
  }

  auto [status, pose] = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link");
  print_api_status("get_end_effector_pose_on_chain(" + chain + ")", status, motion);
  if (status != MotionStatus::SUCCESS || pose.size() != 7) {
    std::cerr << "Failed to get current pose for chain: " << chain << std::endl;
    return false;
  }

  current.poses[chain] = pose;
  std::cout << "Current " << chain << " pose [x, y, z, qx, qy, qz, qw]: ";
  print_vector(pose);
  std::cout << std::endl;
  return true;
}

ExampleInputs make_example_inputs(const CurrentRobotState& current) {
  ExampleInputs inputs;

  const std::vector<std::string> left_arm_joint_names = current.joint_names.at("left_arm");
  const std::vector<std::string> right_arm_joint_names = current.joint_names.at("right_arm");

  PlanRequest request0;
  request0.plan_type = MotionPlanType::MOTION_PLAN;
  request0.enforce_pass = true;
  request0.target = {
      {make_joint_target("left_arm", {1.2, -1.0, -0.2, 1.4, 0.1, -0.3, 0.0},
                         left_arm_joint_names),
       make_cartesian_target("right_arm", {0.26, -0.2324, 0.76428, -0.0453, 0.0152, -0.068, 0.996}, {"torso"})},
      {make_joint_target("left_arm", {1.46326, -1.36281, -0.44721, 1.50544, -0.10183, -0.40826, 0.00287},
                         left_arm_joint_names)},
      {make_joint_target("left_arm", {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0},
                         left_arm_joint_names),
       make_cartesian_target("right_arm", {0.26, -0.2324, 0.76428, -0.0453, 0.0152, -0.068, 0.996}, {"torso"})},
  };

  PlanRequest request1;
  request1.plan_type = MotionPlanType::MOVE_LINE;
  request1.enforce_pass = true;
  request1.target = {
      {make_cartesian_target("left_arm",
                             {0.302504, 0.232819, 0.819552, 0.0413871, 0.0393282, -0.0675238, 0.996083},
                             {"torso"}),
       make_cartesian_target("right_arm",
                             {0.304916, -0.240799, 0.822258, -0.0411131, 0.0379809, 0.0507316, 0.997143},
                             {"torso"})},
      {make_cartesian_target("left_arm",
                             {0.272504, 0.232819, 0.819552, 0.0413871, 0.0393282, -0.0675238, 0.996083},
                             {"torso"})},
      {make_cartesian_target("right_arm",
                             {0.274916, -0.240799, 0.822258, -0.0411131, 0.0379809, 0.0507316, 0.997143},
                             {"torso"})},
  };

  PlanRequest request2;
  request2.plan_type = MotionPlanType::MOTION_PLAN;
  request2.enforce_pass = true;
  request2.target = {
      {make_joint_target("left_arm", {1.2, -1.0, -0.2, 1.4, 0.1, -0.3, 0.0},
                         left_arm_joint_names)},
      {make_joint_target("right_arm", {-1.2, 1.0, 0.2, -1.4, -0.1, 0.3, 0.0},
                         right_arm_joint_names)},
      {make_joint_target("torso", {0.0, 0.0}, {"leg_joint4", "leg_joint5"})},
  };

  inputs.plan_reqs = {request0, request1, request2};

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(true);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_torso");

  return inputs;
}

std::string plan_type_to_string(MotionPlanType plan_type) {
  switch (plan_type) {
    case MotionPlanType::MOTION_PLAN:
      return "MOTION_PLAN";
    case MotionPlanType::TRAJ_PLAN:
      return "TRAJ_PLAN";
    case MotionPlanType::MOVE_LINE:
      return "MOVE_LINE";
    default:
      return "UNKNOWN";
  }
}

void print_joint_names(const std::vector<std::string>& joint_names) {
  if (joint_names.empty()) {
    return;
  }
  std::cout << " joint_names=[";
  for (size_t i = 0; i < joint_names.size(); ++i) {
    std::cout << joint_names[i];
    if (i + 1 < joint_names.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

void print_plan_requests(const std::vector<PlanRequest>& plan_requests) {
  std::cout << "combine_plan plan requests: " << plan_requests.size() << std::endl;
  for (size_t request_index = 0; request_index < plan_requests.size(); ++request_index) {
    const auto& request = plan_requests[request_index];
    std::cout << "  request[" << request_index << "] type=" << plan_type_to_string(request.plan_type)
              << " waypoints=" << request.target.size() << std::endl;
    for (size_t waypoint_index = 0; waypoint_index < request.target.size(); ++waypoint_index) {
      std::cout << "    waypoint[" << waypoint_index << "] targets: " << request.target[waypoint_index].size()
                << std::endl;
      for (const auto& target : request.target[waypoint_index]) {
        if (target.mode == MotionPlanTargetMode::kJoint) {
          std::cout << "      " << target.chain_name << " JOINT q=";
          print_vector(target.joint.joint_positions);
          print_joint_names(target.joint.joint_names);
          std::cout << std::endl;
        } else {
          std::cout << "      " << target.chain_name << " CART pose=";
          print_vector(pose_to_vector(target.cart.pose));
          std::cout << " frame_id=" << target.cart.frame_id
                    << " reference_frame=" << target.cart.reference_frame;
          if (!target.cart.assist_chains.empty()) {
            std::cout << " assist_chains=[";
            size_t assist_chain_index = 0;
            for (const auto& assist_chain : target.cart.assist_chains) {
              std::cout << assist_chain;
              if (++assist_chain_index < target.cart.assist_chains.size()) {
                std::cout << ", ";
              }
            }
            std::cout << "]";
          }
          std::cout << std::endl;
        }
      }
    }
  }
}

void print_traj_result(const std::string& label, const TrajResult& result, GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& traj = std::get<1>(result);
  print_api_status(label, status, motion);
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (traj.empty()) {
    std::cout << "Trajectory map is empty." << std::endl;
    return;
  }
  for (const auto& [chain, points] : traj) {
    std::cout << "  " << chain << " trajectory points: " << points.size() << std::endl;
  }
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  print_no_status_api_completed("GalbotMotion::get_instance()");
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  print_no_status_api_completed("GalbotRobot::get_instance()");

  const bool motion_init_success = motion.init();
  print_api_status("GalbotMotion::init()", motion_init_success);
  if (!motion_init_success) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  const bool robot_init_success = robot.init();
  print_api_status("GalbotRobot::init()", robot_init_success);
  if (!robot_init_success) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  if (!move_robot_to_start_state(robot)) {
    shutdown_robot(robot);
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  CurrentRobotState current;
  if (!capture_current_pose(motion, robot, "left_arm", current) ||
      !capture_current_pose(motion, robot, "right_arm", current)) {
    shutdown_robot(robot);
    return -1;
  }

  const ExampleInputs inputs = make_example_inputs(current);
  print_plan_requests(inputs.plan_reqs);

  std::cout << "Immediate execution is enabled; plan requests are loaded from combine_file_example_plan_reqs.json reference values."
            << std::endl;

  auto result = motion.combine_plan(inputs.plan_reqs, inputs.params);
  print_traj_result("combine_plan", result, motion);

  shutdown_robot(robot);
  return std::get<0>(result) == MotionStatus::SUCCESS ? 0 : 1;
}

Get Chain Joint Names (get_chain_joint_names)

Applicable Scenarios: Query the ordered joint names belonging to a kinematic chain before constructing joint states or targets.

examples/cpp/galbot_motion/src/get_chain_joint_names_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

struct ExampleInputs {
  std::vector<std::string> chain_names;
};

ExampleInputs make_example_inputs() {
  ExampleInputs inputs;

  // get_chain_joint_names(chain_name) input.
  inputs.chain_names = {
      "left_arm",
      "right_arm",
      "leg",
      "head",
      "invalid_chain",
  };

  return inputs;
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  // Initialize SDK interfaces directly in the example so users can see the required order.
  if (!motion.init()) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  if (!robot.init()) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  const ExampleInputs inputs = make_example_inputs();
  for (const auto& chain : inputs.chain_names) {
    // This is the API being demonstrated: query joint names by chain name.
    const auto joint_names = motion.get_chain_joint_names(chain);
    std::cout << "get_chain_joint_names(" << chain << "): ";
    if (joint_names.empty()) {
      std::cout << "<empty>";
    } else {
      for (const auto& name : joint_names) {
        std::cout << name << " ";
      }
    }
    std::cout << std::endl;
  }

  // Clean shutdown releases SDK resources before the process exits.
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return 0;
}

Get Jacobian Matrix (get_jacobian)

Applicable Scenarios: Calculate an end-effector Jacobian for differential kinematics and velocity analysis.

examples/cpp/galbot_motion/src/get_jacobian_example.cpp

#include <chrono>
#include <iomanip>
#include <iostream>
#include <map>
#include <set>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    if (i > 0) {
      std::cout << ", ";
    }
    std::cout << values[i];
  }
  std::cout << "]";
}

void print_joint_state(const std::unordered_map<std::string, std::vector<double>>& joint_state) {
  std::cout << "{" << std::endl;
  for (const auto& pair : joint_state) {
    std::cout << "    " << pair.first << ": ";
    print_vector(pair.second);
    std::cout << std::endl;
  }
  std::cout << "  }" << std::endl;
}

void print_get_jacobian_params(const std::string& chain_name,
                               const std::string& target_frame,
                               const std::string& reference_frame,
                               const std::unordered_map<std::string, std::vector<double>>& joint_state) {
  std::cout << "get_jacobian parameters:" << std::endl;
  std::cout << "  chain_name: " << chain_name << std::endl;
  std::cout << "  target_frame: " << target_frame << std::endl;
  std::cout << "  reference_frame: " << reference_frame << std::endl;
  std::cout << "  joint_state: ";
  print_joint_state(joint_state);
}

void print_jacobian(const std::string& label,
                    const std::tuple<MotionStatus, std::vector<std::vector<double>>>& result,
                    GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& matrix = std::get<1>(result);

  std::cout << "[" << label << "] Status: " << motion.status_to_string(status) << std::endl;
  if (status != MotionStatus::SUCCESS || matrix.empty()) {
    std::cout << "Jacobian computation failed or returned an empty matrix." << std::endl;
    return;
  }

  std::cout << "Jacobian matrix (" << matrix.size() << "x" << matrix[0].size() << "):" << std::endl;
  for (size_t r = 0; r < matrix.size(); ++r) {
    std::cout << "  row " << r << ": ";
    for (double value : matrix[r]) {
      std::cout << std::setw(10) << std::fixed << std::setprecision(5) << value << " ";
    }
    std::cout << std::endl;
  }
}

std::vector<double> make_fixed_joint_values(size_t dof) {
  static const std::vector<double> fixed_values = {-1.2, -0.8, -0.4, 0.0, 0.4, 0.8, 1.2};
  std::vector<double> values;
  values.reserve(dof);
  for (size_t i = 0; i < dof; ++i) {
    values.push_back(fixed_values[i % fixed_values.size()]);
  }
  return values;
}

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  if (!motion.init()) {
    std::cerr << "GalbotMotion initialization failed." << std::endl;
    return 1;
  }
  std::cout << "GalbotMotion initialized successfully" << std::endl;

  if (!robot.init()) {
    std::cerr << "GalbotRobot initialization failed." << std::endl;
    return 1;
  }
  std::cout << "GalbotRobot initialized successfully" << std::endl;

  std::this_thread::sleep_for(std::chrono::seconds(3));

  std::set<std::string> support_chains = motion.get_support_chains();
  std::cout << "support_chains (" << support_chains.size() << "): [";
  for (auto it = support_chains.begin(); it != support_chains.end(); ++it) {
    if (it != support_chains.begin()) {
      std::cout << ", ";
    }
    std::cout << *it;
  }
  std::cout << "]" << std::endl;
  if (support_chains.empty()) {
    support_chains = {"head", "left_arm", "right_arm", "torso", "leg"};
  }

  std::unordered_map<std::string, std::vector<double>> current_chain_joint_state;
  try {
    current_chain_joint_state = motion.get_chain_joint_state();
  } catch (const std::exception& e) {
    std::cerr << "get_chain_joint_state exception: " << e.what() << std::endl;
  }

  const std::string target_frame = "EndEffector";
  const std::string reference_frame = "base_link";

  // Known chain DOF used only when the runtime joint state is unavailable.
  const std::unordered_map<std::string, size_t> fallback_dof = {
      {"head", 2}, {"left_arm", 7}, {"right_arm", 7}, {"leg", 5}};

  for (const auto& chain_name : support_chains) {
    // Prefer the runtime joint count; fall back to a known DOF when it is unavailable.
    size_t dof = 0;
    const auto current_it = current_chain_joint_state.find(chain_name);
    if (current_it != current_chain_joint_state.end() && !current_it->second.empty()) {
      dof = current_it->second.size();
    } else {
      const auto fb = fallback_dof.find(chain_name);
      if (fb != fallback_dof.end()) {
        dof = fb->second;
      }
    }
    if (dof == 0) {
      std::cerr << "Skip chain '" << chain_name << "': unable to determine chain DOF." << std::endl;
      continue;
    }

    std::unordered_map<std::string, std::vector<double>> joint_state = {
        {chain_name, make_fixed_joint_values(dof)}};

    try {
      std::cout << "=== get_jacobian: " << chain_name << " ===" << std::endl;
      print_get_jacobian_params(chain_name, target_frame, reference_frame, joint_state);
      auto result = motion.get_jacobian(chain_name, target_frame, reference_frame, joint_state);
      print_jacobian(chain_name, result, motion);
    } catch (const std::exception& e) {
      std::cerr << chain_name << " exception: " << e.what() << std::endl;
    }
  }

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

  return 0;
}

Get Jacobian from Robot State (get_jacobian_by_state)

Applicable Scenarios: Calculate a Jacobian at an explicitly supplied robot state instead of the current state.

examples/cpp/galbot_motion/src/get_jacobian_by_state_example.cpp

#include <chrono>
#include <iomanip>
#include <iostream>
#include <memory>
#include <set>
#include <string>
#include <thread>
#include <tuple>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    if (i > 0) {
      std::cout << ", ";
    }
    std::cout << values[i];
  }
  std::cout << "]";
}

void print_robot_states(const std::shared_ptr<RobotStates>& robot_states) {
  if (!robot_states) {
    std::cout << "nullptr (use current complete robot state)" << std::endl;
    return;
  }

  std::cout << "RobotStates" << std::endl;
  std::cout << "    whole_body_joint[" << robot_states->whole_body_joint.size() << "]: ";
  print_vector(robot_states->whole_body_joint);
  std::cout << std::endl;
  std::cout << "    base_state[" << robot_states->base_state.size() << "]: ";
  print_vector(robot_states->base_state);
  std::cout << std::endl;
}

void print_get_jacobian_by_state_params(const std::string& chain_name,
                                        const std::string& target_frame,
                                        const std::string& reference_frame,
                                        const std::shared_ptr<RobotStates>& reference_robot_states) {
  std::cout << "get_jacobian_by_state parameters:" << std::endl;
  std::cout << "  chain_name: " << chain_name << std::endl;
  std::cout << "  target_frame: " << target_frame << std::endl;
  std::cout << "  reference_frame: " << reference_frame << std::endl;
  std::cout << "  reference_robot_states: ";
  print_robot_states(reference_robot_states);
}

void print_jacobian(const std::string& label,
                    const std::tuple<MotionStatus, std::vector<std::vector<double>>>& result,
                    GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& matrix = std::get<1>(result);

  std::cout << "[" << label << "] Status: " << motion.status_to_string(status) << std::endl;
  if (status != MotionStatus::SUCCESS || matrix.empty()) {
    std::cout << "Jacobian computation failed or returned an empty matrix." << std::endl;
    return;
  }

  std::cout << "Jacobian matrix (" << matrix.size() << "x" << matrix[0].size() << "):" << std::endl;
  for (size_t r = 0; r < matrix.size(); ++r) {
    std::cout << "  row " << r << ": ";
    for (double value : matrix[r]) {
      std::cout << std::setw(10) << std::fixed << std::setprecision(5) << value << " ";
    }
    std::cout << std::endl;
  }
}

std::vector<double> make_fixed_whole_body_joint(size_t dof) {
  static const std::vector<double> fixed_values = {
      0.3, 0.8, 0.5, 0.0, 0.0, 0.1, -0.2, 1.2, -0.8, 0.4, -1.0,
      0.2, -0.6, 0.0, -1.2, 0.8, -0.4, 1.0, -0.2, 0.6, 0.0};
  std::vector<double> values;
  values.reserve(dof);
  for (size_t i = 0; i < dof; ++i) {
    values.push_back(fixed_values[i % fixed_values.size()]);
  }
  return values;
}

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  if (!motion.init()) {
    std::cerr << "GalbotMotion initialization failed." << std::endl;
    return 1;
  }
  std::cout << "GalbotMotion initialized successfully" << std::endl;

  if (!robot.init()) {
    std::cerr << "GalbotRobot initialization failed." << std::endl;
    return 1;
  }
  std::cout << "GalbotRobot initialized successfully" << std::endl;

  std::this_thread::sleep_for(std::chrono::seconds(3));

  std::set<std::string> support_chains = motion.get_support_chains();
  std::cout << "support_chains (" << support_chains.size() << "): [";
  for (auto it = support_chains.begin(); it != support_chains.end(); ++it) {
    if (it != support_chains.begin()) {
      std::cout << ", ";
    }
    std::cout << *it;
  }
  std::cout << "]" << std::endl;
  if (support_chains.empty()) {
    support_chains = {"head", "left_arm", "right_arm", "torso", "leg"};
  }

  const std::string target_frame = "EndEffector";
  const std::string reference_frame = "base_link";

  size_t whole_body_dof = 21;
  try {
    RobotStates current_state = motion.get_robot_states();
    if (!current_state.whole_body_joint.empty()) {
      whole_body_dof = current_state.whole_body_joint.size();
    }
  } catch (const std::exception& e) {
    std::cerr << "get_robot_states exception: " << e.what() << std::endl;
  }

  auto reference_robot_states = std::make_shared<RobotStates>();
  reference_robot_states->whole_body_joint = make_fixed_whole_body_joint(whole_body_dof);
  reference_robot_states->base_state = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};

  // Compute the Jacobian for every chain using an explicit RobotStates
  for (const auto& chain_name : support_chains) {
    try {
      std::cout << "=== get_jacobian_by_state (explicit RobotStates): " << chain_name << " ===" << std::endl;
      print_get_jacobian_by_state_params(chain_name, target_frame, reference_frame, reference_robot_states);
      auto result = motion.get_jacobian_by_state(chain_name, target_frame, reference_frame, reference_robot_states);
      print_jacobian(chain_name + " explicit RobotStates", result, motion);
    } catch (const std::exception& e) {
      std::cerr << chain_name << " explicit RobotStates exception: " << e.what() << std::endl;
    }
  }

  // Compute the Jacobian for every chain using nullptr (the current robot state)
  for (const auto& chain_name : support_chains) {
    try {
      std::cout << "=== get_jacobian_by_state (nullptr RobotStates): " << chain_name << " ===" << std::endl;
      std::shared_ptr<RobotStates> null_robot_states = nullptr;
      print_get_jacobian_by_state_params(chain_name, target_frame, reference_frame, null_robot_states);
      auto result = motion.get_jacobian_by_state(chain_name, target_frame, reference_frame, null_robot_states);
      print_jacobian(chain_name + " nullptr RobotStates", result, motion);
    } catch (const std::exception& e) {
      std::cerr << chain_name << " nullptr RobotStates exception: " << e.what() << std::endl;
    }
  }

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

  return 0;
}

General Inverse Kinematics

Applicable Scenarios: Solve inverse kinematics using general target and constraint configuration for more complex planning tasks.

examples/cpp/galbot_motion/src/inverse_kinematics_general_example.cpp

#include <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

struct ExampleInputs {
  std::vector<double> left_arm_target_pose;
  MotionPlanWaypoint target_waypoint;
  std::shared_ptr<RobotStates> reference_state;
  std::shared_ptr<Parameter> params;
};

ExampleInputs make_example_inputs() {
  ExampleInputs inputs;

  // Fixed Cartesian pose for the left_arm IK target.
  // Pose layout: [x, y, z, qx, qy, qz, qw], reference frame: base_link.
  inputs.left_arm_target_pose = {
      0.405795,
      0.510630,
      0.872605,
      -0.0701184,
      -0.0226926,
      0.202097,
      0.976589,
  };

  MotionPlanChainTarget left_arm_target;
  left_arm_target.chain_name = "left_arm";
  left_arm_target.mode = MotionPlanTargetMode::kCartesian;
  left_arm_target.cart.chain_name = "left_arm";
  left_arm_target.cart.frame_id = "left_arm_end_effector_mount_link";
  left_arm_target.cart.reference_frame = "base_link";
  left_arm_target.cart.pose = Pose(inputs.left_arm_target_pose);
  left_arm_target.cart.assist_chains.insert("leg");

  // inverse_kinematics_general(target_waypoint, reference_state, params) input: target_waypoint.
  inputs.target_waypoint = {
      left_arm_target,
  };

  // inverse_kinematics_general input: reference_state. nullptr lets the service choose its default seed.
  inputs.reference_state = nullptr;

  // inverse_kinematics_general input: params.
  inputs.params = std::make_shared<Parameter>();
  inputs.params->set_direct_execute(false);
  inputs.params->set_blocking(true);
  inputs.params->set_timeout(20.0);
  inputs.params->set_check_collision(false);
  inputs.params->set_reference_frame("base_link");

  return inputs;
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  // Initialize SDK interfaces directly in the example so users can see the required order.
  if (!motion.init()) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  if (!robot.init()) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  const ExampleInputs inputs = make_example_inputs();
  std::cout << "Fixed target left_arm pose [x, y, z, qx, qy, qz, qw]: ";
  print_vector(inputs.left_arm_target_pose);
  std::cout << std::endl;

  // This is the API being demonstrated: solve joint values for the target waypoint.
  auto [status, joint_map] =
      motion.inverse_kinematics_general(inputs.target_waypoint, inputs.reference_state, inputs.params);

  std::cout << "inverse_kinematics_general status: " << motion.status_to_string(status) << std::endl;
  for (const auto& [chain, joints] : joint_map) {
    std::cout << "  " << chain << " joint names: ";
    for (const auto& name : joints.joint_names) {
      std::cout << name << " ";
    }
    std::cout << std::endl << "  " << chain << " joint positions: ";
    print_vector(joints.joint_positions);
    std::cout << std::endl;
  }

  // Clean shutdown releases SDK resources before the process exits.
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  return status == MotionStatus::SUCCESS ? 0 : 1;
}

General Motion Planning (motion_plan)

Applicable Scenarios: Construct and execute a general motion-planning request for one or more robot chains.

examples/cpp/galbot_motion/src/motion_plan_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"
#include "galbot_sdk_type.hpp"

/*
 * This example focuses on the low-level multi-waypoint overload of
 * GalbotMotion::motion_plan():
 *
 *   motion_plan(const MotionPlanWaypoints& waypoints,
 *               const PlannerConfig& params,
 *               const std::shared_ptr<RobotStates>& start_state = nullptr)
 *
 * It demonstrates the main request-building features of that API:
 * - A MotionPlanWaypoints request is an ordered list of waypoints.
 * - Each waypoint can contain one target per chain, so the planner can
 *   coordinate both arms at the same waypoint index.
 * - MotionPlanChainTarget supports joint-space targets and Cartesian
 *   end-effector pose targets in the same request.
 * - Cartesian targets specify the controlled frame, the reference frame,
 *   and optional assist chains.
 * - Parameter / PlannerConfig carries common planning and execution settings
 *   such as direct execution, blocking behavior, timeout, collision checking,
 *   and reference frame.
 * - The return value is a status plus a trajectory map keyed by chain name.
 *
 * The example first moves the robot to a fixed G3 start posture, then sends a
 * set of G3-specific joint and Cartesian waypoints.
 */
using namespace galbot::sdk;

namespace {

using TrajMap = std::unordered_map<std::string, std::vector<std::vector<double>>>;
using TrajResult = std::tuple<MotionStatus, TrajMap>;

struct CurrentRobotState {
  std::unordered_map<std::string, std::vector<std::string>> joint_names;
  std::unordered_map<std::string, std::vector<double>> joints;
  std::unordered_map<std::string, std::vector<double>> poses;
};

struct ExampleInputs {
  MotionPlanWaypoints waypoints;
  Parameter params;
};

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

std::vector<double> pose_to_vector(const Pose& pose) {
  return {pose.position.x,    pose.position.y,    pose.position.z,   pose.orientation.x,
          pose.orientation.y, pose.orientation.z, pose.orientation.w};
}

void print_api_status(const std::string& api_name, bool success) {
  std::cout << api_name << " status: " << (success ? "SUCCESS" : "FAILED") << std::endl;
}

void print_api_status(const std::string& api_name, MotionStatus status, GalbotMotion& motion) {
  std::cout << api_name << " status: " << motion.status_to_string(status) << " ("
            << (status == MotionStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_api_status(const std::string& api_name, ControlStatus status) {
  std::cout << api_name << " status: " << control_status_to_string(status) << " ("
            << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

void print_no_status_api_completed(const std::string& api_name) {
  std::cout << api_name << " status: no return status; call completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  print_no_status_api_completed("request_shutdown()");
  robot.wait_for_shutdown();
  print_no_status_api_completed("wait_for_shutdown()");
  robot.destroy();
  print_no_status_api_completed("destroy()");
}

bool move_robot_to_home_position(GalbotRobot& robot) {
  std::cout << "Move robot to the G3 home position." << std::endl;
  const std::vector<double> joint_positions = {
      0.5, 1.5, 1.0, 0.0, 0.0,
      0.0, 0.0,
      1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0,
      -1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0};
  const std::vector<std::string> joint_group_names = {"leg", "head", "left_arm", "right_arm"};
  const ControlStatus status =
      robot.set_joint_positions(joint_positions, joint_group_names, {}, true, 0.1, 30.0);
  print_api_status("GalbotRobot::set_joint_positions(home)", status);
  return status == ControlStatus::SUCCESS;
}

MotionPlanChainTarget make_joint_target(const std::string& chain, const std::vector<double>& joint_positions,
                                        const std::vector<std::string>& joint_names = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kJoint;
  target.joint.chain_name = chain;
  target.joint.joint_positions = joint_positions;
  target.joint.joint_names = joint_names;
  return target;
}

MotionPlanChainTarget make_cartesian_target(const std::string& chain, const std::vector<double>& pose,
                                            const std::vector<std::string>& assist_chains = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kCartesian;
  target.cart.chain_name = chain;
  target.cart.frame_id = "EndEffector";
  target.cart.reference_frame = "base_link";
  target.cart.pose = Pose(pose);
  target.cart.assist_chains.insert(assist_chains.begin(), assist_chains.end());
  return target;
}

bool capture_current_joints(GalbotMotion& motion, GalbotRobot& robot, const std::vector<std::string>& chains,
                            CurrentRobotState& current) {
  for (const auto& chain : chains) {
    if (current.joints.find(chain) != current.joints.end()) {
      continue;
    }

    std::vector<std::string> joint_names = motion.get_chain_joint_names(chain);
    print_api_status("get_chain_joint_names(" + chain + ")", !joint_names.empty());
    if (joint_names.empty()) {
      std::cerr << "Failed to get joint names for chain: " << chain << std::endl;
      return false;
    }

    std::vector<double> joint_positions = robot.get_joint_positions({}, joint_names);
    const bool joint_positions_success = joint_positions.size() == joint_names.size();
    print_api_status("GalbotRobot::get_joint_positions(" + chain + ")", joint_positions_success);
    if (!joint_positions_success) {
      std::cerr << "Failed to get current joint positions for chain: " << chain << std::endl;
      return false;
    }

    current.joint_names[chain] = joint_names;
    current.joints[chain] = joint_positions;

    std::cout << "Current " << chain << " joints: ";
    print_vector(joint_positions);
    std::cout << std::endl;
  }
  return true;
}

bool capture_current_pose(GalbotMotion& motion, GalbotRobot& robot, const std::string& chain,
                          CurrentRobotState& current) {
  if (!capture_current_joints(motion, robot, {chain}, current)) {
    return false;
  }

  auto [status, pose] = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link");
  print_api_status("get_end_effector_pose_on_chain(" + chain + ")", status, motion);
  if (status != MotionStatus::SUCCESS || pose.size() != 7) {
    std::cerr << "Failed to get current pose for chain: " << chain << std::endl;
    return false;
  }

  current.poses[chain] = pose;
  std::cout << "Current " << chain << " pose [x, y, z, qx, qy, qz, qw]: ";
  print_vector(pose);
  std::cout << std::endl;
  return true;
}

ExampleInputs make_example_inputs(const CurrentRobotState& current) {
  ExampleInputs inputs;

  const std::vector<std::string> left_arm_joint_names = current.joint_names.at("left_arm");
  const std::vector<std::string> right_arm_joint_names = current.joint_names.at("right_arm");

  // Cartesian targets are absolute poses in base_link, and base_link sits at the
  // chassis, so the leg chain is part of the transform: a hard-coded pose only
  // holds for the robot and leg configuration it was captured on. Anchor them to
  // the start pose measured at run time instead, and let the constants express
  // only how far to move -- keep x / z / orientation, spread outward along y.
  auto spread = [&current](const std::string& chain, double dy) {
    std::vector<double> pose = current.poses.at(chain);
    pose[1] += dy;
    return pose;
  };

  inputs.waypoints = {
      {make_joint_target("left_arm", {1.2, -1.0, -0.2, 1.4, 0.1, -0.3, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.2, 1.0, 0.2, -1.4, -0.1, 0.3, 0.0},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
      {make_joint_target("left_arm", {1.46326, -1.36281, -0.44721, 1.50544, -0.10183, -0.40826, 0.00287},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.46326, 1.36281, 0.44721, -1.50544, 0.10183, 0.40826, -0.00287},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
      {make_cartesian_target("left_arm", spread("left_arm", 0.05), {"torso"}),
       make_cartesian_target("right_arm", spread("right_arm", -0.05), {"torso"})},
      {make_cartesian_target("left_arm", spread("left_arm", 0.10), {"torso"}),
       make_cartesian_target("right_arm", spread("right_arm", -0.10), {"torso"})},
      {make_joint_target("left_arm", {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0},
                         right_arm_joint_names)}
      //  make_joint_target("leg", {0.4992, 1.4991, 1.0005, 0.0, -0.0004},
      //                    {"leg_joint1", "leg_joint2", "leg_joint3", "leg_joint4", "leg_joint5"})},
  };

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(false);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_torso");

  return inputs;
}

void print_joint_names(const std::vector<std::string>& joint_names) {
  if (joint_names.empty()) {
    return;
  }
  std::cout << " joint_names=[";
  for (size_t i = 0; i < joint_names.size(); ++i) {
    std::cout << joint_names[i];
    if (i + 1 < joint_names.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

void print_waypoints(const MotionPlanWaypoints& waypoints) {
  std::cout << "motion_plan waypoints: " << waypoints.size() << std::endl;
  for (size_t i = 0; i < waypoints.size(); ++i) {
    std::cout << "  waypoint[" << i << "] targets: " << waypoints[i].size() << std::endl;
    for (const auto& target : waypoints[i]) {
      if (target.mode == MotionPlanTargetMode::kJoint) {
        std::cout << "    " << target.chain_name << " JOINT q=";
        print_vector(target.joint.joint_positions);
        print_joint_names(target.joint.joint_names);
        std::cout << std::endl;
      } else {
        std::cout << "    " << target.chain_name << " CART pose=";
        print_vector(pose_to_vector(target.cart.pose));
        std::cout << " frame_id=" << target.cart.frame_id
                  << " reference_frame=" << target.cart.reference_frame;
        if (!target.cart.assist_chains.empty()) {
          std::cout << " assist_chains=[";
          size_t assist_chain_index = 0;
          for (const auto& assist_chain : target.cart.assist_chains) {
            std::cout << assist_chain;
            if (++assist_chain_index < target.cart.assist_chains.size()) {
              std::cout << ", ";
            }
          }
          std::cout << "]";
        }
        std::cout << std::endl;
      }
    }
  }
}

void print_traj_result(const std::string& label, const TrajResult& result, GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& traj = std::get<1>(result);
  print_api_status(label, status, motion);
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (traj.empty()) {
    std::cout << "Trajectory map is empty." << std::endl;
    return;
  }
  for (const auto& [chain, points] : traj) {
    std::cout << "  " << chain << " trajectory points: " << points.size() << std::endl;
  }
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  print_no_status_api_completed("GalbotMotion::get_instance()");
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  print_no_status_api_completed("GalbotRobot::get_instance()");

  const bool motion_init_success = motion.init();
  print_api_status("GalbotMotion::init()", motion_init_success);
  if (!motion_init_success) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  const bool robot_init_success = robot.init();
  print_api_status("GalbotRobot::init()", robot_init_success);
  if (!robot_init_success) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  if (!move_robot_to_home_position(robot)) {
    shutdown_robot(robot);
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  CurrentRobotState current;
  if (!capture_current_pose(motion, robot, "left_arm", current) ||
      !capture_current_pose(motion, robot, "right_arm", current)) {
    shutdown_robot(robot);
    return -1;
  }

  const ExampleInputs inputs = make_example_inputs(current);
  print_waypoints(inputs.waypoints);

  std::cout << "Immediate execution is enabled; waypoints use the G3 reference targets in this example."
            << std::endl;

  auto result = motion.motion_plan(inputs.waypoints, inputs.params);
  print_traj_result("motion_plan", result, motion);

  shutdown_robot(robot);
  return std::get<0>(result) == MotionStatus::SUCCESS ? 0 : 1;
}

Linear Cartesian Motion (move_line)

Applicable Scenarios: Plan and execute a straight-line end-effector movement in Cartesian space.

examples/cpp/galbot_motion/src/move_line_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

using TrajMap = std::unordered_map<std::string, std::vector<std::vector<double>>>;
using TrajResult = std::tuple<MotionStatus, TrajMap>;

struct CurrentRobotState {
  std::unordered_map<std::string, std::vector<std::string>> joint_names;
  std::unordered_map<std::string, std::vector<double>> joints;
  std::unordered_map<std::string, std::vector<double>> poses;
};

struct ExampleInputs {
  MotionPlanWaypoints waypoints;
  Parameter params;
};

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

std::vector<double> pose_to_vector(const Pose& pose) {
  return {pose.position.x,    pose.position.y,    pose.position.z,   pose.orientation.x,
          pose.orientation.y, pose.orientation.z, pose.orientation.w};
}

void print_api_status(const std::string& api_name, bool success) {
  std::cout << api_name << " status: " << (success ? "SUCCESS" : "FAILED") << std::endl;
}

void print_api_status(const std::string& api_name, MotionStatus status, GalbotMotion& motion) {
  std::cout << api_name << " status: " << motion.status_to_string(status) << " ("
            << (status == MotionStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_api_status(const std::string& api_name, ControlStatus status) {
  std::cout << api_name << " status: " << control_status_to_string(status) << " ("
            << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

void print_no_status_api_completed(const std::string& api_name) {
  std::cout << api_name << " status: no return status; call completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  print_no_status_api_completed("request_shutdown()");
  robot.wait_for_shutdown();
  print_no_status_api_completed("wait_for_shutdown()");
  robot.destroy();
  print_no_status_api_completed("destroy()");
}

bool move_robot_to_home_position(GalbotRobot& robot) {
  std::cout << "Move robot to the G3 home position." << std::endl;
  const std::vector<double> joint_positions = {
      0.5, 1.5, 1.0, 0.0, 0.0,
      0.0, 0.0,
      1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0,
      -1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0};
  const std::vector<std::string> joint_group_names = {"leg", "head", "left_arm", "right_arm"};
  const ControlStatus status =
      robot.set_joint_positions(joint_positions, joint_group_names, {}, true, 0.1, 30.0);
  print_api_status("GalbotRobot::set_joint_positions(home)", status);
  return status == ControlStatus::SUCCESS;
}

MotionPlanChainTarget make_cartesian_target(const std::string& chain, const std::vector<double>& pose) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kCartesian;
  target.cart.chain_name = chain;
  target.cart.frame_id = "EndEffector";
  target.cart.reference_frame = "base_link";
  target.cart.pose = Pose(pose);
  target.cart.assist_chains.insert("torso");
  return target;
}

bool capture_current_joints(GalbotMotion& motion, GalbotRobot& robot, const std::vector<std::string>& chains,
                            CurrentRobotState& current) {
  for (const auto& chain : chains) {
    if (current.joints.find(chain) != current.joints.end()) {
      continue;
    }

    std::vector<std::string> joint_names = motion.get_chain_joint_names(chain);
    print_api_status("get_chain_joint_names(" + chain + ")", !joint_names.empty());
    if (joint_names.empty()) {
      std::cerr << "Failed to get joint names for chain: " << chain << std::endl;
      return false;
    }

    std::vector<double> joint_positions = robot.get_joint_positions({}, joint_names);
    const bool joint_positions_success = joint_positions.size() == joint_names.size();
    print_api_status("GalbotRobot::get_joint_positions(" + chain + ")", joint_positions_success);
    if (!joint_positions_success) {
      std::cerr << "Failed to get current joint positions for chain: " << chain << std::endl;
      return false;
    }

    current.joint_names[chain] = joint_names;
    current.joints[chain] = joint_positions;

    std::cout << "Current " << chain << " joints: ";
    print_vector(joint_positions);
    std::cout << std::endl;
  }
  return true;
}

bool capture_current_pose(GalbotMotion& motion, GalbotRobot& robot, const std::string& chain,
                          CurrentRobotState& current) {
  if (!capture_current_joints(motion, robot, {chain}, current)) {
    return false;
  }

  auto [status, pose] = motion.get_end_effector_pose_on_chain(chain, "EndEffector", "base_link");
  print_api_status("get_end_effector_pose_on_chain(" + chain + ")", status, motion);
  if (status != MotionStatus::SUCCESS || pose.size() != 7) {
    std::cerr << "Failed to get current pose for chain: " << chain << std::endl;
    return false;
  }
  current.poses[chain] = pose;
  std::cout << "Current " << chain << " pose [x, y, z, qx, qy, qz, qw]: ";
  print_vector(pose);
  std::cout << std::endl;
  return true;
}

ExampleInputs make_example_inputs() {
  ExampleInputs inputs;

  inputs.waypoints = {
      {make_cartesian_target("left_arm",
                             {0.3128974483, 0.2325600253, 0.8390805985, 0.0434438161, 0.0405149863, -0.0695888822,
                              0.9958054821}),
       make_cartesian_target("right_arm",
                             {0.3153029553, -0.2406507514, 0.8417887037, -0.0431098713, 0.0391780159, 0.0525539265,
                              0.9969176028})},
      {make_cartesian_target("left_arm",
                             {0.3128974483, 0.2325600253, 0.8990805985, 0.0434438161, 0.0405149863, -0.0695888822,
                              0.9958054821}),
       make_cartesian_target("right_arm",
                             {0.3153029553, -0.2406507514, 0.9017887037, -0.0431098713, 0.0391780159, 0.0525539265,
                              0.9969176028})},
      {make_cartesian_target("left_arm",
                             {0.2528974483, 0.2325600253, 0.8990805985, 0.0434438161, 0.0405149863, -0.0695888822,
                              0.9958054821}),
       make_cartesian_target("right_arm",
                             {0.2553029553, -0.2406507514, 0.9017887037, -0.0431098713, 0.0391780159, 0.0525539265,
                              0.9969176028})},
      {make_cartesian_target("left_arm",
                             {0.2528974483, 0.2325600253, 0.8390805985, 0.0434438161, 0.0405149863, -0.0695888822,
                              0.9958054821}),
       make_cartesian_target("right_arm",
                             {0.2553029553, -0.2406507514, 0.8417887037, -0.0431098713, 0.0391780159, 0.0525539265,
                              0.9969176028})},
  };

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(false);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_torso");

  return inputs;
}

void print_waypoints(const MotionPlanWaypoints& waypoints) {
  std::cout << "move_line waypoints: " << waypoints.size() << std::endl;
  for (size_t i = 0; i < waypoints.size(); ++i) {
    std::cout << "  waypoint[" << i << "] targets: " << waypoints[i].size() << std::endl;
    for (const auto& target : waypoints[i]) {
      std::cout << "    " << target.chain_name << " CART pose=";
      print_vector(pose_to_vector(target.cart.pose));
      std::cout << " frame_id=" << target.cart.frame_id
                << " reference_frame=" << target.cart.reference_frame;
      if (!target.cart.assist_chains.empty()) {
        std::cout << " assist_chains=[";
        size_t assist_chain_index = 0;
        for (const auto& assist_chain : target.cart.assist_chains) {
          std::cout << assist_chain;
          if (++assist_chain_index < target.cart.assist_chains.size()) {
            std::cout << ", ";
          }
        }
        std::cout << "]";
      }
      std::cout << std::endl;
    }
  }
}

void print_traj_result(const std::string& label, const TrajResult& result, GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& traj = std::get<1>(result);
  print_api_status(label, status, motion);
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (traj.empty()) {
    std::cout << "Trajectory map is empty." << std::endl;
    return;
  }
  for (const auto& [chain, points] : traj) {
    std::cout << "  " << chain << " trajectory points: " << points.size() << std::endl;
  }
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  print_no_status_api_completed("GalbotMotion::get_instance()");
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  print_no_status_api_completed("GalbotRobot::get_instance()");

  const bool motion_init_success = motion.init();
  print_api_status("GalbotMotion::init()", motion_init_success);
  if (!motion_init_success) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  const bool robot_init_success = robot.init();
  print_api_status("GalbotRobot::init()", robot_init_success);
  if (!robot_init_success) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  if (!move_robot_to_home_position(robot)) {
    shutdown_robot(robot);
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  CurrentRobotState current;
  if (!capture_current_pose(motion, robot, "left_arm", current) ||
      !capture_current_pose(motion, robot, "right_arm", current)) {
    shutdown_robot(robot);
    return -1;
  }

  const ExampleInputs inputs = make_example_inputs();
  print_waypoints(inputs.waypoints);

  std::cout << "Immediate execution is enabled; waypoints are loaded from move_line_targets.json reference values."
            << std::endl;

  auto result = motion.move_line(inputs.waypoints, inputs.params);
  print_traj_result("move_line", result, motion);

  shutdown_robot(robot);
  return std::get<0>(result) == MotionStatus::SUCCESS ? 0 : 1;
}

Jacobian Interface Tests

Applicable Scenarios: Verify Jacobian calculation across supported chains, target frames, and explicitly supplied robot states.

examples/cpp/galbot_motion/src/test_get_jacobian.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

static int g_test_count = 0;
static int g_pass_count = 0;
static int g_fail_count = 0;

void print_jacobian(const std::string& label,
                    const std::tuple<MotionStatus, std::vector<std::vector<double>>>& res,
                    GalbotMotion& planner) {
    auto status = std::get<0>(res);
    const auto& matrix = std::get<1>(res);

    std::cout << "[" << label << "] Status: " << planner.status_to_string(status) << std::endl;

    if (status == MotionStatus::SUCCESS && !matrix.empty()) {
        int rows = static_cast<int>(matrix.size());
        int cols = static_cast<int>(matrix[0].size());
        std::cout << "Jacobian matrix (" << rows << "x" << cols << "):" << std::endl;
        for (int r = 0; r < rows; ++r) {
            std::cout << "  row " << r << ": ";
            for (int c = 0; c < cols; ++c) {
                printf("%10.5f ", matrix[r][c]);
            }
            std::cout << std::endl;
        }
    }
    std::cout << std::endl;
}

void run_test(const std::string& name, MotionStatus expected,
              const std::tuple<MotionStatus, std::vector<std::vector<double>>>& res,
              GalbotMotion& planner) {
    g_test_count++;
    auto status = std::get<0>(res);
    const auto& matrix = std::get<1>(res);

    bool ok = (status == expected);
    bool shape_ok = true;
    if (ok && status == MotionStatus::SUCCESS) {
        shape_ok = (!matrix.empty() && matrix[0].size() > 0);
        // Jacobian should be 6 rows (vx,vy,vz,wx,wy,wz)
        if (shape_ok) {
            shape_ok = (matrix.size() == 6);
        }
    }

    if (ok && shape_ok) {
        g_pass_count++;
        std::cout << "[PASS] " << name << std::endl;
    } else {
        g_fail_count++;
        std::cout << "[FAIL] " << name;
        if (!ok) {
            std::cout << " (expected: " << planner.status_to_string(expected)
                      << ", got: " << planner.status_to_string(status) << ")";
        }
        if (!shape_ok && status == MotionStatus::SUCCESS) {
            std::cout << " (unexpected matrix shape)";
        }
        std::cout << std::endl;
    }
    print_jacobian(name, res, planner);
}

int main() {
    auto& planner = GalbotMotion::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!planner.init()) {
        std::cerr << "Planner initialization failed!" << std::endl;
        return -1;
    }
    std::cout << "Planner initialized successfully!" << std::endl;

    if (!robot.init()) {
        std::cerr << "Robot initialization failed!" << std::endl;
        return -1;
    }
    std::cout << "Robot initialized successfully!" << std::endl;

    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    // --- Print supported links (valid target_frame link names for get_jacobian) ---
    std::cout << "\n=== Supported links (valid target_frame for get_jacobian) ===" << std::endl;
    {
        const auto& support_links = planner.get_support_links();
        std::cout << "get_support_links() returned " << support_links.size() << " links:" << std::endl;
        for (const auto& link : support_links) {
            std::cout << "  - " << link << std::endl;
        }

        const auto ee_links = planner.get_link_names(true);
        std::cout << "get_link_names(only_end_effector=true) returned " << ee_links.size()
                  << " end-effector links:" << std::endl;
        for (const auto& link : ee_links) {
            std::cout << "  - " << link << std::endl;
        }
        std::cout << std::endl;
    }

    // Joint configurations for testing
    std::vector<double> left_arm_joints = {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0};
    std::vector<double> right_arm_joints = {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0};

    // --- Test 1: Default parameters (current robot state) ---
    try {
        std::cout << "=== Test 1: get_jacobian with default parameters ===" << std::endl;
        auto res = planner.get_jacobian("left_arm");
        run_test("left_arm default state", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 1 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 2: EndEffector frame, base_link reference ---
    try {
        std::cout << "=== Test 2: EndEffector frame / base_link reference ===" << std::endl;
        auto res = planner.get_jacobian("left_arm", "EndEffector", "base_link");
        run_test("left_arm EndEffector/base_link", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 2 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 3: Tool frame, world reference (NOT supported -> INVALID_INPUT) ---
    try {
        std::cout << "=== Test 3: Tool frame / world reference (expected INVALID_INPUT) ===" << std::endl;
        auto res = planner.get_jacobian("left_arm", "Tool", "world");
        run_test("left_arm Tool/world", MotionStatus::INVALID_INPUT, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 3 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 4: Custom joint state ---
    try {
        std::cout << "=== Test 4: Custom joint state ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> custom_joint_state = {{"left_arm", left_arm_joints}};
        auto custom_param_ptr = std::make_shared<Parameter>();
        custom_param_ptr->set_timeout(5.0);
        auto res = planner.get_jacobian("left_arm", "EndEffector", "base_link", custom_joint_state, custom_param_ptr);
        run_test("left_arm custom joint state", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 4 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 5: Right arm, default state ---
    try {
        std::cout << "=== Test 5: Right arm default state ===" << std::endl;
        auto res = planner.get_jacobian("right_arm");
        run_test("right_arm default state", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 5 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 6: Right arm with custom joint state ---
    try {
        std::cout << "=== Test 6: Right arm custom joint state ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> custom_joint_state = {{"right_arm", right_arm_joints}};
        auto res = planner.get_jacobian("right_arm", "EndEffector", "base_link", custom_joint_state);
        run_test("right_arm custom joint state", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 6 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 7: Invalid chain name ---
    try {
        std::cout << "=== Test 7: Invalid chain name ===" << std::endl;
        auto res = planner.get_jacobian("invalid_chain");
        run_test("invalid_chain", MotionStatus::INVALID_INPUT, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 7 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 8: Left arm, EndEffector frame, world reference ---
    try {
        std::cout << "=== Test 8: EndEffector / world reference ===" << std::endl;
        auto res = planner.get_jacobian("left_arm", "EndEffector", "world");
        run_test("left_arm EndEffector/world", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 8 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 9: Left arm, Tool frame, base_link reference (NOT supported -> INVALID_INPUT) ---
    try {
        std::cout << "=== Test 9: Tool frame / base_link reference (expected INVALID_INPUT) ===" << std::endl;
        auto res = planner.get_jacobian("left_arm", "Tool", "base_link");
        run_test("left_arm Tool/base_link", MotionStatus::INVALID_INPUT, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 9 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 10: Both arms sequentially ---
    try {
        std::cout << "=== Test 10: Both arms ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> both_arms_joint;
        both_arms_joint["left_arm"] = left_arm_joints;
        both_arms_joint["right_arm"] = right_arm_joints;

        auto res_left = planner.get_jacobian("left_arm", "EndEffector", "base_link", both_arms_joint);
        run_test("both_arms left", MotionStatus::SUCCESS, res_left, planner);

        auto res_right = planner.get_jacobian("right_arm", "EndEffector", "base_link", both_arms_joint);
        run_test("both_arms right", MotionStatus::SUCCESS, res_right, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 10 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 11: Empty chain name ---
    try {
        std::cout << "=== Test 11: Empty chain name ===" << std::endl;
        auto res = planner.get_jacobian("");
        run_test("empty_chain", MotionStatus::INVALID_INPUT, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 11 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 12: All zeros joint configuration ---
    try {
        std::cout << "=== Test 12: All zeros joint configuration ===" << std::endl;
        std::vector<double> zero_joints(7, 0.0);
        std::unordered_map<std::string, std::vector<double>> zero_joint_state = {{"left_arm", zero_joints}};
        auto res = planner.get_jacobian("left_arm", "EndEffector", "base_link", zero_joint_state);
        run_test("left_arm zero joints", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 12 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 13: torso chain (torso shares the leg end-effector: leg_end_effector_mount_link) ---
    try {
        std::cout << "=== Test 13: torso chain / EndEffector / base_link ===" << std::endl;
        auto res = planner.get_jacobian("torso", "EndEffector", "base_link");
        run_test("torso EndEffector/base_link", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 13 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // --- Test 14: leg chain with joint values (EndEffector maps to leg_end_effector_mount_link;
    //     G3 leg DOF = 5, so pass five joint values) ---
    try {
        std::cout << "=== Test 14: leg chain / EndEffector / base_link (5 joints) ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> leg_joint_state = {
            {"leg", {0.0, 0.0, 0.0, 0.0, 0.0}}};
        auto res = planner.get_jacobian("leg", "EndEffector", "base_link", leg_joint_state);
        run_test("leg EndEffector/base_link", MotionStatus::SUCCESS, res, planner);
    } catch (const std::exception& e) {
        std::cerr << "[FAIL] Test 14 exception: " << e.what() << std::endl;
        g_test_count++;
        g_fail_count++;
    }

    // Summary
    std::cout << "\n========================================" << std::endl;
    std::cout << "Jacobian test summary: " << g_test_count << " tests, "
              << g_pass_count << " passed, " << g_fail_count << " failed" << std::endl;
    std::cout << "========================================\n" << std::endl;

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

    return (g_fail_count == 0) ? 0 : 1;
}

Trajectory Planning (traj_plan)

Applicable Scenarios: Generate a time-parameterized trajectory for a prepared motion-planning request.

examples/cpp/galbot_motion/src/traj_plan_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_map>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

using TrajMap = std::unordered_map<std::string, std::vector<std::vector<double>>>;
using TrajResult = std::tuple<MotionStatus, TrajMap>;

struct CurrentRobotState {
  std::unordered_map<std::string, std::vector<std::string>> joint_names;
  std::unordered_map<std::string, std::vector<double>> joints;
};

struct ExampleInputs {
  MotionPlanWaypoints waypoints;
  Parameter params;
};

void print_vector(const std::vector<double>& values) {
  std::cout << "[";
  for (size_t i = 0; i < values.size(); ++i) {
    std::cout << values[i];
    if (i + 1 < values.size()) {
      std::cout << ", ";
    }
  }
  std::cout << "]";
}

void print_api_status(const std::string& api_name, bool success) {
  std::cout << api_name << " status: " << (success ? "SUCCESS" : "FAILED") << std::endl;
}

void print_api_status(const std::string& api_name, MotionStatus status, GalbotMotion& motion) {
  std::cout << api_name << " status: " << motion.status_to_string(status) << " ("
            << (status == MotionStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

std::string control_status_to_string(ControlStatus status) {
  switch (status) {
    case ControlStatus::SUCCESS:
      return "SUCCESS";
    case ControlStatus::TIMEOUT:
      return "TIMEOUT";
    case ControlStatus::FAULT:
      return "FAULT";
    case ControlStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case ControlStatus::INIT_FAILED:
      return "INIT_FAILED";
    case ControlStatus::IN_PROGRESS:
      return "IN_PROGRESS";
    case ControlStatus::STOPPED_UNREACHED:
      return "STOPPED_UNREACHED";
    case ControlStatus::DATA_FETCH_FAILED:
      return "DATA_FETCH_FAILED";
    case ControlStatus::PUBLISH_FAIL:
      return "PUBLISH_FAIL";
    case ControlStatus::COMM_DISCONNECTED:
      return "COMM_DISCONNECTED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_api_status(const std::string& api_name, ControlStatus status) {
  std::cout << api_name << " status: " << control_status_to_string(status) << " ("
            << (status == ControlStatus::SUCCESS ? "SUCCESS" : "FAILED") << ")" << std::endl;
}

void print_no_status_api_completed(const std::string& api_name) {
  std::cout << api_name << " status: no return status; call completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  print_no_status_api_completed("request_shutdown()");
  robot.wait_for_shutdown();
  print_no_status_api_completed("wait_for_shutdown()");
  robot.destroy();
  print_no_status_api_completed("destroy()");
}

bool move_robot_to_home_position(GalbotRobot& robot) {
  std::cout << "Move robot to the G3 home position." << std::endl;
  const std::vector<double> joint_positions = {
      0.5, 1.5, 1.0, 0.0, 0.0,
      0.0, 0.0,
      1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0,
      -1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0};
  const std::vector<std::string> joint_group_names = {"leg", "head", "left_arm", "right_arm"};
  const ControlStatus status =
      robot.set_joint_positions(joint_positions, joint_group_names, {}, true, 0.1, 30.0);
  print_api_status("GalbotRobot::set_joint_positions(home)", status);
  return status == ControlStatus::SUCCESS;
}

MotionPlanChainTarget make_joint_target(const std::string& chain, const std::vector<double>& joint_positions,
                                        const std::vector<std::string>& joint_names = {}) {
  MotionPlanChainTarget target;
  target.chain_name = chain;
  target.mode = MotionPlanTargetMode::kJoint;
  target.joint.chain_name = chain;
  target.joint.joint_positions = joint_positions;
  target.joint.joint_names = joint_names;
  return target;
}

bool capture_current_joints(GalbotMotion& motion, GalbotRobot& robot, const std::vector<std::string>& chains,
                            CurrentRobotState& current) {
  for (const auto& chain : chains) {
    if (current.joints.find(chain) != current.joints.end()) {
      continue;
    }

    std::vector<std::string> joint_names = motion.get_chain_joint_names(chain);
    print_api_status("get_chain_joint_names(" + chain + ")", !joint_names.empty());
    if (joint_names.empty()) {
      std::cerr << "Failed to get joint names for chain: " << chain << std::endl;
      return false;
    }

    std::vector<double> joint_positions = robot.get_joint_positions({}, joint_names);
    const bool joint_positions_success = joint_positions.size() == joint_names.size();
    print_api_status("GalbotRobot::get_joint_positions(" + chain + ")", joint_positions_success);
    if (!joint_positions_success) {
      std::cerr << "Failed to get current joint positions for chain: " << chain << std::endl;
      return false;
    }

    current.joint_names[chain] = joint_names;
    current.joints[chain] = joint_positions;

    std::cout << "Current " << chain << " joints: ";
    print_vector(joint_positions);
    std::cout << std::endl;
  }
  return true;
}

ExampleInputs make_example_inputs(const CurrentRobotState& current) {
  ExampleInputs inputs;

  const std::vector<std::string> left_arm_joint_names = current.joint_names.at("left_arm");
  const std::vector<std::string> right_arm_joint_names = current.joint_names.at("right_arm");

  inputs.waypoints = {
      {make_joint_target("left_arm", {1.2, -1.0, -0.2, 1.4, 0.1, -0.3, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.2, 1.0, 0.2, -1.4, -0.1, 0.3, 0.0},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
      {make_joint_target("left_arm", {1.46326, -1.36281, -0.44721, 1.50544, -0.10183, -0.40826, 0.00287},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.46326, 1.36281, 0.44721, -1.50544, 0.10183, 0.40826, -0.00287},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
      {make_joint_target("left_arm", {1.5, -1.36, -0.45, 1.53, -0.1, -0.42, 0.0},
                         left_arm_joint_names),
       make_joint_target("right_arm", {-1.5, 1.36, 0.45, -1.53, 0.1, 0.42, 0.0},
                         right_arm_joint_names),
       make_joint_target("torso", {0.0}, {"leg_joint4"})},
  };

  inputs.params.set_direct_execute(true);
  inputs.params.set_blocking(true);
  inputs.params.set_timeout(120.0);
  inputs.params.set_check_collision(false);
  inputs.params.set_reference_frame("base_link");
  inputs.params.set_actuate("with_torso");

  return inputs;
}

void print_waypoints(const MotionPlanWaypoints& waypoints) {
  std::cout << "traj_plan waypoints: " << waypoints.size() << std::endl;
  for (size_t i = 0; i < waypoints.size(); ++i) {
    std::cout << "  waypoint[" << i << "] targets: " << waypoints[i].size() << std::endl;
    for (const auto& target : waypoints[i]) {
      if (target.mode == MotionPlanTargetMode::kJoint) {
        std::cout << "    " << target.chain_name << " JOINT q=";
        print_vector(target.joint.joint_positions);
        if (!target.joint.joint_names.empty()) {
          std::cout << " joint_names=[";
          for (size_t j = 0; j < target.joint.joint_names.size(); ++j) {
            std::cout << target.joint.joint_names[j];
            if (j + 1 < target.joint.joint_names.size()) {
              std::cout << ", ";
            }
          }
          std::cout << "]";
        }
        std::cout << std::endl;
      } else {
        std::cout << "    " << target.chain_name << " unexpected CART target for traj_plan" << std::endl;
      }
    }
  }
}

void print_traj_result(const std::string& label, const TrajResult& result, GalbotMotion& motion) {
  const auto status = std::get<0>(result);
  const auto& traj = std::get<1>(result);
  print_api_status(label, status, motion);
  if (status != MotionStatus::SUCCESS) {
    return;
  }
  if (traj.empty()) {
    std::cout << "Trajectory map is empty." << std::endl;
    return;
  }
  for (const auto& [chain, points] : traj) {
    std::cout << "  " << chain << " trajectory points: " << points.size() << std::endl;
  }
}

}  // namespace

int main() {
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  print_no_status_api_completed("GalbotMotion::get_instance()");
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  print_no_status_api_completed("GalbotRobot::get_instance()");

  const bool motion_init_success = motion.init();
  print_api_status("GalbotMotion::init()", motion_init_success);
  if (!motion_init_success) {
    std::cerr << "GalbotMotion init FAILED" << std::endl;
    return -1;
  }
  const bool robot_init_success = robot.init();
  print_api_status("GalbotRobot::init()", robot_init_success);
  if (!robot_init_success) {
    std::cerr << "GalbotRobot init FAILED" << std::endl;
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  if (!move_robot_to_home_position(robot)) {
    shutdown_robot(robot);
    return -1;
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(500));

  CurrentRobotState current;
  if (!capture_current_joints(motion, robot, {"left_arm", "right_arm"}, current)) {
    shutdown_robot(robot);
    return -1;
  }

  const ExampleInputs inputs = make_example_inputs(current);
  print_waypoints(inputs.waypoints);

  std::cout << "Immediate execution is enabled; waypoints are loaded from motion_plan_targets.json reference values."
            << std::endl;

  auto result = motion.traj_plan(inputs.waypoints, inputs.params);
  print_traj_result("traj_plan", result, motion);

  shutdown_robot(robot);
  return std::get<0>(result) == MotionStatus::SUCCESS ? 0 : 1;
}

Class: GalbotNavigation

Get Instance / Initialize (get_instance_init)

Applicable Scenarios: Get the navigation module singleton and initialize. Must be called before using navigation functions.

examples/cpp/galbot_navigation/src/get_instance_init_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    std::cout << "Initialization successful!" << std::endl;

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

    return 0;
}

Relocalize / Is Localized / Get Current Pose (relocation)

Applicable Scenarios: Trigger relocalization, check if the robot is successfully localized, and get the robot's current pose in the map.

examples/cpp/galbot_navigation/src/relocation_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << "relocalization success." << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                  << current_pose.position.y << ", " << current_pose.position.z
                  << "), orientation(" << current_pose.orientation.x << ", "
                  << current_pose.orientation.y << ", " << current_pose.orientation.z
                  << ", " << current_pose.orientation.w << ")" << std::endl;

        robot.request_shutdown();
        robot.wait_for_shutdown();
    } else {
        std::cout << "relocalization failed, cannot proceed with navigation." << std::endl;
    }

    robot.destroy();

    return 0;
}

Check Path Reachability and Blocking Navigation to Goal (blocked_navigation)

Applicable Scenarios: Check if the target point is reachable, and if reachable, block and wait for navigation to complete reaching the target. Convenient for simple scenarios.

examples/cpp/galbot_navigation/src/blocked_navigation_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }
    auto res = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << "relocalization success." << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        std::this_thread::sleep_for(std::chrono::milliseconds(2000));

        // Whether to enable obstacle checking (can be set to true in open environments)
        bool enable_collision_check = false;
        // Whether to block and wait for arrival
        bool is_blocking = true;
        // Maximum wait time to reach position
        float timeout_s = 20;

        // Navigate 3 times in loop
        int count = 0;
        while (count++ < 3) {
            std::cout << "No. " << count << " navigation(s)" << std::endl;
            // checkpath navigation target
            if (navigation.check_path_reachability(goal_pose, init_pose)) {
                std::cout << "Path reachable, navigating to target point" << std::endl;
                NavigationStatus status = navigation.navigate_to_goal(
                    goal_pose, enable_collision_check, is_blocking, timeout_s);
                if (status == NavigationStatus::SUCCESS) {
                    std::cout << "Target point reached" << std::endl;
                } else {
                    std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
                }
            } else {
                std::cout << "Path unreachable, cannot navigate to target point" << std::endl;
            }
            // Check path reachability and return to start point
            if (navigation.check_path_reachability(init_pose, goal_pose)) {
                std::cout << "Path reachable, navigating to start point" << std::endl;
                NavigationStatus status = navigation.navigate_to_goal(
                    init_pose, enable_collision_check, is_blocking, timeout_s);
                if (status == NavigationStatus::SUCCESS) {
                    std::cout << "Returned to start point" << std::endl;
                } else {
                    std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
                }
            } else {
                std::cout << "Path unreachable, cannot navigate to start point" << std::endl;
            }
        }

        // Stop navigation
        navigation.stop_navigation();

        // Get current pose again
        current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

    } else {
        std::cout << "relocalization failed." << std::endl;
    }


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

    return 0;
}

Non-blocking Navigation + Polling for Arrival (non_blocking_navigation)

Applicable Scenarios: Start navigation without blocking the current thread, allowing other processing during navigation, and need to poll to determine if the target has been reached. Suitable for scenarios requiring asynchronous processing.

examples/cpp/galbot_navigation/src/non_blocking_navigation_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    auto res = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }

    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(5000));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }
    if (navigation.is_localized()) {
        std::cout << "relocalization success." << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        std::this_thread::sleep_for(std::chrono::milliseconds(2000));

        // Whether to enable obstacle checking (can be set to true in open environments)
        bool enable_collision_check = false;
        // Whether to block and wait for arrival
        bool is_blocking = false;

        // Navigate 2 times in loop, non-blocking wait for arrival
        int count = 0;
        while (count++ < 2) {
            std::cout << "No. " << count << " navigation(s)" << std::endl;
            // checkpath navigation target
            if (navigation.check_path_reachability(goal_pose, init_pose)) {
                std::cout << "Path reachable, navigating to target point" << std::endl;
                NavigationStatus status = navigation.navigate_to_goal(
                    goal_pose, enable_collision_check, is_blocking);
                // wait
                int count_arrival = 0;
                while (!navigation.check_goal_arrival()) {
                    std::cout << "navigate has not arrived" << std::endl;
                    std::this_thread::sleep_for(std::chrono::milliseconds(500));
                    if (++count_arrival > 10) {
                        break;
                    }
                }
                if (navigation.check_goal_arrival()) {
                    std::cout << "Target point reached" << std::endl;
                } else {
                    std::cout << "Navigation failed; target point not reached" << std::endl;
                }
            } else {
                std::cout << "Path unreachable, cannot navigate to target point" << std::endl;
            }
            // Check path reachability and return to start point
            if (navigation.check_path_reachability(init_pose, goal_pose)) {
                std::cout << "Path reachable, navigating to start point" << std::endl;
                NavigationStatus status = navigation.navigate_to_goal(
                    init_pose, enable_collision_check, is_blocking);
                // wait
                int count_arrival = 0;
                while (!navigation.check_goal_arrival()) {
                    std::cout << "navigate has not arrived" << std::endl;
                    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                    if (++count_arrival > 10) {
                        break;
                    }
                }
                if (navigation.check_goal_arrival()) {
                    std::cout << "Target point reached" << std::endl;
                } else {
                    std::cout << "Navigation failed; target point not reached" << std::endl;
                }
            } else {
                std::cout << "Path unreachable, cannot navigate to start point" << std::endl;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }

        // Stop navigation
        navigation.stop_navigation();

        // Get current pose
        current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;
    } else {
        std::cout << "relocalization failed, cannot proceed with navigation." << std::endl;
    }
    
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();

    return 0;
}

Get Navigation Status + Polling for SUCCESS/FAILED or Timeout (get_navigation_status_example)

Applicable Scenarios: Get the execution status of the current navigation task, used for polling in non-blocking navigation.

examples/cpp/galbot_navigation/src/get_navigation_status_example.cpp

/**
 * example: navigation get_navigation_status, SUCCESS/FAILED timeout exit,
 * Avoid deadlock and execute error logic.
 */
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }
    auto res = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }
    Pose goal_pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    const double timeout_s = 20.0;
    const double poll_interval_s = 0.5;

    // Non-blocking navigation
    navigation.navigate_to_goal(goal_pose, true, false, static_cast<float>(timeout_s));
    auto start = std::chrono::steady_clock::now();

    while (true) {
        NavigationTaskStatus status = navigation.get_navigation_status();
        auto elapsed = std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count();

        if (status == NavigationTaskStatus::SUCCESS) {
            std::cout << "Target reached" << std::endl;
            break;
        }
        if (status == NavigationTaskStatus::FAILED) {
            std::cout << "Navigation failed; exit error-handling logic promptly" << std::endl;
            break;
        }
        if (elapsed >= timeout_s) {
            std::cout << "navigationtimeout, exit" << std::endl;
            break;
        }

        if (status == NavigationTaskStatus::RUNNING) {
            std::cout << "Navigating... Status: RUNNING, elapsed: " << elapsed << "s" << std::endl;
        } else {
            std::cout << "Status: UNKNOWN, : " << elapsed << "s" << std::endl;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(poll_interval_s * 1000)));
    }

    // Stop navigation
    navigation.stop_navigation();

    // Get current pose again
    Pose current_pose = navigation.get_current_pose();
    std::cout << "Current pose: Position(" << current_pose.position.x << ", "
            << current_pose.position.y << ", " << current_pose.position.z
            << "), orientation(" << current_pose.orientation.x << ", "
            << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    std::cout << "Resources released successfully" << std::endl;
    return 0;
}

Move Straight to Goal / Stop Navigation (linear_movement)

Applicable Scenarios: Move the robot in a straight line to the target point, or stop the currently ongoing navigation task.

examples/cpp/galbot_navigation/src/linear_movement_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <chrono>
#include <iomanip>
#include <sstream>

using namespace galbot::sdk;

// Helper function: get current time string [HH:MM:SS.ms]
std::string get_timestamp() {
    auto now = std::chrono::system_clock::now();
    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
    std::time_t t = std::chrono::system_clock::to_time_t(now);
    std::tm tm = *std::localtime(&t);

    std::stringstream ss;
    ss << "[" << std::put_time(&tm, "%H:%M:%S") << "." 
       << std::setfill('0') << std::setw(3) << ms.count() << "] ";
    return ss.str();
}

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    std::cout << get_timestamp() << "Starting call to robot.init()..." << std::endl;
    if (robot.init()) {
        std::cout << get_timestamp() << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << get_timestamp() << "Base instance initialization failed!" << std::endl;
        return -1;
    }

    std::cout << get_timestamp() << "Starting call to navigation.init()..." << std::endl;
    if (navigation.init()) {
        std::cout << get_timestamp() << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << get_timestamp() << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    auto res = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    std::cout << get_timestamp() << "Enter relocalization check loop..." << std::endl;
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        std::cout << get_timestamp() << "Calling navigation.relocalize()..." << std::endl;
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << get_timestamp() << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << get_timestamp() << "relocalization success." << std::endl;

        // Get current pose
        std::cout << get_timestamp() << "Calling navigation.get_current_pose()..." << std::endl;
        Pose current_pose = navigation.get_current_pose();
        std::cout << get_timestamp() << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        // Whether to block and wait for arrival
        bool is_blocking = true;
        // Maximum wait time to reach position
        float timeout_s = 20;

        // --- ---
        std::cout << "--------------------------------------------------" << std::endl;
        std::cout << get_timestamp() << "Preparing to execute move_straight_to (linear movement)..." << std::endl;
        
        // Record start time
        auto start_move = std::chrono::system_clock::now();
        
        // Execute move
        NavigationStatus status = navigation.move_straight_to(goal_pose, is_blocking, timeout_s);
        
        // Record end time and calculate elapsed time
        auto end_move = std::chrono::system_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_move - start_move).count();

        if (status == NavigationStatus::SUCCESS) {
            std::cout << get_timestamp() << "Target point reached (elapsed: " << duration << "ms)" << std::endl;
        } else {
            std::cout << get_timestamp() << "navigationfailed, status: " << static_cast<int>(status) 
                    << " (elapsed: " << duration << "ms)" << std::endl;
        }
        std::cout << "--------------------------------------------------" << std::endl;

        // Stop navigation
        std::cout << get_timestamp() << "Calling navigation.stop_navigation()..." << std::endl;
        navigation.stop_navigation();

        // Get current pose again
        std::cout << get_timestamp() << "Calling navigation.get_current_pose()..." << std::endl;
        current_pose = navigation.get_current_pose();
        std::cout << get_timestamp() << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;
    } else {
        std::cout << get_timestamp() << "Relocalization failed, cannot continue navigation." << std::endl;
    }
    
    std::cout << get_timestamp() << "Executing shutdown..." << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    std::cout << get_timestamp() << "Program finished." << std::endl;

    return 0;
}

Complete Running Example (Simple Workflow) (complete_running_example)

Applicable Scenarios: Demonstrates the complete usage workflow of navigation functionality, including initialization, relocalization, navigation to target and other steps for reference.

examples/cpp/galbot_navigation/src/complete_running_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <thread>

using namespace galbot::sdk;

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    auto res = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    Pose goal_pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    // checkrelocalize success
    int count_relocalize = 0;
    while (!navigation.is_localized() && count_relocalize < 20) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        std::cout << "is relocalizing" << std::endl;
        count_relocalize++;
    }

    if (navigation.is_localized()) {
        std::cout << "Relocalization successful!" << std::endl;

        // Get current pose
        Pose current_pose = navigation.get_current_pose();
        std::cout << "Current pose: Position(" << current_pose.position.x << ", "
                << current_pose.position.y << ", " << current_pose.position.z
                << "), orientation(" << current_pose.orientation.x << ", "
                << current_pose.orientation.y << ", " << current_pose.orientation.z
                << ", " << current_pose.orientation.w << ")" << std::endl;

        // Whether to enable obstacle checking (can be set to true in open environments)
        bool enable_collision_check = false;
        // Whether to block and wait for arrival
        bool is_blocking = true;
        // Maximum wait time to reach position
        float timeout_s = 20;

        // checkpath navigation target
        if (navigation.check_path_reachability(goal_pose, init_pose)) {
            std::cout << "Path reachable, navigating to target point" << std::endl;
            // navigation, obstaclecheck, wait, wait 20
            NavigationStatus status = navigation.navigate_to_goal(
                goal_pose, enable_collision_check, is_blocking, timeout_s);
            if (status == NavigationStatus::SUCCESS) {
                std::cout << "Target point reached" << std::endl;
            } else {
                std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
            }
        } else {
            std::cout << "Path unreachable, cannot navigate to target point" << std::endl;
        }

        // Check path reachability and return to start point
        if (navigation.check_path_reachability(init_pose, goal_pose)) {
            std::cout << "Path reachable, navigating to start point" << std::endl;
            // Navigate to the target waypoint with obstacle checking disabled and non-blocking wait for arrival
            is_blocking = false;
            NavigationStatus status = navigation.navigate_to_goal(
                init_pose, enable_collision_check, is_blocking);
            // wait
            int count_arrival = 0;
            while (!navigation.check_goal_arrival()) {
                std::cout << "navigate has not arrived" << std::endl;
                std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                if (++count_arrival > 10) {
                    break;
                }
            }
            if (navigation.check_goal_arrival()) {
                std::cout << "Target point reached" << std::endl;
            } else {
                std::cout << "Navigation failed; target point not reached" << std::endl;
            }
        } else {
            std::cout << "Path unreachable, cannot navigate to start point" << std::endl;
        }

        // checkpath navigation target
        if (navigation.check_path_reachability(goal_pose, init_pose)) {
            std::cout << "Path reachable, navigating to target point" << std::endl;
            // target, wait, wait 10
            is_blocking = true;
            NavigationStatus status = navigation.move_straight_to(
                goal_pose, is_blocking, timeout_s);

            if (status == NavigationStatus::SUCCESS) {
                std::cout << "Target point reached" << std::endl;
            } else {
                std::cout << "navigationfailed, status: " << static_cast<int>(status) << std::endl;
            }
        } else {
            std::cout << "Path unreachable, cannot navigate to target point" << std::endl;
        }

        // Stop navigation
        navigation.stop_navigation();

    } else {
        std::cout << "Relocalization failed!" << std::endl;
    }


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

    return 0;
}

Navigation with Attached Bounding-box Filtering

Applicable Scenarios: Filter navigation collision geometry around an attached object carried by the robot.

examples/cpp/galbot_navigation/src/bounding_box_filter_example.cpp

/**
 * example: add/get/remove navigation bounding boxes and attach/detach box collision objects.
 *
 * These boxes are used by navigation fusion to ignore corresponding box regions,
 * so navigation will not treat the carried/known boxes as obstacles.
 */
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <vector>

using namespace galbot::sdk;

void print_box_info(const BoxInfo& box_info) {
    std::cout << "box_tag: " << box_info.box_tag << std::endl;
    std::cout << "parent_link_name: " << box_info.parent_link_name << std::endl;
    std::cout << "box_size: [" << box_info.box_size.length_x << ", "
              << box_info.box_size.length_y << ", " << box_info.box_size.length_z << "]" << std::endl;
    std::cout << "box_pose: [" << box_info.box_pose.position.x << ", "
              << box_info.box_pose.position.y << ", " << box_info.box_pose.position.z << ", "
              << box_info.box_pose.orientation.x << ", " << box_info.box_pose.orientation.y << ", "
              << box_info.box_pose.orientation.z << ", " << box_info.box_pose.orientation.w << "]" << std::endl;
}

BoxInfo make_box_info(int box_tag, const BoxSize& box_size, const Pose& box_pose,
                      const std::string& parent_link_name) {
    BoxInfo box_info;
    box_info.box_tag = box_tag;
    box_info.box_size = box_size;
    box_info.box_pose = box_pose;
    box_info.parent_link_name = parent_link_name;
    return box_info;
}

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    if (!robot.init()) {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (!navigation.init()) {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }

    BoxSize small_box_size{0.4f, 0.3f, 0.28f};
    Pose small_box_pose(std::vector<double>{0.45, 0.0, 0.25, 0.0, 0.0, 0.0, 1.0});
    Pose attached_box_pose(std::vector<double>{0.2, 0.0, -0.2, 0.0, 0.0, 0.0, 1.0});
    BoxInfo small_box = make_box_info(1001, small_box_size, small_box_pose, "base_link");
    BoxInfo attached_box = make_box_info(2001, small_box_size, attached_box_pose, "left_arm_end_effector_mount_link");

    std::cout << "Add navigation bounding box filters" << std::endl;
    auto add_status = navigation.add_bounding_box(small_box);
    std::cout << "add_bounding_box result: " << static_cast<int>(add_status) << std::endl;

    std::cout << "Get navigation bounding box filters" << std::endl;
    auto boxes = navigation.get_bounding_box();
    std::cout << "box count: " << boxes.size() << std::endl;
    for (const auto& box : boxes) {
        print_box_info(box);
    }

    std::cout << "Attach one box collision object to a robot link" << std::endl;
    std::vector<std::string> ignore_collision_links;
    auto attach_status = navigation.attach_box_to_link(attached_box, ignore_collision_links);
    std::cout << "attach_box_to_link result: " << static_cast<int>(attach_status) << std::endl;

    std::cout << "Detach one box collision object from its robot link" << std::endl;
    auto detach_status = navigation.detach_box_from_link(2001);
    std::cout << "detach_box_from_link result: " << static_cast<int>(detach_status) << std::endl;

    std::cout << "Remove one navigation bounding box filter" << std::endl;
    auto remove_status = navigation.remove_bounding_box(1001);
    std::cout << "remove_bounding_box result: " << static_cast<int>(remove_status) << std::endl;

    boxes = navigation.get_bounding_box();
    std::cout << "box count after remove: " << boxes.size() << std::endl;
    for (const auto& box : boxes) {
        print_box_info(box);
    }

    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    std::cout << "Resources released successfully" << std::endl;
    return 0;
}

Navigate Along a Trajectory (navigate_along_trajectory)

Applicable Scenarios: Execute a navigation route represented by an ordered trajectory of base poses.

examples/cpp/galbot_navigation/src/navigate_along_trajectory_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <vector>
#include <thread>
#include <chrono>

using namespace galbot::sdk;

namespace {

constexpr int kLocalizationRetryLimit = 20;
constexpr int kLocalizationRetrySleepMs = 500;
constexpr float kTaskTimeoutSec = 100.0f;

std::string navigation_target_status_to_string(NavigationTaskStatus status) {
    switch (status) {
        case NavigationTaskStatus::UNKNOWN:
            return "UNKNOWN";
        case NavigationTaskStatus::RUNNING:
            return "RUNNING";
        case NavigationTaskStatus::SUCCESS:
            return "SUCCESS";
        case NavigationTaskStatus::FAILED:
            return "FAILED";
        case NavigationTaskStatus::INTERRUPTED:
            return "INTERRUPTED";
        case NavigationTaskStatus::OCCUPIED:
            return "OCCUPIED";
        case NavigationTaskStatus::COLLISION:
            return "COLLISION";
        case NavigationTaskStatus::CLOSE_TO_OBSTACLE:
            return "CLOSE_TO_OBSTACLE";
        default:
            return "UNKNOWN";
    }
}

bool is_terminal_status(NavigationTaskStatus status) {
    return status == NavigationTaskStatus::SUCCESS || status == NavigationTaskStatus::FAILED ||
           status == NavigationTaskStatus::INTERRUPTED || status == NavigationTaskStatus::OCCUPIED ||
           status == NavigationTaskStatus::COLLISION || status == NavigationTaskStatus::CLOSE_TO_OBSTACLE;
}

void print_task_snapshot(const TaskHandle& handle, const NavigationTaskSnapshot& snapshot) {
    std::cout << "  task_id: " << handle.task_id
              << ", request_sent: " << (handle.request_sent ? "true" : "false")
              << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
    Pose current_pose = navigation.get_current_pose();
    std::cout << indent << "current_pose: ["
              << current_pose.position.x << ", " << current_pose.position.y << ", " << current_pose.position.z << ", "
              << current_pose.orientation.x << ", " << current_pose.orientation.y << ", " << current_pose.orientation.z << ", "
              << current_pose.orientation.w << "]" << std::endl;
}

void monitor_task_until_terminal(GalbotNavigation& navigation, const TaskHandle& handle, float timeout_s) {
    const auto start = std::chrono::steady_clock::now();
    while (true) {
        NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
        print_task_snapshot(handle, snapshot);
        print_current_pose(navigation, "    ");

        const auto now = std::chrono::steady_clock::now();
        const double elapsed = std::chrono::duration<double>(now - start).count();

        if (is_terminal_status(snapshot.status)) {
            return;
        }

        if (elapsed >= timeout_s) {
            std::cout << "  timeout reached while waiting for task completion." << std::endl;
            return;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
}

}  // namespace

bool wait_for_localization(GalbotNavigation& navigation, const Pose& init_pose) {
    int retry = 0;
    while (!navigation.is_localized() && retry < kLocalizationRetryLimit) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(kLocalizationRetrySleepMs));
        ++retry;
    }
    return navigation.is_localized();
}

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }
    auto res = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }

    /* All coordinates are in the "map" frame. */
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    std::vector<Pose> waypoints = {
        {std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}},
        {std::vector<double>{0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0}},
        {std::vector<double>{1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0}}
    };

    /* Wait for data preparation */
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    if (!wait_for_localization(navigation, init_pose)) {
        std::cerr << "localization failed" << std::endl;
        robot.request_shutdown();
        robot.wait_for_shutdown();
        robot.destroy();
        return -1;
    }

    // Print current pose
    print_current_pose(navigation);

    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    std::cout << "set target: waypoints" << std::endl;
    for (size_t i = 0; i < waypoints.size(); ++i) {
        std::cout << "  waypoint_" << i << ": ["
                  << waypoints[i].position.x << ", " << waypoints[i].position.y << ", " << waypoints[i].position.z << ", "
                  << waypoints[i].orientation.x << ", " << waypoints[i].orientation.y << ", " << waypoints[i].orientation.z << ", "
                  << waypoints[i].orientation.w << "]" << std::endl;
    }

    // Whether to enable obstacle checking (can be set to true in open environments)
    bool enable_collision_check = false;
    // The reference frame of the trajectory (can be set to "map" or "base_link", Defaulting to "map")
    const std::string frame_id = "map";
    // Speed scaling ratio for this trajectory execution, 1.0 means full speed.
    float speed_ratio = 1.0f;
    constexpr float timeout_s = kTaskTimeoutSec;

    TaskHandle handle =
        navigation.navigate_along_trajectory(waypoints, frame_id, speed_ratio, enable_collision_check);

    if (handle.request_sent) {
        monitor_task_until_terminal(navigation, handle, timeout_s);
    } else {
        std::cout << "trajectory task submission failed" << std::endl;
    }

    std::cout << "\nfinal task summary" << std::endl;
    NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
    std::cout << "  task_id: " << handle.task_id
              << ", request_sent: " << (handle.request_sent ? "true" : "false")
              << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;

    // Stop navigation
    navigation.stop_navigation();

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

    return 0;
}

Navigate to Goal V2 (navigate_to_goal_v2)

Applicable Scenarios: Navigate to a target pose using the extended goal interface and its additional options.

examples/cpp/galbot_navigation/src/navigate_to_goal_v2_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <array>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

using namespace galbot::sdk;

namespace {

constexpr int kLocalizationRetryLimit = 20;
constexpr int kLocalizationRetrySleepMs = 500;

std::string navigation_status_to_string(NavigationStatus status) {
  switch (status) {
    case NavigationStatus::SUCCESS:
      return "SUCCESS";
    case NavigationStatus::FAIL:
      return "FAIL";
    case NavigationStatus::TIMEOUT:
      return "TIMEOUT";
    case NavigationStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case NavigationStatus::MODE_ERR:
      return "MODE_ERR";
    case NavigationStatus::COMM_ERR:
      return "COMM_ERR";
    case NavigationStatus::WAIT_INITIALIZED:
      return "WAIT_INITIALIZED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
  Pose current_pose = navigation.get_current_pose();
  std::cout << indent << "current_pose: ["
            << current_pose.position.x << ", " << current_pose.position.y << ", " << current_pose.position.z << ", "
            << current_pose.orientation.x << ", " << current_pose.orientation.y << ", " << current_pose.orientation.z << ", "
            << current_pose.orientation.w << "]" << std::endl;
}

bool wait_for_localization(GalbotNavigation& navigation, const Pose& init_pose) {
  int retry = 0;
  while (!navigation.is_localized() && retry < kLocalizationRetryLimit) {
    navigation.relocalize(init_pose);
    std::this_thread::sleep_for(std::chrono::milliseconds(kLocalizationRetrySleepMs));
    ++retry;
  }
  return navigation.is_localized();
}

}  // namespace

int main() {
  auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  if (!robot.init()) {
    std::cerr << "Base instance initialization failed!" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "Navigation instance initialization failed!" << std::endl;
    return -1;
  }

  auto controller_status = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
  if (controller_status != ControlStatus::SUCCESS) {
    std::cerr << "switch controller failed" << std::endl;
    return -1;
  }

  Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

  std::this_thread::sleep_for(std::chrono::milliseconds(3000));

  if (!wait_for_localization(navigation, init_pose)) {
    std::cerr << "localization failed" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  print_current_pose(navigation);

  const Pose relative_goal(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
  const std::array<double, 3> max_vel = {0.5, 0.5, 0.5};
  const std::string pose_frame = "base_link";
  const bool enable_collision_check = true;
  const bool is_blocking = true;
  const float timeout_s = 20.0f;
  const bool omni_plan = false;

  std::cout << "navigate_to_goal_v2 relative target in base_link frame" << std::endl;
  NavigationStatus status = navigation.navigate_to_goal_v2(
      relative_goal, max_vel, pose_frame, enable_collision_check, is_blocking, timeout_s, omni_plan);

  std::cout << "navigate_to_goal_v2 status: " << navigation_status_to_string(status) << std::endl;
  print_current_pose(navigation);

  navigation.stop_navigation();
  std::cout << "navigation stopped" << std::endl;

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


  std::cout << "example completed ..." << std::endl;
  return 0;
}

Velocity Navigation (navigate_with_velocity)

Applicable Scenarios: Control base translation and rotation continuously with velocity commands.

examples/cpp/galbot_navigation/src/navigate_with_velocity_example.cpp

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

std::string navigation_status_to_string(NavigationStatus status) {
  switch (status) {
    case NavigationStatus::SUCCESS:
      return "SUCCESS";
    case NavigationStatus::FAIL:
      return "FAIL";
    case NavigationStatus::TIMEOUT:
      return "TIMEOUT";
    case NavigationStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case NavigationStatus::MODE_ERR:
      return "MODE_ERR";
    case NavigationStatus::COMM_ERR:
      return "COMM_ERR";
    case NavigationStatus::WAIT_INITIALIZED:
      return "WAIT_INITIALIZED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
  Pose current_pose = navigation.get_current_pose();
  std::cout << indent << "current_pose: [" << current_pose.position.x << ", " << current_pose.position.y << ", "
            << current_pose.position.z << ", " << current_pose.orientation.x << ", " << current_pose.orientation.y
            << ", " << current_pose.orientation.z << ", " << current_pose.orientation.w << "]" << std::endl;
}

}  // namespace

int test_base_vel_cmd() {
  auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  if (!robot.init()) {
    std::cerr << "Base instance initialization failed!" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "Navigation instance initialization failed!" << std::endl;
    return -1;
  }

  auto controller_status = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
  if (controller_status != ControlStatus::SUCCESS) {
    std::cerr << "switch controller failed" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(3000));
  print_current_pose(navigation);

  const double vx = 0.3;
  const double vy = 0.0;
  const double vyaw = 0.0;
  const double duration_s = 6.0;
  const bool enable_collision_check = true;

  std::cout << "navigate_with_velocity: vx=" << vx << ", vy=" << vy << ", vyaw=" << vyaw
            << ", duration_s=" << duration_s << std::endl;

  NavigationStatus status = navigation.navigate_with_velocity(vx, vy, vyaw, duration_s, enable_collision_check);

  std::cout << "navigate_with_velocity status: " << navigation_status_to_string(status) << std::endl;

  std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>((duration_s + 2.0) * 1000)));

  navigation.stop_navigation();
  print_current_pose(navigation);
  std::cout << "navigation stopped" << std::endl;

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

  std::cout << "example completed ..." << std::endl;
  return 0;
}

int test_change_vel_cmd() {
  auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  if (!robot.init()) {
    std::cerr << "Base instance initialization failed!" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "Navigation instance initialization failed!" << std::endl;
    return -1;
  }

  auto controller_status = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
  if (controller_status != ControlStatus::SUCCESS) {
    std::cerr << "switch controller failed" << std::endl;
    return -1;
  }

  std::this_thread::sleep_for(std::chrono::milliseconds(3000));
  print_current_pose(navigation);

  {
    double vx = 0.3;
    double vy = 0.0;
    double vyaw = 0.0;
    double duration_s = 6.0;
    bool enable_collision_check = true;

    std::cout << "navigate_with_velocity: vx=" << vx << ", vy=" << vy << ", vyaw=" << vyaw
              << ", duration_s=" << duration_s << std::endl;
    NavigationStatus status = navigation.navigate_with_velocity(vx, vy, vyaw, duration_s, enable_collision_check);
    std::cout << "navigate_with_velocity status 1: " << navigation_status_to_string(status) << std::endl;

    std::this_thread::sleep_for(std::chrono::milliseconds(4000));
  }

  std::cout << "已经运行4s 现在开始下发新的速度指令 =======" << std::endl;
  {
    double vx = 0.1;
    double vy = 0.2;
    double vyaw = 0.0;
    double duration_s = 6.0;
    bool enable_collision_check = true;

    std::cout << "navigate_with_velocity: vx=" << vx << ", vy=" << vy << ", vyaw=" << vyaw
              << ", duration_s=" << duration_s << std::endl;
    NavigationStatus status = navigation.navigate_with_velocity(vx, vy, vyaw, duration_s, enable_collision_check);
    std::cout << "navigate_with_velocity status 2: " << navigation_status_to_string(status) << std::endl;

    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>((duration_s + 2.0) * 1000)));
  }

  navigation.stop_navigation();
  print_current_pose(navigation);
  std::cout << "navigation stopped" << std::endl;

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

  std::cout << "example completed ..." << std::endl;
  return 0;
}

// 根据选择放开不同的注释
int main() {
  test_base_vel_cmd();  // 测试速度导航接口基础功能

  //   test_change_vel_cmd();// 测试中途速度变化命令下发后的导航运动功能

  return 0;
}

Navigation Configuration

Applicable Scenarios: Read and update runtime configuration used by the navigation module.

examples/cpp/galbot_navigation/src/navigation_set_get_config_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <array>
#include <iostream>
#include <string>

using namespace galbot::sdk;

namespace {

std::string navigation_status_to_string(NavigationStatus status) {
  switch (status) {
    case NavigationStatus::SUCCESS:
      return "SUCCESS";
    case NavigationStatus::FAIL:
      return "FAIL";
    case NavigationStatus::TIMEOUT:
      return "TIMEOUT";
    case NavigationStatus::INVALID_INPUT:
      return "INVALID_INPUT";
    case NavigationStatus::MODE_ERR:
      return "MODE_ERR";
    case NavigationStatus::COMM_ERR:
      return "COMM_ERR";
    case NavigationStatus::WAIT_INITIALIZED:
      return "WAIT_INITIALIZED";
    default:
      return "UNKNOWN_STATUS";
  }
}

void print_status(const std::string& name, NavigationStatus status) {
  std::cout << name << ": " << navigation_status_to_string(status) << std::endl;
}

}  // namespace

int main() {
  auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  if (!robot.init()) {
    std::cerr << "Base instance initialization failed!" << std::endl;
    return -1;
  }
  if (!navigation.init()) {
    std::cerr << "Navigation instance initialization failed!" << std::endl;
    return -1;
  }

  std::cout << "dump navigation configs before setting parameters" << std::endl;
  print_status("dump_navigation_configs", navigation.dump_navigation_configs());

  const std::array<double, 3> vel_limit = {0.5, 0.5, 0.5};       // valid range: [0.05, 1.5]
  const std::array<double, 3> acc_limit = {1.0, 1.0, 1.0};       // valid range: [0.05, 6.0]
  const std::array<double, 3> jerk_limit = {5.0, 5.0, 5.0};      // valid range: [0.05, 12.0]
  const std::array<double, 3> arrival_threshold = {0.05, 0.05, 0.05};  // valid range: [0.03, 2.0]
  const double timeout_s = 30.0;

  print_status("set_navigation_velocity_limit",
               navigation.set_navigation_velocity_limit(vel_limit));

  print_status("set_navigation_kinematics_limits",
               navigation.set_navigation_kinematics_limits(vel_limit, acc_limit, jerk_limit));

  print_status("set_navigation_timeout",
               navigation.set_navigation_timeout(timeout_s));

  print_status("set_navigation_arrival_threshold",
               navigation.set_navigation_arrival_threshold(arrival_threshold));

  std::cout << "dump navigation configs after setting parameters" << std::endl;
  print_status("dump_navigation_configs", navigation.dump_navigation_configs());

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

  return 0;
}

Navigate Through Waypoints (navigation_through_waypoints)

Applicable Scenarios: Make the robot visit multiple navigation waypoints in a specified order.

examples/cpp/galbot_navigation/src/navigation_through_waypoints_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <iostream>
#include <string>
#include <thread>
#include <chrono>
#include <vector>

using namespace galbot::sdk;

namespace {

constexpr int kLocalizationRetryLimit = 20;
constexpr int kLocalizationRetrySleepMs = 500;
constexpr int kPollIntervalMs = 1000;
constexpr double kTaskTimeoutSec = 100.0;

std::string navigation_target_status_to_string(NavigationTaskStatus status) {
    switch (status) {
        case NavigationTaskStatus::UNKNOWN:
            return "UNKNOWN";
        case NavigationTaskStatus::RUNNING:
            return "RUNNING";
        case NavigationTaskStatus::SUCCESS:
            return "SUCCESS";
        case NavigationTaskStatus::FAILED:
            return "FAILED";
        case NavigationTaskStatus::INTERRUPTED:
            return "INTERRUPTED";
        case NavigationTaskStatus::OCCUPIED:
            return "OCCUPIED";
        case NavigationTaskStatus::COLLISION:
            return "COLLISION";
        case NavigationTaskStatus::CLOSE_TO_OBSTACLE:
            return "CLOSE_TO_OBSTACLE";
        default:
            return "UNKNOWN";
    }
}

bool is_terminal_status(NavigationTaskStatus status) {
    return status == NavigationTaskStatus::SUCCESS || status == NavigationTaskStatus::FAILED ||
           status == NavigationTaskStatus::INTERRUPTED || status == NavigationTaskStatus::OCCUPIED ||
           status == NavigationTaskStatus::COLLISION || status == NavigationTaskStatus::CLOSE_TO_OBSTACLE;
}

void print_task_snapshot(const TaskHandle& handle, const NavigationTaskSnapshot& snapshot) {
    std::cout << "  task_id: " << handle.task_id
              << ", request_sent: " << (handle.request_sent ? "true" : "false")
              << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
    Pose current_pose = navigation.get_current_pose();
    std::cout << indent << "current_pose: ["
              << current_pose.position.x << ", " << current_pose.position.y << ", " << current_pose.position.z << ", "
              << current_pose.orientation.x << ", " << current_pose.orientation.y << ", " << current_pose.orientation.z << ", "
              << current_pose.orientation.w << "]" << std::endl;
}

void monitor_task_until_terminal(GalbotNavigation& navigation, const TaskHandle& handle, double timeout_s) {
    const auto start = std::chrono::steady_clock::now();
    while (true) {
        NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
        print_task_snapshot(handle, snapshot);
        print_current_pose(navigation, "    ");

        const auto now = std::chrono::steady_clock::now();
        const double elapsed = std::chrono::duration<double>(now - start).count();

        if (is_terminal_status(snapshot.status)) {
            return;
        }

        if (elapsed >= timeout_s) {
            std::cout << "  timeout reached while waiting for task completion." << std::endl;
            return;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(kPollIntervalMs));
    }
}

}  // namespace

bool wait_for_localization(GalbotNavigation& navigation, const Pose& init_pose) {
    int retry = 0;
    while (!navigation.is_localized() && retry < kLocalizationRetryLimit) {
        navigation.relocalize(init_pose);
        std::this_thread::sleep_for(std::chrono::milliseconds(kLocalizationRetrySleepMs));
        ++retry;
    }
    return navigation.is_localized();
}

static Waypoint make_waypoint(const Pose& pose, const WaypointParams& params = WaypointParams()) {
    return Waypoint(pose, params);
}

int main() {
    auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
    auto& robot = GalbotRobot::get_instance(MachineType::G3);

    // Initialize system
    if (robot.init()) {
        std::cout << "Base instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Base instance initialization failed!" << std::endl;
        return -1;
    }
    if (navigation.init()) {
        std::cout << "Navigation instance initialized successfully!" << std::endl;
    } else {
        std::cerr << "Navigation instance initialization failed!" << std::endl;
        return -1;
    }
    auto res = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
    if (res != ControlStatus::SUCCESS) {
        std::cerr << "Failed to switch controller!" << std::endl;
        return -1;
    }

    /* All coordinates are in the "map" frame. */
    Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

    /*
     * Build a waypoint by combining:
     *   1) the target pose
     *   2) an optional WaypointParams object
     *
     * Usage:
     * - Use Waypoint(pose) if you only need the default settings.
     * - Create WaypointParams and set only the fields you want to override.
     * - Pass the params object into the Waypoint constructor to build a target point.
     */
    WaypointParams waypoint_2_params;
    waypoint_2_params.arrival_position_threshold_x = 0.02;
    waypoint_2_params.arrival_position_threshold_y = 0.02;
    waypoint_2_params.arrival_orientation_threshold = 0.08;
    waypoint_2_params.velocity_scale = 0.2;

    WaypointParams waypoint_3_params;
    waypoint_3_params.arrival_position_threshold_x = 0.04;
    waypoint_3_params.arrival_position_threshold_y = 0.04;
    waypoint_3_params.arrival_orientation_threshold = 0.05;
    waypoint_3_params.velocity_scale = 0.8;
    waypoint_3_params.acceleration_scale = 0.8;
    waypoint_3_params.jerk_scale = 0.8;

    std::vector<Waypoint> waypoints = {
        make_waypoint(Pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0})),
        make_waypoint(Pose(std::vector<double>{0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0}), waypoint_2_params),
        make_waypoint(Pose(std::vector<double>{1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 1.0}), waypoint_3_params)
    };

    /* Wait for data preparation */
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    if (!wait_for_localization(navigation, init_pose)) {
        std::cerr << "localization failed" << std::endl;
        robot.request_shutdown();
        robot.wait_for_shutdown();
        robot.destroy();
        return -1;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    std::cout << "set target: waypoints" << std::endl;
    for (size_t i = 0; i < waypoints.size(); ++i) {
        std::cout << "  waypoint_" << i << ": ["
                  << waypoints[i].pose.position.x << ", " << waypoints[i].pose.position.y << ", " << waypoints[i].pose.position.z << ", "
                  << waypoints[i].pose.orientation.x << ", " << waypoints[i].pose.orientation.y << ", " << waypoints[i].pose.orientation.z << ", "
                  << waypoints[i].pose.orientation.w << "]" << std::endl;
    }

    // The reference frame of the trajectory (can be set to "map" or "base_link", Defaulting to "map")
    const std::string frame_id = "map";
    // Whether to enable collision checking (can be set to true in open environments)
    bool enable_collision_check = true;

    TaskHandle handle = navigation.navigate_through_waypoints(waypoints, frame_id, enable_collision_check);

    if (handle.request_sent) {
        monitor_task_until_terminal(navigation, handle, kTaskTimeoutSec);
    } else {
        std::cout << "navigation failed to submit task" << std::endl;
    }

    std::cout << "\nfinal task summary" << std::endl;
    NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
    std::cout << "  task_id: " << handle.task_id
              << ", request_sent: " << (handle.request_sent ? "true" : "false")
              << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;

    // Stop navigation
    navigation.stop_navigation();

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

    return 0;
}

Set Navigation Target (set_navigation_target)

Applicable Scenarios: Set a navigation target separately before starting or managing the navigation task.

examples/cpp/galbot_navigation/src/set_navigation_target_example.cpp

#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

using namespace galbot::sdk;

namespace {

constexpr int kLocalizationRetryLimit = 20;
constexpr int kLocalizationRetrySleepMs = 500;
constexpr int kPollIntervalMs = 500;
constexpr int kMonitorDurationMs = 2000;

std::string navigation_target_status_to_string(NavigationTaskStatus status) {
  switch (status) {
    case NavigationTaskStatus::UNKNOWN:
      return "UNKNOWN";
    case NavigationTaskStatus::RUNNING:
      return "RUNNING";
    case NavigationTaskStatus::SUCCESS:
      return "SUCCESS";
    case NavigationTaskStatus::FAILED:
      return "FAILED";
    case NavigationTaskStatus::INTERRUPTED:
      return "INTERRUPTED";
    case NavigationTaskStatus::OCCUPIED:
      return "OCCUPIED";
    case NavigationTaskStatus::COLLISION:
      return "COLLISION";
    case NavigationTaskStatus::CLOSE_TO_OBSTACLE:
      return "CLOSE_TO_OBSTACLE";
    default:
      return "UNKNOWN";
  }
}


void print_pose(const Pose& pose, const std::string& label) {
  std::cout << label << ": ["
            << pose.position.x << ", " << pose.position.y << ", " << pose.position.z << ", "
            << pose.orientation.x << ", " << pose.orientation.y << ", " << pose.orientation.z << ", "
            << pose.orientation.w << "]" << std::endl;
}

void print_task_snapshot(const TaskHandle& handle, const NavigationTaskSnapshot& snapshot) {
  std::cout << "  task_id: " << handle.task_id
            << ", request_sent: " << (handle.request_sent ? "true" : "false")
            << ", status: " << navigation_target_status_to_string(snapshot.status) << std::endl;
}

void print_current_pose(GalbotNavigation& navigation, const std::string& indent = "  ") {
  Pose current_pose = navigation.get_current_pose();
  std::cout << indent << "current_pose: ["
            << current_pose.position.x << ", " << current_pose.position.y << ", " << current_pose.position.z << ", "
            << current_pose.orientation.x << ", " << current_pose.orientation.y << ", " << current_pose.orientation.z << ", "
            << current_pose.orientation.w << "]" << std::endl;
}

bool wait_for_localization(GalbotNavigation& navigation, const Pose& init_pose) {
  int retry = 0;
  while (!navigation.is_localized() && retry < kLocalizationRetryLimit) {
    navigation.relocalize(init_pose);
    std::this_thread::sleep_for(std::chrono::milliseconds(kLocalizationRetrySleepMs));
    ++retry;
  }
  return navigation.is_localized();
}

void print_task_summary(GalbotNavigation& navigation, const std::vector<TaskHandle>& handles) {
  std::cout << "\nfinal task summary" << std::endl;
  for (const auto& task : handles) {
    const NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(task.task_id);
    print_task_snapshot(task, snapshot);
  }
}


}  // namespace

int main() {

  auto& navigation = GalbotNavigation::get_instance(MachineType::G3);
  auto& robot = GalbotRobot::get_instance(MachineType::G3);

  // Initialize system
  if (robot.init()) {
      std::cout << "Base instance initialized successfully!" << std::endl;
  } else {
      std::cerr << "Base instance initialization failed!" << std::endl;
      return -1;
  }
  if (navigation.init()) {
      std::cout << "Navigation instance initialized successfully!" << std::endl;
  } else {
      std::cerr << "Navigation instance initialization failed!" << std::endl;
      return -1;
  }
  auto controller_status = robot.switch_controller(G3ControllerName::CHASSIS_POSE_CTRL);
  if (controller_status != ControlStatus::SUCCESS) {
    std::cerr << "switch controller failed" << std::endl;
    return -1;
  }

  Pose init_pose(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});

  /* Wait for data preparation */
  std::this_thread::sleep_for(std::chrono::milliseconds(3000)); 

  if (!wait_for_localization(navigation, init_pose)) {
    std::cerr << "localization failed" << std::endl;
    robot.request_shutdown();
    robot.wait_for_shutdown();
    robot.destroy();
    return -1;
  }

  const std::string frame_id = "map";
  float speed_ratio = 0.6f;
  bool enable_collision_check = true;
  const std::vector<Pose> target_points = {
      Pose(std::vector<double>{1.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00}),
      Pose(std::vector<double>{0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00}),
      Pose(std::vector<double>{0.30, 0.00, 0.00, 0.00, 0.00, 0.00, 1.00})
  };

  std::vector<TaskHandle> handles;
  handles.reserve(target_points.size());

  for (size_t i = 0; i < target_points.size(); ++i) {
    std::cout << "set target: dynamic_goal_" << i << ", target_pose: ["
              << target_points[i].position.x << ", " << target_points[i].position.y << ", " << target_points[i].position.z << ", "
              << target_points[i].orientation.x << ", " << target_points[i].orientation.y << ", " << target_points[i].orientation.z << ", "
              << target_points[i].orientation.w << "]" << std::endl;

    TaskHandle handle =
        navigation.set_navigation_target(target_points[i], frame_id, speed_ratio, enable_collision_check);

    handles.push_back(handle);

    const auto start = std::chrono::steady_clock::now();
    while (true) {
      NavigationTaskSnapshot snapshot = navigation.get_navigation_target_status(handle.task_id);
      print_task_snapshot(handle, snapshot);
      print_current_pose(navigation, "    ");

      const auto now = std::chrono::steady_clock::now();
      const auto elapsed_ms =
          std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count();
      if (elapsed_ms >= kMonitorDurationMs) {
        break;
      }

      std::this_thread::sleep_for(std::chrono::milliseconds(kPollIntervalMs));
    }
  }

  print_task_summary(navigation, handles);

  std::cout << "\nstop navigation and shutdown" << std::endl;
  navigation.stop_navigation();

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

  return 0;
}

Class: GalbotPerception

Foundation Stereo: single run_once and save a colorized depth image

Applicable scenarios: Trigger a single stereo depth estimation inference and retrieve the result.

examples/cpp/galbot_perception/src/foundation_stereo_run_once_example.cpp

/**
 * Foundation stereo depth example: single run_once + wait_for_new_result to fetch one inference result.
 */

#include <iostream>
#include <thread>
#include <chrono>

#include "galbot_robot.hpp"
#include "galbot_perception.hpp"
#include "galbot_sdk_type.hpp"
#include "opencv2/opencv.hpp"

using namespace galbot::sdk;

bool run_foundation_stereo_once(GalbotPerception& perception) {
    if (!perception.run_once(PerceptionModule::FOUNDATION_STEREO)) {
        std::cerr << "run_once failed to send command" << std::endl;
        return false;
    }

    std::cout << "Waiting for inference result..." << std::endl;
    if (!perception.wait_for_new_result(PerceptionModule::FOUNDATION_STEREO, 5.0)) {
        std::cerr << "Timed out waiting for inference result" << std::endl;
        return false;
    }

    DetectionResult result;
    if (!perception.get_latest_result(PerceptionModule::FOUNDATION_STEREO, result)) {
        std::cerr << "get_latest_result failed" << std::endl;
        return false;
    }

    if (!result.instanceMask.empty()) {
        cv::Mat depth_map = result.instanceMask;
        std::cout << "Depth map size: " << depth_map.cols << "x" << depth_map.rows
                << ", type: " << depth_map.type() << std::endl;

        double min_val, max_val;
        cv::minMaxLoc(depth_map, &min_val, &max_val);
        std::cout << "Depth value range: [" << min_val << ", " << max_val << "]" << std::endl;

        cv::Mat depth_f;
        depth_map.convertTo(depth_f, CV_32F);
        cv::Mat mask = (depth_f > 0);

        cv::Mat normalized;
        cv::normalize(depth_f, normalized, 0, 255, cv::NORM_MINMAX, CV_8UC1, mask);

        cv::Mat colored;
        cv::applyColorMap(normalized, colored, cv::COLORMAP_TURBO);

        cv::imwrite("foundation_stereo_depth.jpg", colored);
        std::cout << "Depth map saved to foundation_stereo_depth.jpg" << std::endl;
    } else {
        std::cout << "No depth map (instanceMask is empty)" << std::endl;
    }

    return true;
}

int main() {
    auto& robot = GalbotRobot::get_instance(MachineType::G3);
    robot.init();

    auto& perception = GalbotPerception::get_instance(MachineType::G3);
    perception.init({PerceptionModule::FOUNDATION_STEREO});

    // Wait for perception models to load
    std::this_thread::sleep_for(std::chrono::seconds(12));
    std::cout << "Init OK, sending single inference request..." << std::endl;

    run_foundation_stereo_once(perception);

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

    return 0;
}

Class: Parameter

Create and Use Parameter (parameter_use)

Applicable Scenarios: Create a motion planning parameter object used to configure various parameters for motion planning such as velocity limits, acceleration limits, planning time, etc.

examples/cpp/galbot_motion/src/parameter_use_example.cpp

#include <iostream>
#include <vector>
#include <string>
#include <memory>

#include "galbot_motion.hpp"

using namespace galbot::sdk;

int main() {
    // Create Parameter via constructor and set options
    auto p = std::make_shared<Parameter>();

    p->set_blocking(true);            // Set whether to block execution
    p->set_check_collision(false);     // Disable collision detection
    p->set_timeout(5.0);              // Set timeout (seconds)
    p->set_actuate("with_chain_only");// Set drive mode
    p->set_tool_pose(false);           // Whether to consider tool pose
    p->set_reference_frame("base_link");

    std::cout << "--- Parameter p ---" << std::endl;
    std::cout << "blocking: " << (p->get_blocking() ? "True" : "False") << std::endl;
    std::cout << "collision check: " << (p->get_check_collision() ? "True" : "False") << std::endl;
    std::cout << "timeout: " << p->get_timeout() << "s" << std::endl;

    return 0;
}

Utility Functions

status_to_string / JointStates / PoseState (auxi_fun_use)

Applicable Scenarios: Demonstrates how to use utility functions to convert status to strings, create joint state and pose state objects.

examples/cpp/galbot_motion/src/function_use_example.cpp

#include <iostream>
#include <string>

#include "galbot_motion.hpp"

using namespace galbot::sdk;

int main() {

    auto& planner = GalbotMotion::get_instance(MachineType::G3);

    MotionStatus status = MotionStatus::SUCCESS;
    std::string status_str = planner.status_to_string(status);
    
    std::cout << "MotionStatus string: " << status_str << std::endl;

    auto js = std::make_shared<JointStates>();
    auto ps = std::make_shared<PoseState>();

    js->chain_name = "left_arm";
    js->joint_positions = std::vector<double>(7, 0.0); 

    ps->chain_name = "left_arm";
    ps->pose = Pose(std::vector<double>{1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
    ps->reference_frame = "base_link"; 
    ps->frame_id = "EndEffector";

    std::cout << "--- JointStates ---" << std::endl;
    std::cout << "Type: " << typeid(*js).name() << std::endl; // Print class name
    std::cout << "Chain Name: " << js->chain_name << std::endl;
    std::cout << "Joints size: " << js->joint_positions.size() << std::endl;

    std::cout << "\n--- PoseState ---" << std::endl;
    std::cout << "Type: " << typeid(*ps).name() << std::endl;
    std::cout << "Chain Name: " << ps->chain_name << std::endl;
    std::cout << "Pose Z: " << ps->pose.position.z << std::endl;

    return 0;
}

Tutorial Source Examples

The following runnable tutorials are maintained in examples/g1/cpp/tutorials. For walkthroughs and safety notes, see Tutorials.

Example 1. Galbot Control Started

Applicable Scenarios: Basic G1 joint control: execute the dual-arm heart gesture and restore the original pose. Before running, confirm working mode and that the emergency-stop button is released. For gripper control and more control cases, see the galbot_robot examples on this page.

examples/cpp/tutorials/src/example1_galbot_control_started.cpp

#include <chrono>
#include <cctype>
#include <iostream>
#include <thread>
#include <vector>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

void print_list(std::ostream& output, const std::vector<std::string>& values) {
    output << "[";
    for (std::size_t i = 0; i < values.size(); ++i) {
        if (i > 0) {
            output << ", ";
        }
        output << "'" << values[i] << "'";
    }
    output << "]";
}

void print_list(std::ostream& output, const std::vector<double>& values) {
    output << "[";
    for (std::size_t i = 0; i < values.size(); ++i) {
        if (i > 0) {
            output << ", ";
        }
        output << values[i];
    }
    output << "]";
}

const char* control_status_to_string(ControlStatus status) {
    switch (status) {
        case ControlStatus::SUCCESS: return "SUCCESS";
        case ControlStatus::TIMEOUT: return "TIMEOUT";
        case ControlStatus::FAULT: return "FAULT";
        case ControlStatus::INVALID_INPUT: return "INVALID_INPUT";
        case ControlStatus::INIT_FAILED: return "INIT_FAILED";
        case ControlStatus::IN_PROGRESS: return "IN_PROGRESS";
        case ControlStatus::STOPPED_UNREACHED: return "STOPPED_UNREACHED";
        case ControlStatus::DATA_FETCH_FAILED: return "DATA_FETCH_FAILED";
        case ControlStatus::PUBLISH_FAIL: return "PUBLISH_FAIL";
        case ControlStatus::COMM_DISCONNECTED: return "COMM_DISCONNECTED";
        default: return "UNKNOWN";
    }
}

ControlStatus set_joint_positions_with_retry(
        GalbotRobot& robot, const std::vector<double>& positions,
        const std::vector<std::string>& joint_group_names, bool is_blocking,
        double max_speed, double timeout_s, int retry_count,
        std::chrono::seconds retry_delay) {
    ControlStatus status = robot.set_joint_positions(
        positions, joint_group_names, {}, is_blocking, max_speed, timeout_s);
    for (int retry_number = 1;
         status != ControlStatus::SUCCESS && retry_number <= retry_count;
         ++retry_number) {
        std::cerr << "Setting angles for joint group ";
        print_list(std::cerr, joint_group_names);
        std::cerr << " failed with " << control_status_to_string(status)
                  << ", retrying " << retry_number << "/" << retry_count
                  << "..." << std::endl;
        std::this_thread::sleep_for(retry_delay);
        status = robot.set_joint_positions(
            positions, joint_group_names, {}, is_blocking, max_speed, timeout_s);
    }
    return status;
}

void demo_heart_pose(GalbotRobot& robot,
                     const std::vector<std::string>& joint_group_names,
                     const std::vector<std::vector<double>>& position_seq,
                     bool is_blocking, double max_speed, double timeout_s){
    /** Get current joint group angles for subsequent restoration */
    std::vector<double> original_pos = robot.get_joint_positions(joint_group_names, {});
    std::cout << "Current angles of joint group ";
    print_list(std::cout, joint_group_names);
    std::cout << ": ";
    print_list(std::cout, original_pos);
    std::cout << std::endl;

    /** Execute heart pose sequence */
    int pos_idx = 0;
    std::cout << "Starting heart gesture..." << std::endl;
    while (pos_idx < position_seq.size()) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        std::vector<double> pos = position_seq[pos_idx];
        ControlStatus control_status = set_joint_positions_with_retry(
            robot, pos, joint_group_names, is_blocking, max_speed, timeout_s,
            3, std::chrono::seconds(1));
        if (control_status == ControlStatus::SUCCESS) {
            std::cout << "Setting angles for joint group ";
            print_list(std::cout, joint_group_names);
            std::cout << " successful" << std::endl;
            pos_idx++;
        } else {
            std::cerr << "Setting angles for joint group ";
            print_list(std::cerr, joint_group_names);
            std::cerr << " failed: " << control_status_to_string(control_status)
                      << std::endl;
            break;
        }
    }

    /** Restore original joint positions */
    std::cout << "Showing heart gesture for 15 seconds, then restoring original pose..." << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(15));
    ControlStatus restore_status = set_joint_positions_with_retry(
        robot, original_pos, joint_group_names, is_blocking, max_speed, timeout_s,
        3, std::chrono::seconds(2));
    if (restore_status == ControlStatus::SUCCESS) {
        std::cout << "Restoring angles for joint group ";
        print_list(std::cout, joint_group_names);
        std::cout << " successful" << std::endl;
    } else {
        std::cerr << "Restoring angles for joint group ";
        print_list(std::cerr, joint_group_names);
        std::cerr << " failed: " << control_status_to_string(restore_status)
                  << std::endl;
    }
    
}

/* @brief Check if the robot is safe
*/
void check_robot_safety(){
    std::cout << "⚠️  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. \n" << std::endl;

    char key;
    for(;;){
        std::cout << "Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...";
        std::cin >> key;

        if(std::tolower(key) == 'y'){
            std::cout << "User confirmed, continuing execution...\n" << std::endl;
            break;
        }else if(std::tolower(key) == 'n'){
            std::cout << "User not confirmed, program exiting...\n" << std::endl;
            exit(0);
        }else{
            std::cout << "Input error, please enter 'y' or 'n'\n" << std::endl;
        }
    }
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::G3); 

        /* Initialize robot */
        if (robot.init()) {
            std::cout << "Initialization successful" << std::endl;
            std::cout << "Is robot running: "
                      << (robot.is_running() ? "True" : "False") << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
            robot.destroy();
            return 1;
        }

        /* Wait for data preparation */
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        
        /** Get joint names */
        std::vector<std::string> joint_names = robot.get_joint_names();
        if (joint_names.empty()) {
            std::cerr << "Failed to get list of joint names" << std::endl;
        } else {
            std::cout << "List of joint names: ";
            print_list(std::cout, joint_names);
            std::cout << std::endl;
        }

        /** Get joint positions using joint group names, empty returns all joints by default */
        std::vector<std::string> joint_group_names = {"left_arm", "right_arm"};
        std::vector<std::vector<double>> position_seq = {{
             1.0,  0.55, -2.0,  1.5, 0.0, -0.7, 0.0,  // left_arm
            -1.0, -0.55,  2.0, -1.5, 0.0,  0.7, 0.0   // right_arm
        }};
        bool is_blocking = true;
        double max_speed = 0.1;
        double timeout_s = 30;

        demo_heart_pose(robot, joint_group_names, position_seq,
                        is_blocking, max_speed, timeout_s);


        /** Actively send SIGINT exit signal to the robot */
        robot.request_shutdown();
        /** Wait to enter shutdown state */
        robot.wait_for_shutdown();
        /** Release SDK resources */
        robot.destroy();
        std::cout << "Resource release successful" << std::endl;    
    }catch(const std::exception& e){
        std::cout << "Error: " << e.what() << std::endl;
    }

    return 0;
}

Example 2. Arm Manipulation

Applicable Scenarios: Learn forward/inverse kinematics and use the planned result to control an arm.

examples/cpp/tutorials/src/example2_arm_manipulation.cpp

#include<iostream>
#include<fstream>
#include<thread>
#include<chrono>
#include <array>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include "opencv2/opencv.hpp"

#include "galbot_robot.hpp"
#include "galbot_motion.hpp"


using namespace galbot::sdk;

inline std::vector<double> quat_normalize(const std::vector<double>& q)
{
    double norm = std::sqrt(
        q[0]*q[0] + q[1]*q[1] +
        q[2]*q[2] + q[3]*q[3]
    );
    if (norm <= 0.0)
        throw std::runtime_error("Zero-norm quaternion");

    return std::vector<double>{ q[0]/norm, q[1]/norm, q[2]/norm, q[3]/norm };
}
inline std::vector<double> quat_conjugate(const std::vector<double>& q)
{
    return std::vector<double>{ -q[0], -q[1], -q[2], q[3] };
}

inline std::vector<double> quat_multiply(const std::vector<double>& q1, const std::vector<double>& q2)
{
    const double x1 = q1[0];
    const double y1 = q1[1];
    const double z1 = q1[2];
    const double w1 = q1[3];
    const double x2 = q2[0];
    const double y2 = q2[1];
    const double z2 = q2[2];
    const double w2 = q2[3];

    return std::vector<double>{
        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
    };
}

template <typename T>
inline T clamp(const T& v, const T& lo, const T& hi)
{
    return (v < lo) ? lo : (v > hi) ? hi : v;
}

inline double orientation_error_angle(const std::vector<double>& A, const std::vector<double>& B)
{
    std::vector<double> qA = quat_normalize({ A[3], A[4], A[5], A[6] });
    std::vector<double> qB = quat_normalize({ B[3], B[4], B[5], B[6] });

    std::vector<double> q_err = quat_multiply(qB, quat_conjugate(qA));
    q_err = quat_normalize(q_err);

    // Numerically stable
    double qw = clamp(q_err[3], -1.0, 1.0);

    return 2.0 * std::acos(qw);
}

struct PoseError
{
    double position_error_norm;   // meters
    double orientation_error_rad;
    double orientation_error_deg;
};

inline PoseError calculate_error(const std::vector<double>& pose1, const std::vector<double>& pose2)
{
    double dx = pose1[0] - pose2[0];
    double dy = pose1[1] - pose2[1];
    double dz = pose1[2] - pose2[2];

    double pos_err = std::sqrt(dx*dx + dy*dy + dz*dz);
    double rot_err = orientation_error_angle(pose1, pose2);

    return PoseError{
        pos_err,
        rot_err,
        rot_err * 180.0 / M_PI
    };
}

/* @brief Check if the robot is safe
*/
void check_robot_safety(){
    std::cout << "⚠️  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. \n" << std::endl;

    char key;
    for(;;){
        std::cout << "Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...";
        std::cin >> key;

        if(std::tolower(key) == 'y'){
            std::cout << "User confirmed, continuing execution...\n" << std::endl;
            break;
        }else if(std::tolower(key) == 'n'){
            std::cout << "User not confirmed, program exiting...\n" << std::endl;
            exit(0);
        }else{
            std::cout << "Input error, please enter 'y' or 'n'\n" << std::endl;
        }
    }
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::G3); 
        auto& motion = GalbotMotion::get_instance(MachineType::G3); 

        /* Initialize robot */
        if (robot.init()) {
            std::cout << "Initialization successful" << std::endl;
            std::cout << "Is robot running: " << robot.is_running() << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
        }
        
        /** Initialize motion */
        if (motion.init()) {
            std::cout << "Motion initialization successful" << std::endl;
        }else{
            std::cerr << "Motion initialization failed" << std::endl;
        }

        /* Wait for data preparation */
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));

        // set initial joint positions
        std::vector<std::string> joint_groups = {"leg", "head", "left_arm", "right_arm"};
        std::vector<std::string> joint_names = {};

        std::vector<double> joint_pos = {0.5, 1.5, 1.0, 0.0, 0.0, 
            0.0, 0.0,
            1.2, -1.0, -0.2, 1.4, 0.1000, -0.3, 0.0000,
            -1.2, 1.0, 0.2, -1.4, -0.1000, 0.3, 0.0000};
        bool is_block = true;
        double max_speed_rad_s = 0.1;
        double timeout_s = 30.0;

        galbot::sdk::ControlStatus joint_execution_status =
            robot.set_joint_positions(joint_pos, joint_groups, joint_names, is_block, max_speed_rad_s, timeout_s);

        if (joint_execution_status == ControlStatus::SUCCESS) {
            std::cout << "✅ Initial joint positions set successfully!" << std::endl;
        } else {
            std::cerr << "Failed to set initial joint positions!" << std::endl;
        }
        
        /** Define target pose */
        std::map<std::string, std::vector<double>> 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.5, 0.3, 1.0, 0.31, 0.05, -0.22, 0.92}},
            {"right_arm", {0.5, -0.3, 1.0, -0.31, 0.05, -0.22, 0.92}}
        };

        std::string target_chain = "left_arm";
        std::string reference_frame = "base_link";
        std::string target_frame = "EndEffector";
        std::string end_link = "left_arm_end_effector_mount_link";

        /** 1. Get end effector pose on chain */
        std::tuple<MotionStatus, std::vector<double>> ret = motion.get_end_effector_pose_on_chain(
            target_chain, target_frame, reference_frame
        );
        if(std::get<0>(ret) == MotionStatus::SUCCESS){
            std::cout << "✅ Current " << target_chain << "end-effector pose: " << std::get<1>(ret).size() << std::endl;
            for (auto pose : std::get<1>(ret)){
                std::cout << pose << " ";
            }
            std::cout << std::endl;
        }else{
            std::cerr << "Failed to get end effector pose on chain!" << std::endl;
        }

        /** 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
        */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        bool enable_collision_check = false;
        std::tuple<MotionStatus, std::unordered_map<std::string, std::vector<double>>> ret_ik = motion.inverse_kinematics(
            chain_pose_baselink[target_chain],
            {target_chain},
            target_frame, 
            reference_frame,
            {},
            enable_collision_check,
            std::make_shared<Parameter>()
        );
        if (std::get<0>(ret_ik) == MotionStatus::SUCCESS){
            std::cout << "✅ Target" << target_chain << "IK solving successful joint_angles_ik:" << std::endl;
            for (auto joint_angle : std::get<1>(ret_ik)[target_chain]){
                std::cout << joint_angle << " ";
            }
            std::cout << std::endl;
        }else{
            std::cerr << "IK solving failed" << std::endl;
        }

        /** 2.2 Set end-effector pose to target pose tgt_pose_ik by setting 
         * joint group angles joint_angles_ik 
        */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        ControlStatus ret2 = robot.set_joint_positions(
            std::get<1>(ret_ik)[target_chain],
            {target_chain},
            {},
            true,
            0.1,
            20.0
        );
        if(ret2 == ControlStatus::SUCCESS){
            std::cout << "✅ Target " << target_chain << "pose setting successful" << std::endl;
        }else{
            std::cerr << "Target " << target_chain << "pose setting failed" << std::endl;
        }

        /** 2.3 Verify whether the set joint group angles are consistent with the solved angles */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        std::tuple<MotionStatus, std::vector<double>> ret3 = motion.get_end_effector_pose_on_chain(
            target_chain,
            target_frame,
            reference_frame
        );
        if(std::get<0>(ret3) == MotionStatus::SUCCESS){
            std::cout << "✅ Verified " << target_chain << "end-effector pose: " << std::get<1>(ret3).size() << std::endl;
            for (auto pose : std::get<1>(ret3)){
                std::cout << pose << " ";
            }
            std::cout << std::endl;
        }else{
            std::cerr << "Failed to verify end effector pose on chain!" << std::endl;
        }
        
        /** 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 */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        std::tuple<MotionStatus, std::vector<double>> ret_fk = motion.forward_kinematics(
            end_link,
            reference_frame,
            std::get<1>(ret_ik),
            std::make_shared<Parameter>()
        );
        if (std::get<0>(ret_fk) == MotionStatus::SUCCESS){
            std::cout << "✅ Verified " << target_chain << "end-effector pose: " << std::get<1>(ret_fk).size() << std::endl;
            for (auto pose : std::get<1>(ret_fk)){
                std::cout << pose << " ";
            }
            std::cout << std::endl;

            PoseError err = calculate_error(chain_pose_baselink[target_chain], std::get<1>(ret_fk));
            std::cout << "Position error: " << err.position_error_norm << " m" << std::endl;
            std::cout << "Orientation error: " << err.orientation_error_deg << " deg" << std::endl;
        }else{
            std::cerr << "Failed to verify end effector pose on chain!" << std::endl;
        }

        /** 3. Restore to original pose by setting end-effector pose
         *  3.1 Set end-effector pose to restore to original pose
         */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        MotionStatus ret4 = motion.set_end_effector_pose(
            std::get<1>(ret),
            target_chain,
            reference_frame,
            nullptr,
            enable_collision_check,
            true,
            5.0,
            std::make_shared<Parameter>()
        );
        if(ret4 == MotionStatus::SUCCESS){
            std::cout << "✅ Restored " << target_chain << "end-effector pose to original pose" << std::endl;
        }else{
            std::cerr << "Failed to restore " << target_chain << "end-effector pose to original pose" << std::endl;
        }

        /** 3.2 Get end-effector pose and verify whether it has been restored to original pose */
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        std::tuple<MotionStatus, std::vector<double>> ret5 = motion.get_end_effector_pose_on_chain(
            target_chain,
            target_frame,
            reference_frame
        );
        if(std::get<0>(ret5) == MotionStatus::SUCCESS){
            std::cout << "✅ Verified " << target_chain << "end-effector pose: " << std::get<1>(ret5).size() << std::endl;
            for (auto pose : std::get<1>(ret5)){
                std::cout << pose << " ";
            }
            std::cout << std::endl;

            PoseError err = calculate_error(std::get<1>(ret), std::get<1>(ret5));
            std::cout << "Position error: " << err.position_error_norm << " m" << std::endl;
            std::cout << "Orientation error: " << err.orientation_error_deg << " deg" << std::endl;
        }else{
            std::cerr << "Failed to verify end effector pose on chain!" << std::endl;
        }

        /** Actively send SIGINT exit signal to the robot */
        robot.request_shutdown();
        /** Wait to enter shutdown state */
        robot.wait_for_shutdown();
        /** Release SDK resources */
        robot.destroy();
        std::cout << "Resource release successful" << std::endl;    
    }catch(const std::exception& e){
        std::cout << "Error: " << e.what() << std::endl;
    }

    return 0;
}

Example 3. Robot Navigation

Applicable Scenarios: Initialize navigation, localize the robot, and navigate to a target pose.

examples/cpp/tutorials/src/example3_robot_navigation.cpp

#include<iostream>
#include<fstream>
#include<thread>
#include<chrono>
#include <array>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include "opencv2/opencv.hpp"

#include "galbot_robot.hpp"
#include "galbot_motion.hpp"
#include "galbot_navigation.hpp"

using namespace galbot::sdk;

inline std::vector<double> quat_normalize(const std::vector<double>& q)
{
    double norm = std::sqrt(
        q[0]*q[0] + q[1]*q[1] +
        q[2]*q[2] + q[3]*q[3]
    );
    if (norm <= 0.0)
        throw std::runtime_error("Zero-norm quaternion");

    return std::vector<double>{ q[0]/norm, q[1]/norm, q[2]/norm, q[3]/norm };
}
inline std::vector<double> quat_conjugate(const std::vector<double>& q)
{
    return std::vector<double>{ -q[0], -q[1], -q[2], q[3] };
}

inline std::vector<double> quat_multiply(const std::vector<double>& q1, const std::vector<double>& q2)
{
    const double x1 = q1[0];
    const double y1 = q1[1];
    const double z1 = q1[2];
    const double w1 = q1[3];
    const double x2 = q2[0];
    const double y2 = q2[1];
    const double z2 = q2[2];
    const double w2 = q2[3];

    return std::vector<double>{
        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
    };
}

template <typename T>
inline T clamp(const T& v, const T& lo, const T& hi)
{
    return (v < lo) ? lo : (v > hi) ? hi : v;
}

inline double orientation_error_angle(const std::vector<double>& A, const std::vector<double>& B)
{
    std::vector<double> qA = quat_normalize({ A[3], A[4], A[5], A[6] });
    std::vector<double> qB = quat_normalize({ B[3], B[4], B[5], B[6] });

    std::vector<double> q_err = quat_multiply(qB, quat_conjugate(qA));
    q_err = quat_normalize(q_err);

    // Numerically stable
    double qw = clamp(q_err[3], -1.0, 1.0);

    return 2.0 * std::acos(qw);
}

struct PoseError
{
    double position_error_norm;   // meters
    double orientation_error_rad;
    double orientation_error_deg;
};

inline PoseError calculate_error(const std::vector<double>& pose1, const std::vector<double>& pose2)
{
    double dx = pose1[0] - pose2[0];
    double dy = pose1[1] - pose2[1];
    double dz = pose1[2] - pose2[2];

    double pos_err = std::sqrt(dx*dx + dy*dy + dz*dz);
    double rot_err = orientation_error_angle(pose1, pose2);

    return PoseError{
        pos_err,
        rot_err,
        rot_err * 180.0 / M_PI
    };
}


/**
 * @brief Convert quaternion [x, y, z, w] to 3x3 rotation matrix
 * @param qx X component of quaternion
 * @param qy Y component of quaternion
 * @param qz Z component of quaternion
 * @param qw W component (scalar) of quaternion
 * @return 3x3 rotation matrix (CV_64F)
 */
inline cv::Mat quat_to_rotation_matrix(double qx, double qy, double qz, double qw) {
    cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
    
    R.at<double>(0, 0) = 1 - 2*(qy*qy + qz*qz);
    R.at<double>(0, 1) = 2*(qx*qy - qw*qz);
    R.at<double>(0, 2) = 2*(qx*qz + qw*qy);
    
    R.at<double>(1, 0) = 2*(qx*qy + qw*qz);
    R.at<double>(1, 1) = 1 - 2*(qx*qx + qz*qz);
    R.at<double>(1, 2) = 2*(qy*qz - qw*qx);
    
    R.at<double>(2, 0) = 2*(qx*qz - qw*qy);
    R.at<double>(2, 1) = 2*(qy*qz + qw*qx);
    R.at<double>(2, 2) = 1 - 2*(qx*qx + qy*qy);
    
    return R;
}

/**
 * @brief Convert 3x3 rotation matrix to quaternion [x, y, z, w]
 * @param R 3x3 rotation matrix (CV_64F)
 * @return Quaternion as vector [qx, qy, qz, qw]
 */
inline std::vector<double> rotation_matrix_to_quat(const cv::Mat& R) {
    double trace = R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2);
    double qx, qy, qz, qw;
    
    if (trace > 0) {
        double s = 0.5 / std::sqrt(trace + 1.0);
        qw = 0.25 / s;
        qx = (R.at<double>(2, 1) - R.at<double>(1, 2)) * s;
        qy = (R.at<double>(0, 2) - R.at<double>(2, 0)) * s;
        qz = (R.at<double>(1, 0) - R.at<double>(0, 1)) * s;
    } else if ((R.at<double>(0, 0) > R.at<double>(1, 1)) && (R.at<double>(0, 0) > R.at<double>(2, 2))) {
        double s = 2.0 * std::sqrt(1.0 + R.at<double>(0, 0) - R.at<double>(1, 1) - R.at<double>(2, 2));
        qw = (R.at<double>(2, 1) - R.at<double>(1, 2)) / s;
        qx = 0.25 * s;
        qy = (R.at<double>(0, 1) + R.at<double>(1, 0)) / s;
        qz = (R.at<double>(0, 2) + R.at<double>(2, 0)) / s;
    } else if (R.at<double>(1, 1) > R.at<double>(2, 2)) {
        double s = 2.0 * std::sqrt(1.0 + R.at<double>(1, 1) - R.at<double>(0, 0) - R.at<double>(2, 2));
        qw = (R.at<double>(0, 2) - R.at<double>(2, 0)) / s;
        qx = (R.at<double>(0, 1) + R.at<double>(1, 0)) / s;
        qy = 0.25 * s;
        qz = (R.at<double>(1, 2) + R.at<double>(2, 1)) / s;
    } else {
        double s = 2.0 * std::sqrt(1.0 + R.at<double>(2, 2) - R.at<double>(0, 0) - R.at<double>(1, 1));
        qw = (R.at<double>(1, 0) - R.at<double>(0, 1)) / s;
        qx = (R.at<double>(0, 2) + R.at<double>(2, 0)) / s;
        qy = (R.at<double>(1, 2) + R.at<double>(2, 1)) / s;
        qz = 0.25 * s;
    }
    
    return std::vector<double>{qx, qy, qz, qw};
}

/**
 * @brief Convert local pose to global pose
 * 
 * Transforms a pose from local frame to global frame using the start pose as reference.
 * Mathematically: global_pose = start_pose @ local_pose (matrix multiplication)
 * 
 * @param cur_pose Current/start pose (reference frame), [x, y, z, qx, qy, qz, qw]
 * @param local_pose Local pose relative to start, [x, y, z, qx, qy, qz, qw]
 * @return Global pose result, [x, y, z, qx, qy, qz, qw]
 */
Pose local_pose_to_global(Pose cur_pose, Pose local_pose){
    // Create 4x4 transformation matrix from start pose
    cv::Mat T_start = cv::Mat::eye(4, 4, CV_64F);
    cv::Mat R_start = quat_to_rotation_matrix(cur_pose.orientation.x, cur_pose.orientation.y, 
                                               cur_pose.orientation.z, cur_pose.orientation.w);
    R_start.copyTo(T_start(cv::Rect(0, 0, 3, 3)));
    T_start.at<double>(0, 3) = cur_pose.position.x;
    T_start.at<double>(1, 3) = cur_pose.position.y;
    T_start.at<double>(2, 3) = cur_pose.position.z;
    
    // Create 4x4 transformation matrix from local pose
    cv::Mat T_local = cv::Mat::eye(4, 4, CV_64F);
    cv::Mat R_local = quat_to_rotation_matrix(local_pose.orientation.x, local_pose.orientation.y,
                                               local_pose.orientation.z, local_pose.orientation.w);
    R_local.copyTo(T_local(cv::Rect(0, 0, 3, 3)));
    T_local.at<double>(0, 3) = local_pose.position.x;
    T_local.at<double>(1, 3) = local_pose.position.y;
    T_local.at<double>(2, 3) = local_pose.position.z;
    
    // Global transformation = start transformation * local transformation
    cv::Mat T_global = T_start * T_local;
    
    // Extract position and orientation from result
    cv::Mat R_global = T_global(cv::Rect(0, 0, 3, 3));
    std::vector<double> quat_global = rotation_matrix_to_quat(R_global);
    
    Pose result;
    result.position.x = T_global.at<double>(0, 3);
    result.position.y = T_global.at<double>(1, 3);
    result.position.z = T_global.at<double>(2, 3);
    result.orientation.x = quat_global[0];
    result.orientation.y = quat_global[1];
    result.orientation.z = quat_global[2];
    result.orientation.w = quat_global[3];
    
    return result;
}

void demo_square_move(GalbotRobot& robot, GalbotNavigation& navi){
    Pose local_pose(std::vector<double>{0.5, 0.0, 0.0, 0.0, 0.0, 0.707, 0.707});

    for(int i=0; i<4; ++i){
        Pose current_pose = navi.get_current_pose();
        std::cout << "Current pose: [" 
                  << current_pose.position.x << ", "
                  << current_pose.position.y << ", "
                  << current_pose.position.z << ", "
                  << current_pose.orientation.x << ", "
                  << current_pose.orientation.y << ", "
                  << current_pose.orientation.z << ", "
                  << current_pose.orientation.w << "]" << std::endl;

        Pose goal_pose = local_pose_to_global(current_pose, local_pose);
        
        if(navi.check_path_reachability(goal_pose, current_pose)){
            int retry_cnt = 3;
            for(;;){
                // navi.move_to_goal(goal_pose);
                std::this_thread::sleep_for(std::chrono::seconds(1));
                navi.navigate_to_goal(goal_pose, true, true, 30);
                retry_cnt -= 1;
                if(navi.check_goal_arrival() || retry_cnt <= 0){
                    break;
                }else{
                    std::cout << "Navigation failed, retry count: " << retry_cnt << std::endl;
                }
            }
        }
    }
    Pose final_pose = navi.get_current_pose();
    std::cout << "Final pose: [" 
              << final_pose.position.x << ", "
              << final_pose.position.y << ", "
              << final_pose.position.z << ", "
              << final_pose.orientation.x << ", "
              << final_pose.orientation.y << ", "
              << final_pose.orientation.z << ", "
              << final_pose.orientation.w << "]" << std::endl;
}

/* @brief Check if the robot is safe
*/
void check_robot_safety(){
    std::cout << "⚠️  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. \n" << std::endl;

    char key;
    for(;;){
        std::cout << "Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...";
        std::cin >> key;

        if(std::tolower(key) == 'y'){
            std::cout << "User confirmed, continuing execution...\n" << std::endl;
            break;
        }else if(std::tolower(key) == 'n'){
            std::cout << "User not confirmed, program exiting...\n" << std::endl;
            exit(0);
        }else{
            std::cout << "Input error, please enter 'y' or 'n'\n" << std::endl;
        }
    }
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::G3); 
        auto& navi = GalbotNavigation::get_instance(MachineType::G3);        

        /* Initialize robot */
        if (robot.init()) {
            std::cout << "Initialization successful" << std::endl;
            std::cout << "Is robot running: " << robot.is_running() << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
        }
        /** Initialize navigation */
        if (navi.init()) {
            std::cout << "Navigation initialization successful" << std::endl;
        }else{
            std::cerr << "Navigation initialization failed" << std::endl;
        }

        /* Wait for data preparation */
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        
        /** If not localized, relocalize */
        bool is_localized = navi.is_localized();
        std::cout << "Is robot localized: " << is_localized << std::endl;
        while(!is_localized){
            NavigationStatus status = navi.relocalize(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
            std::this_thread::sleep_for(std::chrono::seconds(3));
            std::cout << "Waiting for localization..." << std::endl;
            is_localized = navi.is_localized();
        }

        /** If localized, move the robot */
        demo_square_move(robot, navi);

        /** Actively send SIGINT exit signal to the robot */
        robot.request_shutdown();
        /** Wait to enter shutdown state */
        robot.wait_for_shutdown();
        /** Release SDK resources */
        robot.destroy();
        std::cout << "Resource release successful" << std::endl;    
    }catch(const std::exception& e){
        std::cout << "Error: " << e.what() << std::endl;
    }

    return 0;
}

Example 4. Sensor Data Collection

Applicable Scenarios: Acquire RGB camera, LiDAR, IMU, and other G3 sensor data.

examples/cpp/tutorials/src/example4_sensor_data_collect.cpp

#include<iostream>
#include<fstream>
#include<thread>
#include<chrono>
#include "opencv2/opencv.hpp"

#include "galbot_robot.hpp"

using namespace galbot::sdk;

/* @brief Check if the robot is safe
*/
void check_robot_safety(){
    std::cout << "⚠️  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. \n" << std::endl;

    char key;
    for(;;){
        std::cout << "Please confirm that the robot's emergency stop button is released and there are no obstacles. Continue? (y/n)...";
        std::cin >> key;

        if(std::tolower(key) == 'y'){
            std::cout << "User confirmed, continuing execution...\n" << std::endl;
            break;
        }else if(std::tolower(key) == 'n'){
            std::cout << "User not confirmed, program exiting...\n" << std::endl;
            exit(0);
        }else{
            std::cout << "Input error, please enter 'y' or 'n'\n" << std::endl;
        }
    }
}

struct Point3D {
    float x, y, z;
    uint8_t intensity;
};

std::vector<Point3D> get_xyz_points(const std::shared_ptr<LidarData>& lidar_data, bool remove_nan = false){
    std::vector<Point3D> points;
    if (!lidar_data || lidar_data->data.empty()) return points;

    /** 1. lookup the offset of x, y, z field */
    auto off_x = -1, off_y = -1, off_z = -1;
    auto off_intensity = -1;
    std::cout << "off_x" << off_x << std::endl;
    for (const auto& f: lidar_data->fields) {
        if (f.name == "x") off_x = f.offset;
        else if (f.name == "y") off_y = f.offset;
        else if (f.name == "z") off_z = f.offset;
        else if (f.name == "reflectivity") off_intensity = f.offset;
        std::cout << f.name << " " << f.datatype << " ";
    }
    std::cout << "\n";

    if (off_x == -1 || off_y == -1 || off_z == -1) {
        std::cerr << "Error: the lidar data has no required x/y/z field." << std::endl;
        return points;
    }

    /** 2. read data directly */
    uint32_t num_points = lidar_data->width * lidar_data->height;
    points.reserve(num_points);
    
    const uint8_t* raw_data = lidar_data->data.data();
    const uint32_t point_step = lidar_data->point_step;

    for (uint32_t i=0; i<num_points; ++i) {
        /** get pointer for current point */
        const uint8_t* point_ptr = raw_data + (i * point_step);
        
        /** use reinterpret_cast convert pointer type to read memory */
        /** assume points in lidar data are float32, which is most common data type */
        float x = *reinterpret_cast<const float*>(point_ptr + off_x);
        float y = *reinterpret_cast<const float*>(point_ptr + off_y);
        float z = *reinterpret_cast<const float*>(point_ptr + off_z);
        uint8_t intensity = off_intensity != -1 ? *reinterpret_cast<const uint8_t*>(point_ptr + off_intensity) : 255;
        
        if (remove_nan && (std::isnan(x) || std::isnan(y) || std::isnan(z))) {
            continue;
        }
        points.push_back(Point3D{x, y, z, intensity});
    }
    return points;
}

void save_xyz_to_pcd(const std::vector<Point3D>& points, const std::string& file_path){
    std::ofstream fs(file_path);
    if (!fs.is_open()) {
        std::cerr << "Error: cannot open file " << file_path << " for writing." << std::endl;
        return;
    }

    // PCD 0.7 Header
    fs << "# .PCD v0.7 - Point Cloud Data file format\n"
       << "VERSION 0.7\n"
       << "FIELDS x y z\n"
       << "SIZE 4 4 4\n"
       << "TYPE F F F\n"
       << "COUNT 1 1 1\n"
       << "WIDTH " << points.size() << "\n"
       << "HEIGHT 1\n"
       << "VIEWPOINT 0 0 0 1 0 0 0\n"
       << "POINTS " << points.size() << "\n"
       << "DATA ascii\n";
    
    for (const auto& p: points) {
        fs << p.x << " " << p.y << " " << p.z << "\n";
    }

    fs.close();
    std::cout << "Successfully saved " << points.size() << " points to " << file_path << std::endl;
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::G3); 
        
        /** Get left arm RGB, base LiDAR, and torso IMU data.
            G3 does not have arm-mounted depth cameras. */
        std::unordered_set<SensorType> sensor_types = {
            SensorType::LEFT_ARM_CAMERA,        /** Left arm rgb camera */
            SensorType::BASE_LIDAR,              /** Base LiDAR */
            SensorType::TORSO_IMU              /** Torso IMU */
        };

        /* Initialize robot */
        if (robot.init(sensor_types)) {
            std::cout << "Initialization successful" << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
        }

        /* Wait for data preparation */
        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        
        /** Get rgb image */
        std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(SensorType::LEFT_ARM_CAMERA, RgbOutputFormat::JPEG, true);
        std::shared_ptr<cv::Mat> rgb_img;
        if (rgb_data) {
            std::cout << "Get rgb image suceess" << std::endl;
            rgb_img = rgb_data->convert_to_cv2_mat();
            
            cv::imwrite("rgb_image.png", *rgb_img);
            std::cout << "RGB image saved as 'rgb_image.png'" << std::endl;
        }else{
            std::cerr << "No rgb image data!" << std::endl;
        }

        /** Get lidar data */
        std::shared_ptr<LidarData> lidar_data = robot.get_lidar_data(SensorType::BASE_LIDAR);
        if (lidar_data) {
            std::vector<Point3D> points = get_xyz_points(lidar_data);
            save_xyz_to_pcd(points, "lidar_points.pcd");
        } else {
            std::cerr << "No lidar data!" << std::endl;
        }
        
        /** Get torso imu data */
        std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::TORSO_IMU);
        if (imu_data) {
            std::cout << "Get imu data suceess" << std::endl;
        } else {
            std::cerr << "No imu data!" << std::endl;
        }
        
        /** Actively send SIGINT exit signal to the robot */
        robot.request_shutdown();
        /** Wait to enter shutdown state */
        robot.wait_for_shutdown();
        /** Release SDK resources */
        robot.destroy();
        std::cout << "Resource release successful" << std::endl;    
    }catch(const std::exception& e){
        std::cout << "Error: " << e.what() << std::endl;
    }

    

    return 0;
}

Example 5. Execute VLA

Applicable Scenarios: Demonstrate how a VLA client collects synchronized observations and streams absolute-joint action chunks through set_joint_commands. The sample JSON contains 23 dimensions; unavailable grippers and their dimensions are excluded automatically. The executable requires an action JSON path, for example <SDK_ROOT>/examples/g3/assets/tutorials/example5_vla.json, as its command-line argument. 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. For model output expressed as end-effector EE poses, refer to set_end_effector_command.

examples/cpp/tutorials/src/example5_execute_vla.cpp

/**
 * 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 absolute-joint action chunk frame
 * by frame at the configured control frequency. The sample JSON has 23
 * dimensions; unavailable left/right gripper dimensions are filtered at
 * startup.
 *
 * 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 synchronized observation acquisition and action-chunk
 * execution 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/g3/cpp/galbot_robot/src/set_end_effector_commands_example.cpp.
 *
 * A JSON path is required because an installed executable may not have the
 * same directory layout as the SDK source tree. The SDK-provided sample is:
 * examples/g3/assets/tutorials/example5_vla.json
 *
 * Run: ./tutorials_example5_execute_vla <path/to/example5_vla.json>
 *
 * Before running, release the emergency-stop button and clear the area around
 * the robot. Keep a hand near the emergency-stop button during execution.
 */

#include <algorithm>
#include <chrono>
#include <cctype>
#include <cmath>
#include <cstddef>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <string>
#include <thread>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include "galbot_robot.hpp"

namespace {

using galbot::sdk::ControlStatus;
using galbot::sdk::GalbotRobot;
using galbot::sdk::JointCommand;
using galbot::sdk::SensorType;

constexpr double kControlFrequencyHz = 20.0;
// Number of model action frames returned and executed in one chunk.
constexpr std::size_t kActionChunkSize = 30;
constexpr double kGripperVelocity = 0.5;
constexpr double kGripperEffort = 10.0;
constexpr char kTaskPrompt[] =
    "Based on the current view, predict the next robot action.";

const std::vector<std::string> kActionGroups = {
    "leg",          "head",      "left_arm",
    "left_gripper", "right_arm", "right_gripper",
};

const std::unordered_map<std::string, std::pair<std::size_t, std::size_t>>
    kGroupSlices = {
        {"leg", {0, 5}},           {"head", {5, 7}},
        {"left_arm", {7, 14}},     {"left_gripper", {14, 15}},
        {"right_arm", {15, 22}},   {"right_gripper", {22, 23}},
};

const std::unordered_map<std::string, std::vector<std::string>>
    kGroupJointNames = {
        {"leg",
         {"leg_joint1", "leg_joint2", "leg_joint3", "leg_joint4",
          "leg_joint5"}},
        {"head", {"head_joint1", "head_joint2"}},
        {"left_arm",
         {"left_arm_joint1", "left_arm_joint2", "left_arm_joint3",
          "left_arm_joint4", "left_arm_joint5", "left_arm_joint6",
          "left_arm_joint7"}},
        {"left_gripper", {"left_gripper_joint1"}},
        {"right_arm",
         {"right_arm_joint1", "right_arm_joint2", "right_arm_joint3",
          "right_arm_joint4", "right_arm_joint5", "right_arm_joint6",
          "right_arm_joint7"}},
        {"right_gripper", {"right_gripper_joint1"}},
};

struct VlaObservation {
  std::vector<double> state;
  std::size_t image_bytes = 0;
  std::string prompt;
};

struct MockVlaResponse {
  std::vector<std::vector<double>> actions;
  std::size_t next_cursor = 0;
};

std::vector<std::string> action_joint_names(
    const std::vector<std::string>& action_groups) {
  std::vector<std::string> names;
  for (const auto& group : action_groups) {
    const auto& group_names = kGroupJointNames.at(group);
    names.insert(names.end(), group_names.begin(), group_names.end());
  }
  return names;
}

std::string action_json_path(int argc, char* argv[]) {
  if (argc != 2) {
    throw std::runtime_error(
        "Usage: ./tutorials_example5_execute_vla <path/to/action.json>\n"
        "The SDK sample JSON is located at "
        "<SDK_ROOT>/examples/g3/assets/tutorials/example5_vla.json");
  }
  return argv[1];
}

void confirm_safe_environment() {
  std::cout
      << "WARNING: Release the emergency-stop button, keep the robot in "
         "working mode, and clear people and obstacles from its motion range."
      << std::endl;
  std::cout << "Continue with the VLA execution example? [y/N]: ";

  std::string answer;
  std::cin >> answer;
  std::transform(answer.begin(), answer.end(), answer.begin(),
                 [](unsigned char character) {
                   return static_cast<char>(std::tolower(character));
                 });
  if (answer != "y" && answer != "yes") {
    throw std::runtime_error("Execution cancelled by the user");
  }
}

void validate_action_chunk(
    const std::vector<std::vector<double>>& actions,
    std::size_t expected_dimension = 23) {
  for (std::size_t frame_index = 0; frame_index < actions.size();
       ++frame_index) {
    if (actions[frame_index].size() != expected_dimension) {
      throw std::runtime_error(
          "VLA action frame " + std::to_string(frame_index) + " has " +
          std::to_string(actions[frame_index].size()) +
          " values; expected " + std::to_string(expected_dimension));
    }
    for (double value : actions[frame_index]) {
      if (!std::isfinite(value)) {
        throw std::runtime_error("VLA actions contain NaN or infinity");
      }
    }
  }
}

std::vector<std::vector<double>> load_mock_actions(
    const std::string& json_path) {
  std::ifstream file(json_path);
  if (!file.is_open()) {
    throw std::runtime_error("Mock VLA action file does not exist: " +
                             json_path);
  }

  boost::property_tree::ptree payload;
  boost::property_tree::read_json(file, payload);

  std::vector<std::vector<double>> actions;
  for (const auto& frame_node : payload.get_child("frames")) {
    std::vector<double> frame;
    for (const auto& value_node : frame_node.second) {
      frame.push_back(value_node.second.get_value<double>());
    }
    actions.push_back(std::move(frame));
  }
  const std::size_t declared_frames = payload.get<std::size_t>("num_frames");
  if (declared_frames != actions.size()) {
    throw std::runtime_error(
        "Mock num_frames does not match the number of JSON frames: " +
        std::to_string(declared_frames) + " != " +
        std::to_string(actions.size()));
  }
  validate_action_chunk(actions);
  return actions;
}

VlaObservation collect_observation(
    GalbotRobot& robot,
    const std::vector<std::string>& action_groups) {
  const std::vector<SensorType> cameras = {SensorType::HEAD_LEFT_CAMERA};
  auto observation = robot.get_synced_observation(cameras, true);
  if (!observation) {
    throw std::runtime_error("get_synced_observation failed");
  }

  const auto image_iterator =
      observation->rgb_data_map.find(SensorType::HEAD_LEFT_CAMERA);
  if (image_iterator == observation->rgb_data_map.end() ||
      !image_iterator->second) {
    throw std::runtime_error("Synchronized head-left image is missing");
  }
  if (!observation->joint_state) {
    throw std::runtime_error("Synchronized joint state is missing");
  }

  std::unordered_map<std::string, double> state_by_name;
  for (const auto& joint_state :
       observation->joint_state->joint_state_vec) {
    state_by_name[joint_state.joint_name] = joint_state.position;
  }

  VlaObservation result;
  for (const auto& joint_name : action_joint_names(action_groups)) {
    const auto iterator = state_by_name.find(joint_name);
    if (iterator == state_by_name.end()) {
      throw std::runtime_error("Joint state is missing joint: " + joint_name);
    }
    result.state.push_back(iterator->second);
  }
  result.image_bytes = image_iterator->second->data.size();
  result.prompt = kTaskPrompt;
  return result;
}

MockVlaResponse mock_vla_server(
    const VlaObservation& observation,
    const std::vector<std::vector<double>>& all_actions, std::size_t cursor,
    const std::vector<std::string>& action_groups,
    std::size_t chunk_size = kActionChunkSize) {
  const std::size_t expected_dimension =
      action_joint_names(action_groups).size();
  if (observation.state.size() != expected_dimension) {
    throw std::runtime_error(
        "Observation state must contain " +
        std::to_string(expected_dimension) + " joints");
  }

  const std::size_t end =
      std::min(cursor + chunk_size, all_actions.size());
  MockVlaResponse response;
  response.actions.assign(all_actions.begin() + cursor,
                          all_actions.begin() + end);
  response.next_cursor = end;

  std::cout << "Mock VLA server received one request: prompt='"
            << observation.prompt << "', image_bytes="
            << observation.image_bytes << ", frames=" << cursor << ":" << end
            << std::endl;
  std::cout << "Mock VLA server returned action chunk: shape=("
            << response.actions.size() << ", " << expected_dimension << ")"
            << std::endl;
  return response;
}

std::vector<double> values_for_groups(
    const std::vector<double>& action,
    const std::vector<std::string>& groups) {
  std::vector<double> values;
  for (const auto& group : groups) {
    const auto [start, end] = kGroupSlices.at(group);
    values.insert(values.end(), action.begin() + start, action.begin() + end);
  }
  return values;
}

std::vector<std::string> detect_available_action_groups(GalbotRobot& robot) {
  // Some WBC configurations may still publish a placeholder gripper joint
  // without physical hardware. move_to_first_action() therefore performs a
  // second availability check using the initialization command status.
  std::unordered_set<std::string> available_grippers;
  for (const auto& [group, sdk_group] :
       std::vector<std::pair<std::string, std::string>>{
           {"left_gripper", "left_gripper"},
           {"right_gripper", "right_gripper"}}) {
    if (robot.get_gripper_state(sdk_group)) {
      available_grippers.insert(group);
      std::cout << "Detected " << group << " feedback." << std::endl;
    } else {
      std::cout << "No " << group
                << " detected; its action dimension will be skipped."
                << std::endl;
    }
  }

  std::vector<std::string> active_groups;
  for (const auto& group : kActionGroups) {
    if (group.find("gripper") == std::string::npos ||
        available_grippers.count(group) != 0) {
      active_groups.push_back(group);
    }
  }
  return active_groups;
}

std::vector<std::vector<double>> filter_actions_for_groups(
    const std::vector<std::vector<double>>& actions,
    const std::vector<std::string>& action_groups) {
  std::vector<std::vector<double>> filtered_actions;
  filtered_actions.reserve(actions.size());
  for (const auto& action : actions) {
    filtered_actions.push_back(values_for_groups(action, action_groups));
  }
  return filtered_actions;
}

std::vector<std::string> move_to_first_action(
    GalbotRobot& robot, const std::vector<double>& first_action,
    const std::vector<std::string>& action_groups) {
  const std::vector<std::vector<std::string>> movement_stages = {
      {"leg"},
      {"head", "left_arm", "right_arm"},
  };
  for (const auto& groups : movement_stages) {
    const ControlStatus status = robot.set_joint_positions(
        values_for_groups(first_action, groups), groups, {}, true, 0.2, 10.0);
    if (status != ControlStatus::SUCCESS) {
      throw std::runtime_error(
          "Failed to move a joint group stage to the first JSON action");
    }
  }

  std::vector<std::string> active_groups = action_groups;
  for (const auto& group : {std::string("left_gripper"),
                            std::string("right_gripper")}) {
    if (std::find(active_groups.begin(), active_groups.end(), group) ==
        active_groups.end()) {
      continue;
    }
    const ControlStatus status = robot.set_gripper_command(
        group, first_action[kGroupSlices.at(group).first], 0.1,
        kGripperEffort, true);
    if (status == ControlStatus::DATA_FETCH_FAILED ||
        status == ControlStatus::TIMEOUT) {
      std::cout << "WARNING: " << group << " is unavailable (status="
                << static_cast<int>(status)
                << "); its action dimension will be skipped." << std::endl;
      active_groups.erase(
          std::remove(active_groups.begin(), active_groups.end(), group),
          active_groups.end());
    } else if (status != ControlStatus::SUCCESS) {
      throw std::runtime_error(
          "Failed to initialize " + group + " from the first JSON action; "
          "status=" + std::to_string(static_cast<int>(status)));
    }
  }
  std::cout << "Robot reached the first pose in the VLA action file."
            << std::endl;
  return active_groups;
}

std::vector<JointCommand> build_joint_commands(
    const std::vector<double>& action,
    const std::vector<std::string>& action_groups) {
  const std::size_t expected_dimension =
      action_joint_names(action_groups).size();
  if (action.size() != expected_dimension) {
    throw std::runtime_error(
        "Action must contain " + std::to_string(expected_dimension) +
        " joint values");
  }

  std::vector<JointCommand> commands;
  commands.reserve(action.size());
  std::size_t offset = 0;
  for (const auto& group : action_groups) {
    const bool is_gripper = group.find("gripper") != std::string::npos;
    for (std::size_t index = 0; index < kGroupJointNames.at(group).size();
         ++index) {
      JointCommand command;
      command.position = action[offset++];
      if (is_gripper) {
        command.velocity = kGripperVelocity;
        command.effort = kGripperEffort;
      }
      commands.push_back(command);
    }
  }
  return commands;
}

void execute_action_chunk(
    GalbotRobot& robot,
    const std::vector<std::vector<double>>& actions,
    const std::vector<std::string>& action_groups) {
  validate_action_chunk(actions, action_joint_names(action_groups).size());
  if (actions.empty()) {
    std::cout << "The VLA model returned an empty chunk." << std::endl;
    return;
  }

  const auto period = std::chrono::duration_cast<std::chrono::steady_clock::duration>(
      std::chrono::duration<double>(1.0 / kControlFrequencyHz));
  auto next_tick = std::chrono::steady_clock::now();
  std::cout << "Streaming " << actions.size() << " frames at "
            << kControlFrequencyHz << " Hz with set_joint_commands..."
            << std::endl;

  for (std::size_t frame_index = 0; frame_index < actions.size();
       ++frame_index) {
    const ControlStatus status = robot.set_joint_commands(
        build_joint_commands(actions[frame_index], action_groups),
        action_groups, {}, 0.0);
    if (status != ControlStatus::SUCCESS) {
      throw std::runtime_error("set_joint_commands failed at frame " +
                               std::to_string(frame_index + 1));
    }
    next_tick += period;
    std::this_thread::sleep_until(next_tick);
  }
  std::cout << "Action chunk execution completed." << std::endl;
}

void shutdown_robot(GalbotRobot& robot) {
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "SDK resources released." << std::endl;
}

}  // namespace

int main(int argc, char* argv[]) {
  using galbot::sdk::MachineType;

  GalbotRobot& robot = GalbotRobot::get_instance(MachineType::G3);
  bool initialized = false;
  try {
    // Require an explicit path: the executable location on a robot is not
    // guaranteed to match the SDK source-tree layout.
    const std::string json_path = action_json_path(argc, argv);
    confirm_safe_environment();
    const std::unordered_set<SensorType> enabled_sensors = {
        SensorType::HEAD_LEFT_CAMERA};
    if (!robot.init(enabled_sensors, true)) {
      throw std::runtime_error("GalbotRobot initialization failed");
    }
    initialized = true;
    std::this_thread::sleep_for(std::chrono::seconds(2));

    const auto full_actions = load_mock_actions(json_path);
    std::cout << "Loaded VLA actions from: " << json_path << std::endl;
    std::cout << "Action data shape: (" << full_actions.size()
              << ", 23), including head" << std::endl;
    auto action_groups = detect_available_action_groups(robot);
    action_groups =
        move_to_first_action(robot, full_actions.front(), action_groups);
    const auto all_actions =
        filter_actions_for_groups(full_actions, action_groups);
    std::cout << "Active action groups:";
    for (const auto& group : action_groups) {
      std::cout << " " << group;
    }
    std::cout << "; effective action shape: (" << all_actions.size() << ", "
              << action_joint_names(action_groups).size() << ")" << std::endl;

    std::size_t cursor = 0;
    while (true) {
      const VlaObservation observation =
          collect_observation(robot, action_groups);
      MockVlaResponse response =
          mock_vla_server(observation, all_actions, cursor, action_groups);
      cursor = response.next_cursor;
      if (response.actions.empty()) {
        std::cout << "Mock VLA server has no more actions." << std::endl;
        break;
      }
      execute_action_chunk(robot, response.actions, action_groups);
    }

    std::cout << "VLA execution example finished successfully." << std::endl;
    shutdown_robot(robot);
    initialized = false;
  } catch (const std::exception& error) {
    std::cerr << "Error: " << error.what() << std::endl;
    if (initialized) {
      shutdown_robot(robot);
    }
    return 1;
  }
  return 0;
}

Example 6. Integrated Task: Navigation + Perception + Grasping + Placement

Applicable Scenarios: A reference for complete pick-and-place applications integrating perception, motion planning and control, and navigation. It demonstrates pre-grasp, lift, retreat, navigation between picking and placement, and linear grasp/place motions with move_line.

examples/cpp/tutorials/src/example6_pick_and_place.cpp

// 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 taught grasp approach and place descent use move_line; other end-effector motions use
// set_end_effector_pose. All Cartesian poses below were taught for the bare left-arm
// EndEffector frame relative to base_link on G3.

#include <array>
#include <chrono>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>
#include <tuple>
#include <unordered_set>
#include <vector>

#include "galbot_motion.hpp"
#include "galbot_navigation.hpp"
#include "galbot_robot.hpp"

#include <opencv2/opencv.hpp>

using namespace galbot::sdk;

constexpr double kPlaceNavigationXOffsetM = 0.1;
// Slow the arm trajectory down and make each task waypoint visually distinct.
constexpr double kArmMinMoveTimeS = 3.0;
constexpr double kArmMoveTimeoutS = 30.0;
constexpr double kArmJointSpeedRadS = 0.1;
constexpr auto kWaypointHold = std::chrono::milliseconds(1000);
constexpr auto kGripperSettle = std::chrono::milliseconds(500);
const std::vector<double> kPreGraspPose = {
    0.43645796, 0.18778193, 0.92196164, -0.01950938, 0.02598107, -0.05602237, 0.99790073,
};
const std::vector<double> kGraspPose = {
    0.51412043, 0.18056627, 0.96286294, -0.02264866, 0.00051349, -0.05636788, 0.99815301,
};
const std::vector<double> kLiftGraspPose = {
    0.53047695, 0.15313211, 1.09918264, -0.00801662, -0.19539735, -0.09980387, 0.97559971,
};
const std::vector<double> kRetreatPose = {
    0.43381901, 0.16343561, 1.0412931, -0.02825337, -0.11813149, -0.09585053, 0.98795717,
};
const std::vector<double> kPrePlacePose = {
    0.50956077, 0.20189686, 1.05753193, -0.02514585, -0.11590543, -0.04433032, 0.99195183,
};
const std::vector<double> kPlacePose = {
    0.49092053, 0.19268637, 0.97338715, -0.02437315, -0.01105621, -0.05520225, 0.99811644,
};
const std::vector<std::string> kInitialLegJointNames = {
    "leg_joint1", "leg_joint2", "leg_joint3", "leg_joint4", "leg_joint5",
};
const std::vector<double> kInitialLegJointPositions = {0.4, 1.2, 0.8, 0.0, 0.0};

const std::vector<std::string> kInitialUpperBodyJointNames = {
    "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",
};
const std::vector<double> kInitialUpperBodyJointPositions = {
    0.00001198, 0.0, 1.50008953, -1.3599937, -0.45010296, 1.53000093, -0.09994802, -0.41997507,
    0.00004712, -1.49992013, 1.3599937, 0.44991097, -1.5300498, 0.10004402, 0.4200955, 0.00004887,
};

bool move_to_initial_pose(GalbotRobot& robot) {
  const ControlStatus leg_status =
      robot.set_joint_positions(kInitialLegJointPositions, {}, kInitialLegJointNames, true, 0.2, 20.0);
  if (leg_status != ControlStatus::SUCCESS) {
    std::cout << "❌ Failed to move the legs to the initial pose: status=" << (int) leg_status << std::endl;
    return false;
  }
  std::cout << "✅ Successfully moved the legs to the initial pose: status=" << (int) leg_status << std::endl;

  const ControlStatus upper_body_status = robot.set_joint_positions(
      kInitialUpperBodyJointPositions, {}, kInitialUpperBodyJointNames, true, kArmJointSpeedRadS, 20.0);
  if (upper_body_status != ControlStatus::SUCCESS) {
    std::cout << "❌ Failed to move the head and arms to the initial pose: status=" << (int) upper_body_status
              << std::endl;
    return false;
  }

  std::cout << "✅ Successfully moved the head and arms to the initial pose: status=" << (int) upper_body_status
            << std::endl;
  return true;
}

static std::vector<double> pose_to_vector7(const Pose& p) {
  return {p.position.x, p.position.y, p.position.z, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w};
}

// Quaternion and rotation matrix utilities (without Eigen)
struct QuaternionST {
  double x, y, z, w;

  QuaternionST(double x = 0, double y = 0, double z = 0, double w = 1) : x(x), y(y), z(z), w(w) {}

  // Convert quaternion to 3x3 rotation matrix
  std::array<std::array<double, 3>, 3> to_matrix() const {
    std::array<std::array<double, 3>, 3> mat;

    double xx = x * x, yy = y * y, zz = z * z;
    double xy = x * y, xz = x * z, yz = y * z;
    double wx = w * x, wy = w * y, wz = w * z;

    mat[0][0] = 1 - 2 * (yy + zz);
    mat[0][1] = 2 * (xy - wz);
    mat[0][2] = 2 * (xz + wy);

    mat[1][0] = 2 * (xy + wz);
    mat[1][1] = 1 - 2 * (xx + zz);
    mat[1][2] = 2 * (yz - wx);

    mat[2][0] = 2 * (xz - wy);
    mat[2][1] = 2 * (yz + wx);
    mat[2][2] = 1 - 2 * (xx + yy);

    return mat;
  }

};

// 4x4 transformation matrix
struct Transform {
  std::array<std::array<double, 4>, 4> mat;

  Transform() {
    // Identity matrix
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        mat[i][j] = (i == j) ? 1.0 : 0.0;
      }
    }
  }

  // Set rotation from quaternion
  void set_rotation(const QuaternionST& q) {
    auto rot = q.to_matrix();
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        mat[i][j] = rot[i][j];
      }
    }
  }

  // Set translation
  void set_translation(const std::array<double, 3>& t) {
    mat[0][3] = t[0];
    mat[1][3] = t[1];
    mat[2][3] = t[2];
  }

  // Transform a 3D point
  std::array<double, 3> transform_point(const std::array<double, 3>& p) const {
    std::array<double, 3> result;
    for (int i = 0; i < 3; ++i) {
      result[i] = mat[i][0] * p[0] + mat[i][1] * p[1] + mat[i][2] * p[2] + mat[i][3];
    }
    return result;
  }

};

void navigation_to_goal(GalbotNavigation& nav, const std::vector<double>& goal_pose, int retry_cnt = 3) {
  try {
    auto cur_pose = nav.get_current_pose();
    std::cout << "Current pose: [";
    for (auto val : pose_to_vector7(cur_pose))
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    Pose goal(goal_pose);
    if (nav.check_path_reachability(goal, cur_pose)) {
      retry_cnt = 3;
      NavigationStatus status;

      while (true) {
        status = nav.navigate_to_goal(goal, true, true, 20.0f);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
        retry_cnt--;

        if (nav.check_goal_arrival() || retry_cnt < 0) {
          break;
        } else {
          std::cout << "Navigation failed: status=" << (int) status << ", retrying: " << retry_cnt << std::endl;
        }
      }

      std::cout << "navigate_to_goal return status: " << (int) status << std::endl;
      std::cout << "Has arrived: " << nav.check_goal_arrival() << std::endl;
    } else {
      std::cout << "Path unreachable or unsafe" << std::endl;
    }
  } catch (const std::exception& e) {
    std::cout << "Exception occurred during navigation: " << e.what() << std::endl;
  }
}

std::vector<double> pose_camera_to_base(GalbotRobot& robot, const std::vector<double>& pose_camera) {
  // The G3 TF tree publishes the arm RGB camera under this frame name.
  std::string source_frame = "left_arm_camera";
  std::string target_frame = "base_link";

  auto [base_to_cam, success] = robot.get_transform(target_frame, source_frame);

  if (!success || base_to_cam.size() != 7) {
    std::cout << "Failed to get transform from camera to chassis" << std::endl;
    return {};
  } else {
    std::cout << "base_to_cam: [";
    for (auto val : base_to_cam)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
  }

  Transform base_to_cam_mat;
  QuaternionST quat(base_to_cam[3], base_to_cam[4], base_to_cam[5], base_to_cam[6]);
  base_to_cam_mat.set_rotation(quat);
  base_to_cam_mat.set_translation({base_to_cam[0], base_to_cam[1], base_to_cam[2]});

  std::array<double, 3> cam_pos = {pose_camera[0], pose_camera[1], pose_camera[2]};
  auto pose_base = base_to_cam_mat.transform_point(cam_pos);

  return {pose_base[0], pose_base[1], pose_base[2], 0.0, 0.0, 0.0, 1.0};
}

std::vector<double> detect_object(GalbotRobot& robot) {
  const std::vector<double> k_zero_pose = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};

  try {
    // G3 uses the left-arm RGB camera. The pose below is a placeholder for
    // an application-specific perception result.
    auto rgb_image_data = robot.get_rgb_data(SensorType::LEFT_ARM_CAMERA);

    if (!rgb_image_data) {
      std::cout << "No rgb image data!" << std::endl;
      return k_zero_pose;
    }
    std::cout << "Get rgb image success" << std::endl;

    auto img_ptr = rgb_image_data->convert_to_cv2_mat();
    if (!img_ptr || img_ptr->empty()) {
      std::cout << "Failed to decode rgb image" << std::endl;
      return k_zero_pose;
    }

    // Placeholder perception result in the OpenCV camera frame (x right, y down, z forward).
    // Replace this pose with the output of an application-specific detector.
    std::vector<double> object_pose_camera = {0.0, 0.20, 0.29, 0.0, 0.71, 0.0, 0.71};

    std::cout << "object_pose_camera: [";
    for (auto val : object_pose_camera)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    auto object_pose_base = pose_camera_to_base(robot, object_pose_camera);
    if (object_pose_base.size() != 7) {
      std::cout << "Target pose in chassis coordinate system: (invalid / transform failed)" << std::endl;
      return k_zero_pose;
    }
    std::cout << "Target pose in chassis coordinate system: [";
    for (auto val : object_pose_base)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    return object_pose_base;

  } catch (const std::exception& e) {
    std::cout << "Target detection exception: " << e.what() << std::endl;
    return k_zero_pose;
  }
}

void check_robot_safety() {
  std::cout << "⚠️  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." << std::endl;

  while (true) {
    std::cout << "Please confirm that the robot's emergency stop button is released "
              << "and there are no obstructions, continue? (y/n)..." << std::endl;

    std::string key;
    std::cin >> key;

    if (key == "y" || key == "Y") {
      std::cout << "User confirmed, continuing..." << std::endl;
      break;
    } else if (key == "n" || key == "N") {
      std::cout << "User did not confirm, exiting program..." << std::endl;
      exit(1);
    } else {
      std::cout << "Invalid input, please enter 'y' or 'n'" << std::endl;
    }
  }
}

bool pick_and_place(GalbotRobot& robot, GalbotNavigation& nav, GalbotMotion& motion,
                    const std::vector<double>& object_pose_base, bool navigation_enabled) {
  try {
    // attach_tool() only loads a planning model; it does not detect hardware.
    // Do not add a virtual gripper when no physical gripper feedback exists.
    const auto left_gripper_state = robot.get_gripper_state("left_gripper");
    bool has_left_gripper = static_cast<bool>(left_gripper_state);
    ControlStatus gripper_status = ControlStatus::DATA_FETCH_FAILED;
    if (has_left_gripper) {
      // A gripper-less G3 can still publish placeholder feedback. Confirm
      // hardware availability with the blocking open command that the pick
      // task needs anyway, before changing the planning model.
      gripper_status = robot.set_gripper_command("left_gripper", 0.1, 0.05, 10, true);
      if (gripper_status == ControlStatus::DATA_FETCH_FAILED || gripper_status == ControlStatus::TIMEOUT) {
        has_left_gripper = false;
        std::cout << "⚠️ Left gripper feedback is only a placeholder; status=" << (int) gripper_status << std::endl;
      } else if (gripper_status != ControlStatus::SUCCESS) {
        std::cout << "❌ Failed to open left gripper: status=" << (int) gripper_status << std::endl;
        return false;
      }
    }
    MotionStatus status = MotionStatus::SUCCESS;
    if (has_left_gripper) {
      status = motion.attach_tool("left_arm", "galbot_gripper");
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
      if (status != MotionStatus::SUCCESS) {
        std::cout << "❌ Failed to attach tool: status=" << (int) status << std::endl;
        return false;
      }
      std::cout << "✅ Successfully attached tool: status=" << (int) status << std::endl;
    } else {
      std::cout << "⚠️ Left gripper was not detected; skipping its tool model and gripper commands." << std::endl;
      // Clear a model that may have been left attached by an earlier
      // interrupted/failed run of this example.
      const MotionStatus stale_tool_status = motion.detach_tool("left_arm");
      if (stale_tool_status == MotionStatus::SUCCESS) {
        std::cout << "Cleared a previously attached left-arm tool model." << std::endl;
      }
    }

    // These poses were taught for the bare arm EndEffector frame, not the tool TCP.
    auto motion_params = std::make_shared<Parameter>();
    motion_params->set_direct_execute(true);
    motion_params->set_blocking(true);
    motion_params->set_timeout(kArmMoveTimeoutS);
    motion_params->set_tool_pose(false);
    motion_params->set_check_collision(true);
    motion_params->set_reference_frame("base_link");
    motion_params->set_actuate("with_chain_only");
    if (has_left_gripper) {
      std::cout << "✅ Successfully set left gripper width to 0.1m: status=" << (int) gripper_status << std::endl;
    }

    // These G3 poses were recorded by drag teaching in the base_link frame.
    // Perception remains informational until an application-specific grasp-pose
    // generator replaces the taught trajectory.
    std::cout << "Perception pose: [";
    for (auto val : object_pose_base)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    std::vector<double> pre_grasp_pose = kPreGraspPose;
    std::vector<double> grasp_pose = kGraspPose;
    std::vector<double> lift_grasp_pose = kLiftGraspPose;
    std::vector<double> retreat_pose = kRetreatPose;

    std::cout << "pre_grasp_pose: [";
    for (auto val : pre_grasp_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    status = motion.set_end_effector_pose(pre_grasp_pose, "left_arm", "base_link", nullptr, true, true,
                                          kArmMoveTimeoutS,
                                          motion_params);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute pre_grasp pose command: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed pre_grasp pose command" << std::endl;
    std::cout << "Holding at pre_grasp for 1.0s..." << std::endl;
    std::this_thread::sleep_for(kWaypointHold);

    // Use move_line for the taught final approach to the object.
    MotionPlanChainTarget grasp_target;
    grasp_target.chain_name = "left_arm";
    grasp_target.mode = 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 = Pose(grasp_pose);

    std::cout << "grasp_pose: [";
    for (auto val : grasp_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    auto move_line_result = motion.move_line({{grasp_target}}, *motion_params);
    status = std::get<0>(move_line_result);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute grasp move_line: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed grasp move_line" << std::endl;
    std::cout << "Holding at grasp for 1.0s..." << std::endl;
    std::this_thread::sleep_for(kWaypointHold);

    if (has_left_gripper) {
      const ControlStatus gripper_status = robot.set_gripper_command("left_gripper", 0.02, 0.05, 10, true);
      if (gripper_status != ControlStatus::SUCCESS) {
        std::cout << "❌ Failed to close left gripper: status=" << (int) gripper_status << std::endl;
        return false;
      }
      std::cout << "✅ Successfully closed left gripper: status=" << (int) gripper_status << std::endl;
      std::this_thread::sleep_for(kGripperSettle);
    }

    std::cout << "lift_grasp_pose: [";
    for (auto val : lift_grasp_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    status = motion.set_end_effector_pose(lift_grasp_pose, "left_arm", "base_link", nullptr, true, true,
                                          kArmMoveTimeoutS,
                                          motion_params);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute lift_grasp pose command: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed lift_grasp pose command" << std::endl;
    std::cout << "Holding at lift_grasp for 1.0s..." << std::endl;
    std::this_thread::sleep_for(kWaypointHold);

    std::cout << "retreat_pose: [";
    for (auto val : retreat_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    status = motion.set_end_effector_pose(retreat_pose, "left_arm", "base_link", nullptr, true, true,
                                          kArmMoveTimeoutS,
                                          motion_params);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute retreat pose command: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed retreat pose command" << std::endl;
    std::cout << "Holding at retreat for 1.0s..." << std::endl;
    std::this_thread::sleep_for(kWaypointHold);

    // Return to the initial left-arm joint pose before navigation.
    const std::vector<std::string> left_arm_joint_names(kInitialUpperBodyJointNames.begin() + 2,
                                                        kInitialUpperBodyJointNames.begin() + 9);
    const std::vector<double> left_arm_joint_positions(kInitialUpperBodyJointPositions.begin() + 2,
                                                       kInitialUpperBodyJointPositions.begin() + 9);
    const ControlStatus left_arm_status =
        robot.set_joint_positions(left_arm_joint_positions, {}, left_arm_joint_names, true, kArmJointSpeedRadS, 20.0);
    if (left_arm_status != ControlStatus::SUCCESS) {
      std::cout << "❌ Failed to restore the left-arm initial pose: status=" << (int) left_arm_status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully restored the left-arm initial pose: status=" << (int) left_arm_status << std::endl;

    if (navigation_enabled) {
      auto place_navigation_goal = pose_to_vector7(nav.get_current_pose());
      place_navigation_goal[0] += kPlaceNavigationXOffsetM;
      std::cout << "Place navigation target pose: [";
      for (auto val : place_navigation_goal)
        std::cout << val << " ";
      std::cout << "]" << std::endl;
      navigation_to_goal(nav, place_navigation_goal);
      std::this_thread::sleep_for(std::chrono::seconds(2));
      std::cout << "✅ Navigation stage completed; starting Place" << std::endl;
    } else {
      std::cout << "⚠️ Navigation is unavailable; skipping navigation and continuing with Place at the current "
                   "position."
                << std::endl;
    }

    // Move to the taught pre-place pose, then follow the taught placement approach.
    std::vector<double> pre_place_pose = kPrePlacePose;
    std::vector<double> place_pose = kPlacePose;
    std::cout << "pre_place_pose: [";
    for (auto val : pre_place_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    status = motion.set_end_effector_pose(pre_place_pose, "left_arm", "base_link", nullptr, true, true,
                                          kArmMoveTimeoutS,
                                          motion_params);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute pre_place pose command: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed pre_place pose command" << std::endl;
    std::cout << "Holding at pre_place for 1.0s..." << std::endl;
    std::this_thread::sleep_for(kWaypointHold);

    MotionPlanChainTarget place_target;
    place_target.chain_name = "left_arm";
    place_target.mode = 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 = Pose(place_pose);

    std::cout << "place_pose: [";
    for (auto val : place_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;
    move_line_result = motion.move_line({{place_target}}, *motion_params);
    status = std::get<0>(move_line_result);
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to execute place move_line: status=" << (int) status << std::endl;
      return false;
    }
    std::cout << "✅ Successfully executed place move_line" << std::endl;
    std::cout << "Holding at place for 1.0s..." << std::endl;
    std::this_thread::sleep_for(kWaypointHold);

    if (has_left_gripper) {
      const ControlStatus gripper_status = robot.set_gripper_command("left_gripper", 0.1, 0.05, 10, true);
      if (gripper_status != ControlStatus::SUCCESS) {
        std::cout << "❌ Failed to release left gripper: status=" << (int) gripper_status << std::endl;
        return false;
      }
      std::cout << "✅ Successfully released left gripper: status=" << (int) gripper_status << std::endl;
      std::this_thread::sleep_for(kGripperSettle);
    }

    // Return the left arm to its initial joint pose after placing the object.
    const ControlStatus post_place_arm_status =
        robot.set_joint_positions(left_arm_joint_positions, {}, left_arm_joint_names, true, kArmJointSpeedRadS, 20.0);
    if (post_place_arm_status != ControlStatus::SUCCESS) {
      std::cout << "❌ Failed to restore the left-arm pose after placing: status=" << (int) post_place_arm_status
                << std::endl;
      return false;
    }
    std::cout << "✅ Successfully restored the left-arm pose after placing: status=" << (int) post_place_arm_status
              << std::endl;

    if (has_left_gripper) {
      status = motion.detach_tool("left_arm");
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
      if (status != MotionStatus::SUCCESS) {
        std::cout << "❌ Failed to detach tool: status=" << (int) status << std::endl;
      } else {
        std::cout << "✅ Successfully detached tool: status=" << (int) status << std::endl;
      }
    }

    return true;

  } catch (const std::exception& e) {
    std::cout << "Exception occurred during pick_and_place: " << e.what() << std::endl;
    return false;
  }
}

int main() {
  check_robot_safety();

  // Get robot instances
  auto& robot = GalbotRobot::get_instance(MachineType::G3);
  auto& motion = GalbotMotion::get_instance(MachineType::G3);
  auto& nav = GalbotNavigation::get_instance(MachineType::G3);
  std::shared_ptr<MotionPlanConfig> original_motion_config;
  bool slow_motion_config_applied = false;

  try {
    // Enable sensors
    // G3 does not have arm-mounted depth cameras.
    std::unordered_set<SensorType> enable_sensor_set = {SensorType::LEFT_ARM_CAMERA};

    // Initialize robot
    if (robot.init(enable_sensor_set)) {
      std::cout << "GalbotRobot initialization successful" << std::endl;
    } else {
      std::cout << "GalbotRobot initialization failed" << std::endl;
    }

    if (motion.init()) {
      std::cout << "GalbotMotion initialization successful" << std::endl;
    } else {
      std::cout << "GalbotMotion initialization failed" << std::endl;
    }

    if (nav.init()) {
      std::cout << "GalbotNavigation initialization successful" << std::endl;
    } else {
      std::cout << "GalbotNavigation initialization failed" << std::endl;
    }

    // Wait for data readiness
    std::this_thread::sleep_for(std::chrono::seconds(1));

    if (!move_to_initial_pose(robot)) {
      throw std::runtime_error("Failed to move to the task initial pose");
    }

    // Cartesian motion duration is an MPS-wide setting in this SDK version.
    // Preserve it and restore it before exit so this example has no lasting
    // effect on later motion-planning clients.
    auto [config_status, current_motion_config] = motion.get_motion_plan_config();
    if (config_status != MotionStatus::SUCCESS) {
      throw std::runtime_error("Failed to read motion-plan configuration");
    }
    original_motion_config = std::make_shared<MotionPlanConfig>(current_motion_config);
    auto slow_motion_config = std::make_shared<MotionPlanConfig>();
    auto trajectory_config = slow_motion_config->create_trajectory_plan_config();
    trajectory_config->set_min_move_time(kArmMinMoveTimeS);
    trajectory_config->set_way_point_plan_expected_time(kArmMinMoveTimeS);
    config_status = motion.set_motion_plan_config(slow_motion_config);
    if (config_status != MotionStatus::SUCCESS) {
      throw std::runtime_error("Failed to apply slow motion-plan configuration");
    }
    slow_motion_config_applied = true;

    // Check localization once. If unavailable, Pick still runs and navigation
    // between Pick and Place is skipped.
    bool navigation_enabled = false;
    try {
      navigation_enabled = nav.is_localized();
    } catch (const std::exception& e) {
      std::cout << "⚠️ Navigation status check failed: " << e.what() << std::endl;
    }
    if (navigation_enabled) {
      std::cout << "✅ Robot is localized; Place navigation is enabled." << std::endl;
    } else {
      std::cout << "⚠️ Robot is not localized; navigation will be skipped. This run demonstrates Pick and Place "
                   "at the current position."
                << std::endl;
    }

    std::cout << std::endl;

    // Detect the target before Pick.
    auto object_pose_base = detect_object(robot);
    std::cout << std::endl;

    if (object_pose_base.size() != 7) {
      std::cout << "Skipping pick_and_place: invalid object pose (expected 7 values)" << std::endl;
    } else {
      // Execute Pick, navigate, and then Place.
      if (!pick_and_place(robot, nav, motion, object_pose_base, navigation_enabled)) {
        throw std::runtime_error("Pick-and-place execution failed");
      }
    }
    std::cout << std::endl;

  } catch (const std::exception& e) {
    std::cout << "Exception occurred: " << e.what() << std::endl;
  }

  if (slow_motion_config_applied && original_motion_config) {
    const MotionStatus restore_status = motion.set_motion_plan_config(original_motion_config);
    if (restore_status != MotionStatus::SUCCESS) {
      std::cout << "⚠️ Failed to restore the original motion-plan configuration: status="
                << (int) restore_status << std::endl;
    }
  }

  // Cleanup
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resource release successful" << std::endl;

  return 0;
}