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 (S1 1.2)

Joint Group List

Robot joint group names include: ["torso", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"]

Detailed Information for Each Joint Group

Joint Group	English Name	Number of Joints	Joint Name List
Head	head	2	head_joint1, head_joint2
Torso	torso	1	S1 V1: torso_base_joint; S1 V2: torso_lift_joint1
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

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/torso 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 (torso/head/arm/gripper).
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 (S1)

Joint Group	Typical Scenario
torso	Torso height/pitch adjustment
head	Head orientation / camera aiming
left_arm / right_arm	Arm reaching and manipulation
left_gripper / right_gripper	Grasp width control

Note: S1 also defines passive groups (for example, swerve chassis and camera mount groups) that are usually not used as direct set_joint_positions targets.

Sensor Types and Frames

For sensor data access (get_rgb_data, get_depth_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
HEAD_LIDAR	head_lidar_base_link
BACK_LIDAR	back_lidar_base_link
CHASSIS_LIDAR	chassis_lidar_base_link
HEAD_IMU	head_imu_base_link
BACK_IMU	back_imu_base_link
CHASSIS_IMU	chassis_imu_base_link
LEFT_ARM_CAMERA	left_arm_camera_color_optical_frame
LEFT_ARM_DEPTH_CAMERA	left_arm_camera_color_optical_frame
RIGHT_ARM_CAMERA	right_arm_camera_color_optical_frame
RIGHT_ARM_DEPTH_CAMERA	right_arm_camera_color_optical_frame

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::S1);
    
    // 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: Before program exit, release all resources occupied by the SDK for normal system shutdown.

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::S1);
    
    // 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::S1);
  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::S1);

    // 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 ["torso", "head", "left_arm", "right_arm"] (S1: torso replaces G1 leg)
    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 empty array defaults to torso, head, left_arm, right_arm (S1: torso replaces G1 leg)
    joint_groups = {"head"};
    // Specific joints to control; if provided, this overrides the joint_groups parameter
    std::vector<std::string> joint_names = {};
    // Joint positions; head joint group contains two joints
    // Head angles: within S1 limits [-0.7854, 0.7854] and [-0.6109, 0.6109]
    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::S1);

    // 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 = false;
    // 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;
}

S1 Raw End-Tool Passthrough

These examples show how to encode, transmit, validate, and decode device-specific 64-byte CAN/RS485 frames. Read S1 Raw End-Tool Passthrough before using them.

warning

WBCS must run with endtool_raw_enabled=true in [robot_info.custom_params]. Do not run a raw writer concurrently with a standard controller or another client that writes to the same TIB.

Self-Developed CAN Gripper

Initializes and moves the self-developed two-finger gripper, then monitors passive status reports from the 250 Hz receive stream.

examples/s1/cpp/galbot_robot/src/endtool_raw_grip_can_example.cpp

#include <array>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <cstdint>
#include <cstring>
#include <iomanip>
#include <iostream>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr uint32_t kTxCanId = 0x70A;
constexpr uint32_t kRxCanId = 0x10A;
constexpr uint8_t kWriteCommand = 0x21;
constexpr uint8_t kAnswerCommand = 0x22;
constexpr uint8_t kReportCommand = 0x23;
constexpr uint8_t kInitFunction = 0x04;
constexpr uint8_t kMoveFunction = 0x05;
constexpr float kMinWidthMm = 6.0F;
constexpr float kMaxWidthMm = 120.0F;
constexpr float kMinVelocityMmS = 1.0F;
constexpr float kMaxVelocityMmS = 100.0F;
constexpr float kMinTorqueNm = 1.0F;
constexpr float kMaxTorqueNm = 50.0F;
constexpr auto kStatusPollInterval = std::chrono::milliseconds(100);
constexpr auto kStatusStaleInterval = std::chrono::seconds(1);
constexpr auto kInitialStatusWait = std::chrono::seconds(1);
constexpr auto kStatusPrintInterval = std::chrono::milliseconds(500);
constexpr auto kRawTransportReadyTimeout = std::chrono::seconds(2);
constexpr auto kRawTransportWarmupInterval = std::chrono::milliseconds(200);
constexpr auto kInitTimeout = std::chrono::seconds(20);
constexpr auto kInitCommandGuardInterval = std::chrono::seconds(1);
constexpr auto kMonitorDuration = std::chrono::seconds(10);
constexpr float kPositionToleranceMm = 2.0F;
constexpr float kStoppedVelocityToleranceMmS = 1.0F;

enum class DecodeResult {
  VALID,
  IDLE_SNAPSHOT,
  FRAME_TYPE,
  CAN_ID,
  COMMAND,
  PAYLOAD,
};

struct GripStatus {
  float position_mm = 0.0F;
  float velocity_mm_s = 0.0F;
  float torque_nm = 0.0F;
  uint8_t motion_state = 0;
  uint8_t error_code = 0;
  uint8_t response_command = 0;
};

struct DemoState {
  std::mutex mutex;
  std::condition_variable changed;
  GripStatus last_status;
  bool valid = false;
  uint64_t last_generation = 0;
  uint64_t last_update_ms = 0;
  uint64_t latest_raw_generation = 0;
  uint64_t idle_snapshot_count = 0;
  uint64_t frame_type_reject_count = 0;
  uint64_t can_id_reject_count = 0;
  uint64_t command_reject_count = 0;
  uint64_t payload_reject_count = 0;
  uint64_t non_increasing_generation_count = 0;
};

struct StateSnapshot {
  GripStatus status;
  bool valid = false;
  uint64_t last_generation = 0;
  uint64_t last_update_ms = 0;
  uint64_t latest_raw_generation = 0;
  uint64_t idle_snapshot_count = 0;
  uint64_t frame_type_reject_count = 0;
  uint64_t can_id_reject_count = 0;
  uint64_t command_reject_count = 0;
  uint64_t payload_reject_count = 0;
  uint64_t non_increasing_generation_count = 0;
};

uint64_t steady_now_ms() {
  return static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                   std::chrono::steady_clock::now().time_since_epoch())
                                   .count());
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> make_can_frame(
    uint8_t command, uint8_t function, const uint8_t* payload, std::size_t payload_size) {
  std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> frame{};
  const uint32_t frame_type = 1;
  std::memcpy(frame.data(), &frame_type, sizeof(frame_type));
  std::memcpy(frame.data() + 4, &kTxCanId, sizeof(kTxCanId));
  frame[8] = static_cast<uint8_t>(payload_size + 2);
  frame[12] = command;
  frame[13] = function;
  if (payload != nullptr && payload_size > 0) {
    std::memcpy(frame.data() + 14, payload, payload_size);
  }
  return frame;
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_init() {
  return make_can_frame(kWriteCommand, kInitFunction, nullptr, 0);
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_move(
    float width_mm, float velocity_mm_s, float torque_nm) {
  std::array<uint8_t, 12> payload{};
  std::memcpy(payload.data(), &width_mm, sizeof(width_mm));
  std::memcpy(payload.data() + 4, &velocity_mm_s, sizeof(velocity_mm_s));
  std::memcpy(payload.data() + 8, &torque_nm, sizeof(torque_nm));
  return make_can_frame(kWriteCommand, kMoveFunction, payload.data(), payload.size());
}

DecodeResult decode_status(const EndToolRawData& raw, EndToolSide expected_side, GripStatus& status) {
  // This gripper's CAN response is valid only in the 250 Hz passthrough buffer.
  if (raw.side != expected_side || raw.kind != EndToolRxKind::BUFFER_250HZ) {
    return DecodeResult::FRAME_TYPE;
  }

  uint32_t frame_type = 0;
  uint32_t can_id = 0;
  std::memcpy(&frame_type, raw.frame.data(), sizeof(frame_type));
  std::memcpy(&can_id, raw.frame.data() + 4, sizeof(can_id));
  if (frame_type != 1) {
    return DecodeResult::FRAME_TYPE;
  }
  // A zero CAN ID is an empty slot in the 250 Hz buffer, not a device error.
  if (can_id == 0) {
    return DecodeResult::IDLE_SNAPSHOT;
  }
  if (can_id != kRxCanId) {
    return DecodeResult::CAN_ID;
  }

  // Valid S1 EtherCAT/TIB snapshots can have byte 12 cleared. Identify the
  // device report by its CAN ID and by the command/function fields at byte 16.
  const uint8_t command = raw.frame[16];
  const uint8_t function = raw.frame[17];
  if ((command != kAnswerCommand && command != kReportCommand) || function != kMoveFunction) {
    return DecodeResult::COMMAND;
  }

  GripStatus decoded;
  decoded.error_code = raw.frame[21];
  decoded.response_command = command;
  std::memcpy(&decoded.position_mm, raw.frame.data() + 22, sizeof(decoded.position_mm));
  std::memcpy(&decoded.velocity_mm_s, raw.frame.data() + 26, sizeof(decoded.velocity_mm_s));
  std::memcpy(&decoded.torque_nm, raw.frame.data() + 30, sizeof(decoded.torque_nm));
  decoded.motion_state = raw.frame[34];
  if (!std::isfinite(decoded.position_mm) || !std::isfinite(decoded.velocity_mm_s) ||
      !std::isfinite(decoded.torque_nm) || decoded.position_mm < 0.0F ||
      decoded.position_mm > kMaxWidthMm ||
      std::fabs(decoded.velocity_mm_s) > kMaxVelocityMmS ||
      std::fabs(decoded.torque_nm) > kMaxTorqueNm || decoded.motion_state > 4) {
    // An uninitialized gripper may report 0 mm even though Move accepts 6 mm
    // as its minimum target, so only negative positions are rejected here.
    return DecodeResult::PAYLOAD;
  }

  status = decoded;
  return DecodeResult::VALID;
}

const char* error_text(uint8_t error_code) {
  switch (error_code) {
    case 0x00:
      return "no error";
    case 0x01:
      return "not enabled";
    case 0x08:
      return "overvoltage";
    case 0x09:
      return "undervoltage";
    case 0x0A:
      return "overcurrent";
    case 0x0B:
      return "MOS overheating";
    case 0x0C:
      return "motor coil overheating";
    case 0x0D:
      return "communication lost";
    case 0x0E:
      return "motor overload";
    case 0x10:
      return "locked rotor";
    case 0x11:
      return "locked rotor and not enabled";
    case 0x40:
      return "power loss after calibration";
    case 0x80:
      return "communication error";
    default:
      return "unknown error";
  }
}

StateSnapshot get_snapshot(const std::shared_ptr<DemoState>& state) {
  std::lock_guard<std::mutex> lock(state->mutex);
  StateSnapshot snapshot;
  snapshot.status = state->last_status;
  snapshot.valid = state->valid;
  snapshot.last_generation = state->last_generation;
  snapshot.last_update_ms = state->last_update_ms;
  snapshot.latest_raw_generation = state->latest_raw_generation;
  snapshot.idle_snapshot_count = state->idle_snapshot_count;
  snapshot.frame_type_reject_count = state->frame_type_reject_count;
  snapshot.can_id_reject_count = state->can_id_reject_count;
  snapshot.command_reject_count = state->command_reject_count;
  snapshot.payload_reject_count = state->payload_reject_count;
  snapshot.non_increasing_generation_count = state->non_increasing_generation_count;
  return snapshot;
}

void count_decode_result(DemoState& state, DecodeResult result) {
  switch (result) {
    case DecodeResult::IDLE_SNAPSHOT:
      ++state.idle_snapshot_count;
      break;
    case DecodeResult::FRAME_TYPE:
      ++state.frame_type_reject_count;
      break;
    case DecodeResult::CAN_ID:
      ++state.can_id_reject_count;
      break;
    case DecodeResult::COMMAND:
      ++state.command_reject_count;
      break;
    case DecodeResult::PAYLOAD:
      ++state.payload_reject_count;
      break;
    case DecodeResult::VALID:
      break;
  }
}

void print_status(const char* prefix, uint64_t generation, const GripStatus& status) {
  std::cout << prefix << " generation=" << generation << " source="
            << (status.response_command == kAnswerCommand ? "ANSWER" : "REPORT")
            << " position=" << status.position_mm << "mm velocity=" << status.velocity_mm_s
            << "mm/s torque=" << status.torque_nm
            << "Nm motion_state=" << static_cast<unsigned int>(status.motion_state)
            << " error=0x" << std::hex << static_cast<unsigned int>(status.error_code) << std::dec
            << " (" << error_text(status.error_code) << ")\n";
}

void print_rx_summary(const StateSnapshot& snapshot) {
  std::cout << "RX summary: latest_raw_generation=" << snapshot.latest_raw_generation
            << " latest_valid_generation=" << snapshot.last_generation
            << " idle_snapshots=" << snapshot.idle_snapshot_count
            << " malformed={frame_type:" << snapshot.frame_type_reject_count
            << ",can_id:" << snapshot.can_id_reject_count
            << ",command:" << snapshot.command_reject_count
            << ",payload:" << snapshot.payload_reject_count << "}"
            << " non_increasing_generations=" << snapshot.non_increasing_generation_count
            << '\n';
}

bool parse_side(const std::string& value, EndToolSide& side) {
  if (value == "left" || value == "l" || value == "0") {
    side = EndToolSide::LEFT;
    return true;
  }
  if (value == "right" || value == "r" || value == "1") {
    side = EndToolSide::RIGHT;
    return true;
  }
  return false;
}

bool valid_motion_command(float width_mm, float velocity_mm_s, float torque_nm) {
  return std::isfinite(width_mm) && std::isfinite(velocity_mm_s) && std::isfinite(torque_nm) &&
         width_mm >= kMinWidthMm && width_mm <= kMaxWidthMm &&
         velocity_mm_s >= kMinVelocityMmS && velocity_mm_s <= kMaxVelocityMmS &&
         torque_nm >= kMinTorqueNm && torque_nm <= kMaxTorqueNm;
}

void print_usage(const char* program, std::ostream& output) {
  output << "Usage:\n"
         << "  " << program << " [options]\n"
         << "  " << program
         << " [left|right] [width_mm] [velocity_mm_s] [torque_nm] [init:0|1]"
            " [stop_controller:0|1] [restore_controller:0|1]\n\n"
         << "Options:\n"
         << "  -h, --help                    Show this help message.\n"
         << "  --side <left|right>           Gripper side (default: left).\n"
         << "  --width-mm <6..120>           Target opening in millimeters (default: 50).\n"
         << "  --velocity-mm-s <1..100>      Target velocity in mm/s (default: 50).\n"
         << "  --torque-nm <1..50>           Target torque in Nm (default: 20).\n"
         << "  --init                         Home/calibrate before Move.\n"
         << "  --stop-standard-controller    Best-effort stop of the standard controller.\n"
         << "  --restore-controller          Restart the standard controller on exit.\n\n"
         << "Examples:\n"
         << "  " << program << " --help\n"
         << "  " << program << " --side left --init\n"
         << "  " << program
         << " --side right --width-mm 50 --velocity-mm-s 50 --torque-nm 20 --init\n";
}

}  // namespace

int main(int argc, char** argv) {
  EndToolSide side = EndToolSide::LEFT;
  float width_mm = 50.0F;
  float velocity_mm_s = 50.0F;
  float torque_nm = 20.0F;
  int init_value = 0;
  int stop_controller_value = 0;
  int restore_controller_value = 0;
  try {
    // Named options match the Python example, while the original positional
    // form remains available for existing scripts.
    const bool named_options = argc > 1 && argv[1][0] == '-';
    if (named_options) {
      for (int index = 1; index < argc; ++index) {
        const std::string option = argv[index];
        if (option == "-h" || option == "--help") {
          print_usage(argv[0], std::cout);
          return 0;
        }

        const auto read_value = [&index, argc, argv](const std::string& name) {
          if (index + 1 >= argc) {
            throw std::invalid_argument("missing value for " + name);
          }
          return std::string(argv[++index]);
        };
        if (option == "--side") {
          const std::string value = read_value(option);
          if (!parse_side(value, side)) {
            throw std::invalid_argument("--side must be left or right");
          }
        } else if (option == "--width-mm") {
          width_mm = std::stof(read_value(option));
        } else if (option == "--velocity-mm-s") {
          velocity_mm_s = std::stof(read_value(option));
        } else if (option == "--torque-nm") {
          torque_nm = std::stof(read_value(option));
        } else if (option == "--init") {
          init_value = 1;
        } else if (option == "--stop-standard-controller") {
          stop_controller_value = 1;
        } else if (option == "--restore-controller") {
          restore_controller_value = 1;
        } else {
          throw std::invalid_argument("unknown option: " + option);
        }
      }
    } else {
      if (!parse_side(argc > 1 ? argv[1] : "left", side)) {
        throw std::invalid_argument("side must be left/right/l/r/0/1");
      }
      width_mm = argc > 2 ? std::stof(argv[2]) : width_mm;
      velocity_mm_s = argc > 3 ? std::stof(argv[3]) : velocity_mm_s;
      torque_nm = argc > 4 ? std::stof(argv[4]) : torque_nm;
      init_value = argc > 5 ? std::stoi(argv[5]) : init_value;
      stop_controller_value = argc > 6 ? std::stoi(argv[6]) : stop_controller_value;
      restore_controller_value = argc > 7 ? std::stoi(argv[7]) : restore_controller_value;
    }
  } catch (const std::exception& error) {
    std::cerr << "Argument error: " << error.what() << "\n\n";
    print_usage(argv[0], std::cerr);
    return 1;
  }
  if (!valid_motion_command(width_mm, velocity_mm_s, torque_nm) ||
      (init_value != 0 && init_value != 1) ||
      (stop_controller_value != 0 && stop_controller_value != 1) ||
      (restore_controller_value != 0 && restore_controller_value != 1)) {
    std::cerr << "Argument error: motion values are outside the documented ranges.\n\n";
    print_usage(argv[0], std::cerr);
    return 1;
  }

  const std::string controller_group = side == EndToolSide::LEFT ? "left_gripper" : "right_gripper";
  // Print the resolved values before SDK initialization so command-line
  // mistakes remain visible even when the robot connection cannot be created.
  std::cout << "Parsed options: side=" << (side == EndToolSide::LEFT ? "left" : "right")
            << " width_mm=" << width_mm << " velocity_mm_s=" << velocity_mm_s
            << " torque_nm=" << torque_nm << " init=" << std::boolalpha
            << (init_value == 1) << " stop_standard_controller="
            << (stop_controller_value == 1) << " restore_controller="
            << (restore_controller_value == 1) << std::noboolalpha << '\n';

  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  if (!robot.init()) {
    std::cerr << "FAILED: failed to initialize the S1 SDK.\n";
    return 2;
  }

  std::cout << "WARNING: same-process raw TX is serialized, but there is no cross-client "
               "resource lease.\n";
  std::cout << "WBCS must run with endtool_raw_enabled=true in [robot_info.custom_params].\n";
  if (stop_controller_value == 1) {
    const auto stop_status = robot.stop_controller(controller_group);
    std::cout << "Best-effort stop of " << controller_group << ": "
              << static_cast<int>(stop_status) << '\n';
  }

  auto state = std::make_shared<DemoState>();
  const uint64_t callback_handle = robot.register_endtool_raw_callback(
      [side, state](const EndToolRawData& raw) {
        if (raw.side != side || raw.kind != EndToolRxKind::BUFFER_250HZ) {
          return;
        }

        GripStatus status;
        const DecodeResult decode_result = decode_status(raw, side, status);
        std::unique_lock<std::mutex> lock(state->mutex);
        state->latest_raw_generation = raw.generation;
        if (decode_result != DecodeResult::VALID) {
          count_decode_result(*state, decode_result);
          lock.unlock();
          state->changed.notify_all();
          return;
        }

        if (raw.generation != 0 && state->last_generation != 0 &&
            raw.generation <= state->last_generation) {
          ++state->non_increasing_generation_count;
          lock.unlock();
          state->changed.notify_all();
          return;
        }
        state->last_status = status;
        state->valid = true;
        state->last_generation = raw.generation;
        state->last_update_ms = steady_now_ms();
        lock.unlock();
        state->changed.notify_all();
      });
  if (callback_handle == 0) {
    std::cerr << "FAILED: failed to register the raw RX callback.\n";
    if (restore_controller_value == 1) {
      robot.start_controller(controller_group);
    }
    robot.destroy();
    return 3;
  }

  bool success = true;
  std::cout << "Waiting for the first valid Raw status before TX (timeout=2s)...\n";
  {
    std::unique_lock<std::mutex> lock(state->mutex);
    success = state->changed.wait_for(lock, kRawTransportReadyTimeout, [&state] {
      return state->valid;
    });
  }
  if (!success) {
    const StateSnapshot snapshot = get_snapshot(state);
    std::cerr << "FAILED: raw transport did not deliver a valid gripper status before TX; "
              << "latest_raw_generation=" << snapshot.latest_raw_generation
              << " idle_snapshots=" << snapshot.idle_snapshot_count << '\n';
    print_rx_summary(snapshot);
  } else {
    const StateSnapshot snapshot = get_snapshot(state);
    std::cout << "Raw transport ready: generation=" << snapshot.last_generation
              << "; warming up TX for 200ms...\n";
    // Receiving a valid sample proves that discovery is active. Give the
    // command writer a short additional window to match the WBCS subscriber
    // before publishing the first one-shot Init or Move frame.
    std::this_thread::sleep_for(kRawTransportWarmupInterval);
  }

  if (success && init_value == 1) {
    std::cout << "WARNING: Init performs homing/calibration; do not run it before every Move.\n";
    const uint64_t init_baseline = get_snapshot(state).last_generation;
    if (robot.send_endtool_raw_frame(side, encode_init()) != ControlStatus::SUCCESS) {
      std::cerr << "FAILED: failed to publish the gripper init frame.\n";
      success = false;
    }

    if (success) {
      // A passive Ready report from the pre-Init state can arrive immediately
      // after publication. Keep Move out of the TX path for at least one second
      // so that report cannot prematurely complete this Init attempt.
      const uint64_t ready_not_before_ms =
          steady_now_ms() +
          static_cast<uint64_t>(
              std::chrono::duration_cast<std::chrono::milliseconds>(
                  kInitCommandGuardInterval)
                  .count());
      std::cout << "Waiting for a fresh Ready status (2) after Init "
                   "(minimum guard=1s, timeout=20s)...\n";
      const auto init_deadline = std::chrono::steady_clock::now() + kInitTimeout;
      auto last_print_at = std::chrono::steady_clock::time_point::min();
      uint8_t last_motion_state = 0;
      uint8_t last_error_code = 0;
      bool has_printed_state = false;
      bool ready = false;
      while (std::chrono::steady_clock::now() < init_deadline) {
        // Wake on a raw callback, but periodically re-check the timeout even if
        // the receive stream is silent.
        {
          std::unique_lock<std::mutex> lock(state->mutex);
          state->changed.wait_for(lock, kStatusPollInterval);
        }
        const StateSnapshot snapshot = get_snapshot(state);
        if (!snapshot.valid || snapshot.last_generation <= init_baseline) {
          continue;
        }

        const auto now = std::chrono::steady_clock::now();
        const bool state_changed = !has_printed_state ||
                                   snapshot.status.motion_state != last_motion_state ||
                                   snapshot.status.error_code != last_error_code;
        if (state_changed || now - last_print_at >= kStatusPrintInterval) {
          print_status("Init status:", snapshot.last_generation, snapshot.status);
          last_motion_state = snapshot.status.motion_state;
          last_error_code = snapshot.status.error_code;
          last_print_at = now;
          has_printed_state = true;
        }
        // State 2 is command-ready. State 4 can be a previous Move completion
        // and therefore does not prove that this Init has completed.
        if (snapshot.status.motion_state == 2 &&
            snapshot.last_update_ms >= ready_not_before_ms) {
          ready = true;
          break;
        }
      }
      if (!ready) {
        const StateSnapshot snapshot = get_snapshot(state);
        std::cerr << "FAILED: gripper did not report a fresh command-ready status after Init; "
                  << "latest_valid_generation=" << snapshot.last_generation
                  << " latest_raw_generation=" << snapshot.latest_raw_generation
                  << " idle_snapshots=" << snapshot.idle_snapshot_count << '\n';
        print_rx_summary(snapshot);
        success = false;
      }
    }
  }

  if (success) {
    // Give the passive stream a short chance to expose an uninitialized
    // device before publishing Move. No status is treated as inconclusive.
    {
      std::unique_lock<std::mutex> lock(state->mutex);
      state->changed.wait_for(lock, kInitialStatusWait, [&state] {
        return state->valid;
      });
      if (state->valid &&
          (state->last_status.error_code == 0x01 || state->last_status.motion_state == 0)) {
        std::cerr << "FAILED: gripper is not initialized (device error 0x01/state 0); "
                     "rerun this example with init=1 before sending Move.\n";
        success = false;
      }
    }
  }

  uint64_t move_baseline = 0;
  if (success) {
    move_baseline = get_snapshot(state).last_generation;
    if (robot.send_endtool_raw_frame(side, encode_move(width_mm, velocity_mm_s, torque_nm)) !=
        ControlStatus::SUCCESS) {
      std::cerr << "FAILED: failed to publish the gripper move frame.\n";
      success = false;
    }
  }

  if (success) {
    std::cout << "Move sent. Monitoring only fresh passive status reports for 10 seconds...\n";
    const auto deadline = std::chrono::steady_clock::now() + kMonitorDuration;
    auto last_sdk_print = std::chrono::steady_clock::time_point::min();
    auto last_raw_print = std::chrono::steady_clock::time_point::min();
    float last_sdk_position_mm = 0.0F;
    bool has_sdk_position = false;
    bool has_raw_state = false;
    uint8_t last_raw_motion_state = 0;
    uint8_t last_raw_error_code = 0;
    uint64_t printed_generation = move_baseline;
    bool received_after_move = false;
    bool raw_target_reached = false;
    bool sdk_target_reached = false;
    GripStatus latest_move_status;
    bool has_latest_move_status = false;

    while (std::chrono::steady_clock::now() < deadline) {
      {
        std::unique_lock<std::mutex> lock(state->mutex);
        state->changed.wait_for(lock, kStatusPollInterval);
      }
      const StateSnapshot snapshot = get_snapshot(state);

      if (snapshot.valid && snapshot.last_generation > move_baseline &&
          snapshot.last_generation != printed_generation) {
        received_after_move = true;
        printed_generation = snapshot.last_generation;
        latest_move_status = snapshot.status;
        has_latest_move_status = true;

        const auto now = std::chrono::steady_clock::now();
        const bool state_changed = !has_raw_state ||
                                   snapshot.status.motion_state != last_raw_motion_state ||
                                   snapshot.status.error_code != last_raw_error_code;
        if (state_changed || now - last_raw_print >= kStatusPrintInterval) {
          print_status("Raw status snapshot:", snapshot.last_generation, snapshot.status);
          last_raw_motion_state = snapshot.status.motion_state;
          last_raw_error_code = snapshot.status.error_code;
          last_raw_print = now;
          has_raw_state = true;
        }

        if (snapshot.status.error_code == 0 &&
            std::fabs(snapshot.status.position_mm - width_mm) <= kPositionToleranceMm &&
            std::fabs(snapshot.status.velocity_mm_s) <= kStoppedVelocityToleranceMmS) {
          std::cout << "Move completed within tolerance: requested=" << width_mm
                    << "mm actual=" << snapshot.status.position_mm
                    << "mm (verified by Raw status).\n";
          raw_target_reached = true;
          break;
        }
      }

      const auto sdk_state = robot.get_gripper_state(controller_group);
      if (sdk_state == nullptr) {
        continue;
      }
      const float position_mm = static_cast<float>(sdk_state->width * 1000.0);
      const float velocity_mm_s = static_cast<float>(sdk_state->velocity * 1000.0);
      const auto now = std::chrono::steady_clock::now();
      if (!has_sdk_position || std::fabs(position_mm - last_sdk_position_mm) >= 0.1F ||
          now - last_sdk_print >= kStatusPrintInterval) {
        std::cout << "SDK state: position=" << position_mm << "mm velocity=" << velocity_mm_s
                  << "mm/s effort=" << sdk_state->effort
                  << " is_moving=" << std::boolalpha << sdk_state->is_moving << std::noboolalpha
                  << '\n';
        last_sdk_position_mm = position_mm;
        last_sdk_print = now;
        has_sdk_position = true;
      }
      if (std::fabs(position_mm - width_mm) <= kPositionToleranceMm && !sdk_state->is_moving) {
        std::cout << "Move completed within tolerance: requested=" << width_mm
                  << "mm actual=" << position_mm
                  << "mm (verified by get_gripper_state).\n";
        sdk_target_reached = true;
        break;
      }
    }

    const StateSnapshot snapshot = get_snapshot(state);
    if (!received_after_move) {
      std::cout << "WARNING: no valid gripper status snapshot was received after Move. "
                   "Publication success alone does not prove that the device executed the "
                   "command.\n";
    } else if (steady_now_ms() - snapshot.last_update_ms >
               static_cast<uint64_t>(
                   std::chrono::duration_cast<std::chrono::milliseconds>(kStatusStaleInterval).count())) {
      std::cout << "WARNING: the valid gripper status stream stopped; subsequent receive "
                   "buffers were idle or rejected.\n";
    }
    print_rx_summary(snapshot);

    if (!raw_target_reached && !sdk_target_reached) {
      // Raw verification is authoritative for raw-mode operation. Retain the
      // SDK state as a fallback for configurations that still publish it.
      const auto sdk_state = robot.get_gripper_state(controller_group);
      if (sdk_state != nullptr) {
        const float position_mm = static_cast<float>(sdk_state->width * 1000.0);
        sdk_target_reached = std::fabs(position_mm - width_mm) <= kPositionToleranceMm &&
                             !sdk_state->is_moving;
      }

      if (!sdk_target_reached) {
        std::cerr << "FAILED: self-developed gripper verification failed: Move did not reach "
                     "the requested width: requested="
                  << width_mm << "mm tolerance=" << kPositionToleranceMm << "mm; ";
        if (sdk_state == nullptr) {
          std::cerr << "SDK state=unavailable; ";
        } else {
          const float position_mm = static_cast<float>(sdk_state->width * 1000.0);
          std::cerr << "SDK state={position=" << position_mm
                    << "mm,error=" << std::fabs(position_mm - width_mm)
                    << "mm,is_moving=" << std::boolalpha << sdk_state->is_moving
                    << std::noboolalpha << "}; ";
        }
        if (!has_latest_move_status) {
          std::cerr << "last_raw_status=none\n";
        } else {
          std::cerr << "last_raw_status={generation=" << printed_generation
                    << ",position=" << latest_move_status.position_mm
                    << "mm,velocity=" << latest_move_status.velocity_mm_s
                    << "mm/s,motion_state="
                    << static_cast<unsigned int>(latest_move_status.motion_state)
                    << ",error=0x" << std::hex
                    << static_cast<unsigned int>(latest_move_status.error_code) << std::dec
                    << "}\n";
        }
        success = false;
      }
    }
  }

  robot.unregister_endtool_raw_callback(callback_handle);
  if (restore_controller_value == 1) {
    robot.start_controller(controller_group);
  }
  robot.destroy();
  return success ? 0 : 4;
}

Dahuan Two-Finger CAN Gripper

Configures and moves a Dahuan two-finger gripper and actively polls its status through the raw CAN transport.

examples/s1/cpp/galbot_robot/src/endtool_raw_dahuan2_can_example.cpp

#include <array>
#include <atomic>
#include <chrono>
#include <cstdint>
#include <cstring>
#include <iomanip>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr uint32_t kTxCanId = 0x601;
constexpr uint32_t kRxCanId = 0x201;
constexpr uint8_t kWriteSingleRegister = 0x06;
constexpr uint8_t kReadRegisters = 0x03;
constexpr uint16_t kRegisterInit = 0x0100;
constexpr uint16_t kRegisterForce = 0x0101;
constexpr uint16_t kRegisterTargetPosition = 0x0103;
constexpr uint16_t kRegisterSpeed = 0x0104;
constexpr uint16_t kRegisterStatusBase = 0x0200;

struct Dahuan2Status {
  uint16_t init_status = 0;
  uint16_t grip_status = 0;
  uint16_t position_permille = 0;
  uint16_t speed = 0;
  int16_t current_ma = 0;
  uint16_t error = 0;
};

struct DemoState {
  std::mutex mutex;
  Dahuan2Status status;
  bool has_status = false;
  std::array<uint64_t, 2> generation_by_kind{};
  uint64_t valid_count = 0;
  uint64_t idle_snapshot_count = 0;
  uint64_t rejected_count = 0;
  uint64_t non_increasing_generation_count = 0;
};

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> make_request(
    uint8_t function, uint16_t address, uint16_t value) {
  std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> frame{};
  const uint32_t frame_type = 1;
  std::memcpy(frame.data(), &frame_type, sizeof(frame_type));
  std::memcpy(frame.data() + 4, &kTxCanId, sizeof(kTxCanId));
  frame[8] = 7;  // Five protocol bytes plus the two-byte TIB overhead.
  frame[12] = function;
  frame[13] = static_cast<uint8_t>(address >> 8);
  frame[14] = static_cast<uint8_t>(address);
  frame[15] = static_cast<uint8_t>(value >> 8);
  frame[16] = static_cast<uint8_t>(value);
  return frame;
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_init(bool calibrate) {
  return make_request(kWriteSingleRegister, kRegisterInit, calibrate ? 0x00A5 : 0x0001);
}

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_status_request() {
  return make_request(kReadRegisters, kRegisterStatusBase, 6);
}

bool decode_status(const EndToolRawData& raw, EndToolSide expected_side, Dahuan2Status& status) {
  // Dahuan CAN responses may appear in either receive buffer, so do not filter raw.kind.
  if (raw.side != expected_side) {
    return false;
  }
  // The receive PDO is a snapshot.  A zero marker means no new CAN response
  // arrived in this cycle; the remaining bytes may still contain an old reply.
  if (raw.frame[12] != 0x01) {
    return false;
  }

  uint32_t frame_type = 0;
  uint32_t can_id = 0;
  std::memcpy(&frame_type, raw.frame.data(), sizeof(frame_type));
  std::memcpy(&can_id, raw.frame.data() + 4, sizeof(can_id));
  if (frame_type != 1 || can_id != kRxCanId || raw.frame[16] != kReadRegisters ||
      raw.frame[17] < 12) {
    return false;
  }

  const auto read_be16 = [&raw](std::size_t register_offset) {
    const std::size_t offset = 18 + register_offset * 2;
    return static_cast<uint16_t>((static_cast<uint16_t>(raw.frame[offset]) << 8) |
                                 raw.frame[offset + 1]);
  };
  status.init_status = read_be16(0);
  status.grip_status = read_be16(1);
  status.position_permille = read_be16(2);
  status.speed = read_be16(3);
  status.current_ma = static_cast<int16_t>(read_be16(4));
  status.error = read_be16(5);
  return true;
}

const char* error_text(uint16_t error) {
  switch (error) {
    case 0:
      return "no error";
    case 1:
      return "under voltage";
    case 2:
      return "over voltage";
    case 3:
      return "over current";
    case 4:
      return "over heat";
    case 5:
      return "motor disconnected";
    case 8:
      return "overload";
    case 11:
      return "over speed";
    case 15:
      return "startup error";
    case 32:
      return "encoder error";
    default:
      return "unknown error";
  }
}

bool parse_side(const std::string& value, EndToolSide& side) {
  if (value == "left" || value == "l" || value == "0") {
    side = EndToolSide::LEFT;
    return true;
  }
  if (value == "right" || value == "r" || value == "1") {
    side = EndToolSide::RIGHT;
    return true;
  }
  return false;
}

uint64_t steady_now_ms() {
  return static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                   std::chrono::steady_clock::now().time_since_epoch())
                                   .count());
}

}  // namespace

int main(int argc, char** argv) {
  EndToolSide side = EndToolSide::LEFT;
  int position = 500;
  int speed = 50;
  int force = 50;
  int calibrate_value = 0;
  int duration_s = 10;
  int init_wait_s = 4;
  int run_init_value = 0;
  int stop_controller_value = 0;
  int restore_controller_value = 0;
  try {
    if (!parse_side(argc > 1 ? argv[1] : "left", side)) {
      std::cerr << "Side must be left/right/l/r/0/1.\n";
      return 1;
    }
    position = argc > 2 ? std::stoi(argv[2]) : position;
    speed = argc > 3 ? std::stoi(argv[3]) : speed;
    force = argc > 4 ? std::stoi(argv[4]) : force;
    calibrate_value = argc > 5 ? std::stoi(argv[5]) : calibrate_value;
    duration_s = argc > 6 ? std::stoi(argv[6]) : duration_s;
    init_wait_s = argc > 7 ? std::stoi(argv[7]) : init_wait_s;
    run_init_value = argc > 8 ? std::stoi(argv[8]) : run_init_value;
    stop_controller_value = argc > 9 ? std::stoi(argv[9]) : stop_controller_value;
    restore_controller_value = argc > 10 ? std::stoi(argv[10]) : restore_controller_value;
  } catch (const std::exception&) {
    std::cerr << "Invalid numeric argument.\n";
    return 1;
  }
  const bool calibrate = calibrate_value == 1;
  const bool run_init = calibrate || run_init_value == 1;
  if (position < 0 || position > 1000 || speed < 1 || speed > 100 || force < 20 ||
      force > 100 || (calibrate_value != 0 && calibrate_value != 1) || duration_s < 0 ||
      init_wait_s < 0 || (run_init_value != 0 && run_init_value != 1) ||
      (stop_controller_value != 0 && stop_controller_value != 1) ||
      (restore_controller_value != 0 && restore_controller_value != 1)) {
    std::cerr << "Usage: " << argv[0]
              << " [left|right] [position:0..1000] [speed:1..100] [force:20..100]"
                 " [calibrate:0|1] [duration_s] [init_wait_s]"
                 " [run_init:0|1] [stop_standard_controller:0|1]"
                 " [restore_controller:0|1]\n";
    return 1;
  }

  const std::string controller_group = side == EndToolSide::LEFT ? "left_gripper" : "right_gripper";
  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  if (!robot.init()) {
    std::cerr << "FAILED: failed to initialize the S1 SDK.\n";
    return 2;
  }

  std::cout << "WARNING: raw TX has no server-side exclusive writer lock.\n";
  std::cout << "WBCS must run with endtool_raw_enabled=true.\n";
  if (stop_controller_value == 1) {
    std::cout << "Best-effort stop of " << controller_group << ": "
              << static_cast<int>(robot.stop_controller(controller_group)) << '\n';
  }

  auto last_print_ms = std::make_shared<std::atomic<uint64_t>>(0);
  auto state = std::make_shared<DemoState>();
  const uint64_t callback_handle = robot.register_endtool_raw_callback(
      [side, last_print_ms, state](const EndToolRawData& raw) {
        if (raw.side != side) {
          return;
        }
        if (raw.frame[12] != 0x01) {
          std::lock_guard<std::mutex> lock(state->mutex);
          ++state->idle_snapshot_count;
          return;
        }
        Dahuan2Status status;
        if (!decode_status(raw, side, status)) {
          std::lock_guard<std::mutex> lock(state->mutex);
          ++state->rejected_count;
          return;
        }
        {
          std::lock_guard<std::mutex> lock(state->mutex);
          const std::size_t kind_index =
              raw.kind == EndToolRxKind::BUFFER_1KHZ ? 0U : 1U;
          const uint64_t previous_generation = state->generation_by_kind[kind_index];
          if (raw.generation != 0 && previous_generation != 0 &&
              raw.generation <= previous_generation) {
            ++state->non_increasing_generation_count;
            return;
          }
          state->generation_by_kind[kind_index] = raw.generation;
          state->status = status;
          state->has_status = true;
          ++state->valid_count;
        }
        const uint64_t now_ms = steady_now_ms();
        uint64_t previous = last_print_ms->load(std::memory_order_relaxed);
        if (now_ms - previous < 200 || !last_print_ms->compare_exchange_strong(
                                           previous, now_ms, std::memory_order_relaxed)) {
          return;
        }
        std::cout << "RX kind=" << static_cast<uint32_t>(raw.kind)
                  << " generation=" << raw.generation << " init=" << status.init_status
                  << " grip=" << status.grip_status << " position=" << status.position_permille
                  << "permille speed=" << status.speed << " current=" << status.current_ma
                  << "mA error=0x" << std::hex << status.error << std::dec << " ("
                  << error_text(status.error) << ")\n";
      });
  if (callback_handle == 0) {
    std::cerr << "FAILED: failed to register the raw RX callback.\n";
    if (restore_controller_value == 1) {
      robot.start_controller(controller_group);
    }
    robot.destroy();
    return 3;
  }

  const auto publish = [&robot, side](const auto& frame, const char* description) {
    const auto result = robot.send_endtool_raw_frame(side, frame);
    if (result != ControlStatus::SUCCESS) {
      std::cerr << "FAILED: failed to publish " << description << ": "
                << static_cast<int>(result) << '\n';
      return false;
    }
    return true;
  };
  const auto poll_for = [&publish](int seconds) {
    const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(seconds);
    while (std::chrono::steady_clock::now() < deadline) {
      if (!publish(encode_status_request(), "status request")) {
        return false;
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
    return true;
  };

  bool success = true;
  if (run_init) {
    success = publish(encode_init(calibrate), calibrate ? "calibration command" : "homing command");
  }
  if (success && run_init) {
    std::cout << "Polling initialization state for " << init_wait_s << " seconds...\n";
    success = poll_for(init_wait_s);
  }
  success = success && publish(make_request(kWriteSingleRegister, kRegisterSpeed, speed), "speed command");
  success = success && publish(make_request(kWriteSingleRegister, kRegisterForce, force), "force command");
  success = success && publish(
                           make_request(kWriteSingleRegister, kRegisterTargetPosition, position),
                           "position command");
  uint64_t motion_status_baseline = 0;
  if (success) {
    std::lock_guard<std::mutex> lock(state->mutex);
    // Drop any response left by initialization polling. The following active
    // reads must produce a fresh status before the target can be verified.
    state->has_status = false;
    motion_status_baseline = state->valid_count;
  }
  if (success) {
    std::cout << "Polling status for " << duration_s << " seconds...\n";
    success = poll_for(duration_s);
  }

  if (success) {
    Dahuan2Status status;
    bool has_status = false;
    uint64_t valid_count = 0;
    uint64_t idle_count = 0;
    uint64_t rejected_count = 0;
    uint64_t non_increasing_count = 0;
    {
      std::lock_guard<std::mutex> lock(state->mutex);
      status = state->status;
      has_status = state->has_status;
      valid_count = state->valid_count;
      idle_count = state->idle_snapshot_count;
      rejected_count = state->rejected_count;
      non_increasing_count = state->non_increasing_generation_count;
    }
    if (!has_status || valid_count <= motion_status_baseline) {
      std::cerr << "FAILED: Dahuan2 device verification failed: no fresh status response was "
                   "received: idle_snapshots="
                << idle_count
                << " rejected=" << rejected_count
                << " non_increasing_generations=" << non_increasing_count << '\n';
      success = false;
    } else if (status.error != 0) {
      std::cerr << "FAILED: Dahuan2 device verification failed: gripper status error=0x"
                << std::hex << status.error << std::dec
                << " (" << error_text(status.error) << ")\n";
      success = false;
    } else if (std::abs(static_cast<int>(status.position_permille) - position) > 20) {
      std::cerr << "FAILED: Dahuan2 device verification failed: position did not reach target: "
                   "requested="
                << position
                << " actual=" << status.position_permille << " tolerance=20permille\n";
      success = false;
    } else {
      std::cout << "Position reached: requested=" << position
                << " actual=" << status.position_permille
                << "permille fresh_status_count=" << valid_count << '\n';
    }
  }

  robot.unregister_endtool_raw_callback(callback_handle);
  if (restore_controller_value == 1) {
    robot.start_controller(controller_group);
  }
  robot.destroy();
  return success ? 0 : 4;
}

Kunwei RS485 Six-Axis Force Sensor

Sends periodic RS485 queries without catch-up bursts and validates, monitors, and prints six-axis force/torque responses from the 1 kHz receive stream.

examples/s1/cpp/galbot_robot/src/endtool_raw_kunwei_rs485_example.cpp

#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>

#include "galbot_robot.hpp"

using namespace galbot::sdk;

namespace {

constexpr float kMaxAbsWrenchValue = 1.0e6F;
constexpr auto kStaleTimeout = std::chrono::milliseconds(1000);
constexpr auto kFreezeTimeout = std::chrono::milliseconds(2000);
constexpr auto kWarningInterval = std::chrono::milliseconds(1000);
constexpr auto kStatsInterval = std::chrono::seconds(5);

struct ForceReading {
  float fx = 0.0F;
  float fy = 0.0F;
  float fz = 0.0F;
  float mx = 0.0F;
  float my = 0.0F;
  float mz = 0.0F;
};

enum class DecodeResult {
  OK,
  PREFIX_MISMATCH,
  TERMINATOR_MISMATCH,
  INVALID_NUMERIC,
  ALL_ZERO,
};

struct DemoState {
  std::atomic<uint64_t> query_ok{0};
  std::atomic<uint64_t> query_fail{0};
  std::atomic<uint64_t> rx_1khz{0};
  std::atomic<uint64_t> rejected{0};
  std::atomic<uint64_t> reject_prefix{0};
  std::atomic<uint64_t> reject_terminator{0};
  std::atomic<uint64_t> reject_numeric{0};
  std::atomic<uint64_t> reject_all_zero{0};
  std::atomic<uint64_t> non_increasing_generation{0};
  std::atomic<uint64_t> duplicate_snapshots{0};
  std::atomic<uint64_t> valid{0};
  std::atomic<uint64_t> last_print_ms{0};
  std::atomic<uint64_t> last_valid_ms{0};
  std::atomic<uint64_t> last_generation{0};
  std::atomic<uint64_t> last_payload_change_ms{0};
  std::atomic<uint64_t> last_freeze_warning_ms{0};
  std::atomic<uint64_t> last_stale_warning_ms{0};
  std::mutex payload_mutex;
  std::array<uint8_t, 24> last_payload{};
  bool has_last_payload = false;
};

std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> encode_query() {
  std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE> frame{};
  const uint32_t frame_type = 2;
  const uint32_t device_id = 233;
  const uint32_t data_size = 4;
  std::memcpy(frame.data(), &frame_type, sizeof(frame_type));
  std::memcpy(frame.data() + 4, &device_id, sizeof(device_id));
  std::memcpy(frame.data() + 8, &data_size, sizeof(data_size));
  frame[12] = 0x49;
  frame[13] = 0xAA;
  frame[14] = 0x0D;
  frame[15] = 0x0A;
  return frame;
}

DecodeResult decode_force(
    const std::array<uint8_t, END_TOOL_RAW_FRAME_SIZE>& frame, ForceReading& reading) {
  if ((frame[20] != 0x48 && frame[20] != 0x49) || frame[21] != 0xAA) {
    return DecodeResult::PREFIX_MISMATCH;
  }
  if (frame[46] != 0x0D || frame[47] != 0x0A) {
    return DecodeResult::TERMINATOR_MISMATCH;
  }

  ForceReading decoded;
  std::memcpy(&decoded.fx, frame.data() + 22, sizeof(float));
  std::memcpy(&decoded.fy, frame.data() + 26, sizeof(float));
  std::memcpy(&decoded.fz, frame.data() + 30, sizeof(float));
  std::memcpy(&decoded.mx, frame.data() + 34, sizeof(float));
  std::memcpy(&decoded.my, frame.data() + 38, sizeof(float));
  std::memcpy(&decoded.mz, frame.data() + 42, sizeof(float));

  const float values[] = {decoded.fx, decoded.fy, decoded.fz,
                          decoded.mx, decoded.my, decoded.mz};
  bool all_zero = true;
  for (const float value : values) {
    if (!std::isfinite(value) || std::fabs(value) > kMaxAbsWrenchValue) {
      return DecodeResult::INVALID_NUMERIC;
    }
    all_zero = all_zero && value == 0.0F;
  }
  if (all_zero) {
    return DecodeResult::ALL_ZERO;
  }

  reading = decoded;
  return DecodeResult::OK;
}

bool parse_side(const std::string& value, EndToolSide& side) {
  if (value == "left" || value == "l" || value == "0") {
    side = EndToolSide::LEFT;
    return true;
  }
  if (value == "right" || value == "r" || value == "1") {
    side = EndToolSide::RIGHT;
    return true;
  }
  return false;
}

uint64_t steady_now_ms() {
  return static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                   std::chrono::steady_clock::now().time_since_epoch())
                                   .count());
}

void count_decode_rejection(const std::shared_ptr<DemoState>& state, DecodeResult result) {
  state->rejected.fetch_add(1, std::memory_order_relaxed);
  switch (result) {
    case DecodeResult::PREFIX_MISMATCH:
      state->reject_prefix.fetch_add(1, std::memory_order_relaxed);
      break;
    case DecodeResult::TERMINATOR_MISMATCH:
      state->reject_terminator.fetch_add(1, std::memory_order_relaxed);
      break;
    case DecodeResult::INVALID_NUMERIC:
      state->reject_numeric.fetch_add(1, std::memory_order_relaxed);
      break;
    case DecodeResult::ALL_ZERO:
      state->reject_all_zero.fetch_add(1, std::memory_order_relaxed);
      break;
    case DecodeResult::OK:
      break;
  }
}

}  // namespace

int main(int argc, char** argv) {
  EndToolSide side = EndToolSide::LEFT;
  int duration_s = 10;
  int query_interval_ms = 10;
  int print_interval_ms = 100;
  try {
    if (!parse_side(argc > 1 ? argv[1] : "left", side)) {
      std::cerr << "Side must be left/right/l/r/0/1.\n";
      return 1;
    }
    duration_s = argc > 2 ? std::stoi(argv[2]) : duration_s;
    query_interval_ms = argc > 3 ? std::stoi(argv[3]) : query_interval_ms;
    print_interval_ms = argc > 4 ? std::stoi(argv[4]) : print_interval_ms;
  } catch (const std::exception&) {
    std::cerr << "Invalid numeric argument.\n";
    return 1;
  }
  if (duration_s <= 0 || query_interval_ms <= 0 || print_interval_ms <= 0) {
    std::cerr << "Usage: " << argv[0]
              << " [left|right] [duration_s>0] [query_interval_ms>0] [print_interval_ms>0]\n";
    return 1;
  }

  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  if (!robot.init()) {
    std::cerr << "Failed to initialize the S1 SDK.\n";
    return 2;
  }
  std::cout << "WARNING: same-process raw TX is serialized, but there is no cross-client "
               "resource lease.\n";
  std::cout << "WBCS must run with endtool_raw_enabled=true.\n";

  auto state = std::make_shared<DemoState>();
  const uint64_t callback_handle = robot.register_endtool_raw_callback(
      [side, print_interval_ms, state](const EndToolRawData& raw) {
        if (raw.side != side || raw.kind != EndToolRxKind::BUFFER_1KHZ) {
          return;
        }
        state->rx_1khz.fetch_add(1, std::memory_order_relaxed);

        ForceReading reading;
        const DecodeResult decode_result = decode_force(raw.frame, reading);
        if (decode_result != DecodeResult::OK) {
          count_decode_rejection(state, decode_result);
          return;
        }

        uint64_t previous_generation = state->last_generation.load(std::memory_order_relaxed);
        while (true) {
          if (previous_generation != 0 && raw.generation <= previous_generation) {
            state->non_increasing_generation.fetch_add(1, std::memory_order_relaxed);
            return;
          }
          if (state->last_generation.compare_exchange_weak(
                  previous_generation, raw.generation, std::memory_order_relaxed,
                  std::memory_order_relaxed)) {
            break;
          }
        }

        const uint64_t now_ms = steady_now_ms();
        bool duplicate = false;
        {
          std::lock_guard<std::mutex> lock(state->payload_mutex);
          duplicate = state->has_last_payload &&
                      std::equal(state->last_payload.begin(), state->last_payload.end(),
                                 raw.frame.begin() + 22);
          if (!duplicate) {
            std::copy(raw.frame.begin() + 22, raw.frame.begin() + 46,
                      state->last_payload.begin());
            state->has_last_payload = true;
          }
        }
        if (!duplicate) {
          state->last_payload_change_ms.store(now_ms, std::memory_order_relaxed);
        } else {
          state->duplicate_snapshots.fetch_add(1, std::memory_order_relaxed);
          const uint64_t last_change_ms =
              state->last_payload_change_ms.load(std::memory_order_relaxed);
          uint64_t last_warning_ms =
              state->last_freeze_warning_ms.load(std::memory_order_relaxed);
          if (last_change_ms != 0 &&
              now_ms - last_change_ms >= static_cast<uint64_t>(kFreezeTimeout.count()) &&
              (last_warning_ms == 0 ||
               now_ms - last_warning_ms >= static_cast<uint64_t>(kWarningInterval.count())) &&
              state->last_freeze_warning_ms.compare_exchange_strong(
                  last_warning_ms, now_ms, std::memory_order_relaxed,
                  std::memory_order_relaxed)) {
            std::cerr << "Force payload unchanged for " << (now_ms - last_change_ms)
                      << "ms while generation advances. This may be a held TIB snapshot; an "
                         "exactly steady sensor can also produce identical bytes.\n";
          }
          return;
        }

        state->last_valid_ms.store(now_ms, std::memory_order_relaxed);
        state->valid.fetch_add(1, std::memory_order_relaxed);
        uint64_t previous_print = state->last_print_ms.load(std::memory_order_relaxed);
        if (now_ms - previous_print < static_cast<uint64_t>(print_interval_ms) ||
            !state->last_print_ms.compare_exchange_strong(
                previous_print, now_ms, std::memory_order_relaxed,
                std::memory_order_relaxed)) {
          return;
        }
        std::cout << "generation=" << raw.generation << " Fx=" << reading.fx
                  << " Fy=" << reading.fy << " Fz=" << reading.fz << "kg Mx=" << reading.mx
                  << " My=" << reading.my << " Mz=" << reading.mz << "kg*m\n";
      });
  if (callback_handle == 0) {
    std::cerr << "Failed to register the raw RX callback.\n";
    robot.destroy();
    return 3;
  }

  const auto query = encode_query();
  const auto query_interval = std::chrono::milliseconds(query_interval_ms);
  const auto started_at = std::chrono::steady_clock::now();
  const auto deadline = started_at + std::chrono::seconds(duration_s);
  auto next_query_at = started_at;
  auto next_stats_at = started_at + kStatsInterval;
  auto last_stats_at = started_at;
  uint64_t last_stats_query_count = 0;
  uint64_t last_stats_rx_count = 0;

  while (true) {
    const auto before_sleep = std::chrono::steady_clock::now();
    if (before_sleep < next_query_at) {
      std::this_thread::sleep_until(next_query_at);
    }
    if (std::chrono::steady_clock::now() >= deadline) {
      break;
    }

    const auto result = robot.send_endtool_raw_frame(side, query);
    if (result == ControlStatus::SUCCESS) {
      state->query_ok.fetch_add(1, std::memory_order_relaxed);
    } else {
      const uint64_t failures = state->query_fail.fetch_add(1, std::memory_order_relaxed) + 1;
      if (failures == 1 || failures % 100 == 0) {
        std::cerr << "Query publish failed: status=" << static_cast<int>(result)
                  << " failures=" << failures << '\n';
      }
    }

    const auto after_query = std::chrono::steady_clock::now();
    next_query_at += query_interval;
    // Never catch up with a burst after Publish blocks; doing so only increases FIFO pressure.
    if (next_query_at <= after_query) {
      next_query_at = after_query + query_interval;
    }

    const uint64_t now_ms = steady_now_ms();
    const uint64_t last_valid_ms = state->last_valid_ms.load(std::memory_order_relaxed);
    const uint64_t stale_for_ms =
        last_valid_ms == 0
            ? static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
                                        after_query - started_at)
                                        .count())
            : now_ms - last_valid_ms;
    uint64_t last_stale_warning_ms =
        state->last_stale_warning_ms.load(std::memory_order_relaxed);
    if (stale_for_ms >= static_cast<uint64_t>(kStaleTimeout.count()) &&
        (last_stale_warning_ms == 0 ||
         now_ms - last_stale_warning_ms >= static_cast<uint64_t>(kWarningInterval.count())) &&
        state->last_stale_warning_ms.compare_exchange_strong(
            last_stale_warning_ms, now_ms, std::memory_order_relaxed,
            std::memory_order_relaxed)) {
      std::cerr << "No fresh force response for " << stale_for_ms
                << "ms: query_ok=" << state->query_ok.load(std::memory_order_relaxed)
                << " query_fail=" << state->query_fail.load(std::memory_order_relaxed)
                << " rx_1khz=" << state->rx_1khz.load(std::memory_order_relaxed)
                << " valid=" << state->valid.load(std::memory_order_relaxed)
                << ". Check TIB, sensor power/wiring/baud rate, "
                   "endtool_raw_enabled and WBCS logs.\n";
    }

    if (after_query >= next_stats_at) {
      const uint64_t query_count = state->query_ok.load(std::memory_order_relaxed);
      const uint64_t rx_count = state->rx_1khz.load(std::memory_order_relaxed);
      const double elapsed_s = std::chrono::duration<double>(after_query - last_stats_at).count();
      std::cout << "Rate: query_hz="
                << static_cast<double>(query_count - last_stats_query_count) / elapsed_s
                << " rx_hz=" << static_cast<double>(rx_count - last_stats_rx_count) / elapsed_s
                << " fresh=" << state->valid.load(std::memory_order_relaxed)
                << " duplicates=" << state->duplicate_snapshots.load(std::memory_order_relaxed)
                << " rejects(prefix=" << state->reject_prefix.load(std::memory_order_relaxed)
                << " terminator=" << state->reject_terminator.load(std::memory_order_relaxed)
                << " numeric=" << state->reject_numeric.load(std::memory_order_relaxed)
                << " zero=" << state->reject_all_zero.load(std::memory_order_relaxed) << ")\n";
      last_stats_query_count = query_count;
      last_stats_rx_count = rx_count;
      last_stats_at = after_query;
      next_stats_at = after_query + kStatsInterval;
    }
  }

  robot.unregister_endtool_raw_callback(callback_handle);
  std::cout << "Stopped: query_ok=" << state->query_ok.load()
            << " query_fail=" << state->query_fail.load()
            << " rx_1khz=" << state->rx_1khz.load()
            << " rejected=" << state->rejected.load()
            << " (prefix=" << state->reject_prefix.load()
            << " terminator=" << state->reject_terminator.load()
            << " numeric=" << state->reject_numeric.load()
            << " zero=" << state->reject_all_zero.load()
            << " non_increasing_generation=" << state->non_increasing_generation.load() << ")"
            << " fresh=" << state->valid.load()
            << " duplicates=" << state->duplicate_snapshots.load() << '\n';
  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::S1);

    // 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::S1)
                            .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::S1);

    // 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 names to control, including ["torso", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"] (S1: torso replaces G1 leg)
    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;

    // Execute joint trajectory
    ControlStatus status = robot.execute_joint_trajectory(trajectory, is_traj_block);
    if (status == ControlStatus::SUCCESS) {
        std::cout << "Trajectory execution succeeded!" << std::endl;
    } else {
        std::cerr << "Trajectory execution failed!" << std::endl;
    }

    // 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::S1);

    // 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::S1);

  // 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 ["torso", "head", "left_arm", "right_arm", "left_gripper", "right_gripper"] (S1: torso replaces G1 leg)
  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 S1 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 torso / 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`
 *    - On S1 this corresponds to the `swerve_chassis` task
 *    - 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(S1ControllerName::SWERVE_CHASSIS_POSE_CTRL)` or
 *      `switch_controller(S1ControllerName::SWERVE_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 = S1JointGroup::SWERVE_CHASSIS;
constexpr const char* kChassisSubtaskPose = "swerve_chassis_pose";
constexpr const char* kChassisSubtaskTwist = "swerve_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_swerve_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 = {S1JointGroup::SWERVE_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_swerve_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 = {S1JointGroup::SWERVE_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_swerve_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 torso target\n"
            << "  base_pose    - publish a swerve chassis pose target\n"
            << "  base_twist   - publish a swerve chassis twist target with auto stop\n"
            << "  mixed_pose   - publish torso + swerve chassis pose in one target\n"
            << "  mixed_twist  - publish torso + swerve chassis twist in one target\n"
            << "  quit         - exit example\n"
            << 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::S1);
  if (!robot.init()) {
    std::cerr << "robot init failed" << std::endl;
    return -1;
  }

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

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

  const std::vector<std::string> torso_single_joint = {torso_joint_names.front()};
  constexpr double kJointTimeS = 3.0;
  constexpr double kPoseTimeS = 4.0;
  constexpr double kTwistCommandTimeS = 0.2;
  constexpr double kTwistDurationS = 2.0;
  constexpr double kTorsoJt = 0.60; // torso target height,  meter
  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(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS);
      print_result("joint", robot.PublishTarget(target));
      continue;
    }

    if (command == "base_pose") {
      if (ensure_controller(robot, S1ControllerName::SWERVE_CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_swerve_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, S1ControllerName::SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_swerve_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, S1ControllerName::SWERVE_CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS),
          build_swerve_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, S1ControllerName::SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS),
          build_swerve_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 S1 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 torso / 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`
 *    - On S1 this corresponds to the `swerve_chassis` task
 *    - 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(S1ControllerName::SWERVE_CHASSIS_POSE_CTRL)` or
 *      `switch_controller(S1ControllerName::SWERVE_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 = S1JointGroup::SWERVE_CHASSIS;
constexpr const char* kChassisSubtaskPose = "swerve_chassis_pose";
constexpr const char* kChassisSubtaskTwist = "swerve_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_swerve_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 = {S1JointGroup::SWERVE_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_swerve_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 = {S1JointGroup::SWERVE_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_swerve_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 torso target\n"
            << "  base_pose    - request a swerve chassis pose target\n"
            << "  base_twist   - request a swerve chassis twist target with auto stop\n"
            << "  mixed_pose   - request torso + swerve chassis pose in one target\n"
            << "  mixed_twist  - request torso + swerve chassis twist in one target\n"
            << "  quit         - exit example\n"
            << 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::S1);
  if (!robot.init()) {
    std::cerr << "robot init failed" << std::endl;
    return -1;
  }

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

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

  const std::vector<std::string> torso_single_joint = {torso_joint_names.front()};
  constexpr double kJointTimeS = 3.0;
  constexpr double kPoseTimeS = 4.0;
  constexpr double kTwistCommandTimeS = 0.2;
  constexpr double kTwistDurationS = 2.0;
  constexpr double kTorsoJt = 0.60; // torso target height,  meter

  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(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS);
      print_error_info("joint", robot.RequestTarget(target));
      continue;
    }

    if (command == "base_pose") {
      if (ensure_controller(robot, S1ControllerName::SWERVE_CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_swerve_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, S1ControllerName::SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = build_swerve_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, S1ControllerName::SWERVE_CHASSIS_POSE_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS),
          build_swerve_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, S1ControllerName::SWERVE_CHASSIS_TWIST_CTRL) != ControlStatus::SUCCESS) {
        continue;
      }
      const auto target = merge_targets({
          build_joint_target(S1JointGroup::TORSO, torso_single_joint, {kTorsoJt}, kJointTimeS),
          build_swerve_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 base motion, 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::S1);

    // 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::S1);

    // 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 << "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 << "------------------" << std::endl;
    }
}

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

    // 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::S1);

    // 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::S1);

    // 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 ["torso", "head", "left_arm", "right_arm"] (S1: torso replaces G1 leg)
    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::S1);

    // 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 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::S1);

    // 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::S1);

    // Available IMU sensors on S1 (choose one of the following three):
    // - SensorType::HEAD_IMU: Head lidar IMU
    // - SensorType::BACK_IMU: Rear lidar IMU
    // - SensorType::CHASSIS_IMU: Chassis lidar IMU
    // Initialize sensors
    std::unordered_set<SensorType> sensor_types = {
        SensorType::HEAD_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
    std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::HEAD_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 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::S1);

  // 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_depth_data) (get_camera_data)

Applicable Scenarios: Get RGB image and depth image data, used for visual perception, object detection, SLAM and other tasks.

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;
  }
}

void print_depth_data(const std::shared_ptr<DepthData> depth_data_ptr) {
  if (depth_data_ptr == nullptr) {
    std::cout << "depth_data_ptr is nullptr" << std::endl;
    return;
  }

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

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

  if (img && !img->empty()) {
    cv::Mat img_vis;

    // Image information normalization
    cv::normalize(*img, img_vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);

    // Pseudo-color enhancement
    cv::Mat img_color;
    cv::applyColorMap(img_vis, img_color, cv::COLORMAP_JET);

    // save
    cv::imwrite("check_raw_data.png", *img); 
    cv::imwrite("check_visual_view.jpg", img_color); 

    std::cout << "Image saved: \n"
              << "1. check_raw_data.png -> A fully black image containing real physical depth\n"
              << "2. check_visual_view.jpg -> A colorized image where object contours are visible" << std::endl;
    }
}

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

    // Initialize sensors; only cameras and LiDAR sensors passed during initialization can retrieve data
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::HEAD_LEFT_CAMERA,           // Head left camera
        SensorType::LEFT_ARM_DEPTH_CAMERA,      // Left arm depth 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(10000));

    // 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 depth image data
    std::shared_ptr<DepthData> depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);
    if (depth_data) {
        std::cout << "Depth image data retrieved successfully!" << std::endl;
        print_depth_data(depth_data);
    } else {
        std::cerr << "Failed to get depth 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::S1);

  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_depth_data(
    const std::shared_ptr<DepthData>
        depth_data_ptr) {
  if (depth_data_ptr == nullptr) {
    std::cout << "depth_data_ptr is nullptr" << std::endl;
    return;
  }

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

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

  if (img && !img->empty()) {
    cv::Mat img_vis;

    // Image information normalization
    cv::normalize(*img, img_vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);

    // Pseudo-color enhancement
    cv::Mat img_color;
    cv::applyColorMap(img_vis, img_color, cv::COLORMAP_JET);

    // save
    cv::imwrite("check_raw_data.png", *img); 
    cv::imwrite("check_visual_view.jpg", img_color); 

    std::cout << "Image saved: \n"
              << "1. check_raw_data.png -> A fully black image containing real physical depth\n"
              << "2. check_visual_view.jpg -> A colorized image where object contours are visible" << 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::S1);

    // Initialize sensors; only cameras and LiDAR sensors passed during initialization can retrieve data
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::HEAD_LEFT_CAMERA,       // Head left camera
        SensorType::LEFT_ARM_DEPTH_CAMERA,  // Left arm depth 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(10000));

    // 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::LEFT_ARM_DEPTH_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 depth image data
    std::shared_ptr<DepthData> depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);
    if (depth_data) {
        std::cout << "Depth image data retrieved successfully!" << std::endl;
        print_depth_data(depth_data);
    } else {
        std::cerr << "Failed to get depth image data!" << std::endl;
    }

    // Get sensor extrinsics
    std::pair<std::vector<double>, int64_t> sensor_extrinsic = robot.get_sensor_extrinsic(SensorType::LEFT_ARM_DEPTH_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::S1);

    // Initialize sensors; only cameras and LiDAR sensors passed during initialization can retrieve data
    // Available lidar sensors on S1 (choose one of the following three):
    // - SensorType::HEAD_LIDAR: Head lidar
    // - SensorType::BACK_LIDAR: Rear lidar
    // - SensorType::CHASSIS_LIDAR: Chassis lidar
    std::unordered_set<SensorType> sensor_types =  {
        SensorType::HEAD_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::HEAD_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 input in model inference scenarios, supporting RGB camera, depth camera, and joint data.

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::LEFT_ARM_CAMERA:
      return "LEFT_ARM_CAMERA";
    case SensorType::RIGHT_ARM_CAMERA:
      return "RIGHT_ARM_CAMERA";
    case SensorType::LEFT_ARM_DEPTH_CAMERA:
      return "LEFT_ARM_DEPTH_CAMERA";
    case SensorType::RIGHT_ARM_DEPTH_CAMERA:
      return "RIGHT_ARM_DEPTH_CAMERA";
    default:
      return "UNKNOWN_CAMERA";
  }
}

}  // namespace

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

  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  std::unordered_set<SensorType> enable_sensor_set = {
      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
  };
  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::S1);
    auto& motion = GalbotMotion::get_instance(MachineType::S1);

    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
    // FIXME:EXAMPLE[9]: Parameter names contain "leg" (G1), verify S1 parameter names
    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::S1);
    auto& motion = GalbotMotion::get_instance(MachineType::S1);

    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
    // FIXME:EXAMPLE[10]: Parameter names contain "leg" (G1), verify S1 parameter names
    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;
}

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::S1);

  // 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 = S1ControllerName::LEFT_ARM_PVT_CTRL;
  const std::string controller_str = S1ControllerName::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 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::S1);

    // 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;
}

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 <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 = 35.0;
constexpr auto kVideoStreamTimeout = std::chrono::seconds(10);

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> kPresetTorso = {0.58};
const std::vector<double> kPresetHead = {0.0, 0.0};
const std::vector<double> kPresetLeftArm = {2.0, -1.5, -0.6, -1.7, 0.0, -0.7, 0.0};
const std::vector<double> kPresetRightArm = {-2.0, 1.5, 0.6, 1.7, 0.0, 0.7, 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. Torso 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,
    2.0 + 0.07, -1.5 + 0.04, -0.6, -1.7, 0.0, -0.8 + 0.04, 0.0,
    -2.0 - 0.07, 1.5 - 0.04, 0.6, 1.7, 0.0, 0.8 - 0.04, 0.0
  };
  const std::vector<double> pose_yaw_neg = {
    kHeadSwayYawNegRad, 0.0,
    2.0 - 0.05, -1.5 - 0.03, -0.6, -1.7, 0.0, -0.8 - 0.03, 0.0,
    -2.0 + 0.05, 1.5 + 0.03, 0.6, 1.7, 0.0, 0.8 + 0.03, 0.0
  };

  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;
}

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 head_video_stream_ok = false;

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

  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 (S1, HEAD_LEFT_CAMERA enabled)" << std::endl;
  std::this_thread::sleep_for(std::chrono::duration<double>(5.0));

  bool torso_step_ok = false;
  bool upper_step_ok = false;
  bool demo_step_ok = false;

  const ControlStatus torso_set_status =
      robot.set_joint_positions(kPresetTorso, {"torso"}, {}, true, kSpeedRadS, kTimeoutS);
  torso_step_ok = (torso_set_status == ControlStatus::SUCCESS);
  if (!torso_step_ok) {
    std::cerr << "[FAIL] Torso set_joint_positions not SUCCESS (blocking)" << std::endl;
  }
  print_summary("Torso set_joint_positions (blocking)", torso_step_ok);

  if (torso_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: torso 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 = torso_step_ok && upper_step_ok && demo_step_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("Head camera H.264 video stream", head_video_stream_ok);

  const bool installation_all_checks_passed = body_preset_step_passed && 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 Base Pose (set_base_pose)

Applicable Scenarios: Command the mobile base to move to a target pose in the selected reference frame.

examples/cpp/galbot_robot/src/set_base_pose_example.cpp

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

#include "galbot_robot.hpp"

using namespace galbot::sdk;

// Helper function: calculate quaternion
void set_yaw_orientation(Pose& pose, double yaw) {
    const double half_yaw = 0.5 * yaw;
    pose.orientation.x = 0.0;
    pose.orientation.y = 0.0;
    pose.orientation.z = std::sin(half_yaw);
    pose.orientation.w = std::cos(half_yaw);
}

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

    // 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));

    // ========== Test Version 1: Use Pose struct ==========
    std::cout << "\n[Test 1] Set chassis pose using Pose struct..." << std::endl;
    {
        Pose pose;
        pose.position.x = 0.3;    // Move to x = 0.3 m in the odom frame
        pose.position.y = 0.0;    // Target y coordinate
        pose.position.z = 0.0;    // z coordinate (chassis ignored)
        set_yaw_orientation(pose, 0.0);  // Oriented to 0 radians

        if (robot.set_base_pose(pose, true, 15.0) == ControlStatus::SUCCESS) {
            std::cout << "[Test 1] Pose-struct set succeeded, target position reached." << std::endl;
        } else {
            std::cerr << "[Test 1] Pose-struct set failed or timed out." << std::endl;
        }
    }

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

    // ========== Test Version 2: Simplified coordinate version (default 1s motion time) ==========
    std::cout << "\n[Test 2] Use simplified coordinate version (default 1s motion time)..." << std::endl;
    {
        double x = -0.3;          // Move backward by 0.3 m from the current pose
        double y = 0.0;           // Target y coordinate
        double yaw = 0.0;         // Target orientation
        // std::string frame_id = "base_link";
        std::string frame_id = "rel(0)";
        std::string reference_frame_id = "odom";
        bool is_blocking = true;
        double timeout_s = 15.0;

        if (robot.set_base_pose(x, y, yaw, frame_id, reference_frame_id, is_blocking, timeout_s) == ControlStatus::SUCCESS) {
            std::cout << "[Test 2] Simplified-coordinate set succeeded, target position reached." << std::endl;
        } else {
            std::cerr << "[Test 2] Simplified-coordinate set failed or timed out." << std::endl;
        }
    }

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

    // ========== Test Version 3: Full control version (custom motion time) ==========
    std::cout << "\n[Test 3] Use full control version (custom 5s motion time)..." << std::endl;
    {
        double x = 0.0;           // Target x coordinate (return to origin)
        double y = 0.0;           // Target y coordinate
        double yaw = 0.0;         // Target orientation
        // std::string frame_id = "base_link";
        std::string frame_id = "rel(0)";
        std::string reference_frame_id = "odom";
        double time_from_start_s = 5.0;  // Custom 5-second motion (slower and more stable)
        bool is_blocking = true;
        double timeout_s = 15.0;

        if (robot.set_base_pose(x, y, yaw, frame_id, reference_frame_id, time_from_start_s, is_blocking, timeout_s) == ControlStatus::SUCCESS) {
            std::cout << "[Test 3] Full-control set succeeded, target position reached." << std::endl;
        } else {
            std::cerr << "[Test 3] Full-control set failed or timed out." << std::endl;
        }
    }

    std::cout << "\nAll tests completed!" << std::endl;

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

    return 0;
}

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/s1/en/set_config_reference.md

using namespace galbot::sdk;

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

    // 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 and select the short-distance
    // motion mode. omni_plan is G1-only (passing it on S1 returns INVALID_INPUT), so S1
    // uses adjust_motion_type instead -- 2 = rotate + crab-walk, the recommended S1 mode
    // (avoids joint collisions).
    std::vector<ConfigItem> navigation_fields = {
        ConfigItem{"replan_threshold", 30.0},
        ConfigItem{"no_replan_threshold", 1.0},
        ConfigItem{"adjust_motion_type", static_cast<int64_t>(2)},
    };
    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) together with
    // leg trajectory velocity/acceleration/jerk limits. Per-chain fields are keyed as
    // "{field_name}_{chain_name}"; the leg chain has only 1 joint on S1 (5 on G1), so the
    // array length here is S1-specific.
    std::vector<ConfigItem> motion_plan_fields = {
        ConfigItem{"plan_timeout", 5.0},
        ConfigItem{"max_velocity_leg", std::vector<double>{0.8}},
        ConfigItem{"max_acceleration_leg", std::vector<double>{3.0}},
        ConfigItem{"max_jerk_leg", std::vector<double>{10.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 <vector>
#include "galbot_robot.hpp"
using namespace galbot::sdk;
int main() {
  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  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);
  std::cout << "status: " << static_cast<int>(status) << ", fields: " << fields.size() << 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;
}

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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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;
  }

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

  std::map<std::string, std::vector<double>> chain_joints = {
      {"torso", {1.1}},
      {"head", {0.0000, -0.26}},
      {"left_arm", {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
      {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}}};

  std::vector<double> whole_body_joint;
  std::vector<std::string> keys = {"torso", "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);
  } 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;
}

Jacobian Matrix (get_jacobian)

Applicable Scenarios: Calculate the Jacobian matrix for a target chain and frame based on specified joint values. Used for velocity mapping, differential kinematics, and controller debugging.

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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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;
  // Test all kinematic chains; no chain is excluded.
  // torso/leg are virtual chains handled by the server (leg is treated the same as torso).
  if (support_chains.empty()) {
    support_chains = {"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.
  // S1's get_chain_joint_state() skips leg, but leg still accepts a joint value (DOF = 1).
  const std::unordered_map<std::string, size_t> fallback_dof = {{"leg", 1}};

  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;
}

Jacobian Matrix by Robot State (get_jacobian_by_state)

Applicable Scenarios: Calculate the Jacobian matrix from a complete robot state or the current robot state. Suitable for whole-body state analysis and controller verification.

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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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;
  // torso/leg are virtual chains handled by the server (leg is treated the same as torso).
  if (support_chains.empty()) {
    support_chains = {"left_arm", "right_arm", "torso", "leg"};
  }

  const std::string target_frame = "EndEffector";
  const std::string reference_frame = "base_link";

  size_t whole_body_dof = 17;
  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;
}

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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  // 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 <chrono>
#include <iostream>
#include <memory>
#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_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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));

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

  // Get current end-effector pose from robot as target for subsequent IK scenarios
  std::vector<double> target_pose;
  {
    auto [status, pose] = planner.get_end_effector_pose_on_chain(target_chain, target_frame, reference_frame);
    if (status != MotionStatus::SUCCESS || pose.size() != 7) {
      std::cerr << "Failed to get current end-effector pose, cannot continue test." << std::endl;
      return -1;
    }
    target_pose = pose;
    std::cout << "Current " << target_chain << " End-effector pose: ";
    for (double v : target_pose)
      std::cout << v << " ";
    std::cout << std::endl;
    std::cout << "---------------------------------------------------" << std::endl;
  }

  // Scenario 1: Single-chain inverse kinematics (using default parameters)
  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(target_pose, one_chain);
    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(target_pose, 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 (expected to return INVALID_INPUT)
  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(target_pose, 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: Inverse kinematics with initial reference joint values (using current robot state)
  try {
    std::cout << ">> Running Scenario 4: IK test with initial reference values..." << std::endl;
    std::vector<std::string> one_chain = {target_chain};

    auto current_chain_joints = planner.get_chain_joint_state();
    std::cout << "  Current joint states:" << std::endl;
    for (const auto& [name, joints] : current_chain_joints) {
      std::cout << "    [" << name << "]: ";
      for (double v : joints)
        std::cout << v << " ";
      std::cout << std::endl;
    }

    auto res = planner.inverse_kinematics(target_pose, one_chain, target_frame, reference_frame, current_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: Inverse kinematics based on RobotStates (get full state from robot)
  try {
    std::cout << ">> Running Scenario 5: IK test based on RobotStates..." << std::endl;

    auto robot_states = planner.get_robot_states();
    auto ref_state = std::make_shared<RobotStates>(robot_states);
    ref_state->chain_name = target_chain;

    std::cout << "  Number of whole-body joints: " << ref_state->whole_body_joint.size() << std::endl;
    std::cout << "  Number of chassis states: " << ref_state->base_state.size() << std::endl;

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

    auto res = planner.inverse_kinematics_by_state(target_pose, one_chain, target_frame, reference_frame, ref_state,
                                                   false, params);
    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 <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <vector>

#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;
  }
}

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

  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));

  std::string reference_frame = "base_link";
  std::string target_frame = "EndEffector";
  std::string target_chain = "right_arm";
  std::string end_ee_link = "right_arm_end_effector_mount_link";
  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;

    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 ---
  std::vector<double> current_pose;
  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);
    current_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", current_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 (small offset from current pose) ---
  try {
    std::cout << ">> Scenario 3: Setting end-effector pose..." << std::endl;

    if (current_pose.size() != 7) {
      std::cerr << "❌ Unable to get current pose; skipping Scenario 3" << std::endl;
    } else {
      std::vector<double> target_pose = current_pose;
      target_pose[2] -= 0.05;  // Move downward 5 cm in z direction

      print_pose_info("Target pose", target_pose);

      MotionStatus status = planner.set_end_effector_pose(target_pose,      // 1. target_pose
                                                          target_chain,     // 2. chain_name
                                                          reference_frame,  // 3. reference_frame
                                                          nullptr,          // 4. reference_robot_states
                                                          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 << "✅ Motion execution completed" << std::endl;
      }
    }
  } catch (const std::exception& e) {
    std::cerr << "❌ Pose-setting exception: " << e.what() << std::endl;
  }

  // --- Scenario 4: Get end-effector pose after execution to verify result ---
  try {
    std::cout << ">> Scenario 4: Getting end-effector pose after motion..." << std::endl;

    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("After motion", 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() {
    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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 = {
        {"torso",     {0.65}},
        {"head",      {0.0000, -0.26}},
        {"left_arm",  {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
        {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"torso", "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));

    std::unordered_map<std::string, std::vector<double>> chain_pose_baselink = {
        {"torso",     {-0.0715, 0.0000, 1.2044, 0.0000, 0.0000, 0.0000, 1.0000}},
        {"head",      {-0.0123, 0.0046, 1.4975, -0.0002, 0.1298, -0.0001, 0.9915}},
        {"left_arm",  {0.3851, 0.4190, 1.9214, -0.3816, 0.7681, -0.1496, -0.4920}},
        {"right_arm", {0.2819, -0.3212, 1.9315, 0.3701, 0.7731, 0.1567, -0.4907}}
    };

    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];
        target_joint->joint_positions[4] += 0.1; // 若设备处于home位，则target就是当前点会规划轨迹为空，故加offset

        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() {
    auto& planner = GalbotMotion::get_instance(MachineType::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 = {
        {"torso",     {1.1}},
        {"head",      {0.0000, -0.26}},
        {"left_arm",  {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
        {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"torso", "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());
    }

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

    // --- 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.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.2267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.3267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.4267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}
        };

        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 = {
            {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.2267, 0.4342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.3267, 0.6342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991},
            {0.4267, 0.8342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991}
        };

        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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);
    auto& navigation = GalbotNavigation::get_instance(MachineType::S1);

    // 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 = {
        {"torso",     {1.1}},
        {"head",      {0.0000, -0.26}},
        {"left_arm",  {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
        {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}}
    };

    std::vector<double> whole_body_joint;
    std::vector<std::string> keys = {"torso", "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.99995, -1.60004, 0.599905, -1.69994, 0, -0.799924, 0};
    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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 = "GE103v1";

        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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  auto& navigation = GalbotNavigation::get_instance(MachineType::S1);

  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 = {
        {"torso", {1.1}},
        {"head", {0.0000, -0.26}},
        {"left_arm", {-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0}},
        {"right_arm", {0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0}},
    };
    std::vector<double> whole_body_joint;
    for (const auto& key : {"torso", "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.99995, -1.60004, 0.599905, -1.69994, 0.0, -0.799924, 0.0};
    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;
}

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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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.9999, -1.6000, -0.5999, -1.6999, 0.0000, -0.7999, 0.0000};
    std::vector<double> right_arm_joints = {-2.0000, 1.6001, 0.6001, 1.7000, 0.0000, 0.8000, 0.0000};

    // --- 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 (virtual chain, EndEffector maps to torso_base_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 a joint value (server treats leg the same as torso;
    //     S1 leg DOF = 1, so pass exactly one joint value) ---
    try {
        std::cout << "=== Test 14: leg chain / EndEffector / base_link (1 joint) ===" << std::endl;
        std::unordered_map<std::string, std::vector<double>> leg_joint_state = {{"leg", {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;
}

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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    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 double elapsed =
            std::chrono::duration<double>(std::chrono::steady_clock::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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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;
    }

    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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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(S1ControllerName::SWERVE_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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(S1ControllerName::SWERVE_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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(S1ControllerName::SWERVE_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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  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 = {0.5, 0.5, 0.5};       // 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 double elapsed =
            std::chrono::duration<double>(std::chrono::steady_clock::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::S1);
    auto& robot = GalbotRobot::get_instance(MachineType::S1);

    // 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(S1ControllerName::SWERVE_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 obstacle 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::S1);
  auto& robot = GalbotRobot::get_instance(MachineType::S1);

  // 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(S1ControllerName::SWERVE_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 elapsed_ms =
          std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::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 Inference

Applicable Scenarios: Trigger a single stereo depth-estimation inference and retrieve the resulting depth data.

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 <chrono>
#include <iostream>
#include <thread>

#include "galbot_perception.hpp"
#include "galbot_robot.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::S1);
    robot.init();

    auto& perception = GalbotPerception::get_instance(MachineType::S1);
    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 auxiliary 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::S1);

    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/s1/cpp/tutorials. For walkthroughs and safety notes, see Tutorials.

Example 1. Galbot Control Started

Applicable Scenarios: Basic S1 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<iostream>
#include<fstream>
#include<thread>
#include<chrono>
#include "opencv2/opencv.hpp"

#include "galbot_robot.hpp"

using namespace galbot::sdk;

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, {});
    for (auto a: original_pos){
        std::cout << "Current angles of joint group " << a << 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 = robot.set_joint_positions(
            pos, joint_group_names, {}, is_blocking, max_speed, timeout_s);
        if (control_status == ControlStatus::SUCCESS) {
            pos_idx++;
        } else {
            std::cerr << "Failed to set joint positions." << std::endl;
            break;
        }
    }

    /** Restore original joint positions */
    robot.set_joint_positions(original_pos, joint_group_names, {}, is_blocking, max_speed, timeout_s);
    std::cout << "Restored joint positions of joint group " << 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::S1); 

        /* 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;
        }

        /* 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(true, {"torso", "head", "left_arm", "right_arm"});
        if (joint_names.empty()) {
            std::cerr << "No joint names available!" << std::endl;
        } else {
            for (const auto& name : joint_names) {
                std::cout << name << 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"};
        // Heart pose: all angles verified within S1 joint limits (see robot_config.toml)
        std::vector<std::vector<double>> position_seq = {{
              1.00,  0.40, -2.06, -1.80, -2.92, -0.76, -0.03,// left_arm
             -1.00, -0.40,  2.06,  1.80,  2.92,  0.76,  0.03}}; // right_arm
        bool is_blocking = true;
        double max_speed = 0.3;
        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 <chrono>
#include <cctype>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <stdexcept>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>

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

using namespace galbot::sdk;

void print_motion_status(MotionStatus status)
{
    if (status == MotionStatus::SUCCESS) {
        std::cout << "Execution result: SUCCESS, Execution successful\n";
    } else if (status == MotionStatus::TIMEOUT) {
        std::cout << "Execution result: TIMEOUT, Execution timeout\n";
    } else if (status == MotionStatus::FAULT) {
        std::cout << "Execution result: FAULT, Fault occurred, unable to continue execution\n";
    } else if (status == MotionStatus::INVALID_INPUT) {
        std::cout << "Execution result: INVALID_INPUT, Input parameters do not meet requirements\n";
    } else if (status == MotionStatus::INIT_FAILED) {
        std::cout << "Execution result: INIT_FAILED, Internal communication component creation failed\n";
    } else if (status == MotionStatus::IN_PROGRESS) {
        std::cout << "Execution result: IN_PROGRESS, Moving but not in position\n";
    } else if (status == MotionStatus::STOPPED_UNREACHED) {
        std::cout << "Execution result: STOPPED_UNREACHED, Stopped but did not reach target\n";
    } else if (status == MotionStatus::DATA_FETCH_FAILED) {
        std::cout << "Execution result: DATA_FETCH_FAILED, Data acquisition failed\n";
    } else if (status == MotionStatus::PUBLISH_FAIL) {
        std::cout << "Execution result: PUBLISH_FAIL, Data sending failed\n";
    } else if (status == MotionStatus::COMM_DISCONNECTED) {
        std::cout << "Execution result: COMM_DISCONNECTED, Connection failed\n";
    } else {
        std::cout << "Execution result: status=" << static_cast<int>(status) << "\n";
    }
}

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_val(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);

    double qw = clamp_val(q_err[3], -1.0, 1.0);

    return 2.0 * std::acos(qw);
}

struct PoseError
{
    double position_error_norm;
    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
    };
}

void print_pose_error(const char* label, const PoseError& e)
{
    std::cout << label
              << " position_error_norm: " << e.position_error_norm
              << " m, orientation_error_rad: " << e.orientation_error_rad
              << ", orientation_error_deg: " << e.orientation_error_deg << "\n";
}

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\n";

    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(static_cast<unsigned char>(key)) == 'y') {
            std::cout << "User confirmed, continuing execution...\n\n";
            break;
        }
        if (std::tolower(static_cast<unsigned char>(key)) == 'n') {
            std::cout << "User not confirmed, program exiting...\n\n";
            std::exit(1);
        }
        std::cout << "Input error, please enter 'y' or 'n'\n\n";
    }
}

void print_pose_line(const char* prefix, const std::vector<double>& pose)
{
    std::cout << prefix;
    for (double v : pose) {
        std::cout << v << " ";
    }
    std::cout << "\n";
}

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

        struct SdkCleanup {
            GalbotRobot& r;
            explicit SdkCleanup(GalbotRobot& ref) : r(ref) {}
            ~SdkCleanup()
            {
                r.request_shutdown();
                r.wait_for_shutdown();
                r.destroy();
            }
        } cleanup{robot};

        if (motion.init()) {
            std::cout << "GalbotMotion initialization successful\n";
        } else {
            std::cout << "GalbotMotion initialization failed\n";
        }
        if (robot.init()) {
            std::cout << "GalbotRobot initialization successful\n";
        } else {
            std::cout << "GalbotRobot initialization failed\n";
        }

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

        MotionStatus zero_status = motion.move_whole_body_joint_zero(
            true, 0.2, 15.0, std::make_shared<Parameter>());
        print_motion_status(zero_status);
        if (zero_status != MotionStatus::SUCCESS) {
            std::cout << "❌ move_whole_body_joint_zero failed, stop demo to avoid risky posture.\n";
            return 0;
        }
        std::cout << "✅ whole-body zero completed, start arm manipulation demo from safe pose.\n";

        const std::string target_frame = "EndEffector";
        const std::string reference_frame = "base_link";
        const std::string target_chain = "left_arm";
        const std::string end_link = "left_arm_end_effector_mount_link";

        std::vector<double> original_pose;
        {
            auto ret_pose = motion.get_end_effector_pose_on_chain(
                target_chain, target_frame, reference_frame);
            if (std::get<0>(ret_pose) != MotionStatus::SUCCESS) {
                std::cerr << "❌ Failed to get end-effector pose\n";
                return 0;
            }
            original_pose = std::get<1>(ret_pose);
            std::cout << "✅ Current " << target_chain << " end-effector pose: ";
            print_pose_line("", original_pose);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(800));

        std::vector<double> target_pose = {
            original_pose[0] + 0.04,
            original_pose[1],
            original_pose[2] + 0.02,
            original_pose[3],
            original_pose[4],
            original_pose[5],
            original_pose[6]
        };
        std::cout << "✅ S1 target pose (relative to zero pose): ";
        print_pose_line("", target_pose);

        std::unordered_map<std::string, std::vector<double>> joint_angles_ik;
        {
            const bool enable_collision_check = false;
            auto ret_ik = motion.inverse_kinematics(
                target_pose,
                {target_chain},
                target_frame,
                reference_frame,
                {},
                enable_collision_check,
                std::make_shared<Parameter>());
            if (std::get<0>(ret_ik) != MotionStatus::SUCCESS) {
                std::cerr << "❌ IK solving failed\n";
                return 0;
            }
            joint_angles_ik = std::get<1>(ret_ik);
            std::cout << "✅ Target " << target_chain << " IK solving successful joint_angles_ik: ";
            print_pose_line("", joint_angles_ik[target_chain]);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));

        ControlStatus set_joints_status = robot.set_joint_positions(
            joint_angles_ik[target_chain],
            {target_chain},
            {},
            true,
            0.1,
            20.0);
        if (set_joints_status != ControlStatus::SUCCESS) {
            std::cerr << "❌ Setting joint group angles failed\n";
            return 0;
        }
        std::cout << "✅ Setting " << target_chain << " joint group angles successful.\n";
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));

        {
            auto ret_verify = motion.get_end_effector_pose_on_chain(
                target_chain, target_frame, reference_frame);
            if (std::get<0>(ret_verify) != MotionStatus::SUCCESS) {
                std::cerr << "❌ Failed to get end-effector pose after set joints\n";
                return 0;
            }
            const std::vector<double>& tgt_pose_ik = std::get<1>(ret_verify);
            std::cout << "✅ Getting " << target_chain << " end-effector pose successful: ";
            print_pose_line("", tgt_pose_ik);
            PoseError err = calculate_error(tgt_pose_ik, target_pose);
            print_pose_error("End-effector pose error:", err);
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));

        {
            const bool enable_collision_check = false;
            auto ret_fk = motion.forward_kinematics(
                end_link,
                reference_frame,
                joint_angles_ik,
                std::make_shared<Parameter>());
            if (std::get<0>(ret_fk) != MotionStatus::SUCCESS) {
                std::cerr << "❌ FK solving failed\n";
                return 0;
            }
            const std::vector<double>& tgt_pose_fk = std::get<1>(ret_fk);
            std::cout << "✅ Target " << target_chain << " FK solving successful: ";
            print_pose_line("", tgt_pose_fk);
            PoseError err = calculate_error(tgt_pose_fk, target_pose);
            print_pose_error("FK solving error:", err);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(3000));
        std::cout << "\n";

        {
            const bool enable_collision_check = false;
            MotionStatus ret_restore = motion.set_end_effector_pose(
                original_pose,
                target_chain,
                reference_frame,
                nullptr,
                enable_collision_check,
                true,
                5.0,
                std::make_shared<Parameter>());
            if (ret_restore != MotionStatus::SUCCESS) {
                std::cerr << "❌ Setting end-effector pose failed\n";
                return 0;
            }
            std::cout << "✅ Setting end-effector pose successful: status=" << static_cast<int>(ret_restore) << "\n";
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));

        {
            auto ret_final = motion.get_end_effector_pose_on_chain(
                target_chain, target_frame, reference_frame);
            if (std::get<0>(ret_final) != MotionStatus::SUCCESS) {
                std::cerr << "❌ Failed to get end-effector pose after restore\n";
                return 0;
            }
            const std::vector<double>& original_pose_rec = std::get<1>(ret_final);
            std::cout << "✅ Getting " << target_chain << " end-effector pose successful: ";
            print_pose_line("", original_pose_rec);
            PoseError err = calculate_error(original_pose_rec, original_pose);
            print_pose_error("Restore end-effector pose error:", err);
        }
    } catch (const std::exception& e) {
        std::cerr << "❌ Main program exception: " << e.what() << "\n";
        return 1;
    }

    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::S1); 
        auto& navi = GalbotNavigation::get_instance(MachineType::S1);        

        /* 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 camera, depth, lidar, IMU, and other robot 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;
}

std::vector<Point3D> depth_rgb_to_pointcloud(
    const std::shared_ptr<cv::Mat>& depth_img,
    const std::shared_ptr<cv::Mat>& rgb_img,
    float fx,
    float fy,
    float cx,
    float cy,
    float depth_scale = 0.1){
    
    std::vector<Point3D> points;
    std::cout << "depth_img->type: " << depth_img->type() << std::endl;

    if (depth_img->empty() || rgb_img->empty() || depth_img->rows != rgb_img->rows || depth_img->cols != rgb_img->cols) {
        std::cerr << "Error: depth image or rgb image is empty, or their sizes do not match." << std::endl;
        return points;
    } 
    std::cout << "depth_img->rows: " << depth_img->rows << std::endl;
    std::cout << "depth_img->cols: " << depth_img->cols << std::endl;
    std::cout << "fx: " << fx << std::endl;
    std::cout << "fy: " << fy << std::endl;
    std::cout << "cx: " << cx << std::endl;
    std::cout << "cy: " << cy << std::endl;
    std::cout << "depth_scale: " << depth_scale << std::endl;
    
    points.reserve(depth_img->rows * depth_img->cols);

    /** Handle different depth image types */
    int depth_type = depth_img->type();
    for (int v = 0; v < depth_img->rows; ++v) {
        for (int u = 0; u < depth_img->cols; ++u) {
            float depth = 0.0f;

            if (depth_type == CV_16UC1) {
                /** 16-bit unsigned single channel */
                uint16_t raw_depth = depth_img->at<uint16_t>(v, u);
                depth = static_cast<float>(raw_depth) * depth_scale;
            } else if (depth_type == CV_32F) {
                /** 32-bit float single channel */
                depth = depth_img->at<float>(v, u) * depth_scale;
            } else {
                std::cerr << "Error: unsupported depth image type: " << depth_type << std::endl;
                return points;
            }

            if (depth <= 0.0f) continue;
            float x = (u - cx) * depth / fx;
            float y = (v - cy) * depth / fy;
            uint8_t rgb = rgb_img->at<cv::Vec3b>(v, u)[0];

            points.push_back(Point3D{x, y, depth, rgb});
        }
    }
    return points;
}

int main(){
    check_robot_safety();
    try{
        /* Get robot instance  */
        auto& robot = GalbotRobot::get_instance(MachineType::S1); 
        
        /** Get RGB and depth images from the left arm, depth images from the right arm,  
            base LiDAR data, and torso IMU data */
        std::unordered_set<SensorType> sensor_types = {
            SensorType::LEFT_ARM_CAMERA,        /** Left arm rgb camera */
            SensorType::LEFT_ARM_DEPTH_CAMERA,  /** Left arm depth camera */
            SensorType::BACK_LIDAR,              /** BACK_LIDAR */
            SensorType::BACK_IMU              /** BACK_IMU*/
        };

        /* Initialize robot */
        if (robot.init(sensor_types)) {
            std::cout << "Initialization successful" << std::endl;
        }else{
            std::cerr << "Initialization failed" << std::endl;
            return -1;
        }

        /* 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 depth image */
        std::shared_ptr<DepthData> depth_data = robot.get_depth_data(SensorType::LEFT_ARM_DEPTH_CAMERA);
        std::shared_ptr<cv::Mat> depth_img;
        if (depth_data) {
            std::cout << "Get depth image suceess" << std::endl;
            depth_img = depth_data->convert_to_cv2_mat();

            cv::imwrite("depth_image.png", *depth_img);
            std::cout << "Depth image saved as 'depth_image.png'" << std::endl;
        }else{
            std::cerr << "No depth image data!" << std::endl;
        }

        /** Get lidar data */
        std::shared_ptr<LidarData> lidar_data = robot.get_lidar_data(SensorType::BACK_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 back imu data */
        std::shared_ptr<ImuData> imu_data = robot.get_imu_data(SensorType::BACK_IMU);
        if (imu_data) {
            std::cout << "Get imu data suceess" << std::endl;
        } else {
            std::cerr << "No imu data!" << std::endl;
        }
        
        if (rgb_img && depth_img) {
            /** Set default camera intrinsics */
            std::shared_ptr<CameraInfo> camera_info = std::make_shared<CameraInfo>();
            camera_info->height = 720;
            camera_info->width = 1280;
            camera_info->k = {
                653.4349365234375, 0.0, 639.95159912109375, 
                0.0, 652.48858642578125, 365.29425048828125, 
                0.0, 0.0, 1.0
            };

            std::vector<Point3D> fusion_pcd = depth_rgb_to_pointcloud(
                depth_img,
                rgb_img,
                camera_info->k[0],
                camera_info->k[4],
                camera_info->k[2],
                camera_info->k[5],
                0.1);
            save_xyz_to_pcd(fusion_pcd, "fusion_pcd.pcd");
        }

        /** 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: Convert simulated VLA outputs into safe coordinated robot joint commands.

examples/cpp/tutorials/src/example5_execute_vla.cpp

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

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

using namespace galbot::sdk;

namespace {
constexpr size_t kExpectedS1JointCount = 17;  // torso(1)+head(2)+left_arm(7)+right_arm(7)

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

std::vector<std::string> buildOrderedGroups(
    const std::map<std::string, std::vector<std::vector<double>>>& jointPositions) {
  std::vector<std::string> orderedGroups;
  for (const auto& group : kS1GroupOrder) {
    if (jointPositions.find(group) == jointPositions.end()) {
      throw std::runtime_error("Missing trajectory group: " + group);
    }
    orderedGroups.push_back(group);
  }
  return orderedGroups;
}

std::vector<std::vector<double>> mergeTrajectoryByGroupOrder(
    const std::map<std::string, std::vector<std::vector<double>>>& jointPositions,
    const std::vector<std::string>& orderedGroups) {
  if (orderedGroups.empty()) {
    return {};
  }

  const auto& firstGroupTraj = jointPositions.at(orderedGroups.front());
  if (firstGroupTraj.empty()) {
    return {};
  }

  std::vector<std::vector<double>> mergedTraj = firstGroupTraj;
  for (size_t groupIdx = 1; groupIdx < orderedGroups.size(); ++groupIdx) {
    const auto& groupTraj = jointPositions.at(orderedGroups[groupIdx]);
    if (groupTraj.size() != mergedTraj.size()) {
      throw std::runtime_error("Trajectory point count mismatch in group: " + orderedGroups[groupIdx]);
    }
    for (size_t pointIdx = 0; pointIdx < mergedTraj.size(); ++pointIdx) {
      mergedTraj[pointIdx].insert(mergedTraj[pointIdx].end(), groupTraj[pointIdx].begin(), groupTraj[pointIdx].end());
    }
  }
  return mergedTraj;
}

void printJointGroupSummary(const std::vector<std::string>& jointGroups, const std::vector<JointState>& jointStates) {
  std::cout << "Joint groups to execute: [";
  for (const auto& group : jointGroups) {
    std::cout << group << " ";
  }
  std::cout << "]" << std::endl;
  std::cout << "Joint states size before execution: " << jointStates.size() << std::endl;
}

double computeAbsPositionErrorNorm(const std::vector<double>& target, const std::vector<JointState>& measured) {
  const size_t n = std::min(target.size(), measured.size());
  double errorNorm = 0.0;
  for (size_t i = 0; i < n; ++i) {
    errorNorm += std::abs(target[i] - measured[i].position);
  }
  return errorNorm;
}
}  // namespace

/**
 * Generate target point for trajectory
 */
TrajectoryPoint generateTargetPoint(const std::vector<double>& q, double targetTime = 10.0) {
  TrajectoryPoint jointPosition;
  jointPosition.time_from_start_second = targetTime;

  for (double joint : q) {
    JointCommand jointCmd;
    jointCmd.position = joint;
    jointPosition.joint_command_vec.push_back(jointCmd);
  }

  return jointPosition;
}

/**
 * Generate trajectory for joints
 */
std::shared_ptr<Trajectory> generateTargetTrajectory(const std::vector<std::vector<double>>& trajectory,
                                                     const std::vector<std::string>& jointGroups,
                                                     const std::vector<std::string>& jointNames, double dt = 0.008) {
  if (trajectory.empty()) {
    return nullptr;
  }

  auto traj = std::make_shared<Trajectory>();
  traj->joint_groups = jointGroups;
  traj->joint_names = jointNames;

  double currentTime = 0.0;
  for (const auto& state : trajectory) {
    currentTime += dt;
    TrajectoryPoint trajPoint = generateTargetPoint(state, currentTime);
    traj->points.push_back(trajPoint);
  }

  return traj;
}

/**
 * Print joint states
 */
void printJointStates(const std::vector<JointState>& jointStates) {
  for (const auto& js : jointStates) {
    std::cout << " : position = " << js.position << " , velocity = " << js.velocity
              << " , acceleration = " << js.acceleration << " , effort = " << js.effort << " , current = " << js.current
              << std::endl;
  }
}

/**
 * Fake VLA (Vision-Language-Action) model implementation
 * Generate a small, safe whole-body trajectory based on S1 official zero pose
 */
std::map<std::string, std::vector<std::vector<double>>> fakeVla(
    const std::unordered_map<SensorType, std::shared_ptr<cv::Mat>>& rgbDataDict) {
  std::cout << "Fake VLA executing..." << std::endl;

  const int numPoints = 200;
  std::map<std::string, std::vector<std::vector<double>>> result;

  // S1 zero pose (kept consistent with GalbotMotionS1::move_whole_body_joint_zero)
  const std::vector<double> torso_zero{1.1};
  const std::vector<double> head_zero{0.0, -0.26};
  const std::vector<double> left_arm_zero{-0.47, -0.94, -0.54, -1.92, 0.2, 0.0, 0.0};
  const std::vector<double> right_arm_zero{0.47, 0.94, 0.54, 1.92, -0.2, 0.0, 0.0};

  // Torso: Move linearly from 1.1 to 0.7
  std::vector<std::vector<double>> torsoTraj(numPoints, std::vector<double>(1));
  const double torso_start = 1.1;
  const double torso_end = 0.7;
  for (int i = 0; i < numPoints; ++i) {
    double s = static_cast<double>(i) / (numPoints - 1);  // 0 -> 1
    torsoTraj[i][0] = torso_start + (torso_end - torso_start) * s;
  }

  // Head: Apply small sinusoidal motion on the pitch joint
  std::vector<std::vector<double>> headTraj(numPoints, std::vector<double>(2));
  for (int i = 0; i < numPoints; ++i) {
    double alpha = std::sin(M_PI * i / (numPoints - 1));  // 0 -> 1 -> 0
    headTraj[i][0] = head_zero[0];
    headTraj[i][1] = head_zero[1] + 0.1 * alpha;  // Slightly oscillate around -0.26
  }

  // Arm: Start from zero pose and apply small linear motion on joint 4 for each arm
  auto makeArmTraj = [numPoints](const std::vector<double>& zero, const std::vector<double>& delta) {
    std::vector<std::vector<double>> traj(numPoints, std::vector<double>(zero.size()));
    for (int i = 0; i < numPoints; ++i) {
      double s = static_cast<double>(i) / (numPoints - 1);  // 0 -> 1
      for (size_t j = 0; j < zero.size(); ++j) {
        traj[i][j] = zero[j] + delta[j] * s;
      }
    }
    return traj;
  };

  std::vector<double> left_delta(left_arm_zero.size(), 0.0);
  std::vector<double> right_delta(right_arm_zero.size(), 0.0);
  // Example: make only small motions near the elbow joint; reduce the amplitude further for more conservative behavior
  if (left_delta.size() > 3) {
    left_delta[3] = 0.2;
  }
  if (right_delta.size() > 3) {
    right_delta[3] = -0.2;
  }

  std::vector<std::vector<double>> leftArmTraj = makeArmTraj(left_arm_zero, left_delta);
  std::vector<std::vector<double>> rightArmTraj = makeArmTraj(right_arm_zero, right_delta);

  result["torso"] = torsoTraj;
  result["head"] = headTraj;
  result["left_arm"] = leftArmTraj;
  result["right_arm"] = rightArmTraj;

  return result;
}

std::map<std::string, std::vector<std::vector<double>>> estimate_vla(
    GalbotRobot& robot, const std::unordered_set<SensorType>& enable_sensor_set) {
  /** Get RGB images from enabled cameras */
  std::unordered_map<SensorType, std::shared_ptr<cv::Mat>> rgb_images;
  for (const auto& sensor_type : enable_sensor_set) {
    if (sensor_type == SensorType::LEFT_ARM_CAMERA || sensor_type == SensorType::RIGHT_ARM_CAMERA) {
      std::shared_ptr<RgbData> rgb_data = robot.get_rgb_data(sensor_type, RgbOutputFormat::JPEG, true);
      if (rgb_data) {
        std::shared_ptr<cv::Mat> rgb_image = rgb_data->convert_to_cv2_mat();
        rgb_images[sensor_type] = rgb_image;
      } else {
        std::cerr << "Failed to get RGB data. " << std::endl;
      }
    }
  }
  std::cout << "RGB images size: " << rgb_images.size() << std::endl;

  /** Estimate VLA */
  std::map<std::string, std::vector<std::vector<double>>> vla = fakeVla(rgb_images);
  return vla;
}

/* @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::S1);
    auto& motion = GalbotMotion::get_instance(MachineType::S1);
    auto& navigation = GalbotNavigation::get_instance(MachineType::S1);

    /** Enable sensor type - S1 only has arm cameras, not head cameras */
    std::unordered_set<SensorType> enable_sensor_set = {SensorType::LEFT_ARM_CAMERA, SensorType::RIGHT_ARM_CAMERA};

    /* Initialize robot */
    if (robot.init(enable_sensor_set)) {
      std::cout << "Initialization successful" << std::endl;
      std::cout << "Is robot running: " << robot.is_running() << std::endl;
    } else {
      std::cerr << "Initialization failed" << std::endl;
    }
    if (!motion.init()) {
      std::cerr << "Motion initialization failed" << std::endl;
    } else {
      std::cout << "Motion initialization successful" << std::endl;
    }
    if (!navigation.init()) {
      std::cerr << "Navigation initialization failed" << std::endl;
    } else {
      std::cout << "Navigation initialization successful" << std::endl;
    }

    /* Wait for data preparation */
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    /** Estimate VLA actions */
    std::map<std::string, std::vector<std::vector<double>>> joint_positions = estimate_vla(robot, enable_sensor_set);

    // Generate target trajectory with fixed group order to avoid map-order ambiguity.
    std::vector<std::string> jointGroups = buildOrderedGroups(joint_positions);
    std::vector<std::vector<double>> jointTraj = mergeTrajectoryByGroupOrder(joint_positions, jointGroups);
    if (jointTraj.empty()) {
      throw std::runtime_error("Merged trajectory is empty.");
    }

    // Final joint position check
    std::vector<double> wholeBodyJoint = jointTraj.back();
    if (wholeBodyJoint.size() != kExpectedS1JointCount) {
      throw std::runtime_error("Invalid joint dimension: expected 17, got " + std::to_string(wholeBodyJoint.size()));
    }
    std::vector<double> baseState = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};

    RobotStates checkState;
    checkState.whole_body_joint = wholeBodyJoint;
    checkState.base_state = baseState;

    std::cout << "✅ Final joint position check state: [";
    for (double val : wholeBodyJoint)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    // Check collision
    // std::tuple<MotionStatus, std::vector<bool>> collisionResult =
    // motion.check_collision(std::vector<std::shared_ptr<RobotStates>>{std::make_shared<RobotStates>(checkState)},
    // true); MotionStatus status = std::get<0>(collisionResult); std::vector<bool> collisionRes =
    // std::get<1>(collisionResult);
    auto [status, collisionRes] = motion.check_collision(
        std::vector<std::shared_ptr<RobotStates>>{std::make_shared<RobotStates>(checkState)}, true);
    std::this_thread::sleep_for(std::chrono::seconds(1));

    if (status == MotionStatus::SUCCESS) {
      std::cout << "✅ OK: collision check finished: " << collisionRes[0] << " (False=no collision)" << std::endl;

      auto beforeJointStates = robot.get_joint_states(jointGroups, {});
      printJointGroupSummary(jointGroups, beforeJointStates);
      printJointStates(beforeJointStates);
      std::cout << "Absolute position error norm before execution: "
                << computeAbsPositionErrorNorm(wholeBodyJoint, beforeJointStates) << std::endl;

      // Execute trajectory
      auto trajectory = generateTargetTrajectory(jointTraj, jointGroups, {});
      if (trajectory != nullptr) {
        ControlStatus execStatus = robot.execute_joint_trajectory(*trajectory, true);
        std::this_thread::sleep_for(std::chrono::seconds(1));

        if (execStatus == ControlStatus::SUCCESS) {
          std::cout << "✅ Joint trajectory execution successful." << std::endl;
        } else {
          std::cout << "❌ Joint trajectory execution failed." << std::endl;
          std::cout << "Diagnostic: target points = " << trajectory->points.size()
                    << ", joint dimension = " << wholeBodyJoint.size() << std::endl;
        }

        // Check joint state
        auto jointStates = robot.get_joint_states(jointGroups, {});
        printJointStates(jointStates);
        std::cout << "Absolute position error norm after execution: "
                  << computeAbsPositionErrorNorm(wholeBodyJoint, jointStates) << std::endl;
        std::cout << "✅ Final joint position check state after execution" << std::endl;
      } else {
        std::cout << "❌ Generated trajectory is invalid, cannot execute." << std::endl;
      }
    } else {
      std::cout << "❌ Collision check failed, will not execute the joint trajectory." << 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 6. Integrated Task: Navigation + Perception + Grasping + Placement

Applicable Scenarios: Integrate navigation, perception, grasping, and placement into a complete autonomous task.

examples/cpp/tutorials/src/example6_pick_and_place.cpp

#include <array>
#include <chrono>
#include <cmath>
#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;

namespace {
// Demo mode: do not perform real chassis navigation to avoid on-site collision risk (only print simulated steps)
constexpr bool kDemoMockNavigationOnly = true;

// Resolve concrete names at runtime so both S1 V1 and S1 V2 are supported.
const std::vector<std::string> kS1BodyJointGroupsForSnapshot = {
    S1JointGroup::TORSO, S1JointGroup::HEAD, S1JointGroup::LEFT_ARM, S1JointGroup::RIGHT_ARM};
constexpr std::size_t kS1BodyJointCount = 17;

void mock_navigation_log(const char* step_description) {
  std::cout << "[Mock navigation] " << step_description << "(navigate_to_goal is not called, and the chassis does not move)" << std::endl;
}
}  // namespace

// 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;
  }

  // Create quaternion from rotation matrix
  static QuaternionST from_matrix(const std::array<std::array<double, 3>, 3>& mat) {
    double trace = mat[0][0] + mat[1][1] + mat[2][2];
    QuaternionST q;

    if (trace > 0) {
      double s = 0.5 / std::sqrt(trace + 1.0);
      q.w = 0.25 / s;
      q.x = (mat[2][1] - mat[1][2]) * s;
      q.y = (mat[0][2] - mat[2][0]) * s;
      q.z = (mat[1][0] - mat[0][1]) * s;
    } else if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) {
      double s = 2.0 * std::sqrt(1.0 + mat[0][0] - mat[1][1] - mat[2][2]);
      q.w = (mat[2][1] - mat[1][2]) / s;
      q.x = 0.25 * s;
      q.y = (mat[0][1] + mat[1][0]) / s;
      q.z = (mat[0][2] + mat[2][0]) / s;
    } else if (mat[1][1] > mat[2][2]) {
      double s = 2.0 * std::sqrt(1.0 + mat[1][1] - mat[0][0] - mat[2][2]);
      q.w = (mat[0][2] - mat[2][0]) / s;
      q.x = (mat[0][1] + mat[1][0]) / s;
      q.y = 0.25 * s;
      q.z = (mat[1][2] + mat[2][1]) / s;
    } else {
      double s = 2.0 * std::sqrt(1.0 + mat[2][2] - mat[0][0] - mat[1][1]);
      q.w = (mat[1][0] - mat[0][1]) / s;
      q.x = (mat[0][2] + mat[2][0]) / s;
      q.y = (mat[1][2] + mat[2][1]) / s;
      q.z = 0.25 * s;
    }

    return q;
  }
};

// 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];
  }

  // Matrix multiplication
  Transform operator*(const Transform& other) const {
    Transform result;
    for (int i = 0; i < 4; ++i) {
      for (int j = 0; j < 4; ++j) {
        result.mat[i][j] = 0;
        for (int k = 0; k < 4; ++k) {
          result.mat[i][j] += mat[i][k] * other.mat[k][j];
        }
      }
    }
    return result;
  }

  // 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;
  }

  // Get rotation as quaternion
  QuaternionST get_rotation() const {
    std::array<std::array<double, 3>, 3> rot;
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        rot[i][j] = mat[i][j];
      }
    }
    return QuaternionST::from_matrix(rot);
  }

  // Get translation
  std::array<double, 3> get_translation() const { return {mat[0][3], mat[1][3], mat[2][3]}; }
};

cv::Mat decode_rgb_image(const std::vector<uint8_t>& image_data) {
  cv::Mat nparr(1, image_data.size(), CV_8UC1, (void*) image_data.data());
  cv::Mat img = cv::imdecode(nparr, cv::IMREAD_COLOR);

  if (img.empty()) {
    throw std::runtime_error("Failed to decode RGB Image");
  }
  return img;
}

cv::Mat decode_depth_image(const std::vector<uint8_t>& image_data, float depth_scale, int height = 720,
                           int width = 1280) {
  cv::Mat depth_img(height, width, CV_16UC1);
  std::memcpy(depth_img.data, image_data.data(), image_data.size());

  cv::Mat depth_img_float;
  depth_img.convertTo(depth_img_float, CV_32FC1, 1.0 / depth_scale);

  return depth_img_float;
}

std::vector<double> get_navigation_pose(const std::vector<double>& object_goal_pose, GalbotMotion& motion,
                                        const std::string& arm = "left_arm") {
  if (arm != "left_arm" && arm != "right_arm") {
    throw std::invalid_argument("arm must be left_arm or right_arm");
  }

  try {
    // FIXME:EXAMPLE[8]: Verify S1 end effector link name
    auto [status, ee_pose_in_base] =
        motion.get_end_effector_pose(arm + "_end_effector_mount_link",  // FIXME[8]: verify link name for S1
                                     "base_link");

    double offset_y;
    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to get end effector pose: status=" << (int) status << std::endl;
      offset_y = 0.0;  // Use default if failed
    } else {
      std::cout << "✅ Successfully got end effector pose" << std::endl;
      offset_y = 0.3;
    }

    // Create transformation matrix for base goal pose
    Transform base_goal_pose_mat;
    QuaternionST obj_quat(object_goal_pose[3], object_goal_pose[4], object_goal_pose[5], object_goal_pose[6]);
    base_goal_pose_mat.set_rotation(obj_quat);
    base_goal_pose_mat.set_translation({object_goal_pose[0], object_goal_pose[1], 0.0});

    // Apply offset: move 0.6m backward and offset_y in y direction
    Transform offset_mat;
    offset_mat.set_translation({-0.6, -offset_y, 0.0});

    base_goal_pose_mat = base_goal_pose_mat * offset_mat;

    // Extract pose
    auto pos = base_goal_pose_mat.get_translation();
    auto quat = base_goal_pose_mat.get_rotation();

    return {pos[0], pos[1], pos[2], quat.x, quat.y, quat.z, quat.w};

  } catch (const std::exception& e) {
    std::cout << "Failed to get navigation target pose: " << e.what() << std::endl;
    return {1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0};
  }
}

// Real navigation (not called in the demo flow by default to avoid chassis motion)
void navigation_to_goal(GalbotNavigation& nav, const std::vector<double>& goal_pose, int retry_cnt = 3) {
  if (kDemoMockNavigationOnly) {
    mock_navigation_log("navigate_to_goal skipped");
    (void) nav;
    (void) goal_pose;
    (void) retry_cnt;
    return;
  }
  try {
    auto cur_pose = nav.get_current_pose();
    std::cout << "Current pose: [";
    std::cout << "]" << std::endl;

    if (nav.check_path_reachability(goal_pose, cur_pose)) {
      retry_cnt = 3;
      NavigationStatus status;

      while (true) {
        status = nav.navigate_to_goal(goal_pose, true, true, 20.0);
        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;
  }
}

// GalbotMotionS1::move_whole_body_joint_zero: joint, base end effector z
// Move downward slightly (for camera to look down at table). downward_delta_m: distance to move down from current end-effector height (meters); adjust smaller/larger on-site as needed.
void lower_camera_after_joint_zero(GalbotMotion& motion, const std::string& target_chain,
                                   const std::string& reference_frame, double downward_delta_m = 0.08) {
  try {
    std::cout << "[Camera pose] First run move_whole_body_joint_zero (S1 zero pose consistent with SDK), then move downward relative to end effector by Δz="
              << downward_delta_m << " m" << std::endl;

    auto zero_status = motion.move_whole_body_joint_zero(true, 0.2, 15.0, std::make_shared<Parameter>());
    if (zero_status != MotionStatus::SUCCESS) {
      std::cout << "⚠️ move_whole_body_joint_zero not successful: status=" << static_cast<int>(zero_status)
                << ", still trying to read the end and fine-tune …" << std::endl;
    } else {
      std::cout << "✅ move_whole_body_joint_zero completed successfully" << std::endl;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(800));

    int retry_cnt = 3;
    std::vector<double> cur_ee_pose;
    MotionStatus status;

    while (true) {
      std::tie(status, cur_ee_pose) =
          motion.get_end_effector_pose_on_chain(target_chain, "EndEffector", reference_frame);

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

      if (status == MotionStatus::SUCCESS) {
        std::cout << "✅ Successfully got end effector pose (after zero)" << std::endl;
        break;
      }
      if (retry_cnt < 0) {
        cur_ee_pose = {0.1267, 0.2342, 0.7356, 0.0220, 0.0127, 0.0343, 0.9991};
        std::cout << "❌ Failed to get end effector pose, using default" << std::endl;
        break;
      }
      std::cout << "Failed to get end effector pose: status=" << static_cast<int>(status) << ", retrying: " << retry_cnt
                << std::endl;
    }

    std::vector<double> tgt_ee_pose = cur_ee_pose;
    // base_link: +z is upward; from top-down view, decrease z to move downward
    tgt_ee_pose[2] -= downward_delta_m;
    // Avoid lowering too much at once (can be adjusted according to the on-site workbench height)
    constexpr double kMinEeZBaseLink = 0.15;
    if (tgt_ee_pose[2] < kMinEeZBaseLink) {
      std::cout << "⚠️ Target z=" << tgt_ee_pose[2] << " Below the safety lower limit " << kMinEeZBaseLink << ", clamped" << std::endl;
      tgt_ee_pose[2] = kMinEeZBaseLink;
    }

    std::cout << "Target end effector pose (after downward fine-tuning): [";
    for (auto val : tgt_ee_pose)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    retry_cnt = 3;
    while (true) {
      status = motion.set_end_effector_pose(tgt_ee_pose, target_chain, reference_frame, nullptr, false, true, 5.0,
                                            std::make_shared<Parameter>());

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

      if (status == MotionStatus::SUCCESS) {
        std::cout << "✅ Successfully set end effector pose: status=" << static_cast<int>(status) << std::endl;
        break;
      }
      if (retry_cnt < 0) {
        std::cout << "❌ Failed to set end effector pose after retries: status=" << static_cast<int>(status)
                  << std::endl;
        break;
      }
      std::cout << "Failed to set end effector pose: status=" << static_cast<int>(status) << ", retrying: " << retry_cnt
                << std::endl;
    }
  } catch (const std::exception& e) {
    std::cout << "❌ Exception in lower_camera_after_joint_zero: " << e.what() << std::endl;
  }
}

std::vector<double> detect_target(const cv::Mat& img, const cv::Mat& depth_img) {
  try {
    // NOTE: This is a placeholder function
    // In real-world scenario, implement actual computer vision detection

    // OpenCV frame: x right, y down, z forward
    std::vector<double> default_pose = {0.0, 0.20, 0.29, 0.0, 0.71, 0.0, 0.71};

    return default_pose;
  } catch (const std::exception& e) {
    std::cout << "Target detection exception: " << e.what() << std::endl;
    return {};
  }
}

std::vector<double> pose_camera_to_base(GalbotRobot& robot, const std::vector<double>& pose_camera) {
  std::string source_frame = "left_arm_camera_color_optical_frame";
  std::string target_frame = "base_link";

  auto [base_to_cam, success] = robot.get_transform(target_frame, source_frame);

  if (!success || base_to_cam.empty()) {
    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::string& arm = "left_arm") {
  std::vector<double> object_pose_base;

  try {
    SensorType rgb_sensor, depth_sensor;

    if (arm == "left_arm") {
      rgb_sensor = SensorType::LEFT_ARM_CAMERA;
      depth_sensor = SensorType::LEFT_ARM_DEPTH_CAMERA;
    } else if (arm == "right_arm") {
      rgb_sensor = SensorType::RIGHT_ARM_CAMERA;
      depth_sensor = SensorType::RIGHT_ARM_DEPTH_CAMERA;
    } else {
      throw std::invalid_argument("arm must be left_arm or right_arm");
    }

    // Get RGB image data
    auto rgb_image_data = robot.get_rgb_data(rgb_sensor, RgbOutputFormat::JPEG, true);
    auto depth_data = robot.get_depth_data(depth_sensor);

    cv::Mat img, depth_img;

    if (rgb_image_data && !rgb_image_data->data.empty()) {
      std::cout << "Get rgb image success" << std::endl;
      auto mat_ptr = rgb_image_data->convert_to_cv2_mat();
      if (mat_ptr && !mat_ptr->empty()) {
        img = *mat_ptr;
      } else {
        img = decode_rgb_image(rgb_image_data->data);
      }
    } else {
      std::cout << "No rgb image data (sensor may be unavailable)" << std::endl;
    }

    if (depth_data && depth_data->width > 0 && depth_data->height > 0 && !depth_data->data.empty()) {
      std::cout << "Get depth data success" << std::endl;
    } else {
      std::cout << "No depth data (depth sensor may be unavailable)" << std::endl;
    }

    // Detect target (placeholder)
    auto object_pose_camera = detect_target(img, depth_img);

    if (object_pose_camera.empty()) {
      std::cout << "Target detection failed" << std::endl;
      return {};
    } else {
      std::cout << "object_pose_camera: [";
      for (auto val : object_pose_camera)
        std::cout << val << " ";
      std::cout << "]" << std::endl;
    }

    // Transform to base frame
    object_pose_base = pose_camera_to_base(robot, object_pose_camera);
    std::cout << "Target pose in chassis coordinate system: [";
    for (auto val : object_pose_base)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

  } catch (const std::exception& e) {
    object_pose_base = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
    std::cout << "Target detection exception: " << e.what() << std::endl;
  }

  return object_pose_base;
}

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;
    }
  }
}

void pick_and_place(GalbotRobot& robot, GalbotMotion& motion, const std::vector<double>& object_pose_base,
                    const std::string& target_chain, const std::string& reference_frame) {
  try {
    // Attach tool to end effector
    auto 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;
    } else {
      std::cout << "✅ Successfully attached tool: status=" << (int) status << std::endl;
    }

    // Open left gripper
    auto gripper_status = robot.set_gripper_command("left_gripper", 0.1, 0.05, 10, true);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ Successfully set left gripper width to 0.1m: status=" << (int) gripper_status << std::endl;

    std::cout << "object_pose_base: [";
    for (auto val : object_pose_base)
      std::cout << val << " ";
    std::cout << "]" << std::endl;

    // Reach to target position
    int retry_cnt = 3;
    while (true) {
      status = motion.set_end_effector_pose(object_pose_base, target_chain, reference_frame, nullptr, false, true, 5.0,
                                            std::make_shared<Parameter>());

      std::this_thread::sleep_for(std::chrono::seconds(1));
      retry_cnt--;

      if (status == MotionStatus::SUCCESS || retry_cnt < 0) {
        break;
      } else {
        std::cout << "Failed to set end effector pose: status=" << (int) status << ", retry count: " << retry_cnt
                  << std::endl;
      }
    }

    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to set end effector pose: status=" << (int) status << std::endl;
    } else {
      std::cout << "✅ Successfully set end effector pose: status=" << (int) status << std::endl;
    }

    // Close gripper to grasp object
    gripper_status = robot.set_gripper_command("left_gripper", 0.02, 0.05, 10, true);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ Successfully closed left gripper: status=" << (int) gripper_status << std::endl;

    // Attach target object (box scale values must all be >0; default {} gives 0 and causes INVALID_INPUT)
    std::array<double, 3> grasp_box_scale{0.05, 0.05, 0.05};
    status = motion.attach_target_object("grasped_box", "box", {0, 0, 0, 0, 0, 0, 1}, grasp_box_scale, "", "left_arm",
                                         "ee_base");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to attach target object: status=" << (int) status << std::endl;
    } else {
      std::cout << "✅ Successfully attached target object: status=" << (int) status << std::endl;
    }

    // Return to the initial position: in demo mode, only simulate and do not call chassis navigation
    mock_navigation_log("After grasping, return to the starting point");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ (Mock) Return navigation skipped; chassis not moved" << std::endl;

    // Release target
    gripper_status = robot.set_gripper_command("left_gripper", 0.1, 0.05, 10, true);
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    std::cout << "✅ Successfully released left gripper: status=" << (int) gripper_status << std::endl;

    // Detach target object
    status = motion.detach_target_object("grasped_box");
    std::this_thread::sleep_for(std::chrono::milliseconds(500));

    if (status != MotionStatus::SUCCESS) {
      std::cout << "❌ Failed to detach target object: status=" << (int) status << std::endl;
    } else {
      std::cout << "✅ Successfully detached target object: status=" << (int) status << std::endl;
    }

    // Detach tool from end effector
    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;
    }

  } catch (const std::exception& e) {
    std::cout << "Exception occurred during pick_and_place: " << e.what() << std::endl;
  }
}

int main() {
  check_robot_safety();

  // Get robot instances
  auto& robot = GalbotRobot::get_instance(MachineType::S1);
  auto& motion = GalbotMotion::get_instance(MachineType::S1);
  auto& nav = GalbotNavigation::get_instance(MachineType::S1);

  try {
    // Enable sensors
    std::unordered_set<SensorType> enable_sensor_set = {SensorType::LEFT_ARM_CAMERA, SensorType::LEFT_ARM_DEPTH_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 (kDemoMockNavigationOnly) {
      std::cout << "[Demo mode] Skipped: relocalize and real navigate_to_goal (chassis remains stationary)." << std::endl;
    } else {
      while (true) {
        if (nav.is_localized()) {
          std::cout << "✅ Robot localized successfully, proceeding to navigate to goal..." << std::endl;
          break;
        }
        std::cout << "❌ Navigation not localized, relocalizing..." << std::endl;
        nav.relocalize(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::seconds(2));
      }
    }

    mock_navigation_log("Before starting the task: move near the target point");

    // Save the current body joints using the hardware-version-specific names exposed by the SDK.
    const auto snapshot_joint_names = robot.get_joint_names(true, kS1BodyJointGroupsForSnapshot);
    if (snapshot_joint_names.size() != kS1BodyJointCount) {
      throw std::runtime_error("Failed to resolve complete S1 body joint names for snapshot: got " +
                               std::to_string(snapshot_joint_names.size()) + ", expected " +
                               std::to_string(kS1BodyJointCount));
    }
    auto joint_groups_positions = robot.get_joint_positions({}, snapshot_joint_names);
    std::cout << "Saved body joint snapshot: " << joint_groups_positions.size() << " values (expected "
              << snapshot_joint_names.size() << ")" << std::endl
              << std::endl;
    if (joint_groups_positions.size() != snapshot_joint_names.size()) {
      throw std::runtime_error("Failed to save complete body joint snapshot: got " +
                               std::to_string(joint_groups_positions.size()) + " values for " +
                               std::to_string(snapshot_joint_names.size()) + " joints");
    }

    // joint, end effector (lower_camera_after_joint_zero)
    lower_camera_after_joint_zero(motion, "left_arm", "base_link", 0.08);
    std::cout << std::endl;

    // Detect target
    auto object_pose_base = detect_object(robot, "left_arm");
    std::cout << std::endl;

    // Pick and place
    pick_and_place(robot, motion, object_pose_base, "left_arm", "base_link");
    std::cout << std::endl;

    // Restore joint positions
    int retry_cnt = 3;
    while (true) {
      auto status = robot.set_joint_positions(joint_groups_positions, {}, snapshot_joint_names, true, 0.5, 20);
      std::this_thread::sleep_for(std::chrono::milliseconds(500));

      if (status == ControlStatus::SUCCESS) {
        std::cout << "✅ Successfully set joint positions: status=" << (int) status << std::endl;
        break;
      } else if (retry_cnt < 0) {
        std::cout << "❌ Failed to set joint positions after retries, status=" << (int) status << std::endl;
        break;
      } else {
        retry_cnt--;
        std::cout << "Failed to set joint positions: retry count: " << retry_cnt << std::endl;
      }
    }

  } catch (const std::exception& e) {
    std::cout << "Exception occurred: " << e.what() << std::endl;
  }

  // Cleanup
  robot.request_shutdown();
  robot.wait_for_shutdown();
  robot.destroy();
  std::cout << "Resource release successful" << std::endl;

  return 0;
}