Initial import of ORISYS arm workstation

thirdparty/flexiv_rdk/.clang-format

@@ -0,0 +1,65 @@
+---
+Language: Proto
+BasedOnStyle: Google
+---
+Language: Cpp
+AccessModifierOffset: -4
+AlignAfterOpenBracket: false
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlinesLeft: false
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BasedOnStyle: WebKit
+BinPackArguments: true
+BinPackParameters: true
+BreakBeforeBraces: Mozilla
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: true
+ColumnLimit: 100
+CommentPragmas: "^ IWYU pragma:"
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+ForEachMacros: [foreach, Q_FOREACH, BOOST_FOREACH]
+IndentCaseLabels: true
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+KeepEmptyLinesAtTheStartOfBlocks: true
+MacroBlockBegin: ""
+MacroBlockEnd: ""
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+PenaltyBreakBeforeFirstCallParameter: 100
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 10000
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: false
+SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Cpp11
+TabWidth: 4
+UseTab: Never

thirdparty/flexiv_rdk/.gitattributes

@@ -0,0 +1,6 @@
+*.a filter=lfs diff=lfs merge=lfs -text
+*.lib filter=lfs diff=lfs merge=lfs -text
+*.so filter=lfs diff=lfs merge=lfs -text
+*.pyd filter=lfs diff=lfs merge=lfs -text
+*.stl filter=lfs diff=lfs merge=lfs -text
+*.obj filter=lfs diff=lfs merge=lfs -text

thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/bug_report.md

@@ -0,0 +1,39 @@
+---
+name: Bug Report
+about: Report a bug to help us improve
+title: "[BUG] "
+type: Bug
+labels: bug
+assignees: pzhu-flexiv
+
+---
+
+## Version info
+
+- RDK: [e.g. v1.5]
+- Robot software: [e.g. v3.7]
+- OS: [e.g. Ubuntu 22.04, x86_64]
+
+## How urgent is this bug? Have you found any workarounds?
+
+On a scale of 1 to 10.
+
+## Describe the issue and expected behavior
+
+The issue is ...
+The correct behavior you expect is  ...
+
+## Steps to reproduce
+
+1. '....'
+2. '....'
+3. '....'
+4. See error
+
+## Screenshots
+
+If applicable, add screenshots to help explain your issue.
+
+## Additional context
+
+Add any other context about the issue.

thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/feature_request.md

@@ -0,0 +1,29 @@
+---
+name: Feature Request
+about: Suggest a new feature
+title: "[FEATURE] "
+type: Feature
+labels: enhancement
+assignees: pzhu-flexiv
+
+---
+
+## How urgent is this feature?
+
+On a scale of 1 to 10.
+
+## Is this feature request related to a problem?
+
+This new feature is needed so that ... can be solved.
+
+## Describe the solution you'd like
+
+What you want to happen.
+
+## Describe alternatives you've considered
+
+Any alternative solutions or workarounds you've considered.
+
+## Additional context
+
+Add any other context or screenshots about this feature.

thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/question.md

@@ -0,0 +1,35 @@
+---
+name: Question
+about: Ask a question related to RDK
+title: "[QUESTION] "
+type: Question
+labels: help wanted
+assignees: [pzhu-flexiv, gavin-flexiv, zlin-flexiv, yunhong-flexiv, Aathitian-flexiv, eltonsi-flexiv]
+
+---
+
+## There might already be an answer to your question
+
+Did you know you can find answers to most questions related to RDK in the [manual](https://www.flexiv.com/software/rdk/manual/index.html)? Just search for keywords using the "Search docs" function.
+
+Also, someone might have asked similar questions before, you can search in the [Issues](https://github.com/flexivrobotics/flexiv_rdk/issues?q=type%3AQuestion) tab.
+
+## Looking for more information about certain APIs?
+
+You can first check out [API Overview](https://www.flexiv.com/software/rdk/manual/api_overview.html) from the manual, then dive into the detailed [API documentation](https://www.flexiv.com/software/rdk/api/index.html). See below if you want to learn more about how to use an API.
+
+## Not sure how to get started and write a working RDK program?
+
+You can take a look at the [C++ examples](https://github.com/flexivrobotics/flexiv_rdk/tree/main/example) or [Python examples](https://github.com/flexivrobotics/flexiv_rdk/tree/main/example_py) to get started.
+
+## Is this related to a specific version of RDK?
+
+If yes, please specify.
+
+## What is your country or region?
+
+Please provide your country or region. This helps us determine the most appropriate issue assignees.
+
+## Please describe your question
+
+Texts, screenshots, links, code blocks ...

thirdparty/flexiv_rdk/.github/workflows/ci-cpp.yml

@@ -0,0 +1,34 @@
+name: CI-Cpp
+
+on:
+  push:
+    branches: [main]
+  pull_request:
+    branches: [main]
+  workflow_dispatch:
+
+permissions:
+  contents: read
+
+env:
+  FORCE_JAVASCRIPT_ACTIONS_TO_NODE24: "true"
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  ci-cpp:
+    name: CI-Cpp @ ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        os:
+          - ubuntu-22.04
+          - ubuntu-22.04-arm
+          - ubuntu-24.04
+          - ubuntu-24.04-arm
+          - macos-14
+          - windows-2022
+    steps:
+      - uses: actions/checkout@v6
+      - uses: ./.github/workflows/shared_cpp_steps

thirdparty/flexiv_rdk/.github/workflows/ci-python.yml

@@ -0,0 +1,37 @@
+name: CI-Python
+
+on:
+  push:
+    branches: [main]
+  pull_request:
+    branches: [main]
+  workflow_dispatch:
+
+permissions:
+  contents: read
+
+env:
+  FORCE_JAVASCRIPT_ACTIONS_TO_NODE24: "true"
+
+jobs:
+  ci-python:
+    name: CI-Python @ ${{ matrix.os }} (${{ matrix.python-version }})
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        os:
+          - ubuntu-22.04
+          - ubuntu-22.04-arm
+          - ubuntu-24.04
+          - ubuntu-24.04-arm
+          - macos-14
+          - windows-2022
+        python-version:
+          - "3.10"
+          - "3.12"
+    steps:
+      - uses: actions/checkout@v6
+      - uses: ./.github/workflows/shared_python_steps
+        with:
+          python-version: ${{ matrix.python-version }}

thirdparty/flexiv_rdk/.github/workflows/shared_cpp_steps/action.yml

@@ -0,0 +1,50 @@
+name: "Shared Check Steps for C++"
+runs:
+  using: "composite"
+  # Note: working directory will be reset to the repo root for each new step
+  steps:
+    # Use a fixed CMake version
+    - name: Set up CMake
+      uses: jwlawson/actions-setup-cmake@v2
+      with:
+        cmake-version: '3.28.3'
+
+    # Build and install all dependencies to RDK installation directory.
+    - name: Build and install dependencies
+      shell: bash
+      run: |
+        cmake --version
+        cd thirdparty
+        bash build_and_install_dependencies.sh ~/rdk_install 8
+
+    # Configure CMake, then build and install flexiv_rdk library to RDK installation directory.
+    - name: Build and install library
+      shell: bash
+      run: |
+        mkdir -p build && cd build
+        cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install
+        cmake --build . --target install --config Release
+
+    # Find and link to flexiv_rdk library, then build all examples.
+    - name: Build examples
+      shell: bash
+      run: |
+        cd example
+        mkdir -p build && cd build
+        cmake .. -DCMAKE_PREFIX_PATH=~/rdk_install
+        cmake --build . --config Release -j 8
+
+    # Find and link to flexiv_rdk library, then build all tests.
+    - name: Build tests
+      shell: bash
+      run: |
+        cd test
+        mkdir -p build && cd build
+        cmake .. -DCMAKE_PREFIX_PATH=~/rdk_install
+        cmake --build . --config Release -j 8
+
+    - name: Run unit tests
+      shell: bash
+      run: |
+        cd test/build
+        ctest --output-on-failure

thirdparty/flexiv_rdk/.github/workflows/shared_python_steps/action.yml

@@ -0,0 +1,41 @@
+name: "Shared Check Steps for Python"
+inputs:
+  python-version:
+    description: "Python version used for example compilation checks"
+    required: true
+runs:
+  using: "composite"
+  steps:
+    - name: Set up Python
+      uses: actions/setup-python@v6
+      with:
+        python-version: ${{ inputs.python-version }}
+
+    - name: Install latest flexivrdk
+      shell: bash
+      run: |
+        python -m pip install --upgrade pip
+        python -m pip install flexivrdk
+
+    - name: Byte-compile Python examples
+      shell: bash
+      run: |
+        python - <<'PY'
+        from pathlib import Path
+        import py_compile
+
+        root = Path('example_py')
+        files = sorted(
+            path for path in root.glob('*.py')
+            if path.name.startswith('basics') or path.name.startswith('intermediate')
+        )
+
+        for path in files:
+            py_compile.compile(str(path), doraise=True)
+            print(f'py_compile OK: {path}')
+        PY
+
+    - name: Run unit tests
+      shell: bash
+      run: |
+        python test/test_robot_constructor.py -v

thirdparty/flexiv_rdk/.gitignore

@@ -0,0 +1,13 @@
+.vscode
+.vs
+.venv
+.DS_Store
+*.a
+*.so
+*.lib
+*.dll
+config.h
+build*/
+cloned/
+html/
+__pycache__

thirdparty/flexiv_rdk/CMakeLists.txt

@@ -0,0 +1,140 @@
+cmake_minimum_required(VERSION 3.22.1)
+
+# ===================================================================
+#      PROJECT SETUP
+# ===================================================================
+project(flexiv_rdk VERSION 2.1 LANGUAGES CXX)
+
+# CMake >= 3.30 warns for FindBoost behavior unless CMP0167 is explicitly set.
+if(POLICY CMP0167)
+  cmake_policy(SET CMP0167 OLD)
+endif()
+
+# Configure build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
+endif()
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
+
+# Set static library according to platform
+message(STATUS "OS: ${CMAKE_SYSTEM_NAME}")
+message(STATUS "Processor: ${CMAKE_SYSTEM_PROCESSOR}")
+if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
+    set(RDK_LIB "libflexiv_rdk.x86_64-linux-gnu.a")
+  elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64")
+    set(RDK_LIB "libflexiv_rdk.aarch64-linux-gnu.a")
+  else()
+    message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm64")
+    set(RDK_LIB "libflexiv_rdk.arm64-darwin.a")
+  else()
+    message(FATAL_ERROR "macOS with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "AMD64")
+    set(RDK_LIB "flexiv_rdk.win_amd64.lib")
+  else()
+    message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "QNX")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
+    set(RDK_LIB "libflexiv_rdk.x86_64-qnx.a")
+  elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64le")
+    set(RDK_LIB "libflexiv_rdk.aarch64-qnx.a")
+  else()
+    message(FATAL_ERROR "QNX with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+else()
+  message(FATAL_ERROR "${CMAKE_SYSTEM_NAME} is currently not supported.")
+endif()
+
+# Parse expected hash of the library file
+file(READ ${CMAKE_CURRENT_SOURCE_DIR}/lib/${RDK_LIB}.sha256 EXPECTED_HASH)
+
+# Download the library file from GitHub 
+set(RDK_LIB_URL https://github.com/flexivrobotics/flexiv_rdk/releases/download/v${PROJECT_VERSION}/${RDK_LIB})
+message(STATUS "Downloading ${RDK_LIB_URL} ...")
+file(DOWNLOAD ${RDK_LIB_URL} ${CMAKE_CURRENT_BINARY_DIR}/${RDK_LIB}
+     SHOW_PROGRESS
+     TLS_VERIFY ON
+     EXPECTED_HASH SHA256=${EXPECTED_HASH}
+     STATUS status)
+list(GET status 0 code)
+if(code EQUAL 0)
+  message(STATUS "Download finished")
+else()
+  message(FATAL_ERROR "Download failed: ${status}")
+endif()
+
+# ===================================================================
+#      PROJECT DEPENDENCIES
+# ===================================================================
+# Threads
+set(THREADS_PREFER_PTHREAD_FLAG ON)
+find_package(Threads REQUIRED)
+message(STATUS "Found Threads")
+
+# Eigen3
+find_package(Eigen3 3.4.0 REQUIRED)
+message(STATUS "Found Eigen3 v${Eigen3_VERSION}: ${Eigen3_DIR}")
+
+# spdlog
+find_package(spdlog 1.9.2 REQUIRED)
+message(STATUS "Found spdlog v${spdlog_VERSION}: ${spdlog_DIR}")
+
+# zenoh-c
+find_package(zenohc 1.9.0 REQUIRED)
+message(STATUS "Found zenohc v${zenohc_VERSION}: ${zenohc_DIR}")
+
+# zenoh-cpp
+find_package(zenohcxx 1.9.0 REQUIRED)
+message(STATUS "Found zenohcxx v${zenohcxx_VERSION}: ${zenohcxx_DIR}")
+
+# RBDyn
+set(Boost_USE_STATIC_LIBS OFF)
+find_package(RBDyn 1.9.3 REQUIRED)
+message(STATUS "Found RBDyn v${RBDyn_VERSION}: ${RBDyn_DIR}")
+
+# ===================================================================
+#      CREATE LIBRARY
+# ===================================================================
+# Create static library target with dummy source
+add_library(${PROJECT_NAME} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/lib/dummy.cpp)
+
+# Create an alias of the target with flexiv namespace
+add_library(flexiv::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
+
+# Set include directories
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+# Link to dependencies
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  Threads::Threads
+  Eigen3::Eigen
+  spdlog::spdlog
+  zenohcxx::zenohc
+  RBDyn::RBDyn
+  RBDyn::Parsers
+)
+
+# C++20 required (PUBLIC so the standard propagates to consumers)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_20)
+
+# Use moderate compiler warning option
+if(CMAKE_HOST_UNIX)
+  target_compile_options(${PROJECT_NAME} PUBLIC -Wall -Wextra)
+else()
+  target_compile_options(${PROJECT_NAME} PUBLIC /W1)
+endif()
+
+# ===================================================================
+#      LIBRARY INSTALLATION
+# ===================================================================
+include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/FlexivInstallLibrary.cmake)
+FlexivInstallLibrary()

thirdparty/flexiv_rdk/LICENSE

@@ -0,0 +1,211 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+   APPENDIX: This software depends on the following third-party libraries, and users must comply to their licenses.
+      - Eigen: https://gitlab.com/libeigen/eigen/-/blob/master/LICENSE
+      - boost: https://github.com/boostorg/boost/blob/master/LICENSE_1_0.txt
+      - spdlog: https://github.com/gabime/spdlog/blob/master/LICENSE
+      - tinyxml2: https://github.com/leethomason/tinyxml2/blob/master/LICENSE.txt
+      - yaml-cpp: https://github.com/jbeder/yaml-cpp/blob/master/LICENSE
+      - SpaceVecAlg: https://github.com/jrl-umi3218/SpaceVecAlg/blob/master/LICENSE
+      - RBDyn: https://github.com/jrl-umi3218/RBDyn/blob/master/LICENSE
+      - Zenoh: https://github.com/eclipse-zenoh/zenoh/blob/main/LICENSE

thirdparty/flexiv_rdk/README.md

@@ -0,0 +1,213 @@
+# Flexiv RDK
+
+![Cpp Badge](https://github.com/flexivrobotics/flexiv_rdk/actions/workflows/ci-cpp.yml/badge.svg)
+![Python Badge](https://github.com/flexivrobotics/flexiv_rdk/actions/workflows/ci-python.yml/badge.svg)
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://www.apache.org/licenses/LICENSE-2.0.html)
+
+Flexiv RDK (Robotic Development Kit), a key component of the Flexiv Robotic Software Platform, is a powerful development toolkit that enables the users to create complex and customized robotic applications using APIs that provide both low-level real-time (RT) and high-level non-real-time (NRT) access to Flexiv robots.
+
+## References
+
+[Flexiv RDK Home Page](https://www.flexiv.com/software/rdk) is the main reference. It contains important information including user manual and API documentation. The instructions below serve as a quick reference, and you can find the full documentation at [Flexiv RDK Manual](https://www.flexiv.com/software/rdk/manual).
+
+## LLM Guidance
+
+For AI-assisted code generation in this repository, see [llms.txt](llms.txt).
+
+## Environment Compatibility
+
+| **OS**                | **Platform**    | **C++ compiler kit** | **Python interpreter** |
+| --------------------- | --------------- | -------------------- | ---------------------- |
+| Linux (Ubuntu 22.04+) | x86_64, aarch64 | GCC   v11.4+         | 3.10, 3.12, 3.14       |
+| macOS 12+             | arm64           | Clang v14.0+         | 3.10, 3.12             |
+| Windows 10+           | x86_64          | MSVC  v14.2+         | 3.10, 3.12             |
+| QNX 8.0.3+            | x86_64, aarch64 | QCC   v12.2+         | Not supported          |
+
+## Important Notice
+
+Before trying to run any RDK program, please make sure to carefully go through the [First Time Setup](https://www.flexiv.com/software/rdk/manual/index.html#first-time-setup) chapter in RDK Manual. Otherwise, you might run into blocking issues.
+
+## Quick Start - Python
+
+### Install the Python package
+
+On all supported platforms, the Python package of RDK and its dependencies for a specific Python version can be installed using the `pip` module:
+
+    python3.x -m pip install numpy spdlog flexivrdk
+
+> [!NOTE]
+> Replace `3.x` with a specific Python version.
+
+### Use the installed Python package
+
+After the `flexivrdk` Python package is installed, it can be imported from any Python script. Test with the following commands in a new Terminal, which should start Flexiv RDK:
+
+    python3.x
+    import flexivrdk
+    robot = flexivrdk.Robot("Enlight-L-123456")
+
+The program will start searching for a robot with serial number `Enlight-L-123456`, and will exit after a couple of seconds if the specified robot is not found in the local network.
+
+### Run example Python scripts
+
+To run an example Python script in this repo:
+
+    cd flexiv_rdk/example_py
+    python3.x <example-name>.py <robot-sn>
+
+For example:
+
+    python3.10 ./basics1_display_robot_states.py Enlight-L-123456
+
+## Quick Start - C++
+
+### Prepare build tools
+
+#### Linux
+
+1. Install compiler kit using package manager:
+
+       sudo apt install build-essential
+
+2. Install CMake using package manager:
+
+       sudo apt install cmake
+
+#### macOS
+
+1. Install compiler kit using `xcode` tool:
+
+       xcode-select 
+
+   This will invoke the installation of Xcode Command Line Tools, then follow the prompted window to finish the installation.
+
+2. Install CMake using package manager:
+
+       brew install cmake
+
+#### Windows
+
+1. Install compiler kit: Download and install Microsoft Visual Studio 2019 (MSVC v14.2) or above. Choose "Desktop development with C++" under the *Workloads* tab during installation. You only need to keep the following components for the selected workload:
+   * MSVC ... C++ x64/x86 build tools (Latest)
+   * C++ CMake tools for Windows
+   * Windows 10 SDK or Windows 11 SDK, depending on your actual Windows version
+2. Install CMake: Download `cmake-3.x.x-windows-x86_64.msi` from [CMake download page](https://cmake.org/download/) and install the msi file. The minimum required version is 3.22.1. **Add CMake to system PATH** when prompted, so that `cmake` and `cmake-gui` command can be used from Command Prompt or a bash emulator.
+3. Install bash emulator: Download and install [Git for Windows](https://git-scm.com/install/windows), which comes with a bash emulator Git Bash. The following steps are to be carried out in this bash emulator.
+
+#### QNX
+
+1. Prepare a host computer with Ubuntu 22.04 or higher.
+2. Download and install [QNX SDP 8.0.3](https://blackberry.qnx.com/en/products/foundation-software/qnx-software-development-platform) to the host computer. You'll need a trial or commercial license.
+3. Install CMake on the host computer using package manager:
+
+       sudo apt install cmake
+
+### Install the C++ library
+
+The following steps are mostly the same on all supported platforms, with some variations.
+
+1. Choose a directory for installing the C++ library of RDK and its dependencies. This directory can be under system path or not, depending on whether you want RDK to be globally discoverable by CMake. For example, a new folder named `rdk_install` under the home directory.
+2. In a new Terminal, run the provided script to compile and install all dependencies to the installation directory chosen in step 1:
+
+       cd flexiv_rdk/thirdparty
+
+   On non-QNX:
+
+       bash build_and_install_dependencies.sh ~/rdk_install
+
+   On QNX:
+
+       source <qnx-sdp-dir>/qnxsdp-env.sh
+       bash build_and_install_dependencies.sh ~/rdk_install $(nproc) <path-to-qnx-toolchain-file>
+
+   > [!NOTE]
+   > The QNX toolchain files are located under `flexiv_rdk/cmake` directory, with one for x86_64 target and one for aarch64 target.
+
+3. In the same Terminal, configure the `flexiv_rdk` CMake project:
+
+       cd flexiv_rdk
+       mkdir build && cd build
+
+   On non-QNX:
+
+       cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install
+
+   On QNX:
+
+       cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install -DCMAKE_TOOLCHAIN_FILE=<path-to-qnx-toolchain-file>
+
+   > [!NOTE]
+   > `-D` followed by `CMAKE_INSTALL_PREFIX` sets the absolute path of the installation directory, which should be the one chosen in step 1.
+
+4. Install `flexiv_rdk` C++ library to `CMAKE_INSTALL_PREFIX` path, which may or may not be globally discoverable by CMake depending on the location:
+
+       cd flexiv_rdk/build
+       cmake --build . --target install --config Release
+
+### Use the installed C++ library
+
+After the library is installed as `flexiv_rdk` CMake target, it can be linked from any other CMake projects. Using the provided `flexiv_rdk-examples` project for instance:
+
+    cd flexiv_rdk/example
+    mkdir build && cd build
+
+On non-QNX:
+
+    cmake .. -DCMAKE_PREFIX_PATH=~/rdk_install
+    cmake --build . --config Release -j 4
+
+On QNX:
+
+    cmake .. -DCMAKE_PREFIX_PATH=~/rdk_install -DCMAKE_TOOLCHAIN_FILE=<path-to-qnx-toolchain-file>
+    cmake --build . --config Release -j 4
+
+> [!NOTE]
+> `-D` followed by `CMAKE_PREFIX_PATH` tells the user project's CMake where to find the installed C++ library. This argument can be skipped if the RDK library and its dependencies are installed to a globally discoverable location.
+
+### Run example C++ programs
+
+The steps to run an example C++ program compiled during the previous step vary by OS. 
+
+> [!NOTE]
+> - Replace `<example-name>` with the actual example program to be executed.
+> - Replace `<robot-sn>` with the actual serial number of the robot, for example `Enlight-L-123456`.
+> - Root privilege is required if the real-time scheduler API `flexiv::rdk::Scheduler` is used in the program.
+
+#### Linux and macOS
+
+On UNIX systems, the install location of the dependencies' shared libraries is baked into the executable as an RPATH, so they are found automatically at runtime with no extra setup:
+
+    cd flexiv_rdk/example/build
+    ./<example-name> <robot-sn>
+
+#### Windows - Command Prompt
+
+Windows does not support RPATH, so the install location of the dependencies' shared libraries must be explicitly specified by adding the `bin` folder under the installation directory to `PATH` for the current session, before executing the example programs:
+
+    cd flexiv_rdk\example\build
+    set PATH=%USERPROFILE%\rdk_install\bin;%PATH%
+    Release\<example-name>.exe <robot-sn>
+
+Alternatively, add the `bin` folder to the system or user `PATH` environment variable to make this change permanent instead of per-session.
+
+> [!WARNING]
+> If the `bin` folder is not on `PATH`, the program will exit immediately with no error message on Command Prompt, because Windows terminates the process with exit code `0xC0000135` (`STATUS_DLL_NOT_FOUND`) before any output is produced. Run `echo %ERRORLEVEL%` right after to confirm this is the cause (it prints `-1073741515` on failure, `0` on success).
+
+#### Windows - bash emulator (such as Git Bash)
+
+The same rule applies in a bash emulator, but using bash syntax to set `PATH` for the current session (note the use of `/` as path separator and `:` to delimit entries):
+
+    cd flexiv_rdk/example/build
+    export PATH="$USERPROFILE/rdk_install/bin:$PATH"
+    ./Release/<example-name>.exe <robot-sn>
+
+## API Documentation
+
+The complete and detailed API documentation of the **latest release** can be found at [Flexiv RDK APIs](https://www.flexiv.com/software/rdk/api). The API documentation of a previous release can be generated manually using Doxygen. For example, on Linux:
+
+    sudo apt install doxygen-latex graphviz
+    cd flexiv_rdk
+    git checkout <previous_release_tag>
+    doxygen doc/Doxyfile.in
+
+Open any html file under `flexiv_rdk/doc/html/` with your browser to view the doc.

thirdparty/flexiv_rdk/cmake/FlexivInstallLibrary.cmake

@@ -0,0 +1,103 @@
+# This macro will install ${PROJECT_NAME} to ${CMAKE_INSTALL_PREFIX} when running make install
+#
+# FlexivInstallLibrary() will install all subfolders of ${CMAKE_CURRENT_SOURCE_DIR}/include
+# FlexivInstallLibrary(install_directories) will install only the specified install_directories
+#
+# Requirements:
+# 1. project structure should resemble:
+#    project
+#     - README.md
+#     - CMakeLists.txt that calls this macro
+#     - cmake/${PROJECT_NAME}-config.cmake.in
+#     - include/subfolder/*.h or *.hpp
+# 2. build the library using cmake target functions
+#    - add_library(${PROJECT_NAME} ...) before calling this macro
+#    - target_include_directories(${PROJECT_NAME} ...)
+#    - target_link_libraries(${PROJECT_NAME} ...)
+#    - target_compile_features(${PROJECT_NAME} ...)
+#    - target_compile_options(${PROJECT_NAME} ...)
+#
+# Installed files:
+# - include/subfolder/*.h or *.hpp
+# - lib/lib{PROJECT_NAME}
+# - lib/cmake/{PROJECT_NAME}/
+
+macro(FlexivInstallLibrary)
+    # copy the executables and libraries to the CMAKE_INSTALL_PREFIX DIRECTORY
+    # GNUInstallDirs will set CMAKE_INSTALL* to be the standard relative paths
+    include(GNUInstallDirs)
+    install(TARGETS ${PROJECT_NAME}
+        EXPORT "${PROJECT_NAME}-targets"
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        )
+
+    if(${ARGC} EQUAL 0)
+        # install all subfolders of ${CMAKE_CURRENT_SOURCE_DIR}/include
+        file(GLOB install_directories ${CMAKE_CURRENT_SOURCE_DIR}/include/*)
+        foreach(install_directory ${install_directories})
+            if(IS_DIRECTORY ${install_directory})
+                install(DIRECTORY ${install_directory}
+                        DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+                        FILES_MATCHING 
+                        PATTERN "*.h"
+                        PATTERN "*.hpp"
+                )
+            endif()
+        endforeach()
+    elseif(${ARGC} EQUAL 1)
+        # install specified directories only
+        foreach(install_directory ${ARGV0})
+            install(DIRECTORY ${install_directory}
+                    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+                    FILES_MATCHING 
+                    PATTERN "*.h"
+                    PATTERN "*.hpp"
+                    )
+        endforeach()
+    else()
+        message(FATAL_ERROR "FlexivInstallLibrary take 0 or 1 argument, but given ${ARGC}")
+    endif()
+
+    # Create a *config-version.cmake file so that find_package can have a version specified
+    include(CMakePackageConfigHelpers)
+    write_basic_package_version_file(
+        "${PROJECT_NAME}-config-version.cmake"
+        VERSION ${PROJECT_VERSION}
+        COMPATIBILITY AnyNewerVersion
+        )
+
+    # Copy the *-targets.cmake file to the CMAKE_INSTALL_PREFIX directory
+    install(EXPORT "${PROJECT_NAME}-targets"
+            FILE "${PROJECT_NAME}-targets.cmake"
+            NAMESPACE "flexiv::"
+            DESTINATION "lib/cmake/${PROJECT_NAME}"
+            )
+
+    # Copy the *.-config file to the CMAKE_INSTALL_PREFIX directory. This will specify the dependencies.
+    configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}-config.cmake.in" "${PROJECT_NAME}-config.cmake" @ONLY)
+    install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake"
+                  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake"
+            DESTINATION "lib/cmake/${PROJECT_NAME}"
+            )
+
+    # Replace the dummy static lib with the actual static lib 
+    install(CODE 
+            "file(REMOVE ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/${CMAKE_STATIC_LIBRARY_PREFIX}${PROJECT_NAME}${CMAKE_STATIC_LIBRARY_SUFFIX})")
+    install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${RDK_LIB}
+            DESTINATION ${CMAKE_INSTALL_LIBDIR}
+            RENAME ${CMAKE_STATIC_LIBRARY_PREFIX}${PROJECT_NAME}${CMAKE_STATIC_LIBRARY_SUFFIX}
+            )
+
+    # Use the CPack Package Generator
+    set(CPACK_PACKAGE_VENDOR "Flexiv")
+    set(CPACK_PACKAGE_CONTACT "support@flexiv.com")
+    set(CPACK_PACKAGE_DESCRIPTION "Flexiv RDK (Robotic Development Kit)")
+    set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR})
+    set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR})
+    set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH})
+    set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
+    set(CPACK_RESOURCE_FILE_README  "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
+    include(CPack)
+endmacro()

thirdparty/flexiv_rdk/cmake/flexiv_rdk-config.cmake.in

@@ -0,0 +1,14 @@
+include(CMakeFindDependencyMacro)
+
+# Find dependency
+set(THREADS_PREFER_PTHREAD_FLAG ON)
+set(Boost_USE_STATIC_LIBS OFF)
+find_dependency(Threads REQUIRED)
+find_dependency(Eigen3 3.4.0 REQUIRED)
+find_dependency(spdlog 1.9.2 REQUIRED)
+find_dependency(zenohc 1.9.0 REQUIRED)
+find_dependency(zenohcxx 1.9.0 REQUIRED)
+find_dependency(RBDyn 1.9.3 REQUIRED)
+
+# Add targets file
+include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake")

thirdparty/flexiv_rdk/cmake/qnx_toolchain_aarch64.cmake

@@ -0,0 +1,70 @@
+# QNX toolchain for cross-compilation of aarch64le ("le" - little endian) target
+
+# Set target system
+set(CMAKE_SYSTEM_NAME QNX)
+set(CMAKE_SYSTEM_PROCESSOR aarch64le)
+set(arch gcc_ntoaarch64le)
+
+# specify the cross compiler
+set(CMAKE_C_COMPILER qcc)
+set(CMAKE_C_COMPILER_TARGET ${arch})
+set(CMAKE_CXX_COMPILER q++)
+set(CMAKE_CXX_COMPILER_TARGET ${arch})
+
+# Set qnx build flags
+add_definitions("-D_QNX_SOURCE")
+
+# Set QNX_TARGET
+if (DEFINED ENV{QNX_TARGET})
+    set (QNX_TARGET $ENV{QNX_TARGET})
+else ()
+    message (FATAL_ERROR "NO QNX TARGET ENV DEFINED")
+endif ()
+
+# Set the sysroot to the QNX target directory, so the compiler can find the correct headers and libraries for the target system.
+set(CMAKE_SYSROOT ${QNX_TARGET})
+
+# Set default cmake_install_prefix
+if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT OR NOT DEFINED CMAKE_INSTALL_PREFIX)
+    set(CMAKE_INSTALL_PREFIX ${QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr/local CACHE PATH "cmake install prefix" FORCE)
+endif()
+
+# We want to search:
+# 1. ${CMAKE_PREFIX_PATH}
+# 2. ${CMAKE_INSTALL_PREFIX}
+# 3. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr/local
+# 4. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr
+# 5. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}
+# 6. $ENV{QNX_TARGET}/usr
+#
+# To accomplish 3-6, append CMAKE_FIND_ROOT_PATH with:
+# - $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}
+# - $ENV{QNX_TARGET}/usr
+# 
+# However, CMAKE_FIND_ROOT_PATH will also be prepended to CMAKE_PREFIX_PATH and CMAKE_INSTALL_PREFIX.
+# In order to search these without the prefix,
+# append CMAKE_FIND_ROOT_PATH with:
+# - CMAKE_PREFIX_PATH
+# - CMAKE_INSTALL_PREFIX
+#
+# For details, see Confluence page: "Intro to CMake find_package()"
+if (DEFINED CMAKE_PREFIX_PATH)
+    list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_PREFIX_PATH})
+endif()
+list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_INSTALL_PREFIX})
+list(APPEND CMAKE_FIND_ROOT_PATH ${QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR} ${QNX_TARGET}/usr)
+
+# Remove duplicates, since we are appending
+list(REMOVE_DUPLICATES CMAKE_FIND_ROOT_PATH)
+
+# Search only the paths prefixed with CMAKE_FIND_ROOT_PATH
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY)
+
+# QNX does not support -pthread compiler flag,
+# because pthread is already part of the c library
+# However, FindThreads.cmake incorrectly sets CMAKE_HAVE_LIBC_PTHREAD OFF
+# See: https://gitlab.kitware.com/cmake/cmake/-/issues/21579
+set(CMAKE_HAVE_LIBC_PTHREAD ON)

thirdparty/flexiv_rdk/cmake/qnx_toolchain_x86_64.cmake

@@ -0,0 +1,70 @@
+# QNX toolchain for cross-compilation of x86_64 target
+
+# Set target system
+set(CMAKE_SYSTEM_NAME QNX)
+set(CMAKE_SYSTEM_PROCESSOR x86_64)
+set(arch gcc_ntox86_64)
+
+# Specify the cross compiler
+set(CMAKE_C_COMPILER qcc)
+set(CMAKE_C_COMPILER_TARGET ${arch})
+set(CMAKE_CXX_COMPILER q++)
+set(CMAKE_CXX_COMPILER_TARGET ${arch})
+
+# Set qnx build flags
+add_definitions("-D_QNX_SOURCE")
+
+# Set QNX_TARGET
+if (DEFINED ENV{QNX_TARGET})
+    set (QNX_TARGET $ENV{QNX_TARGET})
+else ()
+    message (FATAL_ERROR "NO QNX TARGET ENV DEFINED")
+endif ()
+
+# Set the sysroot to the QNX target directory, so the compiler can find the correct headers and libraries for the target system.
+set(CMAKE_SYSROOT ${QNX_TARGET})
+
+# Set default cmake_install_prefix
+if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT OR NOT DEFINED CMAKE_INSTALL_PREFIX)
+    set(CMAKE_INSTALL_PREFIX ${QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr/local CACHE PATH "cmake install prefix" FORCE)
+endif()
+
+# We want to search:
+# 1. ${CMAKE_PREFIX_PATH}
+# 2. ${CMAKE_INSTALL_PREFIX}
+# 3. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr/local
+# 4. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr
+# 5. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}
+# 6. $ENV{QNX_TARGET}/usr
+#
+# To accomplish 3-6, append CMAKE_FIND_ROOT_PATH with:
+# - $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}
+# - $ENV{QNX_TARGET}/usr
+# 
+# However, CMAKE_FIND_ROOT_PATH will also be prepended to CMAKE_PREFIX_PATH and CMAKE_INSTALL_PREFIX.
+# In order to search these without the prefix,
+# append CMAKE_FIND_ROOT_PATH with:
+# - CMAKE_PREFIX_PATH
+# - CMAKE_INSTALL_PREFIX
+#
+# For details, see Confluence page: "Intro to CMake find_package()"
+if (DEFINED CMAKE_PREFIX_PATH)
+    list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_PREFIX_PATH})
+endif()
+list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_INSTALL_PREFIX})
+list(APPEND CMAKE_FIND_ROOT_PATH ${QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR} ${QNX_TARGET}/usr)
+
+# Remove duplicates, since we are appending
+list(REMOVE_DUPLICATES CMAKE_FIND_ROOT_PATH)
+
+# Search only the paths prefixed with CMAKE_FIND_ROOT_PATH
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY)
+
+# QNX does not support -pthread compiler flag,
+# because pthread is already part of the c library
+# However, FindThreads.cmake incorrectly sets CMAKE_HAVE_LIBC_PTHREAD OFF
+# See: https://gitlab.kitware.com/cmake/cmake/-/issues/21579
+set(CMAKE_HAVE_LIBC_PTHREAD ON)

thirdparty/flexiv_rdk/example/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.22.1)
+project(flexiv_rdk-examples)
+
+# CMake >= 3.30 warns for FindBoost behavior unless CMP0167 is explicitly set.
+if(POLICY CMP0167)
+  cmake_policy(SET CMP0167 OLD)
+endif()
+
+# Show verbose build info
+SET(CMAKE_VERBOSE_MAKEFILE ON)
+
+message("OS: ${CMAKE_SYSTEM_NAME}")
+message("Processor: ${CMAKE_SYSTEM_PROCESSOR}")
+
+# Configure build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
+endif()
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
+
+# Minimum example list for all OS
+set(EXAMPLE_LIST
+  basics1_display_robot_states
+  basics2_clear_fault
+  basics3_primitive_execution
+  basics4_plan_execution
+  basics5_zero_force_torque_sensors
+  basics6_gripper_control
+  basics7_update_robot_tool
+  basics8_global_variables
+  basics9_logging_behavior
+  intermediate7_robot_dynamics
+)
+
+# Additional examples for Linux and Mac
+if(CMAKE_HOST_UNIX)
+  list(APPEND EXAMPLE_LIST
+    intermediate1_realtime_joint_position_control
+    intermediate2_realtime_joint_impedance_control
+    intermediate3_realtime_joint_torque_control
+    intermediate4_realtime_joint_floating
+    intermediate5_realtime_cartesian_pure_motion_control
+    intermediate6_realtime_cartesian_motion_force_control
+  )
+endif()
+
+# Find flexiv_rdk INTERFACE library
+find_package(flexiv_rdk REQUIRED)
+
+# Build all selected examples
+foreach(example ${EXAMPLE_LIST})
+  add_executable(${example} ${example}.cpp)
+  target_link_libraries(${example} flexiv::flexiv_rdk)
+
+  # C++20 required
+  set_target_properties(${example} PROPERTIES
+    CXX_STANDARD 20
+    CXX_STANDARD_REQUIRED ON)
+endforeach()

thirdparty/flexiv_rdk/example/basics1_display_robot_states.cpp

@@ -0,0 +1,120 @@
+/**
+ * @example basics1_display_robot_states.cpp
+ * This tutorial does the very first thing: check connection with the robot server and print
+ * received robot states.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Print robot states data @ 1Hz */
+void PrintRobotStates(rdk::Robot& robot)
+{
+    while (true) {
+        // Print Available joint groups
+        std::string joint_groups_str;
+        for (const auto& [_, name] : robot.info().all_groups) {
+            joint_groups_str += "[" + name + "] ";
+        }
+        spdlog::info("Available joint groups: {}", joint_groups_str);
+
+        // Print all robot states in JSON format using the built-in ostream operator overloading
+        for (const auto& [group, states] : robot.states()) {
+            spdlog::info("[{}] robot states:", rdk::kJointGroupNames.at(group));
+            std::cout << states << std::endl;
+        }
+
+        // Print all robot actions in JSON format using the built-in ostream operator overloading
+        for (const auto& [group, actions] : robot.actions()) {
+            spdlog::info("[{}] robot actions:", rdk::kJointGroupNames.at(group));
+            std::cout << actions << std::endl;
+        }
+
+        // Print digital inputs and outputs
+        spdlog::info("Digital inputs:");
+        std::cout << rdk::utility::Arr2Str(robot.digital_inputs()) << std::endl;
+        spdlog::info("Digital outputs:");
+        std::cout << rdk::utility::Arr2Str(robot.digital_outputs()) << std::endl;
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial does the very first thing: check connection "
+        "with the robot server and print received robot states.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Print States
+        // =========================================================================================
+        // Use std::thread to do scheduling so that this example can run on all OS, since not all OS
+        // support rdk::Scheduler
+        std::thread low_priority_thread(std::bind(PrintRobotStates, std::ref(robot)));
+
+        // Properly exit thread
+        low_priority_thread.join();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/basics2_clear_fault.cpp

@@ -0,0 +1,72 @@
+/**
+ * @example basics2_clear_fault.cpp
+ * This tutorial clears minor or critical faults, if any, of the connected robot.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial clears minor or critical faults, if any, of "
+        "the connected robot.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Fault Clearing
+        // =========================================================================================
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        } else {
+            spdlog::info("No fault on the connected robot");
+        }
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/basics3_primitive_execution.cpp

@@ -0,0 +1,207 @@
+/**
+ * @example basics3_primitive_execution.cpp
+ * This tutorial executes several basic robot primitives (unit skills). For detailed documentation
+ * on all available primitives, please see [Flexiv Primitives](https://www.flexiv.com/primitives/).
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <algorithm>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial executes several basic robot primitives (unit "
+        "skills). For detailed documentation on all available primitives, please see [Flexiv "
+        "Primitives](https://www.flexiv.com/primitives/).\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Execute Primitives
+        // =========================================================================================
+        // Primitives can only be executed on single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // (1) Move robot to home pose
+        // -----------------------------------------------------------------------------------------
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Switch to primitive execution mode
+        robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+
+        // (2) Move robot joints to target positions
+        // -----------------------------------------------------------------------------------------
+        // Required parameters:
+        //     target: final joint positions, unit: degrees
+        //         {arm joint positions}, {external axis joint positions}
+        // Optional parameters:
+        //     waypoints: waypoints to pass before reaching the target
+        //         (same format as above, but can be more than one)
+        //     vel: TCP linear velocity, unit: m/s
+        spdlog::info("Executing primitive: MoveJ");
+
+        // Send command to robot
+        std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("MoveJ",
+                {
+                    {"target", rdk::JPos({30, -45, 0, 90, 0, 40, 30}, {-50, 30})},
+                    {"waypoints",
+                        std::vector<rdk::JPos> {rdk::JPos({10, -30, 10, 30, 10, 15, 10}, {-15, 10}),
+                            rdk::JPos({20, -60, -10, 60, -10, 30, 20}, {-30, 20})}},
+                });
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // Most primitives won't exit by themselves and require users to explicitly trigger
+        // transitions based on specific primitive states. Here we check if the primitive state
+        // [reachedTarget] becomes true and trigger the transition manually by sending a new
+        // primitive command.
+        while (true) {
+            const auto primitive_states = robot.primitive_states();
+            if (rdk::utility::PrimitiveStateTrueForGroups(
+                    primitive_states, "reachedTarget")) {
+                break;
+            }
+
+            // Print current primitive states
+            spdlog::info("Current primitive states:");
+            for (const auto& [group, pt_states] : primitive_states) {
+                std::cout << rdk::kJointGroupNames.at(group) << ":" << std::endl;
+                std::cout << "primitiveName: " << pt_states.pt_name << std::endl;
+                for (const auto& [name, value] : pt_states.names_and_values) {
+                    std::cout << name << ": " << rdk::utility::FlexivTypes2Str(value);
+                    std::cout << std::endl;
+                }
+            }
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+
+        // (3) Move robot TCP to a target pose in world (base) frame
+        // -----------------------------------------------------------------------------------------
+        // Required parameters:
+        //     target: final TCP pose, unit: m and degrees
+        //         {pos_x, pos_y, pos_z}, {rot_x, rot_y, rot_z}, {ref_frame, ref_point}
+        // Optional parameters:
+        //     waypoints: waypoints to pass before reaching the target
+        //         (same format as above, but can be more than one)
+        //     vel: TCP linear velocity, unit: m/s
+        // NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis
+        spdlog::info("Executing primitive: MoveL");
+
+        // Send command to robot
+        pt_args.clear();
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("MoveL",
+                {{"target",
+                     rdk::Coord({0.3, -0.1, 0.2}, {160, 20, 180}, {"WORLD", "WORLD_ORIGIN"})},
+                    {"waypoints",
+                        std::vector<rdk::Coord> {rdk::Coord({0.1, 0.1, 0.1}, {-160, -20, 180},
+                                                     {"WORLD", "WORLD_ORIGIN"}),
+                            rdk::Coord({0.3, 0.2, 0.1}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})}},
+
+                    {"vel", 0.6}, {"zoneRadius", "Z50"}});
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // Wait for reached target
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "reachedTarget")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+
+        // (4) Another MoveL that uses TCP frame
+        // -----------------------------------------------------------------------------------------
+        // In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START,
+        // which represents the current TCP frame
+        spdlog::info("Executing primitive: MoveL");
+
+        // Example to convert target quaternion [w,x,y,z] to Euler ZYX using utility functions
+        std::array<double, 4> targetQuat = {0.9185587, 0.1767767, 0.3061862, 0.1767767};
+        // ZYX = [30, 30, 30] degrees
+        auto targetEulerDeg = rdk::utility::Rad2Deg(rdk::utility::Quat2EulerZYX(targetQuat));
+
+        // Send command to robot. This motion will hold current TCP position and only do rotation
+        pt_args.clear();
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("MoveL",
+                {
+                    {"target", rdk::Coord({0.0, 0.0, 0.0}, targetEulerDeg, {"TRAJ", "START"})},
+                    {"vel", 0.2},
+                });
+        }
+        robot.ExecutePrimitive(pt_args);
+        // Wait for reached target
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "reachedTarget")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+
+        // All done, stop robot and put into IDLE mode
+        robot.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/basics4_plan_execution.cpp

@@ -0,0 +1,152 @@
+/**
+ * @example basics4_plan_execution.cpp
+ * This tutorial executes a plan selected by the user from a list of available plans. A plan is a
+ * pre-written script to execute a series of robot primitives with pre-defined transition conditions
+ * between 2 adjacent primitives. Users can use Flexiv Elements to compose their own plan and assign
+ * to the robot, which will appear in the plan list.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial executes a plan selected by the user from a "
+        "list of available plans. A plan is a pre-written script to execute a series of robot "
+        "primitives with pre-defined transition conditions between 2 adjacent primitives. Users "
+        "can use Flexiv Elements to compose their own plan and assign to the robot, which will "
+        "appear in the plan list.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Execute Plans
+        // =========================================================================================
+        // Switch to plan execution mode
+        robot.SwitchMode(rdk::Mode::NRT_PLAN_EXECUTION);
+
+        while (true) {
+            // Monitor fault on the connected robot
+            if (robot.fault()) {
+                throw std::runtime_error("Fault occurred on the connected robot, exiting ...");
+            }
+
+            // Get user input
+            spdlog::info("Choose an action:");
+            std::cout << "[1] Show available plans" << std::endl;
+            std::cout << "[2] Execute a plan by index" << std::endl;
+            std::cout << "[3] Execute a plan by name" << std::endl;
+
+            std::string inputBuffer;
+            std::cin >> inputBuffer;
+            int userInput = std::stoi(inputBuffer);
+
+            switch (userInput) {
+                // Get and show plan list
+                case 1: {
+                    auto plan_list = robot.plan_list();
+                    for (size_t i = 0; i < plan_list.size(); i++) {
+                        std::cout << "[" << i << "] " << plan_list[i] << std::endl;
+                    }
+                    std::cout << std::endl;
+                } break;
+                // Execute plan by index
+                case 2: {
+                    spdlog::info("Enter plan index to execute:");
+                    int index;
+                    std::cin >> index;
+                    // Allow the plan to continue its execution even if the RDK program is closed or
+                    // the connection is lost
+                    robot.ExecutePlan(index, true);
+
+                    // Print plan info while the current plan is running
+                    while (robot.busy()) {
+                        spdlog::info("Current plan info:");
+                        std::cout << robot.plan_info() << std::endl;
+                        std::this_thread::sleep_for(std::chrono::seconds(1));
+                    }
+                } break;
+                // Execute plan by name
+                case 3: {
+                    spdlog::info("Enter plan name to execute:");
+                    std::string name;
+                    std::cin >> name;
+                    // Allow the plan to continue its execution even if the RDK program is closed or
+                    // the connection is lost
+                    robot.ExecutePlan(name, true);
+
+                    // Print plan info while the current plan is running
+                    while (robot.busy()) {
+                        spdlog::info("Current plan info:");
+                        std::cout << robot.plan_info() << std::endl;
+                        std::this_thread::sleep_for(std::chrono::seconds(1));
+                    }
+                } break;
+                default:
+                    spdlog::warn("Invalid input");
+                    break;
+            }
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/basics5_zero_force_torque_sensors.cpp

@@ -0,0 +1,123 @@
+/**
+ * @example basics5_zero_force_torque_sensors.cpp
+ * This tutorial zeros the robot's force and torque sensors, which is a recommended (but not
+ * mandatory) step before any operations that require accurate force/torque measurement.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+#include <algorithm>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial zeros the robot's force and torque sensors, "
+        "which is a recommended (but not mandatory) step before any operations that require "
+        "accurate force/torque measurement.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Zero Sensors
+        // =========================================================================================
+        // Get and print the current TCP force/moment readings
+        for (const auto& [group, states] : robot.states()) {
+            spdlog::info(
+                "[{}] TCP force and moment reading in world frame BEFORE sensor zeroing: {} N-Nm",
+                rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(states.tcp_wrench));
+        }
+
+        // Primitives can only be executed on single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors
+        robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+        std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // WARNING: during the process, the robot must not contact anything, otherwise the result
+        // will be inaccurate and affect following operations
+        spdlog::warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot");
+
+        // Wait for primitive to finish
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Sensor zeroing complete");
+
+        // Get and print the current TCP force/moment readings
+        for (const auto& [group, _] : single_arm_groups) {
+            spdlog::info(
+                "[{}] TCP force and moment reading in world frame AFTER sensor zeroing: {} N-Nm",
+                rdk::kJointGroupNames.at(group),
+                rdk::utility::Arr2Str(robot.states().at(group).tcp_wrench));
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/basics6_gripper_control.cpp

@@ -0,0 +1,250 @@
+/**
+ * @example basics6_gripper_control.cpp
+ * This tutorial does position and force (if available) control of grippers supported by Flexiv.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/gripper.hpp>
+#include <flexiv/rdk/tool.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <atomic>
+#include <limits>
+
+using namespace flexiv;
+
+namespace {
+/** Global flag: whether the gripper control tasks are finished */
+std::atomic<bool> g_finished = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn] [gripper_device_name] [gripper_tool_name]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect to. Remove any space, for example: Enlight-L-123456" << std::endl;
+    std::cout << "    gripper_device_name: Full name of the device representing the gripper, can be found in Flexiv Elements->Settings->Device" << std::endl;
+    std::cout << "    gripper_tool_name: Full name of the tool representing the gripper, can be found in Flexiv Elements->Settings->Tool" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Print gripper states data @ 1Hz */
+void PrintGripperStates(rdk::Gripper& gripper)
+{
+    while (!g_finished) {
+        const auto gripper_states = gripper.states();
+        spdlog::info("Current gripper states:");
+        for (const auto& [group, states] : gripper_states) {
+            std::cout << "[" << rdk::kJointGroupNames.at(group) << "]\n" << states << std::endl;
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 4 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+    std::string gripper_device_name = argv[2];
+    std::string gripper_tool_name = argv[3];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial does position and force (if available) "
+        "control of grippers supported by Flexiv.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Gripper Control
+        // =========================================================================================
+        // Instantiate gripper control interface
+        rdk::Gripper gripper(robot);
+
+        // Instantiate tool interface. Gripper is categorized as both a device and a tool. The
+        // device attribute allows a gripper to be interactively controlled by the user; whereas the
+        // tool attribute tells the robot to account for its mass properties and TCP location.
+        rdk::Tool tool(robot);
+
+        // Grippers can only be assigned to single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Enable the specified gripper as a device. This is equivalent to enabling the specified
+        // gripper in Flexiv Elements -> Settings -> Device
+        spdlog::info("Enabling gripper device [{}] for all single-arm groups", gripper_device_name);
+        for (const auto& [group, _] : single_arm_groups) {
+            gripper.Enable(group, gripper_device_name);
+        }
+
+        // Print parameters of the enabled gripper
+        spdlog::info("Gripper params:");
+        const auto gripper_params = gripper.params();
+        for (const auto& [group, params] : gripper_params) {
+            std::cout << "[" << rdk::kJointGroupNames.at(group) << "]\n"
+                      << std::fixed << std::setprecision(2) << "{\n"
+                      << "name: " << params.name << "\nmin_width: " << params.min_width
+                      << "\nmax_width: " << params.max_width << "\nmin_force: " << params.min_force
+                      << "\nmax_force: " << params.max_force << "\nmin_vel: " << params.min_vel
+                      << "\nmax_vel: " << params.max_vel << "\n}" << std::endl;
+        }
+
+        // Switch robot tool to gripper so the gravity compensation and TCP location is updated
+        spdlog::info("Switching robot tool to [{}] for all single-arm groups", gripper_tool_name);
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, gripper_tool_name);
+        }
+
+        // User needs to determine if this gripper requires manual initialization
+        int choice = 0;
+        spdlog::info(
+            "Manually trigger initialization for the gripper now? Choose Yes if it's a 48v Grav "
+            "gripper");
+        std::cout << "[1] No, it has already initialized automatically when power on" << std::endl;
+        std::cout << "[2] Yes, it does not initialize itself when power on" << std::endl;
+        std::cin >> choice;
+
+        // Trigger manual initialization based on choice
+        if (choice == 1) {
+            spdlog::info("Skipped manual initialization");
+        } else if (choice == 2) {
+            for (const auto& [group, _] : single_arm_groups) {
+                gripper.Init(group);
+            }
+            // User determines if the manual initialization is finished
+            spdlog::info(
+                "Triggered manual initialization, press Enter when the initialization is finished "
+                "to continue");
+            std::cin.get();
+            std::cin.get();
+        } else {
+            spdlog::error("Invalid choice");
+            return 1;
+        }
+
+        // Start a separate thread to print gripper states. Use a scope guard so the thread is
+        // always signaled and joined when leaving this scope, including via an exception thrown by
+        // gripper.Move() etc. Otherwise the std::thread destructor would run while still joinable
+        // and call std::terminate(), aborting before the exception reaches the catch block below.
+        std::thread print_thread(PrintGripperStates, std::ref(gripper));
+        auto join_print_thread = [&]() {
+            g_finished = true;
+            if (print_thread.joinable()) {
+                print_thread.join();
+            }
+        };
+        std::shared_ptr<void> print_thread_guard(nullptr, [&](void*) { join_print_thread(); });
+
+        // Position control
+        spdlog::info("Closing gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        spdlog::info("Opening gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+
+        // Stop
+        spdlog::info("Closing gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        spdlog::info("Stopping gripper");
+        for (const auto& [group, _] : gripper_params) {
+            gripper.Stop(group);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        spdlog::info("Closing gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        spdlog::info("Opening gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        spdlog::info("Stopping gripper");
+        for (const auto& [group, _] : gripper_params) {
+            gripper.Stop(group);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+
+        // Force control, if available (sensed force is not zero)
+        const auto gripper_states = gripper.states();
+        bool has_force_control = false;
+        for (const auto& [_, states] : gripper_states) {
+            if (fabs(states.force) > std::numeric_limits<double>::epsilon()) {
+                has_force_control = true;
+                break;
+            }
+        }
+        if (has_force_control) {
+            spdlog::info("Gripper running zero force control");
+            for (const auto& [group, _] : gripper_states) {
+                gripper.Grasp(group, 0);
+            }
+            // Exit after 10 seconds
+            std::this_thread::sleep_for(std::chrono::seconds(10));
+        }
+
+        // Finished, exit all threads
+        for (const auto& [group, _] : gripper_states) {
+            gripper.Stop(group);
+        }
+        spdlog::info("Program finished");
+        // The print thread is signaled and joined by the scope guard above.
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/basics7_update_robot_tool.cpp

@@ -0,0 +1,168 @@
+/**
+ * @example basics7_update_robot_tool.cpp
+ * This tutorial shows how to online update and interact with the robot tools. All changes made to
+ * the robot tool system will take effect immediately without needing to reboot. However, the robot
+ * must be put into IDLE mode when making these changes.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/tool.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to online update and interact with "
+        "the robot tools. All changes made to the robot tool system will take effect immediately "
+        "without needing to reboot. However, the robot must be put into IDLE mode when making "
+        "these changes.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Update Robot Tool
+        // =========================================================================================
+        // Make sure the robot is in IDLE mode
+        robot.SwitchMode(rdk::Mode::IDLE);
+
+        // Instantiate tool interface
+        rdk::Tool tool(robot);
+
+        // Tools can only be assigned to single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Get and print a list of already configured tools currently in the robot's tools pool
+        spdlog::info("All configured tools:");
+        auto tool_list = tool.list();
+        for (size_t i = 0; i < tool_list.size(); i++) {
+            std::cout << "[" << i << "] " << tool_list[i] << std::endl;
+        }
+        std::cout << std::endl;
+
+        // Get and print the current active tool
+        for (const auto& [group, _] : single_arm_groups) {
+            spdlog::info(
+                "[{}] Current active tool: {}", rdk::kJointGroupNames.at(group), tool.name(group));
+        }
+
+        // Set name and parameters for a new tool
+        std::string new_tool_name = "ExampleTool1";
+        rdk::ToolParams new_tool_params;
+        new_tool_params.mass = 0.9;
+        new_tool_params.CoM = {0.0, 0.0, 0.057};
+        new_tool_params.inertia = {2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0};
+        new_tool_params.tcp_location = {0.0, -0.207, 0.09, 0.7071068, 0.7071068, 0.0, 0.0};
+
+        // If there's already a tool with the same name in the robot's tools pool, then remove it
+        // first, because duplicate tool names are not allowed
+        if (tool.exist(new_tool_name)) {
+            spdlog::warn(
+                "Tool with the same name [{}] already exists, removing it now", new_tool_name);
+            // Switch to other tool or no tool (Flange) before removing the current tool
+            for (const auto& [group, _] : single_arm_groups) {
+                tool.Switch(group, "Flange");
+            }
+            tool.Remove(new_tool_name);
+        }
+
+        // Add the new tool
+        spdlog::info("Adding new tool [{}] to the robot", new_tool_name);
+        tool.Add(new_tool_name, new_tool_params);
+
+        // Get and print the tools list again, the new tool should appear at the end
+        spdlog::info("All configured tools:");
+        tool_list = tool.list();
+        for (size_t i = 0; i < tool_list.size(); i++) {
+            std::cout << "[" << i << "] " << tool_list[i] << std::endl;
+        }
+        std::cout << std::endl;
+
+        // Switch to the newly added tool, i.e. set it as the active tool
+        spdlog::info("Switching to tool [{}]", new_tool_name);
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, new_tool_name);
+        }
+
+        // Get and print the current active tool again, should be the new tool
+        for (const auto& [group, _] : single_arm_groups) {
+            spdlog::info(
+                "[{}] Current active tool: {}", rdk::kJointGroupNames.at(group), tool.name(group));
+        }
+
+        // Switch to other tool or no tool (Flange) before removing the current tool
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, "Flange");
+        }
+
+        // Clean up by removing the new tool
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        spdlog::info("Removing tool [{}]", new_tool_name);
+        tool.Remove(new_tool_name);
+
+        spdlog::info("Program finished");
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/basics8_global_variables.cpp

@@ -0,0 +1,129 @@
+/**
+ * @example basics8_global_variables.cpp
+ * This tutorial shows how to get and set global variables.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to get and set global variables.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Get existing global variables
+        // =========================================================================================
+        auto global_vars = robot.global_variables();
+        if (global_vars.empty()) {
+            spdlog::warn("No global variables available");
+            return 1;
+        } else {
+            spdlog::info("Existing global variables and their original values:");
+            for (const auto& var : global_vars) {
+                std::cout << var.first << ": " << rdk::utility::FlexivTypes2Str(var.second)
+                          << std::endl;
+            }
+        }
+
+        // Set global variables
+        // =========================================================================================
+        ///@warning These specified global variables need to be created first using Flexiv Elements
+        spdlog::info("Setting new values to existing global variables");
+        robot.SetGlobalVariables({
+            {"test_bool", 1},
+            {"test_int", 100},
+            {"test_double", 100.123},
+            {"test_string", "Flexiv"},
+            {"test_int_vec", std::vector<int> {1, 2, 3}},
+            {"test_double_vec", std::vector<double> {1.1, 2.2, 3.3}},
+            {"test_string_vec", std::vector<std::string> {"Go", "Flexiv", "Go!"}},
+            {"test_pose", std::vector<double> {0.1, -0.2, 0.3, -90, -45, 120}},
+            {"test_coord", rdk::Coord({0.1, -0.2, 0.3}, {-90, -45, 120}, {"WORLD", "WORLD_ORIGIN"},
+                               {1, 2, 3, 4, 5, 6, 7}, {10, 20, 0, 0, 0, 0})},
+            {"test_coord_array",
+                std::vector<rdk::Coord> {rdk::Coord({1, 2, 3}, {4, 5, 6}, {"WORK", "WorkCoord0"}),
+                    rdk::Coord({10, 20, 30}, {40, 50, 60}, {"WORLD", "WORLD_ORIGIN"},
+                        {1, 2, 3, 4, 5, 6, 7}, {10, 20, 0, 0, 0, 0}),
+                    rdk::Coord({3, 2, 1}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})}},
+        });
+
+        // Get updated global variables
+        // =========================================================================================
+        global_vars = robot.global_variables();
+        if (global_vars.empty()) {
+            spdlog::warn("No global variables available");
+            return 1;
+        } else {
+            spdlog::info("Updated global variables:");
+            for (const auto& var : global_vars) {
+                std::cout << var.first << ": " << rdk::utility::FlexivTypes2Str(var.second)
+                          << std::endl;
+            }
+        }
+
+        spdlog::info("Program finished");
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/basics9_logging_behavior.cpp

@@ -0,0 +1,97 @@
+/**
+ * @example basics9_logging_behavior.cpp
+ * This tutorial shows how to change the logging behaviors of RDK client.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+#include <spdlog/async.h>
+#include <spdlog/sinks/stdout_color_sinks.h>
+#include <spdlog/sinks/basic_file_sink.h>
+
+#include <iostream>
+#include <string>
+
+using namespace flexiv;
+
+namespace {
+constexpr char kDefaultLogPattern[] = "[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v";
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to change the logging behaviors of "
+        "RDK client.\n");
+
+    // Suppress log messages from RDK client
+    // =========================================================================================
+    // Change log level to suppress certain type of log messages from RDK client. The log level
+    // setting will apply globally to all following spdlog messages
+    spdlog::set_level(spdlog::level::warn); ///< Suppress info messages
+    // messages spdlog::set_level(spdlog::level::off); ///< Suppress all messages
+
+    // Instantiate RDK client, all info messages are suppressed
+    try {
+        rdk::Robot robot(robot_sn);
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+    }
+
+    // Output all log messages to a file
+    // =========================================================================================
+    // Create a logger that outputs the messages to both the console and a log file
+    auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
+    auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>("example.log", true);
+    std::vector<spdlog::sink_ptr> sinks {console_sink, file_sink};
+    // Since writing to file takes some time, so we use async logger to avoid blocking the program
+    spdlog::init_thread_pool(10240, 1);
+    auto tp = spdlog::thread_pool();
+    auto logger
+        = std::make_shared<spdlog::async_logger>("global_logger", sinks.begin(), sinks.end(), tp);
+    // Use default pattern for log messages
+    logger->set_pattern(kDefaultLogPattern);
+
+    // Use the logger configured above as the default global logger
+    spdlog::set_default_logger(logger);
+
+    // Allow all messages
+    spdlog::set_level(spdlog::level::info);
+
+    // Instantiate RDK client again, all messages are printed to console and output to a log file
+    try {
+        rdk::Robot robot(robot_sn);
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+    }
+
+    spdlog::warn("This message should also appear in the log file");
+    spdlog::info("Program finished");
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/intermediate1_realtime_joint_position_control.cpp

@@ -0,0 +1,199 @@
+/**
+ * @example intermediate1_realtime_joint_position_control.cpp
+ * This tutorial runs real-time joint position control to hold or sine-sweep all robot joints.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** Sine-sweep trajectory amplitude and frequency */
+constexpr double kSineAmp = 0.035;
+constexpr double kSineFreq = 0.3;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--hold]" << std::endl;
+    std::cout << "    --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
+    const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
+{
+    // Local periodic loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtJointPositionCmd> rt_cmds;
+
+        if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
+            throw std::invalid_argument(
+                "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
+        }
+
+        for (const auto& [group, init_pos] : all_init_pos) {
+            std::vector<double> target_pos(init_pos.size());
+            std::vector<double> target_vel(init_pos.size());
+            std::vector<double> target_acc(init_pos.size());
+
+            if (motion_type == "hold") {
+                target_pos = init_pos;
+            } else {
+                for (size_t i = 0; i < target_pos.size(); ++i) {
+                    target_pos[i]
+                        = init_pos[i]
+                          + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
+                }
+            }
+            rt_cmds[group] = rdk::RtJointPositionCmd(target_pos, target_vel, target_acc);
+        }
+
+        // Send target joint position to RDK server
+        robot.StreamJointPosition(rt_cmds);
+
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time joint position control to hold "
+        "or sine-sweep all robot joints.\n");
+
+    // Type of motion specified by user
+    std::string motion_type = "";
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
+        spdlog::info("Robot holding current pose");
+        motion_type = "hold";
+    } else {
+        spdlog::info("Robot running joint sine-sweep");
+        motion_type = "sine-sweep";
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Real-time Joint Position Control
+        // =========================================================================================
+        // Direct joint control can be executed by single-arm joint groups and the external axis
+        auto exe_groups = robot.info().single_arm_groups;
+        if (exe_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+        // The external axis joint group (if it exists) also supports direct joint control
+        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
+            exe_groups.emplace(rdk::JointGroup::EXT_AXIS,
+                robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
+        }
+
+        // Switch to real-time joint position control mode
+        robot.SwitchMode(rdk::Mode::RT_JOINT_POSITION);
+
+        // Set initial joint positions
+        std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : exe_groups) {
+            all_init_pos[group] = robot_states.at(group).q;
+            spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
+                rdk::utility::Vec2Str(all_init_pos.at(group)));
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
+                              std::cref(all_init_pos)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/intermediate2_realtime_joint_impedance_control.cpp

@@ -0,0 +1,226 @@
+/**
+ * @example intermediate2_realtime_joint_impedance_control.cpp
+ * This tutorial runs real-time joint impedance control to hold or sine-sweep all robot joints.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** Sine-sweep trajectory amplitude and frequency */
+constexpr double kSineAmp = 0.035;
+constexpr double kSineFreq = 0.3;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--hold]" << std::endl;
+    std::cout << "    --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
+    const std::map<rdk::JointGroup, std::string>& single_arm_groups,
+    const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
+{
+    // Local periodic loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtJointPositionCmd> rt_cmds;
+
+        if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
+            throw std::invalid_argument(
+                "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
+        }
+
+        for (const auto& [group, init_pos] : all_init_pos) {
+            std::vector<double> target_pos(init_pos.size());
+            std::vector<double> target_vel(init_pos.size());
+            std::vector<double> target_acc(init_pos.size());
+
+            if (motion_type == "hold") {
+                target_pos = init_pos;
+            } else {
+                for (size_t i = 0; i < target_pos.size(); ++i) {
+                    target_pos[i]
+                        = init_pos[i]
+                          + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
+                }
+            }
+
+            rt_cmds[group] = rdk::RtJointPositionCmd(target_pos, target_vel, target_acc);
+        }
+
+        // Reduce stiffness to half of nominal values after 5 seconds
+        if (loop_counter == 5000) {
+            for (const auto& [group, _] : single_arm_groups) {
+                auto new_Kq = robot.info().K_q_nom.at(group);
+                for (auto& v : new_Kq) {
+                    v *= 0.5;
+                }
+                robot.SetJointImpedance(group, new_Kq);
+                spdlog::info("[{}] Joint stiffness set to: [{}]", rdk::kJointGroupNames.at(group),
+                    rdk::utility::Vec2Str(new_Kq));
+            }
+        }
+
+        // Reset stiffness to nominal values after another 5 seconds
+        if (loop_counter == 10000) {
+            for (const auto& [group, _] : single_arm_groups) {
+                const auto nominal_Kq = robot.info().K_q_nom.at(group);
+                robot.SetJointImpedance(group, nominal_Kq);
+                spdlog::info("[{}] Joint stiffness reset to nominal: [{}]",
+                    rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(nominal_Kq));
+            }
+        }
+
+        // Send commands
+        robot.StreamJointPosition(rt_cmds);
+
+        // Increment loop counter
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time joint impedance control to "
+        "hold or sine-sweep all robot joints.\n");
+
+    // Type of motion specified by user
+    std::string motion_type = "";
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
+        spdlog::info("Robot holding current pose");
+        motion_type = "hold";
+    } else {
+        spdlog::info("Robot running joint sine-sweep");
+        motion_type = "sine-sweep";
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Real-time Joint Impedance Control
+        // =========================================================================================
+        // Direct joint control can be executed by single-arm joint groups and the external axis
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+        // The external axis joint group (if it exists) also supports direct joint control
+        auto exe_groups = single_arm_groups;
+        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
+            exe_groups.emplace(
+                rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
+        }
+
+        // Switch to real-time joint impedance control mode
+        robot.SwitchMode(rdk::Mode::RT_JOINT_IMPEDANCE);
+
+        // Set initial joint positions
+        std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : exe_groups) {
+            all_init_pos[group] = robot_states.at(group).q;
+            spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
+                rdk::utility::Vec2Str(all_init_pos.at(group)));
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
+                              std::cref(single_arm_groups), std::cref(all_init_pos)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/intermediate3_realtime_joint_torque_control.cpp

@@ -0,0 +1,220 @@
+/**
+ * @example intermediate3_realtime_joint_torque_control.cpp
+ * This tutorial runs real-time joint torque control to hold or sine-sweep all robot joints. An
+ * outer position loop is used to generate joint torque commands. This outer position loop + inner
+ * torque loop together is also known as an impedance controller.
+ * @warning The impedance controller in this example is for demo purpose only and has no performance
+ * guarantee. To use the robot's built-in high-performance joint impedance controller, please refer
+ * to intermediate2_realtime_joint_impedance_control.cpp.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** Outer position loop (impedance) gains, values are only for demo purpose */
+const std::vector<double> kImpedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0};
+const std::vector<double> kImpedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0};
+
+/** Sine-sweep trajectory amplitude and frequency */
+constexpr double kSineAmp = 0.035;
+constexpr double kSineFreq = 0.3;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--hold]" << std::endl;
+    std::cout << "    --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
+    const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
+{
+    // Local periodic loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
+            throw std::invalid_argument(
+                "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtJointTorqueCmd> rt_cmds;
+        const auto all_states = robot.states();
+        for (const auto& [group, init_pos] : all_init_pos) {
+            std::vector<double> target_pos(init_pos.size());
+            if (motion_type == "hold") {
+                target_pos = init_pos;
+            } else {
+                for (size_t i = 0; i < target_pos.size(); ++i) {
+                    target_pos[i]
+                        = init_pos[i]
+                          + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
+                }
+            }
+
+            // Compute target joint torque using outer position loop (impedance)
+            std::vector<double> target_torque(target_pos.size());
+            // Set target torque for external axis to zero, as the demo impedance gains are defined
+            // for only the arms
+            if (group != rdk::JointGroup::EXT_AXIS) {
+                const auto& states = all_states.at(group);
+                for (size_t i = 0; i < target_torque.size(); ++i) {
+                    target_torque[i] = kImpedanceKp[i] * (target_pos[i] - states.q[i])
+                                       - kImpedanceKd[i] * states.dtheta[i];
+                }
+            }
+
+            rt_cmds[group] = rdk::RtJointTorqueCmd(target_torque, true, true);
+        }
+
+        // Send target joint torque to RDK server
+        robot.StreamJointTorque(rt_cmds);
+
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time joint torque control to hold "
+        "or sine-sweep all robot joints. An outer position loop is used to generate joint torque "
+        "commands. This outer position loop + inner torque loop together is also known as an "
+        "impedance controller.\n");
+
+    // Type of motion specified by user
+    std::string motion_type = "";
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
+        spdlog::info("Robot holding current pose");
+        motion_type = "hold";
+    } else {
+        spdlog::info("Robot running joint sine-sweep");
+        motion_type = "sine-sweep";
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Real-time Joint Torque Control
+        // =========================================================================================
+        // Direct joint control can be executed by single-arm joint groups and the external axis
+        auto exe_groups = robot.info().single_arm_groups;
+        if (exe_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+        // The external axis joint group (if it exists) also supports direct joint control
+        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
+            exe_groups.emplace(
+                rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
+        }
+
+        // Switch to real-time joint torque control mode
+        robot.SwitchMode(rdk::Mode::RT_JOINT_TORQUE);
+
+        // Set initial joint positions
+        std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : exe_groups) {
+            all_init_pos[group] = robot_states.at(group).q;
+            spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
+                rdk::utility::Vec2Str(all_init_pos.at(group)));
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
+                              std::cref(all_init_pos)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/intermediate4_realtime_joint_floating.cpp

@@ -0,0 +1,167 @@
+/**
+ * @example intermediate4_realtime_joint_floating.cpp
+ * This tutorial runs real-time joint floating with gentle velocity damping, gravity compensation,
+ * and soft protection against position limits. This example is ideal for verifying the system's
+ * whole-loop real-timeliness, accuracy of the robot dynamics model, and joint torque control
+ * performance. If everything works well, all joints should float smoothly.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** Joint velocity damping gains for floating */
+const std::vector<double> kFloatingDamping = {10.0, 10.0, 5.0, 5.0, 1.0, 1.0, 1.0};
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot, const std::map<rdk::JointGroup, std::string>& exe_groups)
+{
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtJointTorqueCmd> rt_cmds;
+        const auto all_states = robot.states();
+        for (const auto& [group, _] : exe_groups) {
+            const auto& states = all_states.at(group);
+            std::vector<double> target_torque(states.q.size());
+
+            // Don't add damping for external axis
+            if (group != rdk::JointGroup::EXT_AXIS) {
+                // Add some velocity damping
+                for (size_t i = 0; i < target_torque.size(); ++i) {
+                    target_torque[i] += -kFloatingDamping[i] * states.dtheta[i];
+                }
+            }
+
+            rt_cmds[group] = rdk::RtJointTorqueCmd(target_torque, true, true);
+        }
+
+        // Send target joint torque to RDK server, enable gravity compensation and joint limits soft
+        // protection
+        robot.StreamJointTorque(rt_cmds);
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time joint floating with gentle "
+        "velocity damping, gravity compensation, and soft protection against position limits. This "
+        "example is ideal for verifying the system's whole-loop real-timeliness, accuracy of the "
+        "robot dynamics model, and joint torque control performance. If everything works well, all "
+        "joints should float smoothly.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Real-time Joint Floating
+        // =========================================================================================
+        // Direct joint control can be executed by single-arm joint groups and the external axis
+        auto exe_groups = robot.info().single_arm_groups;
+        if (exe_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+        // The external axis joint group (if exists) also supports direct joint control
+        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
+            exe_groups.emplace(
+                rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
+        }
+
+        // Switch to real-time joint torque control mode
+        robot.SwitchMode(rdk::Mode::RT_JOINT_TORQUE);
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::cref(exe_groups)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/intermediate5_realtime_cartesian_pure_motion_control.cpp

@@ -0,0 +1,346 @@
+/**
+ * @example intermediate5_realtime_cartesian_pure_motion_control.cpp
+ * This tutorial runs real-time Cartesian-space pure motion control to hold or sine-sweep the robot
+ * TCP. A simple collision detection is also included.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <cmath>
+#include <thread>
+#include <atomic>
+#include <algorithm>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop frequency [Hz] */
+constexpr size_t kLoopFreq = 1000;
+
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** TCP sine-sweep amplitude [m] */
+constexpr double kSwingAmp = 0.1;
+
+/** TCP sine-sweep frequency [Hz] */
+constexpr double kSwingFreq = 0.3;
+
+/** External TCP force threshold for collision detection, value is only for demo purpose [N] */
+constexpr double kExtForceThreshold = 10.0;
+
+/** External joint torque threshold for collision detection, value is only for demo purpose [Nm] */
+constexpr double kExtTorqueThreshold = 5.0;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--hold] [--collision]" << std::endl;
+    std::cout << "    --hold: robot holds current TCP pose, otherwise do a sine-sweep" << std::endl;
+    std::cout << "    --collision: enable collision detection, robot will stop upon collision" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot,
+    const std::map<rdk::JointGroup, std::string>& single_arm_groups,
+    const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_pose,
+    const std::map<rdk::JointGroup, std::vector<double>>& all_init_q, bool enable_hold,
+    bool enable_collision)
+{
+    // Local periodic loop counter
+    static uint64_t loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
+        for (const auto& [group, init_pose] : all_init_pose) {
+            auto target_pose = init_pose;
+
+            // Sine-sweep TCP along Y axis
+            if (!enable_hold) {
+                target_pose[1]
+                    = init_pose[1]
+                      + kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
+            }
+            // Otherwise robot TCP will hold at initial pose
+
+            rt_cmds[group] = rdk::RtCartesianCmd(target_pose);
+        }
+
+        // Send command. Calling this method with only target pose input results in pure motion
+        // control
+        robot.StreamCartesianMotionForce(rt_cmds);
+
+        // Do the following operations in sequence for every 20 seconds
+        switch (loop_counter % (20 * kLoopFreq)) {
+            // Online change reference joint positions at 3 seconds
+            case (3 * kLoopFreq): {
+                const std::vector<double> ref_q
+                    = {0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658};
+                for (const auto& [group, _] : single_arm_groups) {
+                    robot.SetNullSpacePosture(group, ref_q);
+                }
+                spdlog::info("Reference joint positions updated for all groups");
+            } break;
+            // Online change stiffness to half of nominal at 6 seconds
+            case (6 * kLoopFreq): {
+                for (const auto& [group, _] : single_arm_groups) {
+                    auto new_K = robot.info().K_x_nom.at(group);
+                    for (auto& v : new_K) {
+                        v *= 0.5;
+                    }
+                    robot.SetCartesianImpedance(group, new_K);
+                    spdlog::info("[{}] Cartesian stiffness set to: [{}]",
+                        rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(new_K));
+                }
+            } break;
+            // Online change to another reference joint positions at 9 seconds
+            case (9 * kLoopFreq): {
+                const std::vector<double> ref_q
+                    = {-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658};
+                for (const auto& [group, _] : single_arm_groups) {
+                    robot.SetNullSpacePosture(group, ref_q);
+                }
+                spdlog::info("Reference joint positions updated for all groups");
+            } break;
+            // Online reset stiffness to nominal at 12 seconds
+            case (12 * kLoopFreq): {
+                for (const auto& [group, _] : single_arm_groups) {
+                    const auto nominal_K = robot.info().K_x_nom.at(group);
+                    robot.SetCartesianImpedance(group, nominal_K);
+                    spdlog::info("[{}] Cartesian stiffness reset to nominal: [{}]",
+                        rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(nominal_K));
+                }
+            } break;
+            // Online reset reference joint positions to nominal at 14 seconds
+            case (14 * kLoopFreq): {
+                for (const auto& [group, init_q] : all_init_q) {
+                    robot.SetNullSpacePosture(group, init_q);
+                    spdlog::info("[{}] Reference joint positions reset to initial: [{}]",
+                        rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(init_q));
+                }
+            } break;
+            // Online enable max contact wrench regulation at 16 seconds
+            case (16 * kLoopFreq): {
+                std::array<double, rdk::kCartDoF> max_wrench = {10.0, 10.0, 10.0, 2.0, 2.0, 2.0};
+                for (const auto& [group, _] : single_arm_groups) {
+                    robot.SetMaxContactWrench(group, max_wrench);
+                    spdlog::info("[{}] Max contact wrench set to: {}",
+                        rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(max_wrench));
+                }
+            } break;
+            // Disable max contact wrench regulation at 19 seconds
+            case (19 * kLoopFreq): {
+                std::array<double, rdk::kCartDoF> inf;
+                inf.fill(std::numeric_limits<double>::infinity());
+                for (const auto& [group, _] : single_arm_groups) {
+                    robot.SetMaxContactWrench(group, inf);
+                    spdlog::info("[{}] Max contact wrench regulation is disabled",
+                        rdk::kJointGroupNames.at(group));
+                }
+            } break;
+            default:
+                break;
+        }
+
+        // Simple collision detection: stop robot if collision is detected from either end-effector
+        // or robot body
+        if (enable_collision) {
+            for (const auto& [group, states] : robot.states()) {
+                bool collision_detected = false;
+                Eigen::Vector3d ext_force
+                    = {states.tcp_wrench[0], states.tcp_wrench[1], states.tcp_wrench[2]};
+                if (ext_force.norm() > kExtForceThreshold) {
+                    collision_detected = true;
+                }
+                for (const auto& v : states.tau_ext) {
+                    if (fabs(v) > kExtTorqueThreshold) {
+                        collision_detected = true;
+                    }
+                }
+
+                if (collision_detected) {
+                    robot.Stop();
+                    spdlog::warn("[{}] Collision detected, stopping robot and exit program ...",
+                        rdk::kJointGroupNames.at(group));
+                    g_stop_sched = true;
+                    break;
+                }
+            }
+        }
+
+        // Increment loop counter
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space pure motion "
+        "control to hold or sine-sweep the robot TCP. A simple collision detection is also "
+        "included.\n");
+
+    // Type of motion specified by user
+    bool enable_hold = false;
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
+        spdlog::info("Robot holding current TCP pose");
+        enable_hold = true;
+    } else {
+        spdlog::info("Robot running TCP sine-sweep");
+    }
+
+    // Whether to enable collision detection
+    bool enable_collision = false;
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--collision")) {
+        spdlog::info("Collision detection enabled");
+        enable_collision = true;
+    } else {
+        spdlog::info("Collision detection disabled");
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Zero Force-torque Sensor
+        // =========================================================================================
+        // Direct Cartesian control can only be executed by single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+        // IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
+        std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // WARNING: during the process, the robot must not contact anything, otherwise the result
+        // will be inaccurate and affect following operations
+        spdlog::warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot");
+
+        // Wait for primitive to finish
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Sensor zeroing complete");
+
+        // Configure Motion Control
+        // =========================================================================================
+        // The Cartesian motion force modes do pure motion control out of the box, thus nothing
+        // needs to be explicitly configured
+
+        // NOTE: motion control always uses robot world frame, while force control can use
+        // either world or TCP frame as reference frame
+
+        // Start Pure Motion Control
+        // =========================================================================================
+        // Switch to real-time mode for continuous motion control
+        robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
+
+        // Set all Cartesian axis(s) to motion control
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetForceControlAxis(
+                group, std::array<bool, rdk::kCartDoF> {false, false, false, false, false, false});
+        }
+
+        // Save initial poses and joint positions
+        std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
+        std::map<rdk::JointGroup, std::vector<double>> all_init_q;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : single_arm_groups) {
+            all_init_pose[group] = robot_states.at(group).tcp_pose;
+            all_init_q[group] = robot_states.at(group).q;
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(
+            std::bind(PeriodicTask, std::ref(robot), std::cref(single_arm_groups),
+                std::cref(all_init_pose), std::cref(all_init_q), enable_hold, enable_collision),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/intermediate6_realtime_cartesian_motion_force_control.cpp

@@ -0,0 +1,335 @@
+/**
+ * @example intermediate6_realtime_cartesian_motion_force_control.cpp
+ * This tutorial runs real-time Cartesian-space unified motion-force control. The Z axis of the
+ * chosen reference frame will be activated for explicit force control, while the rest axes in the
+ * same reference frame will stay motion controlled.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <cmath>
+#include <thread>
+#include <atomic>
+#include <algorithm>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** TCP sine-sweep amplitude [m] */
+constexpr double kSwingAmp = 0.1;
+
+/** TCP sine-sweep frequency [Hz] */
+constexpr double kSwingFreq = 0.3;
+
+/** Pressing force to apply during the unified motion-force control [N] */
+constexpr double kPressingForce = 5.0;
+
+/** Cartesian linear velocity used to search for contact [m/s] */
+constexpr double kSearchVelocity = 0.02;
+
+/** Maximum distance to travel when searching for contact [m] */
+constexpr double kSearchDistance = 1.0;
+
+/** Maximum contact wrench during contact search for soft contact */
+const std::array<double, rdk::kCartDoF> kMaxWrenchForContactSearch
+    = {10.0, 10.0, 10.0, 3.0, 3.0, 3.0};
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--TCP] [--polish]" << std::endl;
+    std::cout << "    --TCP: use TCP frame as reference frame for force control, otherwise use world frame" << std::endl;
+    std::cout << "    --polish: run a simple polish motion along XY plane in world frame, otherwise hold robot motion in non-force-control axes"
+              << std::endl
+              << std::endl;
+    // clang-format on
+}
+
+/** Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot,
+    const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_pose,
+    rdk::CoordType force_ctrl_frame, bool enable_polish)
+{
+    // Local periodic loop counter
+    static uint64_t loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        // Set Fz according to reference frame to achieve a "pressing down" behavior
+        double Fz = 0.0;
+        if (force_ctrl_frame == rdk::CoordType::WORLD) {
+            Fz = kPressingForce;
+        } else if (force_ctrl_frame == rdk::CoordType::TCP) {
+            Fz = -kPressingForce;
+        }
+        std::array<double, rdk::kCartDoF> target_wrench = {0.0, 0.0, Fz, 0.0, 0.0, 0.0};
+
+        std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
+        for (const auto& [group, init_pose] : all_init_pose) {
+            auto target_pose = init_pose;
+
+            // Apply constant force along Z axis of chosen reference frame, and do a simple polish
+            // motion along XY plane in robot world frame
+            if (enable_polish) {
+                // Create motion command to sine-sweep along Y direction
+                target_pose[1]
+                    = init_pose[1]
+                      + kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
+            }
+
+            rt_cmds[group] = rdk::RtCartesianCmd(target_pose, target_wrench);
+        }
+
+        // Command target pose and target wrench
+        robot.StreamCartesianMotionForce(rt_cmds);
+
+        // Increment loop counter
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space unified "
+        "motion-force control. The Z axis of the chosen reference frame will be activated for "
+        "explicit force control, while the rest axes in the same reference frame will stay motion "
+        "controlled.\n");
+
+    // The reference frame used for force control, see Robot::SetForceControlFrame()
+    auto force_ctrl_frame = rdk::CoordType::WORLD;
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--TCP")) {
+        spdlog::info("Reference frame used for force control: robot TCP frame");
+        force_ctrl_frame = rdk::CoordType::TCP;
+    } else {
+        spdlog::info("Reference frame used for force control: robot world frame");
+    }
+
+    // Whether to enable polish motion
+    bool enable_polish = false;
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--polish")) {
+        spdlog::info("Robot will run a polish motion along XY plane in robot world frame");
+        enable_polish = true;
+    } else {
+        spdlog::info("Robot will hold its motion in all non-force-controlled axes");
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Zero Force-torque Sensor
+        // =========================================================================================
+        // Direct Cartesian control can only be executed by single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+        // IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
+        std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // WARNING: during the process, the robot must not contact anything, otherwise the result
+        // will be inaccurate and affect following operations
+        spdlog::warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot");
+
+        // Wait for primitive to finish
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Sensor zeroing complete");
+
+        // Search for Contact
+        // =========================================================================================
+        // NOTE: there are several ways to do contact search, such as using primitives, or real-time
+        // and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian
+        // control for example.
+        spdlog::info("Searching for contact ...");
+
+        // Set initial poses to current TCP poses
+        std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : single_arm_groups) {
+            all_init_pose[group] = robot_states.at(group).tcp_pose;
+            spdlog::info("[{}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {}",
+                rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(all_init_pose.at(group)));
+        }
+
+        // Use non-real-time mode to make the robot go to a set point with its own motion generator
+        robot.SwitchMode(rdk::Mode::NRT_CARTESIAN_MOTION_FORCE);
+
+        // Search for contact with max contact wrench set to a small value for making soft contact
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetMaxContactWrench(group, kMaxWrenchForContactSearch);
+        }
+
+        // Set target point along -Z direction and expect contact to happen during the travel
+        std::map<rdk::JointGroup, rdk::NrtCartesianCmd> nrt_cmds;
+        for (const auto& [group, init_pose] : all_init_pose) {
+            auto target_pose = init_pose;
+            target_pose[2] -= kSearchDistance;
+            nrt_cmds[group] = rdk::NrtCartesianCmd(target_pose, {}, {}, kSearchVelocity);
+        }
+
+        // Send target point to robot to start searching for contact and limit the velocity. Keep
+        // target wrench 0 at this stage since we are not doing force control yet
+        robot.SendCartesianMotionForce(nrt_cmds);
+
+        // Use a while loop to poll robot states and check if a contact is made
+        bool is_contacted = false;
+        while (!is_contacted) {
+            // Compute norm of sensed external force applied on robot TCP
+            for (const auto& [group, states] : robot.states()) {
+                Eigen::Vector3d ext_force
+                    = {states.tcp_wrench[0], states.tcp_wrench[1], states.tcp_wrench[2]};
+
+                // Contact is considered to be made if sensed TCP force exceeds the threshold
+                if (ext_force.norm() > kPressingForce) {
+                    is_contacted = true;
+                    spdlog::info(
+                        "[{}] Contact detected at robot TCP", rdk::kJointGroupNames.at(group));
+                    break;
+                }
+            }
+            // Check at 1ms interval
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+
+        // Configure Force Control
+        // =========================================================================================
+        // Switch to real-time mode for continuous motion force control
+        robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
+
+        // Set force control reference frame based on program argument. See function doc for more
+        // details
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetForceControlFrame(group, force_ctrl_frame);
+        }
+
+        // Set which Cartesian axis(s) to activate for force control. See function doc for more
+        // details. Here we only active Z axis
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetForceControlAxis(
+                group, std::array<bool, rdk::kCartDoF> {false, false, true, false, false, false});
+        }
+
+        // Uncomment the following line to enable passive force control, otherwise active force
+        // control is used by default. See function doc for more details
+        /*
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetPassiveForceControl(group, true);
+        }
+        */
+
+        // NOTE: motion control always uses robot world frame, while force control can use
+        // either world or TCP frame as reference frame
+
+        // Start Unified Motion Force Control
+        // =========================================================================================
+        // Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis
+        // is activated (i.e. motion control disabled in Z axis) and the motion force control mode
+        // is entered, this way the contact force along Z axis is explicitly regulated and will not
+        // spike after the max contact wrench regulation for motion control is disabled
+        std::array<double, rdk::kCartDoF> inf;
+        inf.fill(std::numeric_limits<double>::infinity());
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetMaxContactWrench(group, inf);
+        }
+
+        // Update initial poses to current TCP poses
+        for (auto& [group, pose] : all_init_pose) {
+            pose = robot.states().at(group).tcp_pose;
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::cref(all_init_pose),
+                              std::ref(force_ctrl_frame), enable_polish),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example/intermediate7_robot_dynamics.cpp

@@ -0,0 +1,158 @@
+/**
+ * @example intermediate7_robot_dynamics.cpp
+ * This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and
+ * gravity torques. Also checks reachability of a Cartesian pose.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/model.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <chrono>
+#include <mutex>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain "
+        "robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian "
+        "pose.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Robot Dynamics
+        // =========================================================================================
+        // Initialize dynamics engine
+        rdk::Model model(robot);
+
+        // Step dynamics engine 5 times
+        for (size_t i = 0; i < 5; i++) {
+            // Mark timer start point
+            auto tic = std::chrono::high_resolution_clock::now();
+
+            // Update robot model in dynamics engine
+            const auto robot_states = robot.states();
+            model.Update(robot_states.at(rdk::JointGroup::ALL).q,
+                robot_states.at(rdk::JointGroup::ALL).dq);
+
+            // Compute gravity vector
+            auto g = model.g();
+
+            // Compute mass matrix
+            auto M = model.M();
+
+            // Compute Jacobian
+            auto J = model.J("flange");
+
+            // Mark timer end point and get loop time
+            auto toc = std::chrono::high_resolution_clock::now();
+            auto computation_time
+                = std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
+
+            // Print time used to compute g, M, J
+            spdlog::info("Computation time = {} us", computation_time);
+            // Print gravity
+            std::cout << "g = \n"
+                      << std::fixed << std::setprecision(5) << g.transpose() << std::endl;
+            // Print mass matrix
+            std::cout << "M = \n" << std::fixed << std::setprecision(5) << M << std::endl;
+            // Print Jacobian
+            std::cout << "J = \n" << std::fixed << std::setprecision(5) << J << std::endl;
+            std::cout << std::endl;
+        }
+
+        // Check IK feasibility for a nearby Cartesian pose on all available single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        const auto robot_states = robot.states();
+        std::map<rdk::JointGroup, rdk::IKParams> ik_params_by_group;
+        for (const auto& [group, _] : single_arm_groups) {
+            auto pose_to_check = robot_states.at(group).tcp_pose;
+            pose_to_check[0] += 0.1;
+            spdlog::info("[{}] Checking IK feasibility of Cartesian pose [{}]",
+                rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(pose_to_check));
+
+            rdk::IKParams ik_params;
+            ik_params.cartesian_pose = pose_to_check;
+            ik_params.seed_q = robot_states.at(group).q;
+            ik_params.free_orientation = false;
+            ik_params_by_group[group] = ik_params;
+        }
+
+        // Print result
+        auto result = model.SolveConstrainedIK(ik_params_by_group);
+        spdlog::info("IK result success = {}", result.success);
+        for (const auto& [group, q] : result.solved_q) {
+            spdlog::info(
+                "[{}] solved_q = [{}]", rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(q));
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/example_py/AGENT_REFERENCE_primitives_only.py

@@ -0,0 +1,409 @@
+#!/usr/bin/env python
+
+"""AGENT_REFERENCE_primitives_only.py
+
+Comprehensive primitive-only reference script for code-generation agents.
+
+Primitives demonstrated (in order):
+    1. Home          - move robot to the platform-defined home posture.
+    2. ZeroFTSensor  - zero force/torque sensors for all active single-arm groups.
+    3. MoveJ         - joint-space move with waypoints and joint-velocity scaling.
+    4. MoveL         - Cartesian linear move in WORLD frame with velocity, blending (zoneRadius).
+    5. MoveL         - multi-waypoint sweep in WORLD frame.
+    6. Home          - return to home before TCP-frame relative move.
+    7. MoveL         - relative move in TCP frame (TRAJ::START), orientation-only rotation.
+    8. MoveJ         - return to upright posture (all joints 0).
+    9. Home          - return to home.
+
+Key patterns shown:
+    - Fault clear, enable, wait operational, mode switch (essential setup).
+    - exec_prim() wrapper accepts group-indexed PrimitiveArgs for per-group primitives.
+    - Transition keys must be looked up per primitive from Flexiv primitive documentation:
+        https://www.flexiv.com/software/primitives
+    - Reading tcp_pose from each joint group to extract live orientation.
+    - Converting quaternion to Euler ZYX degrees using utility.quat2eulerZYX for MoveL orientation.
+    - ZeroFTSensor is executed for all selected groups with explicit completion checks.
+    - MoveL with optional velocity, zoneRadius blending, and waypoints.
+    - MoveJ with optional waypoints list and jntVelScale.
+    - All JPos calls use 7-element arm-joint list; second (external-axis) argument is optional.
+    - All Coord calls use positional args: (position, orientation_eulerZYX_deg, ref_frame_list).
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import argparse
+import time
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+import utility
+
+
+# ──────────────────────────────────────────────────────────────────────────────
+# Helpers
+# ──────────────────────────────────────────────────────────────────────────────
+def normalize_euler_deg(euler_deg):
+    """Normalize one angle or a 3-element Euler list to the robot's [-180, 180] convention."""
+
+    def normalize_angle(angle_deg):
+        normalized = (angle_deg + 180.0) % 360.0 - 180.0
+        if normalized == -180.0 and angle_deg > 0:
+            return 180.0
+        return normalized
+
+    if isinstance(euler_deg, (int, float)):
+        return normalize_angle(euler_deg)
+
+    return [normalize_angle(angle) for angle in euler_deg]
+
+
+def wait_until_operational(robot, dt=1.0, timeout_s=120.0):
+    """Block until robot.operational() is True, or raise TimeoutError."""
+    t0 = time.time()
+    while not robot.operational():
+        if time.time() - t0 > timeout_s:
+            raise TimeoutError("Timed out waiting for robot to become operational")
+        time.sleep(dt)
+
+
+def wait_primitive_transition(robot, transition_keys_by_group, dt=0.2, timeout_s=60.0):
+    """Block until primitive_states() for all groups satisfy their transition key.
+
+    Use the primitive's default transition key unless a custom transition condition
+    is intended. Check primitive docs to confirm available primitive state keys.
+    """
+    t0 = time.time()
+    while True:
+        if utility.primitive_state_true_for_groups(
+            robot.primitive_states(),
+            transition_keys_by_group,
+        ):
+            return
+
+        if time.time() - t0 > timeout_s:
+            raise TimeoutError(
+                "Timed out waiting for primitive transition states: "
+                f"{ {flexivrdk.kJointGroupNames[g]: k for g, k in transition_keys_by_group.items()} }"
+            )
+        time.sleep(dt)
+
+
+def exec_prim(robot, primitive_args_by_group, transition_keys_by_group):
+    """Execute primitive command(s) and block until all transitions are satisfied.
+
+    Args:
+        robot:          flexivrdk.Robot instance.
+        primitive_args_by_group:
+            Dict[JointGroup, flexivrdk.PrimitiveArgs] for per-group primitive commands.
+        transition_keys_by_group:
+            Dict[JointGroup, str] where each value is the transition key for that group.
+            This can be the primitive's default key or another primitive state key.
+            Look up primitive states from:
+            https://www.flexiv.com/software/primitives
+
+        Note:
+        This helper waits on primitive_states() only. If transition depends on
+        robot states, implement a custom wait loop at call site.
+    """
+    if not primitive_args_by_group:
+        raise ValueError("primitive_args_by_group cannot be empty")
+
+    if set(primitive_args_by_group.keys()) != set(transition_keys_by_group.keys()):
+        raise ValueError(
+            "primitive_args_by_group and transition_keys_by_group must have identical groups"
+        )
+
+    robot.ExecutePrimitive(primitive_args_by_group)
+    wait_primitive_transition(robot, transition_keys_by_group)
+
+
+def prepare_robot(robot_sn, logger):
+    """Connect, clear faults, enable robot, and switch to primitive execution mode."""
+    robot = flexivrdk.Robot(robot_sn)
+
+    if robot.fault():
+        logger.warn("Fault detected on connected robot, trying to clear")
+        if not robot.ClearFault():
+            raise RuntimeError("Failed to clear robot fault")
+        logger.info("Fault cleared")
+
+    logger.info("Enabling robot")
+    robot.ServoOn()
+    wait_until_operational(robot)
+    logger.info("Robot is operational")
+
+    robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
+    return robot
+
+
+def current_euler_zyx_deg(robot, group):
+    """Return live TCP orientation of a specific joint group as Euler ZYX [x,y,z] degrees.
+
+    tcp_pose layout: [x, y, z, q_w, q_x, q_y, q_z] (SI units, quaternion).
+    utility.quat2eulerZYX() expects [w, x, y, z] order.
+    The returned Euler values are normalized to the robot's [-180, 180] convention.
+    """
+    all_states = robot.states()
+    if group not in all_states:
+        raise ValueError(
+            f"Requested joint group has no available robot state: "
+            f"{flexivrdk.kJointGroupNames[group]}"
+        )
+
+    pose = all_states[group].tcp_pose
+    return utility.quat2eulerZYX([pose[3], pose[4], pose[5], pose[6]], degree=True)
+
+
+# ──────────────────────────────────────────────────────────────────────────────
+# Main sequence
+# ──────────────────────────────────────────────────────────────────────────────
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove spaces, e.g. Enlight-L-123456",
+    )
+    args = parser.parse_args()
+
+    logger = spdlog.ConsoleLogger("AgentReference")
+
+    try:
+        robot = prepare_robot(args.robot_sn, logger)
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # ── 1) Move to home pose ──────────────────────────────────────────────
+        logger.info("Step 1: Move to home pose")
+        robot.Home()
+
+        # ── 2) ZeroFTSensor ───────────────────────────────────────────────────
+        # Use this primitive's documented default transition condition.
+        # The robot must not be in contact with anything during zeroing.
+        logger.info("Step 2: ZeroFTSensor")
+        logger.warn("Zeroing F/T sensors - ensure nothing contacts the robot")
+        exec_prim(
+            robot,
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "ZeroFTSensor",
+                    {
+                        "dataCollectTime": 0.2,
+                        "enableStaticCheck": False,
+                        "calibExtraPayload": False,
+                    },
+                )
+                for group in single_arm_groups
+            },
+            {group: "terminated" for group in single_arm_groups},
+        )
+        logger.info("F/T sensor zeroing complete")
+
+        # ── 3) MoveJ with waypoints and velocity scaling ──────────────────────
+        # target / waypoints: flexivrdk.JPos(q_m) where q_m is 7-element degrees list.
+        # jntVelScale: percentage of max joint speed [1-100].
+        logger.info("Step 3: MoveJ with waypoints")
+        exec_prim(
+            robot,
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveJ",
+                    {
+                        "target": flexivrdk.JPos([30, -45, 0, 90, 0, 40, 30]),
+                        "waypoints": [
+                            flexivrdk.JPos([10, -30, 10, 30, 10, 15, 10]),
+                            flexivrdk.JPos([20, -60, -10, 60, -10, 30, 20]),
+                        ],
+                        "jntVelScale": 50,  # 50% of max joint speed
+                        "zoneRadius": "Z50",
+                        "targetTolerLevel": 1,
+                        "enableRelativeMove": False,
+                        "jntAccMultiplier": 1,
+                    },
+                )
+                for group in single_arm_groups
+            },
+            {group: "reachedTarget" for group in single_arm_groups},
+        )
+
+        # ── 4) MoveL in WORLD frame with velocity and blending ────────────────
+        # target / waypoints: flexivrdk.Coord(position_m, orientation_eulerZYX_deg, ref_frame).
+        # ref_frame is a 2-element list: [frame_type, frame_origin].
+        # vel: TCP linear speed in m/s.
+        # zoneRadius: blending zone size string, e.g. "Z50". Use "Z0" for exact stops.
+        step4_primitive_args_by_group = {}
+        for group in single_arm_groups:
+            live_euler = current_euler_zyx_deg(robot, group)
+            wp1_euler = normalize_euler_deg(
+                [live_euler[0] + 20.0, live_euler[1] - 20.0, live_euler[2] + 10.0]
+            )
+            wp2_euler = normalize_euler_deg(
+                [live_euler[0] - 30.0, live_euler[1] + 30.0, live_euler[2] - 30.0]
+            )
+            target_euler = normalize_euler_deg(
+                [live_euler[0] + 40.0, live_euler[1] - 20.0, live_euler[2] + 20.0]
+            )
+            logger.info(
+                f"Step 4 [{flexivrdk.kJointGroupNames[group]}] live Euler ZYX (deg): {live_euler}"
+            )
+            logger.info(
+                f"Step 4 [{flexivrdk.kJointGroupNames[group]}] waypoint1 Euler ZYX (deg): {wp1_euler}"
+            )
+            logger.info(
+                f"Step 4 [{flexivrdk.kJointGroupNames[group]}] waypoint2 Euler ZYX (deg): {wp2_euler}"
+            )
+            logger.info(
+                f"Step 4 [{flexivrdk.kJointGroupNames[group]}] target Euler ZYX (deg): {target_euler}"
+            )
+
+            step4_primitive_args_by_group[group] = flexivrdk.PrimitiveArgs(
+                "MoveL",
+                {
+                    "target": flexivrdk.Coord(
+                        [0.65, -0.3, 0.3], target_euler, ["WORLD", "WORLD_ORIGIN"]
+                    ),
+                    "waypoints": [
+                        flexivrdk.Coord(
+                            [0.45, 0.1, 0.3], wp1_euler, ["WORLD", "WORLD_ORIGIN"]
+                        ),
+                        flexivrdk.Coord(
+                            [0.45, -0.3, 0.3], wp2_euler, ["WORLD", "WORLD_ORIGIN"]
+                        ),
+                    ],
+                    "vel": 0.3,  # m/s TCP linear speed
+                    "zoneRadius": "Z50",  # blended corners; use "Z0" for hard stops
+                    "targetTolerLevel": 3,
+                    "acc": 1.5,
+                    "angVel": 150,
+                    "enableFixRefJntPos": False,
+                    "refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
+                    "jerk": 50,
+                    "configOptObj": [0, 0, 0.5],
+                    "enableSixAxisJntCtrl": False,
+                    "enableNullspaceTraj": False,
+                },
+            )
+
+        logger.info("Step 4: MoveL in WORLD frame with speed and blending")
+        exec_prim(
+            robot,
+            step4_primitive_args_by_group,
+            {group: "reachedTarget" for group in single_arm_groups},
+        )
+
+        # ── 5) MoveL multi-point sweep, exact stops ───────────────────────────
+        logger.info("Step 5: MoveL sweep with exact stops")
+        for target_pos in [
+            [0.5, 0.2, 0.4],
+            [0.5, -0.2, 0.4],
+            [0.5, -0.2, 0.2],
+            [0.5, 0.2, 0.2],
+        ]:
+            exec_prim(
+                robot,
+                {
+                    group: flexivrdk.PrimitiveArgs(
+                        "MoveL",
+                        {
+                            "target": flexivrdk.Coord(
+                                target_pos, [180, 0, 180], ["WORLD", "WORLD_ORIGIN"]
+                            ),
+                            "vel": 0.2,
+                            "zoneRadius": "Z0",  # exact stop at each corner
+                            "targetTolerLevel": 3,
+                            "acc": 1.5,
+                            "angVel": 150,
+                            "enableFixRefJntPos": False,
+                            "refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
+                            "jerk": 50,
+                            "configOptObj": [0, 0, 0.5],
+                            "enableSixAxisJntCtrl": False,
+                            "enableNullspaceTraj": False,
+                        },
+                    )
+                    for group in single_arm_groups
+                },
+                {group: "reachedTarget" for group in single_arm_groups},
+            )
+
+        # ── 6) Home before TCP-frame relative move ────────────────────────────
+        logger.info("Step 6: Move to home pose")
+        robot.Home()
+
+        # ── 7) MoveL in TCP frame (TRAJ::START) - relative orientation change ─
+        # TRAJ::START means both translation and orientation targets are relative
+        # to the current TCP pose at primitive start.
+        # Setting position to [0, 0, 0] keeps the TCP at the same location.
+        # Orientation [20, 0, 0] means a +20 deg relative rotation around X.
+        logger.info("Step 7: MoveL relative orientation-only rotation in TCP frame")
+        logger.info("Relative target Euler ZYX (deg): [20.0, 0.0, 0.0]")
+        exec_prim(
+            robot,
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveL",
+                    {
+                        # Zero position delta: stay in place. Apply a 20-degree rotation around X.
+                        "target": flexivrdk.Coord(
+                            [0.0, 0.0, 0.0],
+                            [20.0, 0.0, 0.0],
+                            ["TRAJ", "START"],
+                        ),
+                        "vel": 0.1,
+                        "zoneRadius": "Z0",
+                        "targetTolerLevel": 3,
+                        "acc": 1.0,
+                        "angVel": 120,
+                        "enableFixRefJntPos": False,
+                        "refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
+                        "jerk": 50,
+                        "configOptObj": [0, 0, 0.5],
+                        "enableSixAxisJntCtrl": False,
+                        "enableNullspaceTraj": False,
+                    },
+                )
+                for group in single_arm_groups
+            },
+            {group: "reachedTarget" for group in single_arm_groups},
+        )
+
+        # ── 8) MoveJ to upright posture ───────────────────────────────────────
+        # Upright convention: all arm joints at 0 degrees.
+        logger.info("Step 8: MoveJ to upright posture")
+        exec_prim(
+            robot,
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveJ",
+                    {
+                        "target": flexivrdk.JPos([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
+                        "jntVelScale": 40,
+                        "zoneRadius": "Z50",
+                        "targetTolerLevel": 1,
+                        "enableRelativeMove": False,
+                        "jntAccMultiplier": 1,
+                    },
+                )
+                for group in single_arm_groups
+            },
+            {group: "reachedTarget" for group in single_arm_groups},
+        )
+
+        # ── 9) Home ───────────────────────────────────────────────────────────
+        logger.info("Step 9: Move to home pose")
+        robot.Home()
+
+        logger.info("Reference sequence completed successfully")
+        robot.Stop()
+
+    except Exception as e:
+        logger.error(f"Unhandled exception: {e}")
+        return 1
+
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

thirdparty/flexiv_rdk/example_py/basics1_display_robot_states.py

@@ -0,0 +1,156 @@
+#!/usr/bin/env python
+
+"""basics1_display_robot_states.py
+
+This tutorial does the very first thing: check connection with the robot server and print
+received robot states.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import threading
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def print_robot_states(robot, logger, stop_event):
+    """
+    Print robot states data @ 1Hz.
+
+    """
+
+    while not stop_event.is_set():
+        # Print available joint groups
+        joint_groups_str = " ".join(
+            [f"[{name}]" for name in robot.info().all_groups.values()]
+        )
+        logger.info(f"Available joint groups: {joint_groups_str}")
+
+        # Print all robot states in JSON format using the built-in __str__ overloading
+        for group, states in robot.states().items():
+            logger.info(f"[{flexivrdk.kJointGroupNames[group]}] robot states:")
+            # fmt: off
+            print("{")
+            print(f"timestamp: [{states.timestamp[0]}, {states.timestamp[1]}]")
+            print(f"q: {['%.3f' % i for i in states.q]}")
+            print(f"theta: {['%.3f' % i for i in states.theta]}")
+            print(f"dq: {['%.3f' % i for i in states.dq]}")
+            print(f"dtheta: {['%.3f' % i for i in states.dtheta]}")
+            print(f"tau: {['%.3f' % i for i in states.tau]}")
+            print(f"tau_dot: {['%.3f' % i for i in states.tau_dot]}")
+            print(f"tau_ext: {['%.3f' % i for i in states.tau_ext]}")
+            print(f"tau_interact: {['%.3f' % i for i in states.tau_interact]}")
+            print(f"temperature: {['%.3f' % i for i in states.temperature]}")
+            print(f"flange_pose: {['%.3f' % i for i in states.flange_pose]}")
+            print(f"tcp_pose: {['%.3f' % i for i in states.tcp_pose]}")
+            print(f"tcp_twist: {['%.3f' % i for i in states.tcp_twist]}")
+            print(f"tcp_wrench: {['%.3f' % i for i in states.tcp_wrench]}")
+            print(f"tcp_wrench_local: {['%.3f' % i for i in states.tcp_wrench_local]}")
+            print(f"raw_tcp_wrench: {['%.3f' % i for i in states.raw_tcp_wrench]}")
+            print(f"raw_tcp_wrench_local: {['%.3f' % i for i in states.raw_tcp_wrench_local]}")
+            print(f"raw_ft_sensor: {['%.3f' % i for i in states.raw_ft_sensor]}")
+            print("}", flush=True)
+            # fmt: on
+
+        # Print all robot actions in JSON format using the built-in __str__ overloading
+        for group, actions in robot.actions().items():
+            logger.info(f"[{flexivrdk.kJointGroupNames[group]}] robot actions:")
+            # fmt: off
+            print("{")
+            print(f"timestamp: [{actions.timestamp[0]}, {actions.timestamp[1]}]")
+            print(f"q_d: {['%.3f' % i for i in actions.q_d]}")
+            print(f"dq_d: {['%.3f' % i for i in actions.dq_d]}")
+            print(f"tau_d: {['%.3f' % i for i in actions.tau_d]}")
+            print(f"tcp_pose_d: {['%.3f' % i for i in actions.tcp_pose_d]}")
+            print(f"tcp_twist_d: {['%.3f' % i for i in actions.tcp_twist_d]}")
+            print(f"tcp_wrench_d: {['%.3f' % i for i in actions.tcp_wrench_d]}")
+            print("}", flush=True)
+            # fmt: on
+
+        # Print digital inputs and outputs
+        logger.info("Digital inputs:")
+        print(robot.digital_inputs())
+        logger.info("Digital outputs:")
+        print(robot.digital_outputs())
+        time.sleep(1)
+
+
+def main():
+    # Create an event to signal the thread to stop
+    stop_event = threading.Event()
+
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial does the very first thing: check connection "
+        "with the robot server and print received robot states.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+    # Print States
+    # =============================================================================
+    # Thread for printing robot states
+    print_thread = threading.Thread(
+        target=print_robot_states, args=[robot, logger, stop_event]
+    )
+    print_thread.start()
+
+    # Use main thread to catch keyboard interrupt and exit thread
+    try:
+        while not stop_event.is_set():
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        # Send signal to exit thread
+        logger.info("Stopping print thread")
+        stop_event.set()
+
+    # Wait for thread to exit
+    print_thread.join()
+    logger.info("Print thread exited")
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/basics2_clear_fault.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python
+
+"""basics2_clear_fault.py
+
+This tutorial clears minor or critical faults, if any, of the connected robot.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial clears minor or critical faults, if any, of "
+        "the connected robot.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Fault Clearing
+        # ==========================================================================================
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+        else:
+            logger.info("No fault on the connected robot")
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/basics3_primitive_execution.py

@@ -0,0 +1,223 @@
+#!/usr/bin/env python
+
+"""basics3_primitive_execution.py
+
+This tutorial executes several basic robot primitives (unit skills). For detailed documentation
+on all available primitives, please see [Flexiv Primitives](https://www.flexiv.com/primitives/).
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+import utility
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial executes several basic robot primitives (unit "
+        "skills). For detailed documentation on all available primitives, please see [Flexiv "
+        "Primitives](https://www.flexiv.com/primitives/).\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Execute Primitives
+        # ==========================================================================================
+        # Primitives can only be executed on single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # (1) Move robot to home pose
+        # ------------------------------------------------------------------------------------------
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Switch to primitive execution mode
+        robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
+
+        # (2) Move robot joints to target positions
+        # ------------------------------------------------------------------------------------------
+        # Required parameters:
+        #     target: final joint positions, unit: degrees
+        #         {arm joint positions}, {external axis joint positions, optional}
+        # Optional parameters:
+        #     waypoints: waypoints to pass before reaching the target
+        #         (same format as above, but can be more than one)
+        #     vel: TCP linear velocity, unit: m/s
+        logger.info("Executing primitive: MoveJ")
+
+        # Send command to robot
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveJ",
+                    {
+                        "target": flexivrdk.JPos(
+                            [30, -45, 0, 90, 0, 40, 30], [-50, 30, 0, 0, 0, 0]
+                        ),
+                        "waypoints": [
+                            flexivrdk.JPos(
+                                [10, -30, 10, 30, 10, 15, 10], [-15, 10, 0, 0, 0, 0]
+                            ),
+                            flexivrdk.JPos(
+                                [20, -60, -10, 60, -10, 30, 20], [-30, 20, 0, 0, 0, 0]
+                            ),
+                        ],
+                    },
+                )
+                for group in single_arm_groups
+            }
+        )
+        # Most primitives won't exit by themselves and require users to explicitly trigger
+        # transitions based on specific primitive states. Here we check if the primitive state
+        # [reachedTarget] becomes true and trigger the transition manually by sending a new
+        # primitive command.
+        while True:
+            primitive_states = robot.primitive_states()
+            if utility.primitive_state_true_for_groups(
+                primitive_states, "reachedTarget"
+            ):
+                break
+            # Print current primitive states
+            logger.info("Current primitive states:")
+            for group, pt_states in primitive_states.items():
+                print(f"{flexivrdk.kJointGroupNames[group]}:")
+                print(f"primitiveName: {pt_states.pt_name}")
+                for name, value in pt_states.names_and_values.items():
+                    print(f"{name}: {value}")
+            time.sleep(1)
+
+        # (3) Move robot TCP to a target pose in world (base) frame
+        # ------------------------------------------------------------------------------------------
+        # Required parameters:
+        #     target: final TCP pose, unit: m and degrees
+        #         {pos_x, pos_y, pos_z}, {rot_x, rot_y, rot_z}, {ref_frame, ref_point}
+        # Optional parameters:
+        #     waypoints: waypoints to pass before reaching the target
+        #         (same format as above, but can be more than one)
+        #     vel: TCP linear velocity, unit: m/s
+        # NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis
+        logger.info("Executing primitive: MoveL")
+
+        # Send command to robot
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveL",
+                    {
+                        "target": flexivrdk.Coord(
+                            [0.3, -0.1, 0.2], [160, 20, 180], ["WORLD", "WORLD_ORIGIN"]
+                        ),
+                        "waypoints": [
+                            flexivrdk.Coord(
+                                [0.1, 0.1, 0.1],
+                                [-160, -20, 180],
+                                ["WORLD", "WORLD_ORIGIN"],
+                            ),
+                            flexivrdk.Coord(
+                                [0.3, 0.2, 0.1],
+                                [180, 0, 180],
+                                ["WORLD", "WORLD_ORIGIN"],
+                            ),
+                        ],
+                        "vel": 0.6,
+                        "zoneRadius": "Z50",
+                    },
+                )
+                for group in single_arm_groups
+            }
+        )
+        # Wait for reached target
+        while not utility.primitive_state_true_for_groups(
+            robot.primitive_states(), "reachedTarget"
+        ):
+            time.sleep(1)
+
+        # (4) Another MoveL that uses TCP frame
+        # ------------------------------------------------------------------------------------------
+        # In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START,
+        # which represents the current TCP frame
+        logger.info("Executing primitive: MoveL")
+
+        # Example to convert target quaternion [w,x,y,z] to Euler ZYX using scipy package's 'xyz'
+        # extrinsic rotation
+        target_quat = [0.9185587, 0.1767767, 0.3061862, 0.1767767]
+        # ZYX = [30, 30, 30] degrees
+        eulerZYX_deg = utility.quat2eulerZYX(target_quat, degree=True)
+
+        # Send command to robot. This motion will hold current TCP position and only do rotation
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveL",
+                    {
+                        "target": flexivrdk.Coord(
+                            [0.0, 0.0, 0.0], eulerZYX_deg, ["TRAJ", "START"]
+                        ),
+                        "vel": 0.2,
+                    },
+                )
+                for group in single_arm_groups
+            }
+        )
+
+        # Wait for reached target
+        while not utility.primitive_state_true_for_groups(
+            robot.primitive_states(), "reachedTarget"
+        ):
+            time.sleep(1)
+
+        # All done, stop robot and put into IDLE mode
+        robot.Stop()
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/basics4_plan_execution.py

@@ -0,0 +1,147 @@
+#!/usr/bin/env python
+
+"""basics4_plan_execution.py
+
+This tutorial executes a plan selected by the user from a list of available plans. A plan is a
+pre-written script to execute a series of robot primitives with pre-defined transition conditions
+between 2 adjacent primitives. Users can use Flexiv Elements to compose their own plan and assign
+to the robot, which will appear in the plan list.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial executes a plan selected by the user from a "
+        "list of available plans. A plan is a pre-written script to execute a series of robot "
+        "primitives with pre-defined transition conditions between 2 adjacent primitives. Users "
+        "can use Flexiv Elements to compose their own plan and assign to the robot, which "
+        "will appear in the plan list.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Execute Plans
+        # ==========================================================================================
+        # Switch to plan execution mode
+        robot.SwitchMode(mode.NRT_PLAN_EXECUTION)
+
+        while True:
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            # Get user input
+            logger.info("Choose an action:")
+            print("[1] Show available plans")
+            print("[2] Execute a plan by index")
+            print("[3] Execute a plan by name")
+            user_input = int(input())
+
+            # Get and show plan list
+            if user_input == 1:
+                plan_list = robot.plan_list()
+                for i in range(len(plan_list)):
+                    print(f"[{i}] {plan_list[i]}")
+                print("")
+
+            # Execute plan by index
+            elif user_input == 2:
+                index = int(input("Enter plan index to execute:\n"))
+                # Allow the plan to continue its execution even if the RDK program is closed or
+                # the connection is lost
+                robot.ExecutePlan(index, True)
+
+                # Print plan info while the current plan is running
+                while robot.busy():
+                    plan_info = robot.plan_info()
+                    logger.info("Current plan info:")
+                    print(f"assigned_plan_name: {plan_info.assigned_plan_name}")
+                    print(f"pt_name: {plan_info.pt_name}")
+                    print(f"node_name: {plan_info.node_name}")
+                    print(f"node_path: {plan_info.node_path}")
+                    print(f"node_path_time_period: {plan_info.node_path_time_period}")
+                    print(f"node_path_number: {plan_info.node_path_number}")
+                    print(f"velocity_scale: {plan_info.velocity_scale}")
+                    print(f"waiting_for_step: {plan_info.waiting_for_step}")
+                    print("", flush=True)
+                    time.sleep(1)
+
+            # Execute plan by name
+            elif user_input == 3:
+                name = str(input("Enter plan name to execute:\n"))
+                # Allow the plan to continue its execution even if the RDK program is closed or
+                # the connection is lost
+                robot.ExecutePlan(name, True)
+
+                # Print plan info while the current plan is running
+                while robot.busy():
+                    plan_info = robot.plan_info()
+                    logger.info("Current plan info:")
+                    print(f"assigned_plan_name: {plan_info.assigned_plan_name}")
+                    print(f"pt_name: {plan_info.pt_name}")
+                    print(f"node_name: {plan_info.node_name}")
+                    print(f"node_path: {plan_info.node_path}")
+                    print(f"node_path_time_period: {plan_info.node_path_time_period}")
+                    print(f"node_path_number: {plan_info.node_path_number}")
+                    print(f"velocity_scale: {plan_info.velocity_scale}")
+                    print(f"waiting_for_step: {plan_info.waiting_for_step}")
+                    print("", flush=True)
+                    time.sleep(1)
+
+            else:
+                logger.warn("Invalid input")
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/basics5_zero_force_torque_sensors.py

@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+"""basics5_zero_force_torque_sensors.py
+
+This tutorial zeros the robot's force and torque sensors, which is a recommended (but not
+mandatory) step before any operations that require accurate force/torque measurement.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+import utility
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial zeros the robot's force and torque sensors, "
+        "which is a recommended (but not mandatory) step before any operations that require "
+        "accurate force/torque measurement.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Zero Sensors
+        # ==========================================================================================
+        # Get and print the current TCP force/moment readings
+        for group, states in robot.states().items():
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] TCP force and moment reading in world frame BEFORE sensor zeroing: {states.tcp_wrench} N-Nm"
+            )
+
+        # Primitives can only be executed on single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors
+        robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
+                for group in single_arm_groups
+            }
+        )
+
+        # WARNING: during the process, the robot must not contact anything, otherwise the result
+        # will be inaccurate and affect following operations
+        logger.warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot"
+        )
+
+        # Wait for primitive to finish
+        while not utility.primitive_state_true_for_groups(robot.primitive_states(), "terminated"):
+            time.sleep(1)
+        logger.info("Sensor zeroing complete")
+
+        # Get and print the current TCP force/moment readings
+        for group, states in robot.states().items():
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] TCP force and moment reading in world frame AFTER sensor zeroing: {states.tcp_wrench} N-Nm"
+            )
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/basics6_gripper_control.py

@@ -0,0 +1,230 @@
+#!/usr/bin/env python
+
+"""basics6_gripper_control.py
+
+This tutorial does position and force (if available) control of grippers supported by Flexiv.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import sys
+import argparse
+import threading
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def print_gripper_states(gripper, logger, stop_event):
+    """
+    Print gripper states data @ 1Hz.
+
+    """
+    while not stop_event.is_set():
+        # Print all gripper states, round all float values to 2 decimals
+        logger.info("Current gripper states:")
+        gripper_states = gripper.states()
+        for group, states in gripper_states.items():
+            print(f"[{flexivrdk.kJointGroupNames[group]}]")
+            print(f"width: {round(states.width, 2)}")
+            print(f"force: {round(states.force, 2)}")
+            print(f"is_moving: {states.is_moving}")
+        print("", flush=True)
+        time.sleep(1)
+
+
+def main():
+    # Create an event to signal the thread to stop
+    stop_event = threading.Event()
+
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "gripper_device_name",
+        help="Full name of the device representing the gripper, can be found in Flexiv Elements->Settings->Device",
+    )
+    argparser.add_argument(
+        "gripper_tool_name",
+        help="Full name of the tool representing the gripper, can be found in Flexiv Elements->Settings->Tool",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial does position and force (if available) "
+        "control of grippers supported by Flexiv.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Gripper Control
+        # ==========================================================================================
+        # Instantiate gripper control interface
+        gripper = flexivrdk.Gripper(robot)
+
+        # Instantiate tool interface. Gripper is categorized as both a device and a tool. The
+        # device attribute allows a gripper to be interactively controlled by the user; whereas the
+        # tool attribute tells the robot to account for its mass properties and TCP location.
+        tool = flexivrdk.Tool(robot)
+
+        # Grippers can only be assigned to single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # Enable the specified gripper as a device. This is equivalent to enabling the specified
+        # gripper in Flexiv Elements -> Settings -> Device
+        logger.info(
+            f"Enabling gripper device [{args.gripper_device_name}] for all available single-arm joint groups"
+        )
+        for group in single_arm_groups:
+            gripper.Enable(group, args.gripper_device_name)
+
+        # Print parameters of the enabled gripper
+        logger.info("Gripper params:")
+        gripper_params = gripper.params()
+        for group, params in gripper_params.items():
+            print(f"[{flexivrdk.kJointGroupNames[group]}]")
+            print(f"name: {params.name}")
+            print(f"min_width: {round(params.min_width, 2)}")
+            print(f"max_width: {round(params.max_width, 2)}")
+            print(f"min_force: {round(params.min_force, 2)}")
+            print(f"max_force: {round(params.max_force, 2)}")
+            print(f"min_vel: {round(params.min_vel, 2)}")
+            print(f"max_vel: {round(params.max_vel, 2)}")
+            print("", flush=True)
+
+        # Switch robot tool to gripper so the gravity compensation and TCP location is updated
+        logger.info(
+            f"Switching robot tool to [{args.gripper_tool_name}] for all available single-arm joint groups"
+        )
+        for group in single_arm_groups:
+            tool.Switch(group, args.gripper_tool_name)
+
+        # User needs to determine if this gripper requires manual initialization
+        logger.info(
+            "Manually trigger initialization for the gripper now? Choose Yes if it's a 48v Grav "
+            "gripper"
+        )
+        print("[1] No, it has already initialized automatically when power on")
+        print("[2] Yes, it does not initialize itself when power on")
+        choice = int(input(""))
+
+        # Trigger manual initialization based on input
+        if choice == 1:
+            logger.info("Skipped manual initialization")
+        elif choice == 2:
+            for group in single_arm_groups:
+                gripper.Init(group)
+            # User determines if the manual initialization is finished
+            logger.info(
+                "Triggered manual initialization, press Enter when the initialization is finished to continue"
+            )
+            input()
+        else:
+            logger.error("Invalid choice")
+            return 1
+
+        # Start a separate thread to print gripper states
+        print_thread = threading.Thread(
+            target=print_gripper_states, args=[gripper, logger, stop_event]
+        )
+        print_thread.start()
+
+        # Position control
+        logger.info("Closing gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(2)
+        logger.info("Opening gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(2)
+
+        # Stop
+        logger.info("Closing gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(0.5)
+        logger.info("Stopping gripper")
+        for group in gripper_params:
+            gripper.Stop(group)
+        time.sleep(2)
+        logger.info("Closing gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(2)
+        logger.info("Opening gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(0.5)
+        logger.info("Stopping gripper")
+        for group in gripper_params:
+            gripper.Stop(group)
+        time.sleep(2)
+
+        # Force control, if available (sensed force is not zero)
+        gripper_states = gripper.states()
+        has_force_control = any(
+            abs(states.force) > sys.float_info.epsilon
+            for states in gripper_states.values()
+        )
+        if has_force_control:
+            logger.info("Gripper running zero force control")
+            for group in gripper_states:
+                gripper.Grasp(group, 0)
+            # Exit after 10 seconds
+            time.sleep(10)
+
+        # Finished
+        for group in gripper_states:
+            gripper.Stop(group)
+
+        # Stop all threads
+        logger.info("Stopping print thread")
+        stop_event.set()
+        print_thread.join()
+        logger.info("Print thread exited")
+        logger.info("Program finished")
+
+    except Exception as e:
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/basics7_update_robot_tool.py

@@ -0,0 +1,159 @@
+#!/usr/bin/env python
+
+"""basics7_update_robot_tool.py
+
+This tutorial shows how to online update and interact with the robot tools. All changes made to
+the robot tool system will take effect immediately without needing to reboot. However, the robot
+must be put into IDLE mode when making these changes.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to online update and interact with "
+        "the robot tools. All changes made to the robot tool system will take effect immediately "
+        "without needing to reboot. However, the robot must be put into IDLE mode when making "
+        "these changes.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Update Robot Tool
+        # ==========================================================================================
+        # Make sure the robot is in IDLE mode
+        robot.SwitchMode(mode.IDLE)
+
+        # Instantiate tool interface
+        tool = flexivrdk.Tool(robot)
+
+        # Tools can only be assigned to single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # Get and print a list of already configured tools currently in the robot's tools pool
+        logger.info("All configured tools:")
+        tool_list = tool.list()
+        for i in range(len(tool_list)):
+            print(f"[{i}] {tool_list[i]}")
+        print()
+
+        # Get and print the current active tool
+        for group in single_arm_groups:
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Current active tool: [{tool.name(group)}]"
+            )
+
+        # Set name and parameters for a new tool
+        new_tool_name = "ExampleTool1"
+        new_tool_params = flexivrdk.ToolParams()
+        new_tool_params.mass = 0.9
+        new_tool_params.CoM = [0.0, 0.0, 0.057]
+        new_tool_params.inertia = [2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0]
+        new_tool_params.tcp_location = [
+            0.0,
+            -0.207,
+            0.09,
+            0.7071068,
+            0.7071068,
+            0.0,
+            0.0,
+        ]
+
+        # If there's already a tool with the same name in the robot's tools pool, then remove it
+        # first, because duplicate tool names are not allowed
+        if tool.exist(new_tool_name):
+            logger.warn(
+                f"Tool with the same name [{new_tool_name}] already exists, removing it now"
+            )
+            # Switch to other tool or no tool (Flange) before removing the current tool
+            for group in single_arm_groups:
+                tool.Switch(group, "Flange")
+            tool.Remove(new_tool_name)
+
+        # Add the new tool
+        logger.info(f"Adding new tool [{new_tool_name}] to the robot")
+        tool.Add(new_tool_name, new_tool_params)
+
+        # Get and print the tools list again, the new tool should appear at the end
+        logger.info("All configured tools:")
+        tool_list = tool.list()
+        for i in range(len(tool_list)):
+            print(f"[{i}] {tool_list[i]}")
+        print()
+
+        # Switch to the newly added tool, i.e. set it as the active tool
+        logger.info(f"Switching to tool [{new_tool_name}]")
+        for group in single_arm_groups:
+            tool.Switch(group, new_tool_name)
+
+        # Get and print the current active tool again, should be the new tool
+        for group in single_arm_groups:
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Current active tool: [{tool.name(group)}]"
+            )
+
+        # Switch to other tool or no tool (Flange) before removing the current tool
+        for group in single_arm_groups:
+            tool.Switch(group, "Flange")
+
+        # Clean up by removing the new tool
+        time.sleep(2)
+        logger.info(f"Removing tool [{new_tool_name}]")
+        tool.Remove(new_tool_name)
+
+        logger.info("Program finished")
+
+    except Exception as e:
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/basics8_global_variables.py

@@ -0,0 +1,129 @@
+#!/usr/bin/env python
+
+"""basics8_global_variables.py
+
+This tutorial shows how to get and set global variables.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to get and set global variables.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Get existing global variables
+        # =========================================================================================
+        global_vars = robot.global_variables()
+        if not global_vars:
+            logger.warn("No global variables available")
+            return 1
+        else:
+            logger.info("Existing global variables and their original values:")
+            for key, value in global_vars.items():
+                print(f"{key}: {value}")
+
+        # Set global variables
+        # =========================================================================================
+        # WARNING: These specified global variables need to be created first using Flexiv Elements
+        logger.info("Setting new values to existing global variables")
+        robot.SetGlobalVariables(
+            {
+                "test_bool": 1,
+                "test_int": 100,
+                "test_double": 100.123,
+                "test_string": "Flexiv",
+                "test_int_vec": [1, 2, 3],
+                "test_double_vec": [1.1, 2.2, 3.3],
+                "test_string_vec": ["Go", "Flexiv", "Go!"],
+                "test_pose": [0.1, -0.2, 0.3, -90, -45, 120],
+                "test_coord": flexivrdk.Coord(
+                    [0.1, -0.2, 0.3],
+                    [-90, -45, 120],
+                    ["WORLD", "WORLD_ORIGIN"],
+                    [1, 2, 3, 4, 5, 6, 7],
+                    [10, 20, 0, 0, 0, 0],
+                ),
+                "test_coord_array": [
+                    flexivrdk.Coord([1, 2, 3], [4, 5, 6], ["WORK", "WorkCoord0"]),
+                    flexivrdk.Coord(
+                        [10, 20, 30],
+                        [40, 50, 60],
+                        ["WORLD", "WORLD_ORIGIN"],
+                        [1, 2, 3, 4, 5, 6, 7],
+                        [10, 20, 0, 0, 0, 0],
+                    ),
+                    flexivrdk.Coord(
+                        [3, 2, 1], [180, 0, 180], ["WORLD", "WORLD_ORIGIN"]
+                    ),
+                ],
+            }
+        )
+
+        # Get updated global variables
+        # =========================================================================================
+        global_vars = robot.global_variables()
+        if not global_vars:
+            logger.warn("No global variables available")
+            return 1
+        else:
+            logger.info("Updated global variables:")
+            for key, value in global_vars.items():
+                print(f"{key}: {value}")
+
+        logger.info("Program finished")
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/intermediate1_non_realtime_joint_position_control.py

@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+
+"""intermediate1_non_realtime_joint_position_control.py
+
+This tutorial runs non-real-time joint position control to hold or sine-sweep all robot joints.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    # Required arguments
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "frequency", help="Command frequency, 1 to 100 [Hz]", type=int
+    )
+    # Optional arguments
+    argparser.add_argument(
+        "--hold",
+        action="store_true",
+        help="Robot holds current joint positions, otherwise do a sine-sweep",
+    )
+    args = argparser.parse_args()
+
+    # Check if arguments are valid
+    frequency = args.frequency
+    assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs non-real-time joint position control to "
+        "hold or sine-sweep all robot joints.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Non-real-time Joint Position Control
+        # ==========================================================================================
+        # Direct joint control can be executed by single-arm joint groups and the external axis
+        exe_groups = robot.info().single_arm_groups
+        if not exe_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+        # The external axis joint group (if it exists) also supports direct joint control
+        if flexivrdk.JointGroup.EXT_AXIS in robot.info().all_groups:
+            exe_groups[flexivrdk.JointGroup.EXT_AXIS] = robot.info().all_groups[
+                flexivrdk.JointGroup.EXT_AXIS
+            ]
+
+        # Switch to non-real-time joint position control mode
+        robot.SwitchMode(mode.NRT_JOINT_POSITION)
+
+        period = 1.0 / frequency
+        loop_time = 0
+        logger.info(
+            f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
+        )
+
+        # Use current robot joint positions as initial positions
+        all_init_pos = {}
+        robot_states = robot.states()
+        for group in exe_groups:
+            all_init_pos[group] = robot_states[group].q.copy()
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Initial joint positions: {all_init_pos[group]}"
+            )
+
+        # Joint sine-sweep amplitude [rad]
+        SWING_AMP = 0.1
+
+        # TCP sine-sweep frequency [Hz]
+        SWING_FREQ = 0.3
+
+        # Send command periodically at user-specified frequency
+        while True:
+            # Use sleep to control loop period
+            time.sleep(period)
+
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            cmds = {}
+            sine_offset = SWING_AMP * math.sin(2 * math.pi * SWING_FREQ * loop_time)
+            for group, init_pos in all_init_pos.items():
+                target_pos = init_pos.copy()
+                if not args.hold:
+                    for i in range(len(target_pos)):
+                        target_pos[i] += sine_offset
+
+                zero_vel = [0.0] * len(init_pos)
+                max_vel = [2.0] * len(init_pos)
+                max_acc = [3.0] * len(init_pos)
+                cmds[group] = flexivrdk.NrtJointPositionCmd(
+                    target_pos, zero_vel, max_vel, max_acc
+                )
+
+            robot.SendJointPosition(cmds)
+
+            # Increment loop time
+            loop_time += period
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/intermediate2_non_realtime_joint_impedance_control.py

@@ -0,0 +1,177 @@
+#!/usr/bin/env python
+
+"""intermediate2_non_realtime_joint_impedance_control.py
+
+This tutorial runs non-real-time joint impedance control to hold or sine-sweep all robot joints.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install spdlog
+import numpy as np  # pip install numpy
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    # Required arguments
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "frequency", help="Command frequency, 1 to 100 [Hz]", type=int
+    )
+    # Optional arguments
+    argparser.add_argument(
+        "--hold",
+        action="store_true",
+        help="Robot holds current joint positions, otherwise do a sine-sweep",
+    )
+    args = argparser.parse_args()
+
+    # Check if arguments are valid
+    frequency = args.frequency
+    assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs non-real-time joint impedance control to "
+        "hold or sine-sweep all robot joints.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Non-real-time Joint Impedance Control
+        # ==========================================================================================
+        # Switch to non-real-time joint impedance control mode
+        robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE)
+
+        # Direct joint control can be executed by single-arm joint groups and the external axis
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+        # The external axis joint group (if it exists) also supports direct joint control
+        exe_groups = dict(single_arm_groups)
+        if flexivrdk.JointGroup.EXT_AXIS in robot.info().all_groups:
+            exe_groups[flexivrdk.JointGroup.EXT_AXIS] = robot.info().all_groups[
+                flexivrdk.JointGroup.EXT_AXIS
+            ]
+
+        period = 1.0 / frequency
+        loop_counter = 0
+        logger.info(
+            f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
+        )
+
+        # Use current robot joint positions as initial positions
+        all_init_pos = {}
+        robot_states = robot.states()
+        for group in exe_groups:
+            all_init_pos[group] = robot_states[group].q.copy()
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Initial joint positions: {all_init_pos[group]}"
+            )
+
+        # Joint sine-sweep amplitude [rad]
+        SWING_AMP = 0.1
+
+        # TCP sine-sweep frequency [Hz]
+        SWING_FREQ = 0.3
+
+        # Send command periodically at user-specified frequency
+        while True:
+            # Use sleep to control loop period
+            time.sleep(period)
+
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            # Reduce stiffness to half of nominal values after 5 seconds
+            if loop_counter == 5 / period:
+                for group in single_arm_groups:
+                    new_Kq = np.multiply(robot.info().K_q_nom[group], 0.5).tolist()
+                    robot.SetJointImpedance(group, new_Kq)
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Joint stiffness set to: {new_Kq}"
+                    )
+
+            # Reset stiffness to nominal values after another 5 seconds
+            if loop_counter == 10 / period:
+                for group in single_arm_groups:
+                    nominal_Kq = robot.info().K_q_nom[group]
+                    robot.SetJointImpedance(group, nominal_Kq)
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Joint stiffness reset to nominal: {nominal_Kq}"
+                    )
+
+            # Send commands
+            cmds = {}
+            sine_offset = SWING_AMP * math.sin(
+                2 * math.pi * SWING_FREQ * loop_counter * period
+            )
+            for group, init_pos in all_init_pos.items():
+                target_pos = init_pos.copy()
+                if not args.hold:
+                    for i in range(len(target_pos)):
+                        target_pos[i] += sine_offset
+
+                zero_vel = [0.0] * len(init_pos)
+                max_vel = [2.0] * len(init_pos)
+                max_acc = [3.0] * len(init_pos)
+                cmds[group] = flexivrdk.NrtJointPositionCmd(
+                    target_pos, zero_vel, max_vel, max_acc
+                )
+
+            robot.SendJointPosition(cmds)
+
+            # Increment loop counter
+            loop_counter += 1
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/intermediate3_non_realtime_cartesian_pure_motion_control.py

@@ -0,0 +1,280 @@
+#!/usr/bin/env python
+
+"""intermediate3_non_realtime_cartesian_pure_motion_control.py
+
+This tutorial runs non-real-time Cartesian-space pure motion control to hold or sine-sweep the robot
+TCP. A simple collision detection is also included.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install spdlog
+import numpy as np  # pip install numpy
+import flexivrdk  # pip install flexivrdk
+import utility
+
+# Global constants
+# ==================================================================================================
+# TCP sine-sweep amplitude [m]
+SWING_AMP = 0.1
+
+# TCP sine-sweep frequency [Hz]
+SWING_FREQ = 0.3
+
+# External TCP force threshold for collision detection, value is only for demo purpose [N]
+EXT_FORCE_THRESHOLD = 10.0
+
+# External joint torque threshold for collision detection, value is only for demo purpose [Nm]
+EXT_TORQUE_THRESHOLD = 5.0
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    # Required arguments
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "frequency", help="Command frequency, 1 to 100 [Hz]", type=int
+    )
+    # Optional arguments
+    argparser.add_argument(
+        "--hold",
+        action="store_true",
+        help="Robot holds current TCP pose, otherwise do a sine-sweep",
+    )
+    argparser.add_argument(
+        "--collision",
+        action="store_true",
+        help="Enable collision detection, robot will stop upon collision",
+    )
+    args = argparser.parse_args()
+
+    # Check if arguments are valid
+    frequency = args.frequency
+    assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs non-real-time Cartesian-space pure "
+        "motion control to hold or sine-sweep the robot TCP. A simple collision detection is also "
+        "included.\n"
+    )
+
+    # Print based on arguments
+    if args.hold:
+        logger.info("Robot holding current TCP pose")
+    else:
+        logger.info("Robot running TCP sine-sweep")
+
+    if args.collision:
+        logger.info("Collision detection enabled")
+    else:
+        logger.info("Collision detection disabled")
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Zero Force-torque Sensor
+        # =========================================================================================
+        # Direct Cartesian control can only be executed by single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
+        # IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
+                for group in single_arm_groups
+            }
+        )
+
+        # WARNING: during the process, the robot must not contact anything, otherwise the result
+        # will be inaccurate and affect following operations
+        logger.warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot"
+        )
+
+        # Wait for primitive to finish
+        while not utility.primitive_state_true_for_groups(robot.primitive_states(), "terminated"):
+            time.sleep(1)
+        logger.info("Sensor zeroing complete")
+
+        # Configure Motion Control
+        # =========================================================================================
+        # The Cartesian motion force modes do pure motion control out of the box, thus nothing
+        # needs to be explicitly configured
+
+        # NOTE: motion control always uses robot world frame, while force control can use
+        # either world or TCP frame as reference frame
+
+        # Start Pure Motion Control
+        # =========================================================================================
+        # Switch to non-real-time mode for discrete motion control
+        robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
+
+        # Set all Cartesian axis(s) to motion control
+        for group in single_arm_groups:
+            robot.SetForceControlAxis(group, [False, False, False, False, False, False])
+
+        # Save initial poses and joint positions
+        all_init_pose = {}
+        all_init_q = {}
+        robot_states = robot.states()
+        for group in single_arm_groups:
+            all_init_pose[group] = robot_states[group].tcp_pose.copy()
+            all_init_q[group] = robot_states[group].q.copy()
+
+        # Periodic Task
+        # =========================================================================================
+        # Set loop period
+        period = 1.0 / frequency
+        loop_counter = 0
+        logger.info(
+            f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
+        )
+
+        # Send command periodically at user-specified frequency
+        while True:
+            # Use sleep to control loop period
+            time.sleep(period)
+
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            cmds = {}
+            sine_offset = SWING_AMP * math.sin(
+                2 * math.pi * SWING_FREQ * loop_counter * period
+            )
+            for group, init_pose in all_init_pose.items():
+                target_pose = init_pose.copy()
+
+                # Sine-sweep TCP along Y axis
+                if not args.hold:
+                    target_pose[1] += sine_offset
+
+                cmds[group] = flexivrdk.NrtCartesianCmd(target_pose)
+
+            # Send command. Calling this method with only target pose input results
+            # in pure motion control
+            robot.SendCartesianMotionForce(cmds)
+
+            #  Do the following operations in sequence for every 20 seconds
+            time_elapsed = loop_counter * period
+            # Online change reference joint positions at 3 seconds
+            if time_elapsed % 20.0 == 3.0:
+                ref_q = [0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658]
+                for group in single_arm_groups:
+                    robot.SetNullSpacePosture(group, ref_q)
+                logger.info(f"Reference joint positions set to: {ref_q}")
+            # Online change stiffness to half of nominal at 6 seconds
+            elif time_elapsed % 20.0 == 6.0:
+                for group in single_arm_groups:
+                    new_K = np.multiply(robot.info().K_x_nom[group], 0.5).tolist()
+                    robot.SetCartesianImpedance(group, new_K)
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Cartesian stiffness set to: {new_K}"
+                    )
+            # Online change to another reference joint positions at 9 seconds
+            elif time_elapsed % 20.0 == 9.0:
+                ref_q = [-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658]
+                for group in single_arm_groups:
+                    robot.SetNullSpacePosture(group, ref_q)
+                logger.info(f"Reference joint positions set to: {ref_q}")
+            # Online reset stiffness to nominal at 12 seconds
+            elif time_elapsed % 20.0 == 12.0:
+                for group in single_arm_groups:
+                    nominal_K = robot.info().K_x_nom[group]
+                    robot.SetCartesianImpedance(group, nominal_K)
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Cartesian stiffness reset to nominal: {nominal_K}"
+                    )
+            # Online reset reference joint positions to nominal at 14 seconds
+            elif time_elapsed % 20.0 == 14.0:
+                for group, init_q in all_init_q.items():
+                    robot.SetNullSpacePosture(group, init_q)
+                logger.info("Reference joint positions are reset")
+            # Online enable max contact wrench regulation at 16 seconds
+            elif time_elapsed % 20.0 == 16.0:
+                max_wrench = [10.0, 10.0, 10.0, 2.0, 2.0, 2.0]
+                for group in single_arm_groups:
+                    robot.SetMaxContactWrench(group, max_wrench)
+                logger.info(f"Max contact wrench set to: {max_wrench}")
+            # Disable max contact wrench regulation at 19 seconds
+            elif time_elapsed % 20.0 == 19.0:
+                for group in single_arm_groups:
+                    robot.SetMaxContactWrench(group, [float("inf")] * 6)
+                logger.info("Max contact wrench regulation is disabled")
+
+            # Simple collision detection: stop robot if collision is detected at
+            # end-effector
+            if args.collision:
+                for group, states in robot.states().items():
+                    collision_detected = False
+                    ext_force = np.array(states.tcp_wrench[:3])
+                    if np.linalg.norm(ext_force) > EXT_FORCE_THRESHOLD:
+                        collision_detected = True
+
+                    for v in states.tau_ext:
+                        if abs(v) > EXT_TORQUE_THRESHOLD:
+                            collision_detected = True
+
+                    if collision_detected:
+                        robot.Stop()
+                        logger.warn(
+                            f"[{flexivrdk.kJointGroupNames[group]}] Collision detected, stopping robot and exit program ..."
+                        )
+                        return
+
+            # Increment loop counter
+            loop_counter += 1
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/intermediate4_non_realtime_cartesian_motion_force_control.py

@@ -0,0 +1,300 @@
+#!/usr/bin/env python
+
+"""intermediate4_non_realtime_cartesian_motion_force_control.py
+
+This tutorial runs non-real-time Cartesian-space unified motion-force control. The Z axis of the
+chosen reference frame will be activated for explicit force control, while the rest axes in the
+same reference frame will stay motion controlled.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install spdlog
+import numpy as np  # pip install numpy
+import flexivrdk  # pip install flexivrdk
+import utility
+
+# Global constants
+# ==================================================================================================
+# TCP sine-sweep amplitude [m]
+SWING_AMP = 0.1
+
+# TCP sine-sweep frequency [Hz]
+SWING_FREQ = 0.3
+
+# Pressing force to apply during the unified motion-force control [N]
+PRESSING_FORCE = 5.0
+
+# Cartesian linear velocity used to search for contact [m/s]
+SEARCH_VELOCITY = 0.02
+
+# Maximum distance to travel when searching for contact [m]
+SEARCH_DISTANCE = 1.0
+
+# Maximum contact wrench during contact search for soft contact
+MAX_WRENCH_FOR_CONTACT_SEARCH = [10.0, 10.0, 10.0, 3.0, 3.0, 3.0]
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    # Required arguments
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "frequency", help="Command frequency, 1 to 100 [Hz]", type=int
+    )
+    # Optional arguments
+    argparser.add_argument(
+        "--TCP",
+        action="store_true",
+        help="Use TCP frame as reference frame for force control, otherwise use world frame",
+    )
+    argparser.add_argument(
+        "--polish",
+        action="store_true",
+        help="Run a simple polish motion along XY plane in world frame, otherwise hold robot motion in non-force-control axes",
+    )
+    args = argparser.parse_args()
+
+    # Check if arguments are valid
+    frequency = args.frequency
+    assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs non-real-time Cartesian-space unified "
+        "motion-force control. The Z axis of the chosen reference frame will be activated for "
+        "explicit force control, while the rest axes in the same reference frame will stay motion "
+        "controlled.\n"
+    )
+
+    # The reference frame to use, see Robot::SendCartesianMotionForce() for more details
+    force_ctrl_frame = flexivrdk.CoordType.WORLD
+    if args.TCP:
+        logger.info("Reference frame used for force control: robot TCP frame")
+        force_ctrl_frame = flexivrdk.CoordType.TCP
+    else:
+        logger.info("Reference frame used for force control: robot world frame")
+
+    # Whether to enable polish motion
+    if args.polish:
+        logger.info(
+            "Robot will run a polish motion along XY plane in robot world frame"
+        )
+    else:
+        logger.info("Robot will hold its motion in all non-force-controlled axes")
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Zero Force-torque Sensor
+        # =========================================================================================
+        # Direct Cartesian control can only be executed by single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
+        # IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
+                for group in single_arm_groups
+            }
+        )
+
+        # WARNING: during the process, the robot must not contact anything, otherwise the result
+        # will be inaccurate and affect following operations
+        logger.warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot"
+        )
+
+        # Wait for primitive to finish
+        while not utility.primitive_state_true_for_groups(
+            robot.primitive_states(), "terminated"
+        ):
+            time.sleep(1)
+        logger.info("Sensor zeroing complete")
+
+        # Search for Contact
+        # =========================================================================================
+        # NOTE: there are several ways to do contact search, such as using primitives, or real-time
+        # and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian
+        # control for example.
+        logger.info("Searching for contact ...")
+
+        # Set initial poses to current TCP poses
+        all_init_pose = {}
+        for group in single_arm_groups:
+            all_init_pose[group] = robot.states()[group].tcp_pose.copy()
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {all_init_pose[group]}"
+            )
+
+        # Use non-real-time mode to make the robot go to a set point with its own motion generator
+        robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
+
+        # Search for contact with max contact wrench set to a small value for making soft contact
+        for group in single_arm_groups:
+            robot.SetMaxContactWrench(group, MAX_WRENCH_FOR_CONTACT_SEARCH)
+
+        # Set target point along -Z direction and expect contact to happen during the travel
+        cmds = {}
+        for group, init_pose in all_init_pose.items():
+            target_pose = init_pose.copy()
+            target_pose[2] -= SEARCH_DISTANCE
+            cmds[group] = flexivrdk.NrtCartesianCmd(
+                target_pose, [0.0] * 6, [0.0] * 6, SEARCH_VELOCITY
+            )
+
+        # Send target point to robot to start searching for contact and limit the velocity. Keep
+        # target wrench 0 at this stage since we are not doing force control yet
+        robot.SendCartesianMotionForce(cmds)
+
+        # Use a while loop to poll robot states and check if a contact is made
+        is_contacted = False
+        while not is_contacted:
+            # Compute norm of sensed external force applied on robot TCP
+            for group, states in robot.states().items():
+                ext_force = np.array(states.tcp_wrench[:3])
+
+                # Contact is considered to be made if sensed TCP force exceeds the threshold
+                if np.linalg.norm(ext_force) > PRESSING_FORCE:
+                    is_contacted = True
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Contact detected at robot TCP"
+                    )
+                    break
+
+            # Check at 1ms interval
+            time.sleep(0.001)
+
+        # Configure Force Control
+        # =========================================================================================
+        # Switch to non-real-time mode for discrete motion force control
+        robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
+
+        # Set force control reference frame based on program argument. See function doc for more
+        # details
+        for group in single_arm_groups:
+            robot.SetForceControlFrame(group, force_ctrl_frame)
+
+        # Set which Cartesian axis(s) to activate for force control. See function doc for more
+        # details. Here we only active Z axis
+        for group in single_arm_groups:
+            robot.SetForceControlAxis(group, [False, False, True, False, False, False])
+
+        # Uncomment the following line to enable passive force control, otherwise active force
+        # control is used by default. See function doc for more details
+        # for group in single_arm_groups:
+        #     robot.SetPassiveForceControl(group, True)
+
+        # NOTE: motion control always uses robot world frame, while force control can use
+        # either world or TCP frame as reference frame
+
+        # Start Unified Motion Force Control
+        # =========================================================================================
+        # Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis
+        # is activated (i.e. motion control disabled in Z axis) and the motion force control mode
+        # is entered, this way the contact force along Z axis is explicitly regulated and will not
+        # spike after the max contact wrench regulation for motion control is disabled
+        for group in single_arm_groups:
+            robot.SetMaxContactWrench(group, [float("inf")] * 6)
+
+        # Update initial poses to current TCP poses
+        for group in single_arm_groups:
+            all_init_pose[group] = robot.states()[group].tcp_pose.copy()
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {all_init_pose[group]}"
+            )
+
+        # Periodic Task
+        # =========================================================================================
+        # Set loop period
+        period = 1.0 / frequency
+        logger.info(
+            f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
+        )
+
+        # Periodic loop counter
+        loop_counter = 0
+
+        # Send command periodically at user-specified frequency
+        while True:
+            # Use sleep to control loop period
+            time.sleep(period)
+
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            # Set Fz according to reference frame to achieve a "pressing down" behavior
+            Fz = 0.0
+            if force_ctrl_frame == flexivrdk.CoordType.WORLD:
+                Fz = PRESSING_FORCE
+            elif force_ctrl_frame == flexivrdk.CoordType.TCP:
+                Fz = -PRESSING_FORCE
+            target_wrench = [0.0, 0.0, Fz, 0.0, 0.0, 0.0]
+
+            cmds = {}
+            for group, init_pose in all_init_pose.items():
+                target_pose = init_pose.copy()
+                if args.polish:
+                    target_pose[1] += SWING_AMP * math.sin(
+                        2 * math.pi * SWING_FREQ * loop_counter * period
+                    )
+
+                cmds[group] = flexivrdk.NrtCartesianCmd(target_pose, target_wrench)
+
+            robot.SendCartesianMotionForce(cmds)
+
+            # Increment loop counter
+            loop_counter += 1
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/intermediate5_robot_dynamics.py

@@ -0,0 +1,132 @@
+#!/usr/bin/env python
+
+"""intermediate5_robot_dynamics.py
+
+This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and
+gravity torques. Also checks reachability of a Cartesian pose.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain "
+        "robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian "
+        "pose.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Robot Dynamics
+        # ==========================================================================================
+        # Initialize dynamics engine
+        model = flexivrdk.Model(robot)
+
+        # Step dynamics engine 5 times
+        for i in range(5):
+            # Update robot model in dynamics engine
+            robot_states = robot.states()
+            model.Update(
+                robot_states[flexivrdk.JointGroup.ALL].q,
+                robot_states[flexivrdk.JointGroup.ALL].dq,
+            )
+
+            # Compute gravity vector
+            g = model.g()
+
+            # Compute mass matrix
+            M = model.M()
+
+            # Compute Jacobian
+            J = model.J("flange")
+
+            # Print result
+            logger.info("g = ")
+            print(g, flush=True)
+            logger.info("M = ")
+            print(M, flush=True)
+            logger.info("J = ")
+            print(J, flush=True)
+            print()
+
+        # Check IK feasibility for a nearby Cartesian pose on all available single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        robot_states = robot.states()
+        ik_params_by_group = {}
+        for group in single_arm_groups:
+            pose_to_check = robot_states[group].tcp_pose.copy()
+            pose_to_check[0] += 0.1
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Checking IK feasibility of Cartesian pose {pose_to_check}"
+            )
+            ik_params = flexivrdk.IKParams()
+            ik_params.cartesian_pose = pose_to_check
+            ik_params.seed_q = robot_states[group].q.copy()
+            ik_params.free_orientation = False
+            ik_params_by_group[group] = ik_params
+
+        # Print result
+        result = model.SolveConstrainedIK(ik_params_by_group)
+        logger.info(f"IK result success = {result.success}")
+        for group, q in result.solved_q.items():
+            logger.info(f"[{flexivrdk.kJointGroupNames[group]}] solved_q = {q}")
+
+    except Exception as e:
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()

thirdparty/flexiv_rdk/example_py/utility.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+
+"""utility.py
+
+Utility methods.
+
+Import this module as `import utility` and use functions through the module namespace,
+for example `utility.quat2eulerZYX(...)` and `utility.primitive_state_true_for_groups(...)`.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import math
+
+# pip install scipy
+from scipy.spatial.transform import Rotation as R
+
+
+def quat2eulerZYX(quat, degree=False):
+    """
+    Convert quaternion to Euler angles with ZYX axis rotations.
+
+    Parameters
+    ----------
+    quat : float list
+        Quaternion input in [w,x,y,z] order.
+    degree : bool
+        Return values in degrees, otherwise in radians.
+
+    Returns
+    ----------
+    float list
+        Euler angles in [x,y,z] order, radian by default unless specified otherwise.
+        Returned angles are normalized by scipy to a principal range. In degree mode,
+        this is typically within [-180, 180].
+    """
+
+    # Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation
+    # NOTE: scipy uses [x,y,z,w] order to represent quaternion. The returned Euler values
+    # are a normalized representation of the same physical orientation.
+    eulerZYX = (
+        R.from_quat([quat[1], quat[2], quat[3], quat[0]])
+        .as_euler("xyz", degrees=degree)
+        .tolist()
+    )
+
+    return eulerZYX
+
+
+def primitive_state_true_for_groups(primitive_states, state_name):
+    """
+    Check whether primitive states are true for all groups included in primitive_states.
+
+    Parameters
+    ----------
+    primitive_states : dict
+        Primitive states keyed by joint group.
+    state_name : str or dict
+        Primitive state name to check for all groups, or per-group state names keyed by joint
+        group, e.g. "reachedTarget" or {group: "terminated"}.
+
+    Returns
+    ----------
+    bool
+        True if all included groups contain the requested primitive state and its integer value is
+        non-zero.
+    """
+
+    def primitive_state_true(primitive_state, requested_state_name):
+        if requested_state_name not in primitive_state.names_and_values:
+            return False
+
+        value = primitive_state.names_and_values[requested_state_name]
+        return isinstance(value, int) and value != 0
+
+    if isinstance(state_name, dict):
+        return all(
+            group in state_name
+            and primitive_state_true(primitive_state, state_name[group])
+            for group, primitive_state in primitive_states.items()
+        )
+
+    return all(
+        primitive_state_true(primitive_state, state_name)
+        for primitive_state in primitive_states.values()
+    )
+
+
+def list2str(ls):
+    """
+    Convert a list to a string.
+
+    Parameters
+    ----------
+    ls : list
+        Source list of any size.
+
+    Returns
+    ----------
+    str
+        A string with format "ls[0] ls[1] ... ls[n] ", i.e. each value
+        followed by a space, including the last one.
+    """
+
+    ret_str = ""
+    for i in ls:
+        ret_str += str(i) + " "
+    return ret_str

thirdparty/flexiv_rdk/include/flexiv/rdk/data.hpp

@@ -0,0 +1,896 @@
+/**
+ * @file data.hpp
+ * @brief Header file containing various constant expressions, data structures, and enums.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_DATA_HPP_
+#define FLEXIV_RDK_DATA_HPP_
+
+#include <array>
+#include <vector>
+#include <string>
+#include <ostream>
+#include <variant>
+#include <chrono>
+#include <map>
+
+namespace flexiv::rdk {
+/** Cartesian-space degrees of freedom */
+constexpr size_t kCartDoF = 6;
+
+/** Joint-space degrees of freedom of Flexiv's serial robots */
+constexpr size_t kSerialJointDoF = 7;
+
+/** Size of pose array (3 position + 4 quaternion) */
+constexpr size_t kPoseSize = 7;
+
+/** Number of digital IO ports: 16 on the control box + 2 in each wrist connector * maximum 2 wrists
+ */
+constexpr size_t kIOPorts = 16 + 2 * 2;
+
+/** Maximum number of external axes */
+constexpr size_t kMaxExtAxes = 6;
+
+/**
+ * @brief All supported product models of the robot.
+ */
+enum class ProductModel
+{
+    UNKNOWN = 0,
+    Enlight_L,  /// Enlight-L standard version
+    Enlight_LL, /// Enlight-LL: Dual Enlight-L with customizable mounting poses
+    MICO_Core,  /// MICO-Core: Dual Enlight-L with fixed-mounting upper body
+    MICO_Plus,  /// MICO-Plus: MICO-Core with pan-tilt torso
+    MICO_Ultra, /// MICO-Ultra: MICO-Plus with mobile base
+};
+
+/** Map ProductModel enums to strings */
+inline const std::map<ProductModel, std::string> kProductModelNames
+    = {{ProductModel::UNKNOWN, "UNKNOWN"}, {ProductModel::Enlight_L, "Enlight-L"},
+        {ProductModel::Enlight_LL, "Enlight-LL"}, {ProductModel::MICO_Core, "MICO-Core"},
+        {ProductModel::MICO_Plus, "MICO-Plus"}, {ProductModel::MICO_Ultra, "MICO-Ultra"}};
+
+/**
+ * @brief All possible joint groups of the robot.
+ */
+enum class JointGroup
+{
+    UNKNOWN = -1, ///< Unknown group
+    ALL = 0,      ///< The full system, including all actuated joints
+    ARMS = 2,     ///< The dual arms as a whole, only applicable to dual-arm robots
+    ARM_1 = 3,    ///< The 1st single arm in a dual-arm robot or the only arm in a single-arm robot
+    ARM_2 = 4,    ///< The 2nd single arm in a dual-arm robot, not applicable to single-arm robots
+    EXT_AXIS = 5, ///< External axis(es) for workspace extension
+
+    FIRST = ALL,
+    LAST = EXT_AXIS,
+};
+
+/** Map JointGroup enums to strings */
+inline const std::map<JointGroup, std::string> kJointGroupNames {
+    {JointGroup::UNKNOWN, "UNKNOWN"},
+    {JointGroup::ALL, "ALL"},
+    {JointGroup::ARMS, "ARMS"},
+    {JointGroup::ARM_1, "ARM_1"},
+    {JointGroup::ARM_2, "ARM_2"},
+    {JointGroup::EXT_AXIS, "EXT_AXIS"},
+};
+
+/**
+ * @brief All possible operational statuses of the robot. Except for the first two, the other
+ * enumerators indicate the cause of the robot being not ready to operate.
+ * @see Robot::operational_status().
+ */
+enum class OperationalStatus
+{
+    UNKNOWN = 0,        ///< Unknown status.
+    READY,              ///< Ready to be operated.
+    BOOTING,            ///< System still booting, please wait.
+    ESTOP_NOT_RELEASED, ///< E-Stop is not released.
+    NOT_SERVO_ON,       ///< Not servo on, call ServoOn() to send the signal.
+    RELEASING_BRAKE,    ///< Brake release in progress, please wait.
+    MINOR_FAULT,        ///< Minor fault occurred, call ClearFault() to try clearing it.
+    CRITICAL_FAULT,     ///< Critical fault occurred, call ClearFault() to try clearing it.
+    IN_REDUCED_STATE,   ///< In reduced state, see reduced().
+    IN_RECOVERY_STATE,  ///< In recovery state, see recovery().
+    IN_MANUAL_MODE,     ///< In Manual mode, need to switch to Auto (Remote) mode.
+    IN_AUTO_MODE,       ///< In regular Auto mode, need to switch to Auto (Remote) mode.
+};
+
+/** Map OperationalStatus enums to strings */
+inline const std::map<OperationalStatus, std::string> kOpStatusNames {
+    {OperationalStatus::UNKNOWN, "Unknown status"},
+    {OperationalStatus::READY, "Ready"},
+    {OperationalStatus::BOOTING, "System booting"},
+    {OperationalStatus::ESTOP_NOT_RELEASED, "E-Stop not released"},
+    {OperationalStatus::NOT_SERVO_ON, "Not servo on"},
+    {OperationalStatus::RELEASING_BRAKE, "Releasing brakes"},
+    {OperationalStatus::MINOR_FAULT, "Minor fault occurred"},
+    {OperationalStatus::CRITICAL_FAULT, "Critical fault occurred"},
+    {OperationalStatus::IN_REDUCED_STATE, "In reduced state"},
+    {OperationalStatus::IN_RECOVERY_STATE, "In recovery state"},
+    {OperationalStatus::IN_MANUAL_MODE, "In Manual mode"},
+    {OperationalStatus::IN_AUTO_MODE, "In regular Auto mode"},
+};
+
+/**
+ * @brief Type of commonly-used reference coordinates.
+ */
+enum class CoordType
+{
+    WORLD, ///< World frame (fixed).
+    TCP,   ///< TCP frame (move with the robot's end effector).
+};
+
+/**
+ * @struct RobotEvent
+ * @brief Information about a robot event.
+ * @see Robot::event_log().
+ */
+struct RobotEvent
+{
+    enum Level
+    {
+        UNKNOWN = 0, ///< Not set
+        INFO,        ///< Informational event
+        WARNING,     ///< Warning event
+        ERROR,       ///< Error event
+        CRITICAL,    ///< Critical error event
+    };
+
+    /** Level of the event */
+    Level level = UNKNOWN;
+
+    /** Unique ID of the event */
+    int id = 0;
+
+    /** Brief description of the event */
+    std::string description = "";
+
+    /** Consequences caused by the event */
+    std::string consequences = "";
+
+    /** Probable causes of the event */
+    std::string probable_causes = "";
+
+    /** Recommended actions after the event */
+    std::string recommended_actions = "";
+
+    /** Timestamp (since epoch) of the event */
+    std::chrono::time_point<std::chrono::system_clock> timestamp;
+};
+
+/**
+ * @struct RobotInfo
+ * @brief General information about the connected robot.
+ * @see Robot::info().
+ */
+struct RobotInfo
+{
+    /** Product model of the robot */
+    ProductModel product_model = {};
+
+    /** Serial number of the robot */
+    std::string serial_num = {};
+
+    /** Software version of the robot */
+    std::string software_ver = {};
+
+    /** Type of license */
+    std::string license_type = {};
+
+    /** All available joint groups in the robot and their names */
+    std::map<JointGroup, std::string> all_groups = {};
+
+    /** Available single-arm joint groups in the robot and their names */
+    std::map<JointGroup, std::string> single_arm_groups = {};
+
+    /** Joint-space degrees of freedom for each joint group */
+    std::map<JointGroup, size_t> DoF = {};
+
+    /** Nominal Cartesian motion stiffness of all available single-arm joint groups when in
+     * Cartesian motion-force control modes: \f$ K_x^{nom} \in \mathbb{R}^{6 \times 1} \f$. Consists
+     * of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$ \mathbb{R}^{3 \times 1} \f$
+     * angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, k_{Rz}]^T \f$.
+     * Unit: \f$ [N/m]:[Nm/rad] \f$. */
+    std::map<JointGroup, std::array<double, kCartDoF>> K_x_nom = {};
+
+    /** Nominal joint motion stiffness of all available single-arm joint groups when in joint
+     * impedance control modes: \f$ K_q^{nom} \in \mathbb{R}^{n \times 1} \f$.
+     * Unit: \f$ [Nm/rad] \f$. */
+    std::map<JointGroup, std::vector<double>> K_q_nom = {};
+
+    /** Lower software limits of joint positions of all available joint groups: \f$ q_{min} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> q_min = {};
+
+    /** Upper software limits of joint positions of all available joint groups: \f$ q_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> q_max = {};
+
+    /** Upper software limits of joint velocities of all available joint groups: \f$ \dot{q}_{max}
+     * \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$. */
+    std::map<JointGroup, std::vector<double>> dq_max = {};
+
+    /** Upper software limits of joint torques of all available joint groups: \f$ \tau_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$. */
+    std::map<JointGroup, std::vector<double>> tau_max = {};
+};
+
+/**
+ * @struct RobotStates
+ * @brief Robot states data in joint and Cartesian space for a joint group.
+ */
+struct RobotStates
+{
+    /** Current time since epoch of the robot system. The pair consists of {seconds since epoch,
+     * nanoseconds since last full second} */
+    std::pair<int, int> timestamp = {};
+
+    /**
+     * Measured joint positions from the link-side encoder: \f$ q \in \mathbb{R}^{n \times 1} \f$.
+     * This is the direct measurement of joint positions. Unit: \f$ [rad] or [m] \f$.
+     * @note If a joint has only one encoder, then \f$ \theta = q \f$.
+     */
+    std::vector<double> q = {};
+
+    /**
+     * Measured joint positions from the motor-side encoder: \f$ \theta \in \mathbb{R}^{n \times 1}
+     * \f$. This is the indirect measurement of joint positions. \f$ \theta = q + \Delta \f$, where
+     * \f$ \Delta \f$ is the joint's internal deflection between motor and link. Unit: \f$ [rad] or
+     * [m] \f$.
+     * @note If a joint has only one encoder, then \f$ \theta = q \f$.
+     */
+    std::vector<double> theta = {};
+
+    /**
+     * Measured joint velocities from the link-side encoder: \f$ \dot{q} \in \mathbb{R}^{n \times
+     * 1} \f$. This is the direct but more noisy measurement of joint velocities. Unit: \f$ [rad/s]
+     * or [m/s] \f$.
+     * @note If a joint has only one encoder, then \f$ \dot{\theta} = \dot{q} \f$.
+     */
+    std::vector<double> dq = {};
+
+    /**
+     * Measured joint velocities from the motor-side encoder: \f$ \dot{\theta} \in \mathbb{R}^{n
+     * \times 1} \f$. This is the indirect but less noisy measurement of joint velocities. Unit: \f$
+     * [rad/s] or [m/s] \f$.
+     * @note If a joint has only one encoder, then \f$ \dot{\theta} = \dot{q} \f$.
+     */
+    std::vector<double> dtheta = {};
+
+    /**
+     * Measured joint torques: \f$ \tau \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
+     * @note If a joint has no torque measurement, the corresponding value will be 0.
+     */
+    std::vector<double> tau = {};
+
+    /**
+     * Numerical derivative of measured joint torques: \f$ \dot{\tau} \in \mathbb{R}^{n \times 1}
+     * \f$. Unit: \f$ [Nm/s] \f$.
+     * @note If a joint has no torque measurement, the corresponding value will be 0.
+     */
+    std::vector<double> tau_dot = {};
+
+    /**
+     * Estimated external joint torques: \f$ \hat \tau_{ext} \in \mathbb{R}^{n \times 1} \f$.
+     * Produced by any external contact (with robot body or end-effector) that does not belong to
+     * the known robot model. Unit: \f$ [Nm] \f$.
+     * @note If a joint has no torque measurement, the corresponding value will be 0.
+     */
+    std::vector<double> tau_ext = {};
+
+    /**
+     * Estimated interaction joint torques: \f$ \hat \tau_{int} \in \mathbb{R}^{n \times 1} \f$.
+     * Produced by any interaction forces at the TCP. Unit: \f$ [Nm] \f$.
+     * @note If a joint has no torque measurement, the corresponding value will be 0.
+     */
+    std::vector<double> tau_interact = {};
+
+    /**
+     * Measured joint temperatures: \f$ temp \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [°C] \f$.
+     * @note If a joint has no temperature measurement, the corresponding value will be 0.
+     */
+    std::vector<double> temperature = {};
+
+    /**
+     * Measured flange pose w.r.t. world frame: \f$ ^{O}T_{flange} \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
+     */
+    std::array<double, kPoseSize> flange_pose = {};
+
+    /**
+     * Measured TCP pose w.r.t. world frame: \f$ ^{O}T_{TCP} \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
+     */
+    std::array<double, kPoseSize> tcp_pose = {};
+
+    /**
+     * Measured TCP twist w.r.t. world frame: \f$ ^{O}\dot{X} \in \mathbb{R}^{6 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear velocity and \f$ \mathbb{R}^{3 \times 1}
+     * \f$ angular velocity: \f$ [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T \f$. Unit: \f$
+     * [m/s]:[rad/s] \f$.
+     */
+    std::array<double, kCartDoF> tcp_twist = {};
+
+    /**
+     * Measured or estimated external wrench applied on TCP w.r.t. world frame: \f$ ^{O}F_{ext} \in
+     * \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
+     * \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
+     * Unit: \f$ [N]:[Nm] \f$.
+     */
+    std::array<double, kCartDoF> tcp_wrench = {};
+
+    /**
+     * Measured or estimated external wrench applied on TCP w.r.t. local frame: \f$ ^{TCP}F_{ext}
+     * \in \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
+     * \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
+     * Unit: \f$ [N]:[Nm] \f$.
+     */
+    std::array<double, kCartDoF> tcp_wrench_local = {};
+
+    /**
+     * Unfiltered tcp_wrench. The data is more noisy but has no filter latency.
+     */
+    std::array<double, kCartDoF> raw_tcp_wrench = {};
+
+    /**
+     * Unfiltered tcp_wrench_local. The data is more noisy but has no filter latency.
+     */
+    std::array<double, kCartDoF> raw_tcp_wrench_local = {};
+
+    /**
+     * Raw reading from the force-torque (FT) sensor w.r.t. flange frame: \f$ ^{flange}F_{raw}
+     * \in \mathbb{R}^{6 \times 1} \f$. The value is 0 if no FT sensor is installed. Consists of
+     * \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$
+     * [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$ [N]:[Nm] \f$.
+     */
+    std::array<double, kCartDoF> raw_ft_sensor = {};
+};
+
+/**
+ * @struct RobotActions
+ * @brief Robot actions data in joint and Cartesian space for a joint group.
+ */
+struct RobotActions
+{
+    /** Current time since epoch of the robot system. The pair consists of {seconds since epoch,
+     * nanoseconds since last full second} */
+    std::pair<int, int> timestamp = {};
+
+    /**
+     * Desired joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] or [m] \f$.
+     */
+    std::vector<double> q_d = {};
+
+    /**
+     * Desired joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] or
+     * [m/s] \f$.
+     */
+    std::vector<double> dq_d = {};
+
+    /**
+     * Desired joint torques excluding the compensation of nonlinear dynamics: \f$ \tau_d \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
+     * @note Nonlinear dynamics include: gravity, centrifugal, and Coriolis. If the robot is static,
+     * tau_d will be zeros.
+     * @note If a joint has no torque control capability, the corresponding value will be 0.
+     */
+    std::vector<double> tau_d = {};
+
+    /**
+     * Desired TCP pose w.r.t. world frame: \f$ {^{O}T_{TCP}}_d \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]~[] \f$.
+     */
+    std::array<double, kPoseSize> tcp_pose_d = {};
+
+    /**
+     * Desired TCP twist w.r.t. world frame: \f$ {^{O}\dot{X}}_d \in \mathbb{R}^{6 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear velocity and \f$ \mathbb{R}^{3 \times 1}
+     * \f$ angular velocity: \f$ [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T \f$. Unit: \f$
+     * [m/s]:[rad/s] \f$.
+     */
+    std::array<double, kCartDoF> tcp_twist_d = {};
+
+    /**
+     * Desired TCP wrench w.r.t. the current force control frame: \f$ {^{ctrl}F_{ext}}_d \in
+     * \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
+     * \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$
+     * [N]:[Nm] \f$.
+     */
+    std::array<double, kCartDoF> tcp_wrench_d = {};
+};
+
+/**
+ * @struct PlanInfo
+ * @brief Information of the on-going primitive/plan.
+ * @see Robot::plan_info().
+ */
+struct PlanInfo
+{
+    /** Current primitive name */
+    std::string pt_name = {};
+
+    /** Current node name */
+    std::string node_name = {};
+
+    /** Current node path */
+    std::string node_path = {};
+
+    /** Current node path time period */
+    std::string node_path_time_period = {};
+
+    /** Current node path number */
+    std::string node_path_number = {};
+
+    /** Assigned plan name */
+    std::string assigned_plan_name = {};
+
+    /** Velocity scale */
+    double velocity_scale = {};
+
+    /** Waiting for user signal to step the breakpoint */
+    bool waiting_for_step = {};
+};
+
+/**
+ * @struct JPos
+ * @brief Data structure representing the customized data type "JPOS" in Flexiv Elements.
+ * @warning Here [m] is used as the unit of length, whereas [mm] is used in Flexiv Elements. The
+ * conversion is automatically done when exchanging "JPOS" data type with the robot via functions
+ * like Robot::ExecutePrimitive(), Robot::SetGlobalVariables(), etc.
+ */
+struct JPos
+{
+    /** Default constructor */
+    JPos() = default;
+
+    /**
+     * @brief Custom constructor.
+     * @param[in] q_m Sets struct member [q_m].
+     * @param[in] q_e Sets struct member [q_e]. Leave empty if there's no external axis.
+     */
+    JPos(const std::array<double, kSerialJointDoF>& q_m,
+        const std::array<double, kMaxExtAxes>& q_e = {})
+    : q_m(q_m)
+    , q_e(q_e)
+    {
+    }
+
+    /** Joint positions of the robot manipulator. Unit: [degree] */
+    std::array<double, kSerialJointDoF> q_m = {};
+
+    /** Joint positions (linear or angular) of the external axes. Unit: [m] or [degree]
+     * @note If the number of external axes \f$ n_e < kMaxExtAxes \f$, set the first \f$ n_e \f$
+     * elements and leave the rest 0. Leave the whole array empty if there's no external axis. */
+    std::array<double, kMaxExtAxes> q_e = {};
+
+    /** String representation of all data in the struct, separated by space */
+    std::string str() const;
+};
+
+/**
+ * @struct Coord
+ * @brief Data structure representing the customized data type "COORD" in Flexiv Elements.
+ * @warning Here [m] is used as the unit of length, whereas [mm] is used in Flexiv Elements. The
+ * conversion is automatically done when exchanging "COORD" data type with the robot via functions
+ * like Robot::ExecutePrimitive(), Robot::SetGlobalVariables(), Robot::global_variables(), etc.
+ */
+struct Coord
+{
+    /** Default constructor */
+    Coord() = default;
+
+    /**
+     * @brief Custom constructor.
+     * @param[in] position Sets struct member [position].
+     * @param[in] orientation Sets struct member [orientation].
+     * @param[in] ref_frame Sets struct member [ref_frame].
+     * @param[in] ref_q_m Sets struct member [ref_q_m]. Leave empty to use default values.
+     * @param[in] ref_q_e Sets struct member [ref_q_e]. Leave empty if there's no external axis.
+     */
+    Coord(const std::array<double, kCartDoF / 2>& position,
+        const std::array<double, kCartDoF / 2>& orientation,
+        const std::array<std::string, 2>& ref_frame,
+        const std::array<double, kSerialJointDoF>& ref_q_m = {},
+        const std::array<double, kMaxExtAxes>& ref_q_e = {})
+    : position(position)
+    , orientation(orientation)
+    , ref_frame(ref_frame)
+    , ref_q_m(ref_q_m)
+    , ref_q_e(ref_q_e)
+    {
+    }
+
+    /** Position in [ref_frame]. Unit: [m] */
+    std::array<double, kCartDoF / 2> position = {};
+
+    /** Orientation in terms of Euler angles in [ref_frame]. Unit: [degree] */
+    std::array<double, kCartDoF / 2> orientation = {};
+
+    /** Name of the reference frame "root::branch" represented as {"root", "branch"}.
+     *  Refer to Flexiv Elements for available options. Some common ones are:
+     * - World origin: {"WORLD", "WORLD_ORIGIN"}
+     * - Current pose: {"TRAJ", "START"}
+     * - A work coordinate: {"WORK", "WorkCoord0"}
+     * - A global variable: {"GVAR", "MyCoord0"}
+     */
+    std::array<std::string, 2> ref_frame = {};
+
+    /** Reference joint positions of the robot manipulator. Only effective on robots with redundant
+     * degrees of freedom. Unit: [degree]
+     * @note Leave empty to use default values. However, this array cannot be empty if [ref_q_e] has
+     * values */
+    std::array<double, kSerialJointDoF> ref_q_m = {};
+
+    /** Reference joint positions (linear or angular) of the external axes. Only effective on
+     * robots with redundant degrees of freedom and external axes. Unit: [m] or [degree]
+     * @note If the number of external axes \f$ n_e < kMaxExtAxes \f$, set the first \f$ n_e \f$
+     * elements and leave the rest 0. Leave the whole array empty if there's no external axis. */
+    std::array<double, kMaxExtAxes> ref_q_e = {};
+
+    /** String representation of all data in the struct, separated by space */
+    std::string str() const;
+};
+
+/**
+ * @brief Modes for synchronous motions.
+ * @see PrimitiveArgs::sync_motion_mode
+ */
+enum class SyncMotionMode
+{
+    DISABLE = 0,    ///< Don't sync with any target
+    ARM1_TCP = 1,   ///< Sync with arm1 tcp
+    ARM2_TCP = 2,   ///< sync with arm2 tcp
+    POSITIONER = 3, ///< sync with positioner
+};
+
+/** Alias of the variant that holds all possible types of data exchanged with Flexiv robots */
+using FlexivDataTypes = std::variant<int, double, std::string, rdk::JPos, rdk::Coord,
+    std::vector<int>, std::vector<double>, std::vector<std::string>, std::vector<rdk::JPos>,
+    std::vector<rdk::Coord>>;
+
+/**
+ * @struct PrimitiveArgs
+ * @brief Arguments of a primitive command.
+ * @see Robot::ExecutePrimitive().
+ * @see [Flexiv Primitives documentation](https://www.flexiv.com/primitives/)
+ */
+struct PrimitiveArgs
+{
+    /** Default constructor */
+    PrimitiveArgs() = default;
+
+    /** Custom constructor */
+    PrimitiveArgs(
+        const std::string& pt_name, const std::map<std::string, FlexivDataTypes>& input_params)
+    : pt_name(pt_name)
+    , input_params(input_params)
+    {
+    }
+
+    /** Name of the primitive to execute. For example, "Home", "MoveL", "ZeroFTSensor", etc. */
+    std::string pt_name = {};
+
+    /** Input parameter names and values of this primitive. Use int 1 and 0 to represent booleans.
+     * E.g. {{"target", rdk::Coord({0.65, -0.3, 0.2}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})},
+     * {"vel", 0.6}, {"zoneRadius", "Z50"}}.
+     * @warning The primitive input parameters may not use SI units, please refer to the Flexiv
+     * Primitives documentation for exact unit definition. */
+    std::map<std::string, FlexivDataTypes> input_params = {};
+
+    /** True: The external axis motion is actively controlled by this primitive. When this primitive
+     * is executed, the external axis will move to the position defined in target points or
+     * waypoints. False: The external axis motion is not actively controlled by this primitive.
+     * @note This setting is only effective when the external axis exists. */
+    bool external_axis_control = true;
+
+    /** Mode for synchronous motions between the TCP of this primitive and the selected arm or
+     * positioner. Once configured, the motion of the TCP will be automatically adjusted in
+     * accordance with the movement of the selected arm or positioner.
+     * @note Only certain primitives support this feature, this setting will be ignored if this
+     * primitive does not support it. */
+    SyncMotionMode sync_motion_mode = SyncMotionMode::DISABLE;
+};
+
+/**
+ * @struct PrimitiveStates
+ * @brief States data of a primitive.
+ * @see Robot::primitive_states().
+ */
+struct PrimitiveStates
+{
+    /** Name of the currently running primitive */
+    std::string pt_name = {};
+
+    /** Names and corresponding values of this primitive's states. Booleans are represented by int 1
+     * and 0. For example:
+     * {{"reachedTarget", 1}, {"timePeriod", 5.6}, {"forceOffset", {0.1, 0.2, -1.3}}}.
+     */
+    std::map<std::string, FlexivDataTypes> names_and_values = {};
+};
+
+/**
+ * @struct RtJointTorqueCmd
+ * @brief Commands data for real-time joint torque control.
+ * @see Robot::StreamJointTorque().
+ */
+struct RtJointTorqueCmd
+{
+    /** Default constructor */
+    RtJointTorqueCmd() = default;
+
+    /** Custom constructor */
+    RtJointTorqueCmd(const std::vector<double>& tau_d, bool enable_gravity_comp = true,
+        bool enable_soft_limits = true, double friction_comp_scale = 100.0)
+    : tau_d(tau_d)
+    , enable_gravity_comp(enable_gravity_comp)
+    , enable_soft_limits(enable_soft_limits)
+    , friction_comp_scale(friction_comp_scale)
+    {
+    }
+
+    /** Target joint torques: \f$ \tau_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$ */
+    std::vector<double> tau_d = {};
+
+    /** Enable/disable robot gravity compensation */
+    bool enable_gravity_comp = true;
+
+    /** Enable/disable soft limits to keep the joints from moving outside allowed position range,
+     * which will trigger a safety fault that requires recovery operation */
+    bool enable_soft_limits = true;
+
+    /** Percentage of joint friction to be compensated. Valid range: [0, 100]. Setting to 100 means
+     * to compensate all joint friction, and 0 means no friction compensation at all.
+     * Under-compensation increases natural damping of the joints, which can be useful in some
+     * cases, e.g. zero-torque floating. */
+    double friction_comp_scale = 100.0;
+};
+
+/**
+ * @struct RtJointPositionCmd
+ * @brief Commands data for real-time joint position control.
+ * @see Robot::StreamJointPosition().
+ */
+struct RtJointPositionCmd
+{
+    /** Default constructor */
+    RtJointPositionCmd() = default;
+
+    /** Custom constructor */
+    RtJointPositionCmd(const std::vector<double>& q_d, const std::vector<double>& dq_d,
+        const std::vector<double>& ddq_d)
+    : q_d(q_d)
+    , dq_d(dq_d)
+    , ddq_d(ddq_d)
+    {
+    }
+
+    /** Target joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$ */
+    std::vector<double> q_d = {};
+
+    /** Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s]
+     * \f$ */
+    std::vector<double> dq_d = {};
+
+    /** Target joint accelerations: \f$ \ddot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$
+     * [rad/s^2] \f$ */
+    std::vector<double> ddq_d = {};
+};
+
+/**
+ * @struct NrtJointPositionCmd
+ * @brief Commands data for non-real-time joint position control.
+ * @see Robot::SendJointPosition().
+ */
+struct NrtJointPositionCmd
+{
+    /** Default constructor */
+    NrtJointPositionCmd() = default;
+
+    /** Custom constructor */
+    NrtJointPositionCmd(const std::vector<double>& q_d, const std::vector<double>& dq_d,
+        const std::vector<double>& dq_max, const std::vector<double>& ddq_max)
+    : q_d(q_d)
+    , dq_d(dq_d)
+    , dq_max(dq_max)
+    , ddq_max(ddq_max)
+    {
+    }
+
+    /** Target joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$ */
+    std::vector<double> q_d = {};
+
+    /** Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Each joint will
+     * maintain this amount of velocity when it reaches the target position. Unit: \f$ [rad/s] \f$
+     */
+    std::vector<double> dq_d = {};
+
+    /** Maximum joint velocities for the planned trajectory: \f$ \dot{q}_{max} \in \mathbb{R}^{n
+     * \times 1} \f$. Unit: \f$ [rad/s] \f$ */
+    std::vector<double> dq_max = {};
+
+    /** Maximum joint accelerations for the planned trajectory: \f$ \ddot{q}_{max} \in \mathbb{R}^{n
+     * \times 1} \f$. Unit: \f$ [rad/s^2] \f$ */
+    std::vector<double> ddq_max = {};
+};
+
+/**
+ * @struct RtCartesianCmd
+ * @brief Commands data for real-time Cartesian motion-force control.
+ * @see Robot::StreamCartesianMotionForce().
+ */
+struct RtCartesianCmd
+{
+    /** Default constructor */
+    RtCartesianCmd() = default;
+
+    /** Custom constructor */
+    RtCartesianCmd(const std::array<double, kPoseSize>& pose_d,
+        const std::array<double, kCartDoF>& wrench_d = {},
+        const std::array<double, kCartDoF>& twist_d = {},
+        const std::array<double, kCartDoF>& acc_d = {})
+    : pose_d(pose_d)
+    , wrench_d(wrench_d)
+    , twist_d(twist_d)
+    , acc_d(acc_d)
+    {
+    }
+
+    /** Target TCP pose in world frame: \f$ {^{O}T_{TCP}}_{d} \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
+    std::array<double, kPoseSize> pose_d = {};
+
+    /** Target TCP wrench in the force control reference frame (configured by
+     * SetForceControlFrame()): \f$ ^{O}F_d \in \mathbb{R}^{6 \times 1} \f$. The robot will track
+     * the target wrench using an explicit force controller. Consists of \f$ \mathbb{R}^{3 \times 1}
+     * \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T
+     * \f$. Unit: \f$ [N]:[Nm] \f$ */
+    std::array<double, kCartDoF> wrench_d = {};
+
+    /** Target TCP twist in world frame: \f$ ^{O}\dot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
+     * Providing properly calculated target twist can improve the robot's overall tracking
+     * performance at the cost of reduced robustness. Leaving this input 0 can maximize robustness
+     * at the cost of reduced tracking performance. Consists of \f$ \mathbb{R}^{3 \times 1} \f$
+     * linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular velocity. Unit: \f$ [m/s]:[rad/s] \f$ */
+    std::array<double, kCartDoF> twist_d = {};
+
+    /** Target TCP acceleration in world frame: \f$ ^{O}\ddot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
+     * Feeding forward target acceleration can improve the robot's tracking performance for highly
+     * dynamic motions, but it's also okay to leave this input 0. Consists of \f$ \mathbb{R}^{3
+     * \times 1} \f$ linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular acceleration. Unit: \f$
+     * [m/s^2]:[rad/s^2] \f$ */
+    std::array<double, kCartDoF> acc_d = {};
+};
+
+/**
+ * @struct NrtCartesianCmd
+ * @brief Commands data for non-real-time Cartesian motion-force control.
+ * @see Robot::SendCartesianMotionForce().
+ */
+struct NrtCartesianCmd
+{
+    /** Default constructor */
+    NrtCartesianCmd() = default;
+
+    /** Custom constructor */
+    NrtCartesianCmd(const std::array<double, kPoseSize>& pose_d,
+        const std::array<double, kCartDoF>& wrench_d = {},
+        const std::array<double, kCartDoF>& twist_d = {}, double max_linear_vel = 0.5,
+        double max_angular_vel = 1.0, double max_linear_acc = 2.0, double max_angular_acc = 5.0)
+    : pose_d(pose_d)
+    , wrench_d(wrench_d)
+    , twist_d(twist_d)
+    , max_linear_vel(max_linear_vel)
+    , max_angular_vel(max_angular_vel)
+    , max_linear_acc(max_linear_acc)
+    , max_angular_acc(max_angular_acc)
+    {
+    }
+
+    /** Target TCP pose in world frame: \f$ {^{O}T_{TCP}}_{d} \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
+    std::array<double, kPoseSize> pose_d = {};
+
+    /** Target TCP wrench in the force control reference frame (configured by
+     * SetForceControlFrame()): \f$ ^{O}F_d \in \mathbb{R}^{6 \times 1} \f$. The robot will track
+     * the target wrench using an explicit force controller. Consists of \f$ \mathbb{R}^{3 \times 1}
+     * \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T
+     * \f$. Unit: \f$ [N]:[Nm] \f$ */
+    std::array<double, kCartDoF> wrench_d = {};
+
+    /** Target TCP twist in world frame: \f$ ^{O}\dot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
+     * Providing properly calculated target twist can improve the robot's overall tracking
+     * performance at the cost of reduced robustness. Leaving this input 0 can maximize robustness
+     * at the cost of reduced tracking performance. Consists of \f$ \mathbb{R}^{3 \times 1} \f$
+     * linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular velocity. Unit: \f$ [m/s]:[rad/s] \f$ */
+    std::array<double, kCartDoF> twist_d = {};
+
+    /** Maximum Cartesian linear velocity when moving to the target pose. A safe value is provided
+     * as default. Unit: \f$ [m/s] \f$ */
+    double max_linear_vel = 0.5;
+
+    /** Maximum Cartesian angular velocity when moving to the target pose. A safe value is provided
+     * as default. Unit: \f$ [rad/s] \f$ */
+    double max_angular_vel = 1.0;
+
+    /** Maximum Cartesian linear acceleration when moving to the target pose. A safe value is
+     * provided as default. Unit: \f$ [m/s^2] \f$ */
+    double max_linear_acc = 2.0;
+
+    /** Maximum Cartesian angular acceleration when moving to the target pose. A safe value is
+     * provided as default. Unit: \f$ [rad/s^2] \f$ */
+    double max_angular_acc = 5.0;
+};
+
+/**
+ * @struct NrtCartesianMultiWaypointCmd
+ * @brief Commands data for non-real-time Cartesian multi-waypoint motion-force control.
+ * @see Robot::SendCartesianMotionForceMultiWaypoint().
+ */
+struct NrtCartesianMultiWaypointCmd
+{
+    /** Default constructor */
+    NrtCartesianMultiWaypointCmd() = default;
+
+    /** Custom constructor */
+    NrtCartesianMultiWaypointCmd(const std::vector<NrtCartesianCmd>& waypoints)
+    : waypoints(waypoints)
+    {
+    }
+
+    /** Sequence of Cartesian waypoints to execute. Each waypoint uses the same data layout as
+     * NrtCartesianCmd. */
+    std::vector<NrtCartesianCmd> waypoints = {};
+};
+
+/**
+ * @brief Operator overloading to out stream all members of RobotEvent in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] robot_event RobotEvent data structure to out stream.
+ * @return Updated ostream instance.
+ * @note The event timestamp is converted to local timezone when printed.
+ */
+std::ostream& operator<<(std::ostream& ostream, const RobotEvent& robot_event);
+
+/**
+ * @brief Operator overloading to out stream all members of RobotInfo in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] robot_info RobotInfo data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const RobotInfo& robot_info);
+
+/**
+ * @brief Operator overloading to out stream all members of RobotStates in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] robot_states RobotStates data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const RobotStates& robot_states);
+
+/**
+ * @brief Operator overloading to out stream all members of RobotActions in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] robot_actions RobotActions data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const RobotActions& robot_actions);
+
+/**
+ * @brief Operator overloading to out stream all members of PlanInfo in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] plan_info PlanInfo data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const PlanInfo& plan_info);
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_DATA_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/device.hpp

@@ -0,0 +1,114 @@
+/**
+ * @file device.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_DEVICE_HPP_
+#define FLEXIV_RDK_DEVICE_HPP_
+
+#include "robot.hpp"
+#include <map>
+
+namespace flexiv::rdk {
+
+using DeviceParamDataTypes
+    = std::variant<int, double, std::string, std::vector<double>, std::vector<std::string>>;
+
+/**
+ * @class Device
+ * @brief Interface to control the peripheral device(s) connected to the robot.
+ */
+class Device
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the device control interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Device(const Robot& robot);
+    virtual ~Device();
+
+    /**
+     * @brief [Non-blocking] A list of all existing devices.
+     * @return Device names as a string list.
+     */
+    std::vector<std::string> list() const;
+
+    /**
+     * @brief [Non-blocking] Whether the specified device already exists.
+     * @param[in] name Name of the device to check.
+     * @return True if the specified device exists.
+     */
+    bool exist(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Whether the specified device is enabled.
+     * @param[in] name Name of the device to check.
+     * @return True if the specified device is enabled.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    bool enabled(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Whether the specified device is connected.
+     * @param[in] name Name of the device to check.
+     * @return True if the specified device is connected.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    bool connected(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Configuration parameters of the specified device.
+     * @param[in] name Name of the device to get parameters for.
+     * @return A map of {param_name, param_value}. Booleans are represented by int 1 and 0. For
+     * example, {{"maxVel", 0.5}, {"absolutePosition", {0.7, -0.4, 0.05}}, {"conveyorName",
+     * "conveyor0"}}.
+     * @throw std::invalid_argument if the specified device does not exist.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::map<std::string, DeviceParamDataTypes> params(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Enable the specified device.
+     * @param[in] name Name of the device to enable.
+     * @throw std::invalid_argument if the specified device does not exist.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Enable(const std::string& name);
+
+    /**
+     * @brief [Blocking] Disable the specified device.
+     * @param[in] name Name of the device to disable.
+     * @throw std::invalid_argument if the specified device does not exist.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Disable(const std::string& name);
+
+    /**
+     * @brief [Blocking] Send command(s) to the specified device.
+     * @param[in] name Name of the device to send command(s) to.
+     * @param[in] commands A map of {command_name, command_value}. For example, {{"setSpeed", 6000},
+     * {"openLaser", true}}. All commands in the map will be sent to the device simultaneously. Make
+     * sure the command name(s) are valid and can be accepted by the specified device.
+     * @throw std::invalid_argument if the specified device does not exist.
+     * @throw std::logic_error if the specified device is not enabled yet.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Command(const std::string& name,
+        const std::map<std::string, std::variant<bool, int, double>>& commands);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_DEVICE_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/file_io.hpp

@@ -0,0 +1,128 @@
+/**
+ * @file file_io.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_FILE_IO_HPP_
+#define FLEXIV_RDK_FILE_IO_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/**
+ * @class FileIO
+ * @brief Interface to exchange files with the robot. Only certain types of file can be transferred.
+ */
+class FileIO
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the file exchange interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    FileIO(const Robot& robot);
+    virtual ~FileIO();
+
+    /**
+     * @brief [Blocking] A list of all trajectory files currently stored in the connected robot.
+     * @return Trajectory filenames as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> traj_files_list() const;
+
+    /**
+     * @brief [Blocking] A list of all user projects currently stored in the connected robot.
+     * @return Project names as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> projects_list() const;
+
+    /**
+     * @brief [Blocking] Upload a local trajectory file (.traj) to the robot.
+     * @param[in] file_dir Relative or absolute path of the directory that contains the file to
+     * upload, e.g. /home/user/Documents/. Do not include the file name here.
+     * @param[in] file_name Full name of the trajectory file to upload, including the extension.
+     * E.g. PolishSpiral.traj. Do not include the directory path here.
+     * @throw std::invalid_argument if failed to find or load the specified file.
+     * @throw std::runtime_error if failed to upload the file.
+     * @note This function blocks until the file is successfully uploaded.
+     */
+    void UploadTrajFile(const std::string& file_dir, const std::string& file_name);
+
+    /**
+     * @brief [Blocking] Download a trajectory file (.traj) from the robot.
+     * @param[in] file_name Full name of the trajectory file to download, including the extension.
+     * E.g. PolishSpiral.traj.
+     * @return String content of the trajectory file.
+     * @throw std::invalid_argument if the specified file does not exist.
+     * @throw std::runtime_error if failed to download the file.
+     * @note This function blocks until the file is successfully downloaded.
+     */
+    std::string DownloadTrajFile(const std::string& file_name);
+
+    /**
+     * @brief [Blocking] Download a trajectory file (.traj) from the robot and save to the specified
+     * directory.
+     * @param[in] file_name Full name of the trajectory file to download, including the extension.
+     * E.g. "PolishSpiral.traj".
+     * @param[in] save_dir Relative or absolute path of the directory to save the downloaded file.
+     * E.g. /home/user/Documents/. Do not include the file name here. The specified directory will
+     * be created if not yet exist.
+     * @throw std::invalid_argument if the specified file does not exist.
+     * @throw std::runtime_error if failed to download or save the file.
+     * @throw std::filesystem::filesystem_error if failed to create the specified directory.
+     * @note This function blocks until the file is successfully downloaded.
+     */
+    void DownloadTrajFile(const std::string& file_name, const std::string& save_dir);
+
+    /**
+     * @brief [Blocking] Upload a local project to the robot.
+     * @param[in] project_dir Relative or absolute path of the project directory, which should
+     * contain a .proj file and a .plan file.
+     * @throw std::invalid_argument if failed to find the specified directory or the project files
+     * are incomplete.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to upload the project.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the project is successfully uploaded.
+     * @warning If the uploaded project was created by Flexiv Elements with a software package
+     * version different from the connected robot, the project might not be executable before it's
+     * converted to the compatible version.
+     */
+    void UploadProject(const std::string& project_dir);
+
+    /**
+     * @brief [Blocking] Download a project from the robot and save to the specified directory.
+     * @param[in] project_name Full name of the project to download. E.g. "polish_table".
+     * @param[in] save_dir Relative or absolute path of the directory to save the downloaded
+     * project. E.g. /home/user/Documents/.
+     * @throw std::invalid_argument if the specified project does not exist.
+     * @throw std::runtime_error if failed to download or save the project.
+     * @throw std::filesystem::filesystem_error if failed to create the specified directory.
+     * @note This function blocks until the project is successfully downloaded.
+     */
+    void DownloadProject(const std::string& project_name, const std::string& save_dir);
+
+    /**
+     * @brief [Blocking] Download all collision mesh files of the connected robot and save to the
+     * specified directory.
+     * @param[in] save_dir Relative or absolute path of the directory to save the downloaded
+     * mesh files. E.g. /home/user/Documents/.
+     * @throw std::runtime_error if failed to download or save the mesh files.
+     * @throw std::filesystem::filesystem_error if failed to create the specified directory.
+     * @note This function blocks until all collision mesh files are successfully downloaded.
+     */
+    void DownloadCollisionMesh(const std::string& save_dir);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_FILE_IO_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/gripper.hpp

@@ -0,0 +1,182 @@
+/**
+ * @file gripper.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_GRIPPER_HPP_
+#define FLEXIV_RDK_GRIPPER_HPP_
+
+#include "robot.hpp"
+#include <map>
+#include <memory>
+
+namespace flexiv::rdk {
+
+/**
+ * @struct GripperParams
+ * @brief Data structure containing the gripper parameters.
+ * @see Gripper::params().
+ */
+struct GripperParams
+{
+    /** Gripper name */
+    std::string name = {};
+
+    /** Minimum finger opening width [m] */
+    double min_width = {};
+
+    /** Maximum finger opening width [m] */
+    double max_width = {};
+
+    /** Minimum finger moving velocity [m/s] */
+    double min_vel = {};
+
+    /** Maximum finger moving velocity [m/s] */
+    double max_vel = {};
+
+    /** Minimum grasping force [N] */
+    double min_force = {};
+
+    /** Maximum grasping force [N] */
+    double max_force = {};
+};
+
+/**
+ * @struct GripperStates
+ * @brief Data structure containing the gripper states.
+ * @see Gripper::states().
+ */
+struct GripperStates
+{
+    /** Measured finger opening width [m] */
+    double width = {};
+
+    /** Measured finger force. Positive: opening force, negative: closing force.
+     * Reads 0 if the enabled gripper has no force sensing capability [N] */
+    double force = {};
+
+    /** Whether the gripper fingers are moving */
+    bool is_moving = {};
+};
+
+/**
+ * @brief Operator overloading to out stream all members of GripperStates in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] gripper_states GripperStates data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const GripperStates& gripper_states);
+
+/**
+ * @class Gripper
+ * @brief Interface to control the gripper installed on the robot. Because gripper is also a type of
+ * robot device, this API uses the same underlying infrastructure as rdk::Device, but with functions
+ * tailored specifically for gripper controls.
+ */
+class Gripper
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the gripper control interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Gripper(const Robot& robot);
+    virtual ~Gripper();
+
+    /**
+     * @brief [Blocking] Enable the specified gripper as a device used by the specified joint group.
+     * @param[in] group Joint group that uses this gripper. Only existing single-arm joint groups
+     * like ARM_1 and ARM_2 are accepted.
+     * @param[in] name Name of the gripper to enable.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if the specified gripper does not exist.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot or failed
+     * to sync gripper parameters.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning There's no enforced check on whether the enabled device is a gripper or not. Using
+     * this function to enable a non-gripper device will likely lead to undefined behaviors.
+     */
+    void Enable(JointGroup group, const std::string& name);
+
+    /**
+     * @brief [Blocking] Disable the gripper currently used by the specified joint group.
+     * @param[in] group Joint group whose gripper to be disabled.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Disable(JointGroup group);
+
+    /**
+     * @brief [Blocking] Manually trigger the initialization of the enabled gripper used by the
+     * specified joint group. This step is not needed for grippers that automatically initialize
+     * upon power-on.
+     * @param[in] group Joint group whose gripper is to be initialized.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning This function does not wait for the initialization sequence to finish, the user may
+     * need to implement wait after calling this function before commanding the gripper.
+     */
+    void Init(JointGroup group);
+
+    /**
+     * @brief [Blocking] Command the gripper used by the specified joint group to grasp with direct
+     * force control. This function requires the enabled gripper to support direct force control.
+     * @param[in] group Joint group whose gripper to send this command to.
+     * @param[in] force Target gripping force. Positive: closing force, negative: opening force.
+     * Valid range: [GripperParams::min_force, GripperParams::max_force]. Unit: \f$ [N] \f$.
+     * @throw std::invalid_argument if [force] is outside the valid range.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Grasp(JointGroup group, double force);
+
+    /**
+     * @brief [Blocking] Command the gripper used by the specified joint group to move the fingers
+     * with position control.
+     * @param[in] group Joint group whose gripper to send this command to.
+     * @param[in] width Target opening width. Valid range: [GripperParams::min_width,
+     * GripperParams::max_width]. Unit: \f$ [m] \f$.
+     * @param[in] velocity Closing/opening velocity, cannot be 0. Valid range:
+     * [GripperParams::min_vel, GripperParams::max_vel]. Unit: \f$ [m/s] \f$.
+     * @param[in] force_limit Maximum contact force during movement. Valid range:
+     * [GripperParams::min_force, GripperParams::max_force]. Unit: \f$ [N] \f$.
+     * @throw std::invalid_argument if any input parameter is outside its valid range.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Move(JointGroup group, double width, double velocity, double force_limit);
+
+    /**
+     * @brief [Blocking] Stop and hold the gripper used by the specified joint group.
+     * @param[in] group Joint group whose gripper to send this command to.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Stop(JointGroup group);
+
+    /**
+     * @brief [Non-blocking] Parameters of all enabled grippers.
+     * @return Gripper parameters mapped by joint group.
+     */
+    std::map<JointGroup, GripperParams> params() const;
+
+    /**
+     * @brief [Non-blocking] Current states data of all enabled grippers.
+     * @return Gripper states data mapped by joint group.
+     */
+    std::map<JointGroup, GripperStates> states() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_GRIPPER_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/maintenance.hpp

@@ -0,0 +1,64 @@
+/**
+ * @file maintenance.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_MAINTENANCE_HPP_
+#define FLEXIV_RDK_MAINTENANCE_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/**
+ * @class Maintenance
+ * @brief Interface to run maintenance operations on the robot.
+ */
+class Maintenance
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the robot maintenance interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Maintenance(const Robot& robot);
+    virtual ~Maintenance();
+
+    /**
+     * @brief [Blocking] Calibrate all joint torque sensors. The robot will first move to a proper
+     * calibration posture, then start the low-level calibration of all joint torque sensors.
+     * Trigger this calibration if the sensed joint torques have noticeable deviations from true
+     * values. See below for more details.
+     * @param[in] group The joint group to run calibration for.
+     * @param[in] cali_posture Joint positions to move to before starting the calibration: \f$
+     * q_cali \in \mathbb{R}^{n \times 1} \f$. If left empty, the robot will use the recommended
+     * upright posture for calibration. Otherwise the specified posture will be used, which is NOT
+     * recommended. Valid range: [RobotInfo::q_min, RobotInfo::q_max]. Unit: \f$ [rad] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, if [cali_posture] contains any value outside the valid range, or if its size
+     * does not match robot DoF.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if fault occurred during the calibration or failed to save the
+     * calibration result.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the calibration is finished.
+     * @warning The robot needs to be rebooted for the calibration result to take effect.
+     * @par How to determine when this calibration is needed?
+     * 1. When the robot is static and there's no payload or external force exerted on it, if
+     * RobotStates::tcp_wrench_local still gives greater than 5N reading, then this calibration
+     * should be triggered once.
+     * 2. When running the "intermediate4_realtime_joint_floating.cpp" example, if the joints drift
+     * swiftly toward one direction, then this calibration should be triggered once.
+     */
+    void CalibrateJointTorqueSensors(
+        JointGroup group, const std::vector<double>& cali_posture = {});
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_MAINTENANCE_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/mode.hpp

@@ -0,0 +1,126 @@
+/**
+ * @file mode.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_MODE_HPP_
+#define FLEXIV_RDK_MODE_HPP_
+
+#include <array>
+#include <map>
+#include <string>
+
+namespace flexiv::rdk {
+
+/**
+ * @enum Mode
+ * @brief Robot control modes. The robot needs to be switched into the correct control mode before
+ * the corresponding commands can be sent. Refer to Flexiv RDK Manual for more details.
+ * @see Robot::SwitchMode().
+ */
+enum class Mode
+{
+    /** Mode not set */
+    UNKNOWN = 0,
+
+    /**
+     * No operation to execute, the robot holds position and waits for new command.
+     */
+    IDLE,
+
+    /**
+     * Run real-time joint torque control to track continuous commands @1kHz.
+     * @note Real-time (RT) mode.
+     * @see Robot::StreamJointTorque().
+     */
+    RT_JOINT_TORQUE,
+
+    /**
+     * Run real-time joint impedance control to track continuous commands @ 1kHz.
+     * @note Real-time (RT) mode.
+     * @see Robot::StreamJointPosition().
+     */
+    RT_JOINT_IMPEDANCE,
+
+    /**
+     * Run non-real-time joint impedance control to track discrete commands (smoothened by internal
+     * motion generator).
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::SendJointPosition().
+     */
+    NRT_JOINT_IMPEDANCE,
+
+    /**
+     * Run real-time joint position control to track continuous commands @ 1kHz.
+     * @note Real-time (RT) mode.
+     * @see Robot::StreamJointPosition().
+     */
+    RT_JOINT_POSITION,
+
+    /**
+     * Run non-real-time joint position control to track discrete commands (smoothened by internal
+     * motion generator).
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::SendJointPosition().
+     */
+    NRT_JOINT_POSITION,
+
+    /**
+     * Execute pre-configured robot task plans.
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::ExecutePlan().
+     */
+    NRT_PLAN_EXECUTION,
+
+    /**
+     * Execute robot primitives (unit skills).
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::ExecutePrimitive().
+     * @see [Flexiv Primitives](https://www.flexiv.com/primitives/) documentation.
+     */
+    NRT_PRIMITIVE_EXECUTION,
+
+    /**
+     * Run real-time Cartesian motion-force control to track continuous commands @ 1kHz.
+     * @note Real-time (RT) mode.
+     * @see Robot::StreamCartesianMotionForce().
+     */
+    RT_CARTESIAN_MOTION_FORCE,
+
+    /**
+     * Run non-real-time Cartesian motion-force control to track discrete commands, smoothened by
+     * internal motion generator.
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::SendCartesianMotionForce().
+     */
+    NRT_CARTESIAN_MOTION_FORCE,
+
+    /**
+     * Run non-real-time super primitives, i.e. composite tasks that consist of various components
+     * like motion, force, vision, planning, etc.
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::SendCartesianMotionForce().
+     * @see Robot::SendCartesianMotionForceMultiWaypoint().
+     */
+    NRT_SUPER_PRIMITIVE,
+};
+
+/** Map Mode enums to strings */
+inline const std::map<Mode, std::string> kModeNames {
+    {Mode::UNKNOWN, "UNKNOWN"},
+    {Mode::IDLE, "IDLE"},
+    {Mode::RT_JOINT_TORQUE, "RT_JOINT_TORQUE"},
+    {Mode::RT_JOINT_IMPEDANCE, "RT_JOINT_IMPEDANCE"},
+    {Mode::NRT_JOINT_IMPEDANCE, "NRT_JOINT_IMPEDANCE"},
+    {Mode::RT_JOINT_POSITION, "RT_JOINT_POSITION"},
+    {Mode::NRT_JOINT_POSITION, "NRT_JOINT_POSITION"},
+    {Mode::NRT_PLAN_EXECUTION, "NRT_PLAN_EXECUTION"},
+    {Mode::NRT_PRIMITIVE_EXECUTION, "NRT_PRIMITIVE_EXECUTION"},
+    {Mode::RT_CARTESIAN_MOTION_FORCE, "RT_CARTESIAN_MOTION_FORCE"},
+    {Mode::NRT_CARTESIAN_MOTION_FORCE, "NRT_CARTESIAN_MOTION_FORCE"},
+    {Mode::NRT_SUPER_PRIMITIVE, "NRT_SUPER_PRIMITIVE"},
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_MODE_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/model.hpp

@@ -0,0 +1,254 @@
+/**
+ * @file model.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_MODEL_HPP_
+#define FLEXIV_RDK_MODEL_HPP_
+
+#include "robot.hpp"
+#include <Eigen/Eigen>
+#include <memory>
+
+namespace flexiv::rdk {
+
+/**
+ * @struct IKParams
+ * @brief Per-joint-group IK input parameters for SolveConstrainedIK().
+ */
+struct IKParams
+{
+    /** Cartesian pose to be solved. */
+    std::array<double, kPoseSize> cartesian_pose = {};
+
+    /** Seed joint positions for IK solver. Unit: [rad]. */
+    std::vector<double> seed_q = {};
+
+    /** Whether to only constrain Cartesian position and allow orientation to vary. */
+    bool free_orientation = false;
+};
+
+/**
+ * @struct IKResult
+ * @brief Result from SolveConstrainedIK().
+ */
+struct IKResult
+{
+    /** Whether the IK solution is successful. */
+    bool success = false;
+
+    /** Solved joint positions by active joint group. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> solved_q = {};
+};
+
+/**
+ * @class Model
+ * @brief Interface to obtain certain model data of the robot, including kinematics and dynamics.
+ */
+class Model
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the robot model interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @param[in] gravity_vector Earth's gravity vector in world frame. Default to \f$ [0.0, 0.0,
+     * -9.81]^T \f$. Unit: \f$ [m/s^2] \f$.
+     * @throw std::runtime_error if the initialization sequence failed.
+     * @throw std::logic_error if the connected robot does not have an RDK professional license; or
+     * the parsed robot model is not supported.
+     */
+    Model(const Robot& robot,
+        const Eigen::Vector3d& gravity_vector = Eigen::Vector3d(0.0, 0.0, -9.81));
+    virtual ~Model();
+
+    /**
+     * @brief [Non-blocking] Names of all links in the robot model.
+     * @return Name vector in the same order as the robot's kinematic chain.
+     */
+    std::vector<std::string> link_names() const;
+
+    /**
+     * @brief [Non-blocking] Names of all actuated joints in the robot model.
+     * @return Name vector in the same order as the robot's kinematic chain.
+     */
+    std::vector<std::string> joint_names() const;
+
+    /**
+     * @brief [Blocking] Reload (refresh) parameters of the robot model stored locally in this class
+     * using the latest data synced from the connected robot. Tool model is also synced.
+     * @throw std::runtime_error if failed to sync model data.
+     * @throw std::logic_error if the synced robot model contains invalid data.
+     * @note This function blocks until the model parameters are synced and reloaded.
+     * @note This function does not affect the kinematics functions.
+     * @warning Parameters of the locally-stored robot model must be manually refreshed using this
+     * function whenever a physical change is made to the connected robot (e.g. a tool is added or
+     * changed). Otherwise the locally computed functions will return incorrect results.
+     */
+    void Reload();
+
+    /**
+     * @brief [Non-blocking] Update the configuration (posture) of the locally-stored robot model so
+     * that the locally computed functions return results based on the updated configuration.
+     * @param[in] full_q Current joint positions of the whole robot and matching the order of
+     * joint_names(): \f$ q \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$.
+     * @param[in] full_dq Current joint velocities of the whole robot and matching the order of
+     * joint_names(): \f$ \dot{q} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$.
+     * @throw std::invalid_argument if size of any input vector does not match robot full DoF.
+     */
+    void Update(const std::vector<double>& full_q, const std::vector<double>& full_dq);
+
+    //========================================== DYNAMICS ==========================================
+    /**
+     * @brief [Non-blocking] Compute the time derivative of Jacobian matrix at the specified frame
+     * w.r.t. world frame.
+     * @param[in] link_name Name of the link whose frame is the specified one.
+     * @return Time derivative of the Jacobian matrix: \f$ ^{O}\dot{J_i} \in \mathbb{R}^{m \times n}
+     * \f$.
+     * @throw std::invalid_argument if [link_name] does not exist.
+     * @note Call Update() before this function.
+     * @see link_names().
+     */
+    Eigen::MatrixXd dJ(const std::string& link_name);
+
+    /**
+     * @brief [Non-blocking] Compute the mass matrix in generalized coordinates, i.e. joint space.
+     * @return Symmetric positive definite mass matrix: \f$ M(q) \in \mathbb{S}^{n \times n}_{++}
+     * \f$. Unit: \f$ [kgm^2] \f$.
+     * @note Call Update() before this function.
+     */
+    Eigen::MatrixXd M();
+
+    /**
+     * @brief [Non-blocking] Compute the Coriolis/centripetal matrix in generalized coordinates,
+     * i.e. joint space.
+     * @return Coriolis/centripetal matrix: \f$ C(q,\dot{q}) \in \mathbb{R}^{n \times n} \f$.
+     * @note Call Update() before this function.
+     */
+    Eigen::MatrixXd C();
+
+    /**
+     * @brief [Non-blocking] Compute the gravity force vector in generalized coordinates, i.e.
+     * joint space.
+     * @return Gravity force vector: \f$ g(q) \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
+     * @note Call Update() before this function.
+     */
+    Eigen::VectorXd g();
+
+    /**
+     * @brief [Non-blocking] Compute the Coriolis force vector in generalized coordinates, i.e.
+     * joint space.
+     * @return Coriolis force vector: \f$ c(q,\dot{q}) \in \mathbb{R}^{n \times 1} \f$. Unit: \f$
+     * [Nm] \f$.
+     * @note Call Update() before this function.
+     */
+    Eigen::VectorXd c();
+
+    //========================================= KINEMATICS =========================================
+    /**
+     * @brief [Non-blocking] Compute the Jacobian matrix at the specified frame w.r.t. world frame.
+     * @param[in] link_name Name of the link whose frame is the specified one.
+     * @return Jacobian matrix: \f$ ^{O}J_i \in \mathbb{R}^{m \times n} \f$.
+     * @throw std::invalid_argument if [link_name] does not exist.
+     * @note Call Update() before this function.
+     * @see link_names().
+     */
+    Eigen::MatrixXd J(const std::string& link_name);
+
+    /**
+     * @brief [Non-blocking] Compute the transformation matrix of the specified frame w.r.t. world
+     * frame.
+     * @param[in] link_name Name of the link whose frame is the specified one.
+     * @return Transformation matrix: \f$ ^{O}T_i \in \mathbb{R}^{4 \times 4} \f$.
+     * @throw std::invalid_argument if [link_name] does not exist.
+     * @note Call Update() before this function.
+     * @see link_names().
+     */
+    Eigen::Isometry3d T(const std::string& link_name);
+
+    /**
+     * @brief [Blocking] Sync the actual kinematic parameters of the connected robot into the
+     * template URDF file.
+     * @param[in] template_urdf_path Path to the template URDF file that can be generated in
+     * [flexiv_description](https://github.com/flexivrobotics/flexiv_description). This template
+     * URDF file will be updated when the sync is finished.
+     * @throw std::invalid_argument if failed to load the template URDF file.
+     * @throw std::runtime_error if failed to sync the URDF file.
+     * @note This function blocks until the sync is finished.
+     * @par Why is this function needed?
+     * The template URDF contains kinematic parameters of the latest version of robot hardware,
+     * which might be different from older versions. This function is therefore provided to sync the
+     * actual kinematic parameters of the connected robot into the template URDF.
+     */
+    void SyncURDF(const std::string& template_urdf_path);
+
+    /**
+     * @brief [Blocking] Sync the actual kinematic parameters of the connected robot into the
+     * template YAML file.
+     * @param[in] template_yaml_path Path to the template YAML file located at
+     * [flexiv_description/config/.../default_kinematics.yaml]. This template YAML file will be
+     * updated when the sync is finished.
+     * @return Total number of joints that have been successfully synced.
+     * @throw std::invalid_argument if failed to load the template YAML file.
+     * @throw std::runtime_error if failed to sync the YAML file.
+     * @note This function blocks until the sync is finished.
+     */
+    size_t SyncKinematicsYAML(const std::string& template_yaml_path);
+
+    /**
+     * @brief [Blocking] Solve constrained IK using one or more active joint groups.
+     * @param[in] ik_params_by_group IK input parameters mapped by active joint group. Joint groups
+     * not included in this map are treated as inactive. Only single-arm joint groups like ARM_1 and
+     * ARM_2 are accepted.
+     * @return Solver result. [solved_q] contains one entry per requested active joint group.
+     * @throw std::invalid_argument if input map is empty, if any joint group is not an existing
+     * single-arm joint group, or if any [seed_q] size does not match DoF of its joint group.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    IKResult SolveConstrainedIK(const std::map<JointGroup, IKParams>& ik_params_by_group);
+
+    /**
+     * @brief [Blocking] Score of each joint group's current configuration (posture), calculated
+     * from the manipulability measurements.
+     * @return Configuration score mapped by joint group as {translation_score,
+     * orientation_score}. The quality of configuration based on the score can be interpreted as:
+     * poor = [0, 20), medium = [20, 40), good = [40, 100].
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     * @warning A poor configuration score means the robot is near or at singularity, which can lead
+     * to degraded Cartesian performance. Use configuration with high scores for better
+     * manipulability and task results.
+     */
+    std::map<JointGroup, std::pair<double, double>> configuration_score() const;
+
+    //======================================= MULTI-CONTACT ========================================
+    /**
+     * @brief [Non-blocking] Estimated multi-contact forces applied on each link of applicable joint
+     * groups, calculated using the force-torque sensors installed in every joint of the robot.
+     * @return \f$ f_c \in \mathbb{R}^{n \times 1} \f$ mapped by joint group. Each vector element is
+     * a \f$ \mathbb{R}^{3 \times 1} \f$ force vector w.r.t. the corresponding link frame. Only
+     * contains joint groups that are capable of multi-contact estimation.
+     * @warning This data is only available on certain robot models. An empty vector will be
+     * returned if the connected robot does not support multi-contact estimation.
+     */
+    std::map<JointGroup, std::vector<Eigen::Vector3d>> multi_contact_forces() const;
+
+    /**
+     * @brief [Non-blocking] Estimated multi-contact positions on each link of applicable joint
+     * groups, calculated using the force-torque sensors installed in every joint of the robot.
+     * @return \f$ p_c \in \mathbb{R}^{n \times 1} \f$ mapped by joint group. Each vector element is
+     * a \f$ \mathbb{R}^{3 \times 1} \f$ position vector w.r.t. the corresponding link frame. Only
+     * contains joint groups that are capable of multi-contact estimation.
+     * @warning This data is only available on certain robot models. An empty vector will be
+     * returned if the connected robot does not support multi-contact estimation.
+     */
+    std::map<JointGroup, std::vector<Eigen::Vector3d>> multi_contact_positions() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_MODEL_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/robot.hpp

@@ -0,0 +1,896 @@
+/**
+ * @file robot.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_ROBOT_HPP_
+#define FLEXIV_RDK_ROBOT_HPP_
+
+#include "data.hpp"
+#include "mode.hpp"
+#include <vector>
+#include <memory>
+#include <exception>
+#include <map>
+
+namespace flexiv::rdk {
+
+/**
+ * @class Robot
+ * @brief Main interface to control the robot, containing several function categories and background
+ * services.
+ */
+class Robot
+{
+public:
+    /**
+     * @brief [Blocking] Instantiate the robot control interface. RDK services will be started and
+     * establish connection with the target robot.
+     * @param[in] robot_sn Serial number of the robot to connect. The accepted formats are:
+     * "Enlight-L-123456" and "Enlight L-123456". See ProductModel enum for all supported models.
+     * @param[in] verbose Enable/disable info and warning prints.
+     * @param[in] lite Whether to create a lite instance. See details below. A normal instance must
+     * have already connected to the robot for lite instances to function properly.
+     * @throw std::invalid_argument if the format of [robot_sn] is invalid.
+     * @throw std::runtime_error if the initialization sequence failed.
+     * @throw std::logic_error if the connected robot does not have a valid RDK license, or is
+     * incompatible with this RDK library version, or is an unsupported product model.
+     * @warning This constructor blocks until the initialization sequence is successfully finished
+     * and connection with the robot is established.
+     * @par Lite Instance
+     * Creating multiple instances of rdk::Robot that connect to the same robot is allowed. However,
+     * every additional instance brings extra overhead that can restrict the total number of
+     * instances due to limited computation power and network bandwidth. To mitigate the performance
+     * impact, it is recommended to create a lite instance of rdk::Robot if it only needs to send
+     * one-shot requests, which are functions marked with the [Blocking] tag. A lite instance does
+     * not receive robot states or system status, nor send motion commands.
+     */
+    Robot(const std::string& robot_sn, bool verbose = true, bool lite = false);
+    virtual ~Robot();
+
+    //========================================= ACCESSORS ==========================================
+    /**
+     * @brief [Non-blocking] Whether this rdk::Robot instance is a lite one.
+     * @return True: is a lite instance; false: not a lite instance.
+     */
+    bool lite() const;
+
+    /**
+     * @brief [Non-blocking] Whether the connection with the robot is established.
+     * @return True: connected; false: disconnected.
+     */
+    bool connected() const;
+
+    /**
+     * @brief [Non-blocking] General information about the robot.
+     * @return Value copy of RobotInfo struct.
+     */
+    RobotInfo info() const;
+
+    /**
+     * @brief [Non-blocking] Current control mode of the robot.
+     * @return Mode enum.
+     */
+    Mode mode() const;
+
+    /**
+     * @brief [Non-blocking] Current states data of all existing joint groups of the robot.
+     * @return Robot states data mapped by joint group.
+     * @warning Cartesian states of non-single-arm joint groups are not populated.
+     */
+    std::map<JointGroup, RobotStates> states() const;
+
+    /**
+     * @brief [Non-blocking] Current actions data of all existing joint groups of the robot.
+     * @return Robot actions data mapped by joint group.
+     * @warning Cartesian actions of non-single-arm joint groups are not populated.
+     */
+    std::map<JointGroup, RobotActions> actions() const;
+
+    /**
+     * @brief [Non-blocking] Whether specific joint groups of the robot have come to a complete
+     * stop.
+     * @return Stopped flag mapped by joint group. True: this joint group has stopped; false: this
+     * joint group is still moving.
+     */
+    std::map<JointGroup, bool> stopped() const;
+
+    /**
+     * @brief [Non-blocking] Whether all joint groups of the robot have come to a complete stop.
+     * @return True: all stopped; false: not all stopped.
+     */
+    bool all_stopped() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is ready to be operated, which requires the
+     * following conditions to be met: enabled, brakes fully released, in auto mode, no fault, and
+     * not in reduced state.
+     * @return True: operational (operational_status() == READY); false: not operational.
+     * @warning The robot won't execute any user command until it's ready to be operated.
+     */
+    bool operational() const;
+
+    /**
+     * @brief [Non-blocking] Current operational status of the robot.
+     * @return OperationalStatus enum.
+     */
+    OperationalStatus operational_status() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is busy. This includes any user commanded operations
+     * that requires the robot to execute. For example, plans, primitives, Cartesian and joint
+     * motions, etc.
+     * @return True: busy; false: idle.
+     * @warning Some exceptions exist for primitives, see ExecutePrimitive() for more details.
+     */
+    bool busy() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is in fault state.
+     * @return True: robot has fault; false: robot normal.
+     */
+    bool fault() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is in reduced state.
+     * @return True: in reduced state; false: not in reduced state.
+     * @par Reduced state
+     * The robot will enter reduced state if a) the safety input for reduced state goes low or b)
+     * robot TCP passes through any safety plane. The safety limits are lowered in reduced state
+     * compared to normal state. Specific values for the safety limits can be configured in Flexiv
+     * Elements under Settings -> Safety Configuration. Please refer to the robot user manual for
+     * more details about system reduced state.
+     */
+    bool reduced() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is in recovery state.
+     * @return True: in recovery state; false: not in recovery state.
+     * @note Please use Flexiv Elements to get the robot out of recovery state.
+     * @par Recovery state
+     * The robot will enter recovery state if it needs to recover from joint position limit
+     * violation (a critical system fault that requires a recovery operation, during which the
+     * joints that moved outside the allowed position range will need to move very slowly back into
+     * the allowed range). Please refer to the robot user manual for more details about system
+     * recovery state.
+     */
+    bool recovery() const;
+
+    /**
+     * @brief [Non-blocking] Whether all connected emergency stops (E-stops) are released.
+     * @return True: released; false: pressed.
+     */
+    bool estop_released() const;
+
+    /**
+     * @brief [Non-blocking] Whether the 3-position enabling device (e.g. enabling button on the
+     * motion bar) is in the ON position (the middle position).
+     * @return True: ON; false: OFF.
+     */
+    bool enabling_device_on() const;
+
+    /**
+     * @brief [Non-blocking] Whether the timeliness failure limit has been reached within a
+     * 1-second moving window.
+     * @return True: reached limit; false: not reached limit.
+     * @see SetTimelinessFailureLimit().
+     */
+    bool reached_timeliness_failure_limit() const;
+
+    /**
+     * @brief [Non-blocking] Robot events stored since the last successful instantiation of this
+     * class.
+     * @return Event log as a vector of RobotEvent, with the first element being the
+     * oldest event and the last element being the latest event.
+     * @warning Events before the last successful instantiation of this class are not stored.
+     */
+    std::vector<RobotEvent> event_log() const;
+
+    //======================================= SYSTEM CONTROL =======================================
+    /**
+     * @brief [Blocking] Servo on the robot. If E-stop is released and there's no fault, the robot
+     * will release brakes, and becomes operational a few seconds later.
+     * @throw std::logic_error if the robot is not connected or E-stop is not released.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void ServoOn();
+
+    /**
+     * @brief [Blocking] Move the specified joint groups to the home posture using Home primitive.
+     * @param[in] groups Joint group(s) to home. Only existing single-arm joint groups like ARM_1
+     * and ARM_2 are accepted. If left empty, home all applicable joint groups.
+     * @throw std::invalid_argument if [groups] contains joint groups that are not existing
+     * single-arm joint groups in the connected robot.
+     * @throw std::runtime_error if failed to execute the Home primitive.
+     * @note The robot's control mode will be temporarily switched to NRT_PRIMITIVE_EXECUTION and
+     * restored to the previous mode before returning.
+     * @note This function blocks until all specified joint groups have reached the home posture.
+     */
+    void Home(const std::vector<JointGroup>& groups = {});
+
+    /**
+     * @brief [Blocking] Force robot brakes to engage or release during normal operation.
+     * Restrictions apply, see warning.
+     * @param[in] engage True: engage brakes; false: release brakes.
+     * @throw std::logic_error if the connected robot is not a medical model or is moving.
+     * @throw std::runtime_error if failed to engage/release the brakes.
+     * @note This function blocks until the brakes are successfully engaged/released.
+     * @warning This function is accessible only if a) the connected robot is a medical model AND
+     * b) the robot is not moving.
+     */
+    void Brake(bool engage);
+
+    /**
+     * @brief [Blocking] Switch the robot to a new control mode and wait for the mode transition
+     * to finish.
+     * @param[in] mode Control mode to switch to.
+     * @throw std::invalid_argument if the requested mode is invalid or unlicensed.
+     * @throw std::logic_error if robot is in an unknown control mode or is not operational.
+     * @throw std::runtime_error if failed to transit the robot into the specified control mode
+     * after several attempts.
+     * @note This function blocks until the robot has successfully transited into the specified
+     * control mode.
+     * @warning If the robot is still moving when this function is called, it will automatically
+     * stop before making the mode transition.
+     */
+    void SwitchMode(Mode mode);
+
+    /**
+     * @brief [Blocking] Stop the robot and transit its control mode to IDLE.
+     * @throw std::runtime_error if failed to stop the robot.
+     * @note This function blocks until the robot comes to a complete stop.
+     */
+    void Stop();
+
+    /**
+     * @brief [Blocking] Try to clear minor or critical fault for the robot without a power cycle.
+     * @param[in] timeout_sec Maximum time in seconds to wait for the fault to be successfully
+     * cleared. Normally, a minor fault should take no more than 3 seconds to clear, and a critical
+     * fault should take no more than 30 seconds to clear.
+     * @return True: successfully cleared fault; false: failed to clear fault.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the fault is successfully cleared or [timeout_sec] has
+     * elapsed.
+     * @warning Clearing a critical fault through this function without a power cycle requires a
+     * dedicated device, which may not be installed in older robot models.
+     */
+    bool ClearFault(unsigned int timeout_sec = 30);
+
+    /**
+     * @brief [Blocking] Set values to global variables that already exist in the robot.
+     * @param[in] global_vars A map of {global_var_name, global_var_value(s)}. Use int 1 and 0 to
+     * represent booleans. For example, {{"camera_offset", {0.1, -0.2, 0.3}}, {"start_plan", 1}}.
+     * @throw std::length_error if [global_vars] is empty.
+     * @throw std::invalid_argument if any of the specified global variables does not exist.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the global variables are successfully set.
+     * @warning The specified global variables need to be created first using Flexiv Elements.
+     * @see global_variables().
+     */
+    void SetGlobalVariables(const std::map<std::string, FlexivDataTypes>& global_vars);
+
+    /**
+     * @brief [Blocking] Existing global variables and their current values.
+     * @return A map of {global_var_name, global_var_value(s)}. Booleans are represented by int 1
+     * and 0. For example, {{"camera_offset", {0.1, -0.2, 0.3}}, {"start_plan", 1}}.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     * @see SetGlobalVariables().
+     */
+    std::map<std::string, FlexivDataTypes> global_variables() const;
+
+    /**
+     * @brief [Non-blocking] Set the maximum tolerable percentage of real-time commands that arrived
+     * the robot too late within a 1-second moving window. This only applies to the real-time
+     * control modes and functions marked as "Real-time (RT)" will throw an exception when this
+     * limit is reached.
+     * @param[in] percentage Percentage limit, default to 2%. Valid range: [0, 100].
+     * @throw std::invalid_argument if [percentage] is outside the valid range.
+     * @see reached_timeliness_failure_limit().
+     */
+    void SetTimelinessFailureLimit(double percentage = 2.0);
+
+    //======================================= PLAN EXECUTION =======================================
+    /**
+     * @brief [Blocking] Execute a plan by specifying its index.
+     * @param[in] index Index of the plan to execute, can be obtained via plan_list().
+     * @param[in] continue_exec Whether to continue executing the plan when the RDK program is
+     * closed or the connection is lost.
+     * @param[in] block_until_started Whether to wait for the commanded plan to finish loading
+     * and start execution before the function returns. Depending on the amount of computation
+     * needed to get the plan ready, the loading process typically takes no more than 200 ms.
+     * @throw std::invalid_argument if [index] is outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot or the
+     * robot is not operational.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered if
+     * [block_until_started] is disabled, or until the plan has started execution if
+     * [block_until_started] is enabled.
+     * @note busy() can be used to check if the plan has finished.
+     */
+    void ExecutePlan(
+        unsigned int index, bool continue_exec = false, bool block_until_started = true);
+
+    /**
+     * @brief [Blocking] Execute a plan by specifying its name.
+     * @param[in] name Name of the plan to execute, can be obtained via plan_list().
+     * @param[in] continue_exec Whether to continue executing the plan when the RDK program is
+     * closed or the connection is lost.
+     * @param[in] block_until_started Whether to wait for the commanded plan to finish loading
+     * and start execution before the function returns. Depending on the amount of computation
+     * needed to get the plan ready, the loading process typically takes no more than 200 ms.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot or the
+     * robot is not operational.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered if
+     * [block_until_started] is disabled, or until the plan has started execution if
+     * [block_until_started] is enabled.
+     * @note busy() can be used to check if the plan has finished.
+     */
+    void ExecutePlan(
+        const std::string& name, bool continue_exec = false, bool block_until_started = true);
+
+    /**
+     * @brief [Blocking] Pause or resume the execution of the current plan.
+     * @param[in] toggle True: pause plan; false: resume plan.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning Internal plans (not created by user) cannot be resumed due to safety concerns.
+     */
+    void PausePlan(bool toggle);
+
+    /**
+     * @brief [Blocking] Stop the execution of the current plan.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void StopPlan();
+
+    /**
+     * @brief [Blocking] A list of all available plans.
+     * @return Available plans as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> plan_list() const;
+
+    /**
+     * @brief [Blocking] Detailed information about the executing plan. Contains plan name,
+     * primitive name, node name, node path, node path time period, etc.
+     * @return PlanInfo value copy.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until a reply is received.
+     */
+    PlanInfo plan_info() const;
+
+    /**
+     * @brief [Blocking] Enable or disable the breakpoint mode during plan execution. When enabled,
+     * the executing plan will pause at the pre-defined breakpoints. Use StepBreakpoint() to
+     * continue the execution and pause at the next breakpoint.
+     * @param[in] is_enabled True: enable; false: disable. By default, breakpoint mode is disabled.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void SetBreakpointMode(bool is_enabled);
+
+    /**
+     * @brief [Blocking] If breakpoint mode is enabled, step to the next breakpoint. The plan
+     * execution will continue and pause at the next breakpoint.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     * @note Use PlanInfo::waiting_for_step to check if the plan is waiting for user signal to step
+     * the breakpoint.
+     */
+    void StepBreakpoint();
+
+    /**
+     * @brief [Blocking] Set overall velocity scale of robot motions during plan and primitive
+     * execution.
+     * @param[in] velocity_scale Percentage scale to adjust the overall velocity of robot motions.
+     * Valid range: [0, 100]. Setting to 100 means to move with 100% of specified motion velocity,
+     * and 0 means not moving at all.
+     * @throw std::invalid_argument if [velocity_scale] is outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION, NRT_PRIMITIVE_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void SetVelocityScale(unsigned int velocity_scale);
+
+    //==================================== PRIMITIVE EXECUTION =====================================
+    /**
+     * @brief [Blocking] Execute primitive(s) on specified joint group(s).
+     * @param[in] primitive_args Primitive arguments mapped by joint group. Only existing
+     * single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] block_until_started Whether to wait for the commanded primitive to finish loading
+     * and start execution before the function returns. Depending on the amount of computation
+     * needed to get the primitive ready, the loading process typically takes no more than 200 ms.
+     * @throw std::invalid_argument if [primitive_args] contains joint groups that are not
+     * existing single-arm joint groups in the connected robot.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot or the
+     * robot is not operational.
+     * @note Applicable control modes: NRT_PRIMITIVE_EXECUTION.
+     * @note This function blocks until the request is successfully delivered if
+     * [block_until_started] is disabled, or until the primitive has started execution if
+     * [block_until_started] is enabled. After this function returns, the primitive will still be
+     * running, thus the users need to implement their own blocking after calling this function. See
+     * examples for reference.
+     * @warning The primitive input parameters may not use SI units, please refer to the Flexiv
+     * Primitives documentation for exact unit definition.
+     * @warning Most primitives won't exit by themselves and require users to explicitly trigger
+     * transitions based on specific primitive states. In such case, busy() will stay true even if
+     * it seems everything is done for that primitive.
+     */
+    void ExecutePrimitive(
+        const std::map<JointGroup, PrimitiveArgs>& primitive_args, bool block_until_started = true);
+
+    /**
+     * @brief [Blocking] States data of the primitive(s) that are currently running.
+     * @return Primitive states data mapped by joint group. Only joint groups that are currently
+     * running primitives will be included.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::map<JointGroup, PrimitiveStates> primitive_states() const;
+
+    //==================================== DIRECT JOINT CONTROL ====================================
+    /**
+     * @brief [Non-blocking] Continuously stream joint torque commands of the specified joint
+     * group(s) to the robot. The commands are tracked by a high-performance joint torque
+     * controller.
+     * @param[in] cmds Real-time joint torque commands mapped by joint group. Only existing
+     * single-arm joint groups and external axis joint groups are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm or external axis joint group in the connected robot, if size of any input vector
+     * does not match robot DoF, or [friction_comp_scale] is outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: RT_JOINT_TORQUE.
+     * @note Real-time (RT).
+     * @warning Always stream smooth and continuous commands to avoid sudden movements.
+     */
+    void StreamJointTorque(const std::map<JointGroup, RtJointTorqueCmd>& cmds);
+
+    /**
+     * @brief [Non-blocking] Continuously stream joint position, velocity, and acceleration commands
+     * of the specified joint group(s) to the robot. The commands are tracked by either the joint
+     * impedance controller or the joint position controller, depending on the control mode.
+     * @param[in] cmds Real-time joint position-velocity-acceleration commands mapped by joint
+     * group. Only existing single-arm joint groups and external axis joint groups are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm or external axis joint group in the connected robot, or if size of any input
+     * vector does not match robot DoF.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: RT_JOINT_IMPEDANCE, RT_JOINT_POSITION.
+     * @note Real-time (RT).
+     * @warning Always stream smooth and continuous commands to avoid sudden movements.
+     * @see SetJointImpedance().
+     */
+    void StreamJointPosition(const std::map<JointGroup, RtJointPositionCmd>& cmds);
+
+    /**
+     * @brief [Non-blocking] Discretely send joint position and velocity commands of the specified
+     * joint group(s) to the robot. An internal motion generator will smoothen the discrete
+     * commands, which are then tracked by either the joint impedance controller or the joint
+     * position controller, depending on the control mode.
+     * @param[in] cmds Non-real-time joint position-velocity-acceleration commands mapped by joint
+     * group. Only existing single-arm joint groups and external axis joint groups are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm or external axis joint group in the connected robot, if size of any input vector
+     * does not match robot DoF, or [dq_max] or [ddq_max] contains any non-positive value.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: NRT_JOINT_IMPEDANCE, NRT_JOINT_POSITION.
+     * @warning Calling this function a second time while the motion from the previous call is still
+     * ongoing will trigger an online re-planning of the joint trajectory, such that the previous
+     * command is aborted and the new command starts to execute.
+     * @see SetJointImpedance().
+     */
+    void SendJointPosition(const std::map<JointGroup, NrtJointPositionCmd>& cmds);
+
+    /**
+     * @brief [Blocking] Set impedance properties of the robot's joint motion controller used in
+     * the joint impedance control modes.
+     * @param[in] group The joint group to set impedance properties for. Only existing single-arm
+     * joint groups and external axis joint groups are accepted.
+     * @param[in] K_q Joint motion stiffness: \f$ K_q \in \mathbb{R}^{n \times 1} \f$.
+     * Setting motion stiffness of a joint axis to 0 will make this axis free-floating. Valid range:
+     * [0, RobotInfo::K_q_nom]. Unit: \f$ [Nm/rad] \f$.
+     * @param[in] Z_q Joint motion damping ratio: \f$ Z_q \in \mathbb{R}^{n \times 1} \f$.
+     * Valid range: [0.3, 0.8]. The default value 0.7 will be used if left empty.
+     * @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
+     * group in the connected robot, if [K_q] or [Z_q] contains any value outside the valid range,
+     * or if size of any input vector does not match robot DoF.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning Changing damping ratio [Z_q] to a non-nominal value may lead to performance and
+     * stability issues, please use with caution.
+     */
+    void SetJointImpedance(
+        JointGroup group, const std::vector<double>& K_q, const std::vector<double>& Z_q = {});
+
+    /**
+     * @brief [Blocking] Set maximum contact torques for the robot's joint motion controller used in
+     * the joint impedance control modes. The controller will regulate its output to maintain
+     * contact torques with the environment under the set values.
+     * @param[in] group The joint group to set maximum contact torques for. Only existing single-arm
+     * joint groups and external axis joint groups are accepted.
+     * @param[in] max_torques Maximum contact torques: \f$ tau_q \in \mathbb{R}^{n \times 1} \f$.
+     * Valid range: [0, RobotInfo::tau_max]. Unit: \f$ [Nm] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
+     * group in the connected robot, if [max_torques] contains any value outside the valid range, or
+     * if its size does not match robot DoF.
+     * @throw std::logic_error if the robot is not in an applicable control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void SetMaxContactTorque(JointGroup group, const std::vector<double>& max_torques);
+
+    /**
+     * @brief [Blocking] Set inertia shaping scales for the robot's joint motion controller used in
+     * the joint impedance control modes.
+     * @param[in] group The joint group to set inertia shaping scales for. Only existing single-arm
+     * joint groups and external axis joint groups are accepted.
+     * @param[in] inertia_scales Inertia shaping scales: \f$ \sigma_q \in \mathbb{R}^{n \times 1}
+     * \f$. Valid range: [0.75, 1.0]. The nominal (safe) value is 1.0, which means no shaping.
+     * @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
+     * group in the connected robot, if [inertia_scales] contains any value outside the valid range,
+     * or if its size does not match robot DoF.
+     * @throw std::logic_error if the robot is not in an applicable control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
+     * @note This function blocks until the request is successfully delivered.
+     * @par Joint inertia shaping
+     * In joint impedance control modes, it is possible to shape down the natural inertia of the
+     * joints to make them behave as if they are lighter. The parameter [inertia_scales] sets the
+     * scale of shaped/natural inertia for each joint. Smaller scale corresponds to lighter inertia.
+     */
+    void SetJointInertiaScale(JointGroup group, const std::vector<double>& inertia_scales);
+
+    //================================== DIRECT CARTESIAN CONTROL ==================================
+    /**
+     * @brief [Non-blocking] Continuously stream Cartesian motion and/or force commands of the
+     * specified joint group(s) to the robot. The commands are tracked by a unified motion-force
+     * controller that allows force control in zero or more Cartesian axes and motion control in
+     * the other axes.
+     * @param[in] cmds Real-time Cartesian motion/force commands mapped by joint group. Only
+     * existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm joint group in the connected robot.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE.
+     * @note Real-time (RT).
+     * @warning Always stream smooth and continuous motion commands to avoid sudden movements. The
+     * force commands don't need to be continuous.
+     * @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
+     * instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
+     * towards -Z direction, so that upon contact, the sensed force will be +5 N.
+     * @par How to achieve pure motion control?
+     * Use SetForceControlAxis() to disable force control for all Cartesian axes to achieve pure
+     * motion control. This function does pure motion control by default.
+     * @par How to achieve pure force control?
+     * Use SetForceControlAxis() to enable force control for all Cartesian axes to achieve pure
+     * force control, active or passive.
+     * @par How to achieve unified motion-force control?
+     * Use SetForceControlAxis() to enable force control for one or more Cartesian axes and leave
+     * the rest axes motion-controlled, then provide target pose for the motion-controlled axes and
+     * target wrench for the force-controlled axes.
+     * @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
+     * SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
+     */
+    void StreamCartesianMotionForce(const std::map<JointGroup, RtCartesianCmd>& cmds);
+
+    /**
+     * @brief [Non-blocking] Discretely send Cartesian motion and/or force commands of the specified
+     * joint group(s) to the robot. An internal motion generator will smoothen the discrete
+     * commands, which are then tracked by a unified motion-force controller that allows force
+     * control in zero or more Cartesian axes and motion control in the other axes. The robot's
+     * internal motion generator will smoothen the discrete commands.
+     * @param[in] cmds Non-real-time Cartesian motion/force commands mapped by joint group. Only
+     * existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm joint group in the connected robot, or if any of the last 4 input parameters is
+     * not positive.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: NRT_CARTESIAN_MOTION_FORCE, NRT_SUPER_PRIMITIVE.
+     * @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
+     * instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
+     * towards -Z direction, so that upon contact, the sensed force will be +5 N.
+     * @par How to achieve pure motion control?
+     * Use SetForceControlAxis() to disable force control for all Cartesian axes to achieve pure
+     * motion control. This function does pure motion control by default.
+     * @par How to achieve pure force control?
+     * Use SetForceControlAxis() to enable force control for all Cartesian axes to achieve pure
+     * force control, active or passive.
+     * @par How to achieve unified motion-force control?
+     * Use SetForceControlAxis() to enable force control for one or more Cartesian axes and leave
+     * the rest axes motion-controlled, then provide target pose for the motion-controlled axes and
+     * target wrench for the force-controlled axes.
+     * @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
+     * SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
+     */
+    void SendCartesianMotionForce(const std::map<JointGroup, NrtCartesianCmd>& cmds);
+
+    /**
+     * @brief [Non-blocking] Discretely send Cartesian multi-waypoint motion and/or force commands
+     * for the robot to track using non-real-time super primitives.
+     * @param[in] cmds Non-real-time Cartesian multi-waypoint commands mapped by joint group. Only
+     * existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm joint group in the connected robot, or if any waypoint list is empty or any
+     * waypoint's last 4 input parameters is not positive.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: NRT_SUPER_PRIMITIVE.
+     * @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
+     * instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
+     * towards -Z direction, so that upon contact, the sensed force will be +5 N.
+     * @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
+     * SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
+     */
+    void SendCartesianMotionForceMultiWaypoint(
+        const std::map<JointGroup, NrtCartesianMultiWaypointCmd>& cmds);
+
+    /**
+     * @brief [Blocking] Set impedance properties of the robot's Cartesian motion controller
+     * used in the Cartesian motion-force control modes.
+     * @param[in] group The joint group to set Cartesian impedance for. Only existing single-arm
+     * joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] K_x Cartesian motion stiffness: \f$ K_x \in \mathbb{R}^{6 \times 1} \f$.
+     * Setting motion stiffness of a motion-controlled Cartesian axis to 0 will make this axis
+     * free-floating. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$
+     * \mathbb{R}^{3 \times 1} \f$ angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, k_{Rz}]^T
+     * \f$. Valid range: [0, RobotInfo::K_x_nom]. Unit: \f$ [N/m]:[Nm/rad] \f$.
+     * @param[in] Z_x Cartesian motion damping ratio: \f$ Z_x \in \mathbb{R}^{6 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear damping ratio and \f$ \mathbb{R}^{3 \times
+     * 1} \f$ angular damping ratio: \f$ [\zeta_x, \zeta_y, \zeta_z, \zeta_{Rx}, \zeta_{Ry},
+     * \zeta_{Rz}]^T \f$. Valid range: [0.3, 0.8]. The nominal (safe) value is provided as default.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if [K_x] or [Z_x] contains any value outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning Changing damping ratio [Z_x] to a non-nominal value may lead to performance and
+     * stability issues, please use with caution.
+     */
+    void SetCartesianImpedance(JointGroup group, const std::array<double, kCartDoF>& K_x,
+        const std::array<double, kCartDoF>& Z_x = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7});
+
+    /**
+     * @brief [Blocking] Set maximum contact wrench for the motion control part of the Cartesian
+     * motion-force control modes. The controller will regulate its output to maintain contact
+     * wrench (force and moment) with the environment under the set values.
+     * @param[in] group The joint group to set maximum contact wrench for. Only existing
+     * single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] max_wrench Maximum contact wrench (force and moment): \f$ F_{max} \in
+     * \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ maximum force and
+     * \f$ \mathbb{R}^{3 \times 1} \f$ maximum moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
+     * Unit: \f$ [N]:[Nm] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if [max_wrench] contains any negative value.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note The maximum contact wrench regulation only applies to the motion control part.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning The maximum contact wrench regulation cannot be enabled if any of the rotational
+     * Cartesian axes is enabled for moment control.
+     */
+    void SetMaxContactWrench(JointGroup group, const std::array<double, kCartDoF>& max_wrench);
+
+    /**
+     * @brief [Blocking] Set reference joint positions for the null-space posture control module
+     * used in the Cartesian motion-force control modes.
+     * @param[in] group The joint group to set null-space posture for. Only existing single-arm
+     * joint groups and external axis joint groups are accepted.
+     * @param[in] ref_positions Reference joint positions for the null-space posture control:
+     * \f$ q_{ns} \in \mathbb{R}^{n \times 1} \f$. Valid range: [RobotInfo::q_min,
+     * RobotInfo::q_max]. Unit: \f$ [rad] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm or external axis
+     * joint group in the connected robot, if [ref_positions] contains any value outside the valid
+     * range, or if its size does not match robot DoF.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning The reference joint positions will be automatically reset to the robot's current
+     * joint positions upon re-entering the applicable control modes.
+     * @par Null-space posture control
+     * Similar to human arm, a robotic arm with redundant joint-space degree(s) of freedom (DoF > 6)
+     * can change its overall posture without affecting the ongoing primary task. This is achieved
+     * through a technique called "null-space control". After the reference joint positions of a
+     * desired robot posture are set using this function, the robot's null-space control module will
+     * try to pull the arm as close to this posture as possible without affecting the primary
+     * Cartesian motion-force control task.
+     */
+    void SetNullSpacePosture(JointGroup group, const std::vector<double>& ref_positions);
+
+    /**
+     * @brief [Blocking] Set weights of the three optimization objectives while computing the
+     * robot's null-space posture. Change the weights to optimize robot performance for different
+     * use cases.
+     * @param[in] group The joint group to set null-space objectives for. Only existing
+     * single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] linear_manipulability Increase this weight to improve the robot's capability to
+     * translate freely in Cartesian space, i.e. a broader range of potential translation movements.
+     * Valid range: [0.0, 1.0].
+     * @param[in] angular_manipulability Increase this weight to improve the robot's capability to
+     * rotate freely in Cartesian space, i.e. a broader range of potential rotation movements. Valid
+     * range: [0.0, 1.0].
+     * @param[in] ref_positions_tracking Increase this weight to make the robot track closer to the
+     * reference joint positions specified using SetNullSpacePosture(). Valid range: [0.1, 1.0].
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if any of the input parameters is outside its valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note The default value is provided for each parameter.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning The optimization weights will be automatically reset to the provided default values
+     * upon re-entering the applicable control modes.
+     */
+    void SetNullSpaceObjectives(JointGroup group, double linear_manipulability = 0.0,
+        double angular_manipulability = 0.0, double ref_positions_tracking = 0.5);
+
+    /**
+     * @brief [Blocking] Set Cartesian axes to enable force control while in the Cartesian
+     * motion-force control modes. Axes not enabled for force control will be motion-controlled.
+     * @param[in] group The joint group to set force control axes for. Only existing single-arm
+     * joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] enabled_axes Flags to enable/disable force control for certain Cartesian axes in
+     * the force control reference frame (configured by SetForceControlFrame()). The axis order is
+     * \f$ [X, Y, Z, Rx, Ry, Rz] \f$.
+     * @param[in] max_linear_vel For linear Cartesian axes that are enabled for force control, limit
+     * the moving velocity to these values as a protection mechanism in case of contact loss. The
+     * axis order is \f$ [X, Y, Z] \f$. Valid range: [0.005, 2.0]. Unit: \f$ [m/s] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if [max_linear_vel] contains any value outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @note If not set, force control is disabled for all Cartesian axes by default.
+     * @warning The maximum linear velocity protection for force control axes is only effective
+     * under active force control (i.e. passive force control disabled), see
+     * SetPassiveForceControl().
+     */
+    void SetForceControlAxis(JointGroup group, const std::array<bool, kCartDoF>& enabled_axes,
+        const std::array<double, kCartDoF / 2>& max_linear_vel = {1.0, 1.0, 1.0});
+
+    /**
+     * @brief [Blocking] Set reference frame for force control while in the Cartesian motion-force
+     * control modes. The force control frame is defined by specifying its transformation with
+     * regard to the root coordinate.
+     * @param[in] group The joint group to set force control frame for. Only existing single-arm
+     * joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] root_coord Reference coordinate of [T_in_root].
+     * @param[in] T_in_root Transformation from [root_coord] to the user-defined force control
+     * frame: \f$ ^{root}T_{force} \in \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3
+     * \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w,
+     * q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$. If root coordinate is a fixed one (e.g. WORLD),
+     * then the force control frame will also be fixed; if root coordinate is a moving one (e.g.
+     * TCP), then the force control frame will also be moving with the root coordinate. An identity
+     * transformation is provided as default.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if [root_coord] is invalid.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @note If not set, the robot will use WORLD origin as the force control frame by default.
+     * @par Force control frame
+     * In Cartesian motion-force control modes, the reference frame of motion control is always the
+     * world frame, but the reference frame of force control can be an arbitrary one. While the
+     * world frame is the commonly used global coordinate, the current TCP frame is a dynamic local
+     * coordinate whose transformation with regard to the world frame changes as the robot TCP
+     * moves. When using world frame with no transformation as the force control frame, the
+     * force-controlled axes and motion-controlled axes are guaranteed to be orthogonal. Otherwise,
+     * the force-controlled axes and motion-controlled axes are NOT guaranteed to be orthogonal
+     * because different reference frames are used. In this case, it's recommended but not required
+     * to set the target pose such that the actual robot motion direction(s) are orthogonal to force
+     * direction(s). If they are not orthogonal, the motion control's vector component(s) in the
+     * force direction(s) will be eliminated.
+     */
+    void SetForceControlFrame(JointGroup group, CoordType root_coord,
+        const std::array<double, kPoseSize>& T_in_root = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0});
+
+    /**
+     * @brief [Blocking] Enable or disable passive force control for the Cartesian motion-force
+     * control modes. When enabled, an open-loop force controller will be used to feed forward the
+     * target wrench, i.e. passive force control. When disabled, a closed-loop force controller will
+     * be used to track the target wrench, i.e. active force control.
+     * @param[in] group The joint group to toggle passive force control for. Only existing
+     * single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] is_enabled True: enable; false: disable. By default, passive force control is
+     * disabled and active force control is used.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @note If not set, the passive force control is disabled by default.
+     * @par Difference between active and passive force control
+     * Active force control uses a feedback loop to reduce the error between target wrench and
+     * measured wrench. This method results in better force tracking performance, but at the cost of
+     * additional Cartesian damping which could potentially decrease motion tracking performance. On
+     * the other hand, passive force control simply feeds forward the target wrench. This methods
+     * results in worse force tracking performance, but is more robust and does not introduce
+     * additional Cartesian damping. The choice of active or passive force control depends on the
+     * actual application.
+     */
+    void SetPassiveForceControl(JointGroup group, bool is_enabled);
+
+    //======================================== IO CONTROL ========================================
+    /**
+     * @brief [Blocking] Set one or more digital output ports, including 16 on the control box plus
+     * 2 inside each wrist connector.
+     * @param[in] digital_outputs A map of {port_index, port_value}. For [port_index], the valid
+     * range is [0, 17]. For [port_value], true: set port high, false: set port low. For example,
+     * {{1, true}, {3, false}, {10, true}}.
+     * @throw std::invalid_argument if any provided port index is outside the valid range.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void SetDigitalOutputs(const std::map<unsigned int, bool>& digital_outputs);
+
+    /**
+     * @brief [Non-blocking] Current reading from all digital input ports, including 16 on the
+     * control box plus 2 in each wrist connector.
+     * @return A boolean array whose index corresponds to that of the digital input ports.
+     * True: port high; false: port low.
+     */
+    std::array<bool, kIOPorts> digital_inputs() const;
+
+    /**
+     * @brief [Non-blocking] Current reading from all digital output ports, including 16 on the
+     * control box plus 2 in each wrist connector.
+     * @return A boolean array whose index corresponds to that of the digital output ports.
+     * True: port high; false: port low.
+     */
+    std::array<bool, kIOPorts> digital_outputs() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+
+    friend class Device;
+    friend class FileIO;
+    friend class Gripper;
+    friend class Maintenance;
+    friend class Model;
+    friend class Safety;
+    friend class Tool;
+    friend class WorkCoord;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_ROBOT_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/safety.hpp

@@ -0,0 +1,168 @@
+/**
+ * @file safety.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_SAFETY_HPP_
+#define FLEXIV_RDK_SAFETY_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/** Number of safety IO ports */
+constexpr size_t kSafetyIOPorts = 8;
+
+/**
+ * @struct SafetyLimits
+ * @brief Data structure containing configurable robot safety limits.
+ */
+struct SafetyLimits
+{
+    /** Lower safety limits of joint positions for each joint group: \f$ q_{min} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> q_min = {};
+
+    /** Upper safety limits of joint positions for each joint group: \f$ q_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> q_max = {};
+
+    /** Upper safety limits of joint velocities for each joint group when the robot is in normal
+     * state: \f$ \dot{q}^{normal}_{max} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$. */
+    std::map<JointGroup, std::vector<double>> dq_max_normal = {};
+
+    /** Upper safety limits of joint velocities for each joint group when the robot is in reduced
+     * state: \f$ \dot{q}^{reduced}_{max} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$.
+     * @see Robot::reduced() */
+    std::map<JointGroup, std::vector<double>> dq_max_reduced = {};
+};
+
+/**
+ * @class Safety
+ * @brief Interface to manage safety settings of the robot. A password is required to authenticate
+ * this interface.
+ * @note As mentioned in the function doc, certain safety settings are only applicable to the arms
+ * but not the external axis.
+ */
+class Safety
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the safety settings interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @param[in] password Password to authorize making changes to the robot's safety settings.
+     * @throw std::invalid_argument if the provided password is incorrect.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Safety(const Robot& robot, const std::string& password);
+    virtual ~Safety();
+
+    /**
+     * @brief [Non-blocking] Default values of the safety limits of the connected robot.
+     * @return SafetyLimits value copy.
+     */
+    SafetyLimits default_limits() const;
+
+    /**
+     * @brief [Non-blocking] Current values of the safety limits of the connected robot.
+     * @return SafetyLimits value copy.
+     */
+    SafetyLimits current_limits() const;
+
+    /**
+     * @brief [Non-blocking] Current reading from all safety input ports.
+     * @return A boolean array whose index corresponds to that of the safety input ports.
+     * True: port high; false: port low.
+     */
+    std::array<bool, kSafetyIOPorts> safety_inputs() const;
+
+    /**
+     * @brief [Blocking] Set safety limits on the joint positions of the manipulator, which will
+     * honor this setting when making movements.
+     * @param[in] min_positions Minimum joint positions: \f$ q_{min} \in \mathbb{R}^{n \times 1}
+     * \f$. Valid range: [default_min_joint_positions, default_max_joint_positions]. Unit: \f$ [rad]
+     * \f$.
+     * @param[in] max_positions Maximum joint positions: \f$ q_{max} \in \mathbb{R}^{n \times 1}
+     * \f$. Valid range: [default_min_joint_positions, default_max_joint_positions]. Unit: \f$ [rad]
+     * \f$.
+     * @param[in] group Joint group to apply the limits to.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, if [min_positions] or [max_positions] contains any value outside the valid
+     * range, or if size of any input vector does not match manipulator DoF.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning A reboot is required for the updated safety settings to take effect. After reboot,
+     * make sure to use current_limits() to double check the updated safety limits are correct.
+     */
+    void SetJointPositionLimits(JointGroup group, const std::vector<double>& min_positions,
+        const std::vector<double>& max_positions);
+
+    /**
+     * @brief [Blocking] Set safety limits on the joint velocities of the manipulator, which will
+     * honor this setting when making movements under the normal state.
+     * @param[in] max_velocities Maximum joint velocities for normal state: \f$ \dot{q}_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Valid range: [0.8727, joint_velocity_normal_limits]. Unit: \f$
+     * [rad/s] \f$.
+     * @param[in] group Joint group to apply the limits to.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, if [max_velocities] contains any value outside the valid range, or if its
+     * size does not match manipulator DoF.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning A reboot is required for the updated safety settings to take effect. After reboot,
+     * make sure to use current_limits() to double check the updated safety limits are correct.
+     */
+    void SetJointVelocityNormalLimits(JointGroup group, const std::vector<double>& max_velocities);
+
+    /**
+     * @brief [Blocking] Set safety limits on the joint velocities of the manipulator, which will
+     * honor this setting when making movements under the reduced state.
+     * @param[in] max_velocities Maximum joint velocities for reduced state: \f$ \dot{q}_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Valid range: [0.8727, joint_velocity_normal_limits]. Unit: \f$
+     * [rad/s] \f$.
+     * @param[in] group Joint group to apply the limits to.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, if [max_velocities] contains any value outside the valid range, or if its
+     * size does not match manipulator DoF.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning A reboot is required for the updated safety settings to take effect. After reboot,
+     * make sure to use current_limits() to double check the updated safety limits are correct.
+     */
+    void SetJointVelocityReducedLimits(JointGroup group, const std::vector<double>& max_velocities);
+
+    /**
+     * @brief [Blocking] Change settings of the regulator on the joint output torques of the
+     * manipulator and external axes. The regulator will limit the joints' total output torques so
+     * that the measured torques are less likely to exceed safety limits under large payload or
+     * fast/abrupt motions.
+     * @param[in] limiting_factor Factor to limit the total output torques: \f$ \tau^{o}_{max} =
+     * \tau_{max} \times factor \f$.
+     * @param[in] error_threshold If the unregulated output torque of any joint exceeds the
+     * configured regulator limit for more than [error_threshold] cycles consecutively, the robot
+     * will trigger a minor fault and stop.
+     * @throw std::invalid_argument if [limiting_factor] or [error_threshold] is not positive.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @note This setting takes effect immediately and doesn't require a reboot.
+     * @warning Setting the limiting factor too small can cause performance degradation.
+     */
+    void SetJointOutputTorqueRegulator(
+        double limiting_factor = 1.3, unsigned int error_threshold = 50);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_SAFETY_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/scheduler.hpp

@@ -0,0 +1,113 @@
+/**
+ * @file scheduler.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_SCHEDULER_HPP_
+#define FLEXIV_RDK_SCHEDULER_HPP_
+
+#include <string>
+#include <functional>
+#include <memory>
+
+namespace flexiv::rdk {
+
+/**
+ * @class Scheduler
+ * @brief Real-time scheduler that can simultaneously run multiple periodic tasks. Parameters for
+ * each task are configured independently.
+ */
+class Scheduler
+{
+public:
+    /**
+     * @brief [Blocking] Instantiate a real-time scheduler.
+     * @throw std::runtime_error if the initialization sequence failed.
+     * @warning This constructor blocks until the initialization sequence is successfully finished.
+     */
+    Scheduler();
+    virtual ~Scheduler();
+
+    /**
+     * @brief [Non-blocking] Add a new periodic task to the scheduler's task pool. Each task in the
+     * pool is assigned to a dedicated thread with independent thread configuration.
+     * @param[in] callback Callback function of user task.
+     * @param[in] task_name A unique name for this task.
+     * @param[in] interval Execution interval of this periodic task [ms]. The minimum available
+     * interval is 1 ms, equivalent to 1 kHz loop frequency.
+     * @param[in] priority Priority for this task thread, can be set to
+     * min_priority()–max_priority() for real-time scheduling, or 0 for non-real-time scheduling.
+     * When the priority is set to use real-time scheduling, this thread becomes a real-time thread
+     * and can only be interrupted by threads with higher priority. When the priority is set to use
+     * non-real-time scheduling (i.e. 0), this thread becomes a non-real-time thread and can be
+     * interrupted by any real-time threads. The common practice is to set priority of the most
+     * critical tasks to max_priority() or near, and set priority of other non-critical tasks to 0
+     * or near. To avoid race conditions, the same priority should be assigned to only one task.
+     * @param[in] cpu_affinity CPU core for this task thread to bind to, can be set to 2–(num_cores
+     * - 1). This task thread will only run on the specified CPU core. If left with the default
+     * value (-1), then this task thread will not bind to any CPU core, and the system will decide
+     * which core to run this task thread on according to the system's own strategy. The common
+     * practice is to bind the high-priority task to a dedicated spare core, and bind low-priority
+     * tasks to other cores or just leave them unbound (cpu_affinity = -1).
+     * @throw std::logic_error if the scheduler is already started or is not fully initialized yet.
+     * @throw std::invalid_argument if the specified interval/priority/affinity is invalid or the
+     * specified task name is duplicate.
+     * @throw std::runtime_error if an error is triggered by the client computer.
+     * @note Setting CPU affinity on macOS has no effect, as its Mach kernel takes full control of
+     * thread placement so CPU binding is not supported.
+     * @warning Calling this function after start() is not allowed.
+     * @warning For maximum scheduling performance, setting CPU affinity to 0 or 1 is not allowed:
+     * core 0 is usually the default core for system processes and can be crowded; core 1 is
+     * reserved for the scheduler itself.
+     */
+    void AddTask(std::function<void(void)>&& callback, const std::string& task_name, int interval,
+        int priority, int cpu_affinity = -1);
+
+    /**
+     * @brief [Blocking] Start all added tasks. A dedicated thread will be created for each added
+     * task and the periodic execution will begin.
+     * @throw std::logic_error if the scheduler is not initialized yet.
+     * @throw std::runtime_error if failed to start the tasks.
+     * @note This function blocks until all added tasks are started.
+     */
+    void Start();
+
+    /**
+     * @brief [Blocking] Stop all added tasks. The periodic execution will stop and all task threads
+     * will be closed with the resources released.
+     * @throw std::logic_error if the scheduler is not initialized or the tasks are not started yet.
+     * @throw std::runtime_error if failed to stop the tasks.
+     * @note Calling start() again can restart the added tasks.
+     * @note This function blocks until all task threads have exited and resources are released.
+     * @warning This function cannot be called from within a task thread.
+     */
+    void Stop();
+
+    /**
+     * @brief [Non-blocking] Maximum available priority for user tasks.
+     * @return The maximum priority that can be set for a user task with real-time scheduling policy
+     * when calling AddTask().
+     */
+    int max_priority() const;
+
+    /**
+     * @brief [Non-blocking] Minimum available priority for user tasks.
+     * @return The minimum priority that can be set for a user task with real-time scheduling policy
+     * when calling AddTask().
+     */
+    int min_priority() const;
+
+    /**
+     * @brief [Non-blocking] Number of tasks added to the scheduler.
+     * @return Number of added tasks.
+     */
+    size_t num_tasks() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_SCHEDULER_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/tool.hpp

@@ -0,0 +1,187 @@
+/**
+ * @file tool.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_TOOL_HPP_
+#define FLEXIV_RDK_TOOL_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/**
+ * @struct ToolParams
+ * @brief Data structure containing robot tool parameters.
+ * @see Tool::params().
+ */
+struct ToolParams
+{
+    /** Total mass. Unit: \f$ [kg] \f$ */
+    double mass = 0.0;
+
+    /** Center of mass in robot flange frame: \f$ [x, y, z] \f$. Unit: \f$ [m] \f$ */
+    std::array<double, 3> CoM = {};
+
+    /** Inertia at center of mass: \f$ [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] \f$. Unit: \f$ [kg m^2] \f$ */
+    std::array<double, 6> inertia = {};
+
+    /** Position and orientation of the tool center point (TCP) in flange frame. Consists of \f$
+     * \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x,
+     * y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
+    std::array<double, kPoseSize> tcp_location = {};
+};
+
+/**
+ * @class Tool
+ * @brief Interface to manage tools of the robot. All updates take effect immediately without a
+ * power cycle. However, the robot must be in IDLE mode when applying changes.
+ */
+class Tool
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the robot tool interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Tool(const Robot& robot);
+    virtual ~Tool();
+
+    /**
+     * @brief [Blocking] A list of all configured tools.
+     * @return Tool names as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> list() const;
+
+    /**
+     * @brief [Blocking] Name of the active tool currently used by the specified joint group.
+     * @param[in] group Joint group to get tool name for. Only existing single-arm joint groups
+     * like ARM_1 and ARM_2 are accepted.
+     * @return Name of the active tool. Return "Flange" if there's no active tool.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::string name(JointGroup group) const;
+
+    /**
+     * @brief [Blocking] Whether the specified tool already exists.
+     * @param[in] name Name of the tool to check.
+     * @return True if the specified tool exists.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    bool exist(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Parameters of the active tool currently used by the specified joint group.
+     * @param[in] group Joint group to get tool parameters for. Only existing single-arm joint
+     * groups like ARM_1 and ARM_2 are accepted.
+     * @return ToolParams data struct.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    ToolParams params(JointGroup group) const;
+
+    /**
+     * @brief [Blocking] Parameters of the specified tool.
+     * @param[in] name Name of the tool to get parameters for, must be an existing one.
+     * @return ToolParams data struct.
+     * @throw std::invalid_argument if the specified tool does not exist.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    ToolParams params(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Add a new tool with user-specified parameters.
+     * @param[in] name Name of the new tool, must be unique.
+     * @param[in] params Parameters of the new tool.
+     * @throw std::invalid_argument if the specified tool already exists.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Add(const std::string& name, const ToolParams& params);
+
+    /**
+     * @brief [Blocking] Switch active tool for the specified joint group. All following operations
+     * will default to use this tool.
+     * @param[in] group Joint group to switch active tool for. Only existing single-arm joint
+     * groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] name Name of the tool to switch to, must be an existing one.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot.
+     * @throw std::invalid_argument if the specified tool does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Switch(JointGroup group, const std::string& name);
+
+    /**
+     * @brief [Blocking] Update the parameters of an existing tool.
+     * @param[in] name Name of the tool to update, must be an existing one.
+     * @param[in] params New parameters for the specified tool.
+     * @throw std::invalid_argument if the specified tool does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Update(const std::string& name, const ToolParams& params);
+
+    /**
+     * @brief [Blocking] Remove an existing tool.
+     * @param[in] name Name of the tool to remove, must be an existing one but cannot be "Flange".
+     * @throw std::invalid_argument if the specified tool does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode or trying to remove
+     * "Flange".
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Remove(const std::string& name);
+
+    /**
+     * @brief [Blocking] Calibrate the payload parameters (mass, CoM, and inertia) of a tool.
+     * @param[in] tool_mounted Whether the tool to be calibrated is mounted on the robot flange when
+     * triggering this calibration process. See details below.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if fault occurred during the calibration or failed to get the
+     * calibration result.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the calibration is finished.
+     * @warning [tcp_location] in the returned struct will be zeros and should be ignored.
+     * @par How to properly calibrate the payload parameters of a tool?
+     * 1. Call Switch("Flange") to disable any active tool from the robot software.
+     * 2. Physically mount the tool to be calibrated to robot flange.
+     * 3. Call this function with [tool_mounted] set to TRUE, then wait for completion. If the robot
+     * has a force-torque (FT) sensor, then the returned result will be accurate enough and the
+     * optional steps can be skipped. If the robot does not have an FT sensor, then the optional
+     * steps are recommended to improve accuracy.
+     * 4. (Optional) Physically unmount the tool from robot flange.
+     * 5. (Optional) Call this function again but with [tool_mounted] set to FALSE, then wait for
+     * completion.
+     * 6. Review the returned result and call Add() or Update() to apply the calibrated payload
+     * parameters to a new or existing tool. Note that [tcp_location] in the returned struct is
+     * invalid and cannot be used directly.
+     */
+    ToolParams CalibratePayloadParams(bool tool_mounted);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_TOOL_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/utility.hpp

@@ -0,0 +1,245 @@
+/**
+ * @file utility.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_UTILITY_HPP_
+#define FLEXIV_RDK_UTILITY_HPP_
+
+#include "data.hpp"
+#include <Eigen/Eigen>
+#include <algorithm>
+#include <sstream>
+
+namespace flexiv::rdk {
+
+namespace utility {
+
+/** @brief Whether the specified group represents a single arm. */
+inline bool IsSingleArmGroup(JointGroup group)
+{
+    return group == JointGroup::ARM_1 || group == JointGroup::ARM_2;
+}
+
+/**
+ * @brief Check whether one primitive state is true.
+ * @param[in] primitive_states Primitive states.
+ * @param[in] state_name Primitive state name to check, e.g. "reachedTarget".
+ * @return True if the state exists and its integer value is non-zero.
+ */
+inline bool PrimitiveStateTrue(
+    const PrimitiveStates& primitive_states, const std::string& state_name)
+{
+    const auto state_it = primitive_states.names_and_values.find(state_name);
+    if (state_it == primitive_states.names_and_values.end()) {
+        return false;
+    }
+
+    if (!std::holds_alternative<int>(state_it->second)) {
+        return false;
+    }
+
+    return std::get<int>(state_it->second) != 0;
+}
+
+/**
+ * @brief Check whether one primitive state is true for all groups included in [primitive_states].
+ * @param[in] primitive_states Primitive states keyed by joint group.
+ * @param[in] state_name Primitive state name to check, e.g. "reachedTarget".
+ * @return True if all included groups contain the state and its integer value is non-zero.
+ */
+inline bool PrimitiveStateTrueForGroups(
+    const std::map<JointGroup, PrimitiveStates>& primitive_states, const std::string& state_name)
+{
+    return std::all_of(primitive_states.begin(), primitive_states.end(),
+        [&](const auto& entry) { return PrimitiveStateTrue(entry.second, state_name); });
+}
+
+/**
+ * @brief Check whether one primitive state is true for each group included in [primitive_states],
+ * using a per-group state name.
+ * @param[in] primitive_states Primitive states keyed by joint group.
+ * @param[in] state_names Primitive state names keyed by joint group.
+ * @return True if every included group has a matching state name entry and its integer value is
+ * non-zero.
+ */
+inline bool PrimitiveStateTrueForGroups(
+    const std::map<JointGroup, PrimitiveStates>& primitive_states,
+    const std::map<JointGroup, std::string>& state_names)
+{
+    return std::all_of(primitive_states.begin(), primitive_states.end(), [&](const auto& entry) {
+        if (!state_names.contains(entry.first)) {
+            return false;
+        }
+        return PrimitiveStateTrue(entry.second, state_names.at(entry.first));
+    });
+}
+
+/**
+ * @brief Convert quaternion to Euler angles with ZYX axis rotations.
+ * @param[in] quat Quaternion input in [w,x,y,z] order.
+ * @return Euler angles in [x,y,z] order [rad].
+ * @note The return value, when converted to degrees, is the same Euler angles used by Move
+ * primitives.
+ */
+inline std::array<double, 3> Quat2EulerZYX(const std::array<double, 4>& quat)
+{
+    // Form quaternion
+    Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
+
+    // The returned array is in [z,y,x] order
+    auto euler_ZYX = q.toRotationMatrix().eulerAngles(2, 1, 0);
+
+    // Convert to general [x,y,z] order
+    return (std::array<double, 3> {euler_ZYX[2], euler_ZYX[1], euler_ZYX[0]});
+}
+
+/**
+ * @brief Convert radians to degrees for a single value.
+ */
+inline double Rad2Deg(double rad)
+{
+    constexpr double kPi = 3.14159265358979323846;
+    return (rad / kPi * 180.0);
+}
+
+/**
+ * @brief Convert radians to degrees for an array.
+ */
+template <size_t N>
+inline std::array<double, N> Rad2Deg(const std::array<double, N>& rad_arr)
+{
+    std::array<double, N> deg_arr;
+    for (size_t i = 0; i < N; i++) {
+        deg_arr[i] = Rad2Deg(rad_arr[i]);
+    }
+    return deg_arr;
+}
+
+/**
+ * @brief Convert radians to degrees for a vector.
+ */
+inline std::vector<double> Rad2Deg(const std::vector<double>& rad_vec)
+{
+    std::vector<double> deg_vec;
+    for (const auto& v : rad_vec) {
+        deg_vec.push_back(Rad2Deg(v));
+    }
+    return deg_vec;
+}
+
+/**
+ * @brief Convert an std::vector to a string.
+ * @param[in] vec std::vector of any type and size.
+ * @param[in] decimal Decimal places to keep for each floating-point number in the vector.
+ * @param[in] separator Character to separate between numbers.
+ * @return The converted string.
+ */
+template <typename T>
+inline std::string Vec2Str(
+    const std::vector<T>& vec, size_t decimal = 3, const std::string& separator = " ")
+{
+    std::string padding = "";
+    std::ostringstream oss;
+    oss.precision(decimal);
+    oss << std::fixed;
+
+    for (const auto& v : vec) {
+        oss << padding << v;
+        padding = separator;
+    }
+    return oss.str();
+}
+
+/**
+ * @brief Convert an std::array to a string.
+ * @param[in] arr std::array of any type and size.
+ * @param[in] decimal Decimal places to keep for each floating-point number in the array.
+ * @param[in] separator Character to separate between numbers.
+ * @return The converted string.
+ */
+template <typename T, size_t N>
+inline std::string Arr2Str(
+    const std::array<T, N>& arr, size_t decimal = 3, const std::string& separator = " ")
+{
+    std::vector<T> vec(N);
+    std::copy(arr.begin(), arr.end(), vec.begin());
+    return Vec2Str(vec, decimal, separator);
+}
+
+/**
+ * @brief Convert the commonly used std::variant to a string.
+ * @param[in] variant std::variant used by multiple rdk::Robot functions.
+ * @param[in] decimal Decimal places to keep for each floating-point number in the variant.
+ * @param[in] separator Character to separate between numbers in the vector.
+ * @return The converted string.
+ */
+inline std::string FlexivTypes2Str(
+    const rdk::FlexivDataTypes& variant, size_t decimal = 3, const std::string& separator = " ")
+{
+    if (auto* val = std::get_if<int>(&variant)) {
+        return Vec2Str(std::vector<int> {*val}, decimal);
+    } else if (auto* val = std::get_if<double>(&variant)) {
+        return Vec2Str(std::vector<double> {*val}, decimal);
+    } else if (auto* val = std::get_if<std::string>(&variant)) {
+        return *val;
+    } else if (auto* val = std::get_if<rdk::Coord>(&variant)) {
+        return (*val).str();
+    } else if (auto* vec = std::get_if<std::vector<int>>(&variant)) {
+        return Vec2Str(*vec, decimal, separator);
+    } else if (auto* vec = std::get_if<std::vector<double>>(&variant)) {
+        return Vec2Str(*vec, decimal, separator);
+    } else if (auto* vec = std::get_if<std::vector<std::string>>(&variant)) {
+        return Vec2Str(*vec, decimal, separator);
+    } else if (auto* vec = std::get_if<std::vector<rdk::Coord>>(&variant)) {
+        std::string ret;
+        // Separate two Coord by " : "
+        for (const auto& v : (*vec)) {
+            ret += v.str() + " : ";
+        }
+        // Remove the trailing " : "
+        if (!ret.empty()) {
+            ret.erase(ret.size() - 3);
+        }
+        return ret;
+    }
+    return "";
+}
+
+/**
+ * @brief Check if any provided strings exist in the program arguments.
+ * @param[in] argc Argument count passed to main() of the program.
+ * @param[in] argv Argument vector passed to main() of the program, where argv[0] is the program
+ * name.
+ * @param[in] ref_strings Reference strings to check against.
+ * @return True if the program arguments contain one or more reference strings.
+ */
+inline bool ProgramArgsExistAny(int argc, char** argv, const std::vector<std::string>& ref_strings)
+{
+    for (int i = 0; i < argc; i++) {
+        for (const auto& v : ref_strings) {
+            if (v == std::string(argv[i])) {
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+/**
+ * @brief Check if one specific string exists in the program arguments.
+ * @param[in] argc Argument count passed to main() of the program.
+ * @param[in] argv Argument vector passed to main() of the program, with argv[0] being the program
+ * name.
+ * @param[in] ref_strings Reference string to check against.
+ * @return True if the program arguments contain this specific reference string.
+ */
+inline bool ProgramArgsExist(int argc, char** argv, const std::string& ref_strings)
+{
+    return ProgramArgsExistAny(argc, argv, {ref_strings});
+}
+
+} /* namespace utility */
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_UTILITY_HPP_ */

thirdparty/flexiv_rdk/include/flexiv/rdk/work_coord.hpp

@@ -0,0 +1,105 @@
+/**
+ * @file work_coord.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_WORK_COORD_HPP_
+#define FLEXIV_RDK_WORK_COORD_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/**
+ * @class WorkCoord
+ * @brief Interface to manage work coordinates of the robot. All updates take effect immediately
+ * without a power cycle. However, the robot must be in IDLE mode when applying changes.
+ */
+class WorkCoord
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the work coordinate interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    WorkCoord(const Robot& robot);
+    virtual ~WorkCoord();
+
+    /**
+     * @brief [Blocking] A list of all configured work coordinates.
+     * @return Work coordinate names as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> list() const;
+
+    /**
+     * @brief [Blocking] Whether the specified work coordinate already exists.
+     * @param[in] name Name of the work coordinate to check.
+     * @return True if the specified work coordinate exists.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    bool exist(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Pose of an existing work coordinate.
+     * @param[in] name Name of the work coordinate to get pose for, must be an existing one.
+     * @return Pose of the work coordinate in world frame: \f$ ^{O}T_{work} \in \mathbb{R}^{7 \times
+     * 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1}
+     * \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::array<double, kPoseSize> pose(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Add a new work coordinate with user-specified parameters.
+     * @param[in] name Name of the new work coordinate, must be unique.
+     * @param[in] pose Pose of the new work coordinate in world frame: \f$ ^{O}T_{work} \in
+     * \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$
+     * \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$
+     * [m]:[] \f$.
+     * @throw std::invalid_argument if the specified work coordinate already exists.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Add(const std::string& name, const std::array<double, kPoseSize>& pose);
+
+    /**
+     * @brief [Blocking] Update the pose of an existing work coordinate.
+     * @param[in] name Name of the work coordinate to update, must be an existing one.
+     * @param[in] pose New pose for the specified work coordinate in world frame: \f$ ^{O}T_{work}
+     * \in \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$
+     * \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$
+     * [m]:[] \f$.
+     * @throw std::invalid_argument if the specified work coordinate does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Update(const std::string& name, const std::array<double, kPoseSize>& pose);
+
+    /**
+     * @brief [Blocking] Remove an existing work coordinate.
+     * @param[in] name Name of the work coordinate to remove, must be an existing one.
+     * @throw std::invalid_argument if the specified work coordinate does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Remove(const std::string& name);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_WORK_COORD_HPP_ */

thirdparty/flexiv_rdk/lib/dummy.cpp

@@ -0,0 +1,5 @@
+/**
+ * @file dummy.cpp
+ * @brief A dummy source file to add portability to the installed flexiv_rdk library
+ */
+void dummy() { }

thirdparty/flexiv_rdk/lib/flexiv_rdk.win_amd64.lib.sha256

@@ -0,0 +1 @@
+8159b1304cd25f4cbde0aafc37d8e41ea8edf77804b0680cee5abf9442ba7557

thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a.sha256

@@ -0,0 +1 @@
+d1267bfa2f54b30e044c92c5468bdfdeb80126fc99e22d1a9623e73315b070a7

thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-qnx.a.sha256

@@ -0,0 +1 @@
+temporarily_suspended

thirdparty/flexiv_rdk/lib/libflexiv_rdk.arm64-darwin.a.sha256

@@ -0,0 +1 @@
+24b0f511b6679d935c6d26e7dab512e534b80da32583699e4b1f2872f10eabde

thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a.sha256

@@ -0,0 +1 @@
+f5a0aab1fe2195ac28fd10764c6a3deb81d25789d7b04acb1e87e029f6924aea

thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-qnx.a.sha256

@@ -0,0 +1 @@
+temporarily_suspended

thirdparty/flexiv_rdk/llms.txt

@@ -0,0 +1,215 @@
+# llms.txt - Guidance for generating Flexiv robot control code in this repository
+
+Scope: This file captures repository-specific, API-accurate knowledge for LLMs that generate Python control scripts using `flexivrdk`.
+
+## 0) Authoritative References
+- RDK Manual: https://www.flexiv.com/software/rdk/manual
+- API Documentation (latest): https://www.flexiv.com/software/rdk/api
+- Python package page: https://pypi.org/project/flexivrdk
+- Primitive catalog and documentation: https://www.flexiv.com/software/primitives
+
+Use these sources for API behavior details and argument meaning when in doubt.
+
+## 0.1) C++ and Python API Parity
+- In this project, Python RDK is generated from pybind bindings of C++ RDK.
+- Function names and function documentation are expected to match between C++ and Python APIs.
+- Generation rule: if Python docs are sparse, use the corresponding C++ API docs as canonical backup for names, parameters, and semantics.
+- Verification rule: before emitting a Python API call, verify parameter count, optionality, and fixed-size container constraints against API docs or headers.
+- Priority references for verification: API docs first, then C++ headers (for exact sizes/types when docs are ambiguous).
+
+## 1) Preferred Script Pattern (Python)
+- Use examples under `example_py/` as style reference.
+- Typical imports:
+  - `import argparse`
+  - `import time`
+  - `import spdlog`
+  - `import flexivrdk`
+- Typical flow:
+  1. Parse `robot_sn` argument.
+  2. `robot = flexivrdk.Robot(robot_sn)`.
+  3. If `robot.fault()`: call `robot.ClearFault()` and handle failure.
+  4. `robot.ServoOn()` and wait until `robot.operational()`.
+  5. Get applicable joint groups from `robot.info()`:
+     - for primitive execution, use `single_arm_groups = robot.info().single_arm_groups`
+     - for inspection/logging of all groups, use `robot.info().all_groups`
+  6. If needed, use `robot.Home()` (or `robot.Home([...])`) as the blocking convenience wrapper for homing.
+  7. Switch to primitive execution with `robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)`.
+  8. Execute primitives with `robot.ExecutePrimitive({group: flexivrdk.PrimitiveArgs(name, params) for group in single_arm_groups})`.
+  9. Wait for primitive completion using the primitive's documented Default Transition Condition from each running group's primitive state map.
+  10. End with `robot.Stop()`.
+
+## 2) Primitive Execution Conventions
+- Use explicit primitive names such as `Home`, `MoveJ`, `MoveL`.
+- For each primitive call, wrap parameters in `flexivrdk.PrimitiveArgs(name, params)` and send as a map keyed by joint group.
+- Use only existing single-arm joint groups, typically the keys of `robot.info().single_arm_groups`, with `robot.ExecutePrimitive(...)`.
+- Keep transitions explicit in code (sequential primitive calls) unless user asks for asynchronous behavior.
+- Do not assume `robot.ExecutePrimitive(...)` blocks until completion. It blocks until request delivery or primitive start; users must still wait on primitive states.
+- Primitive transition rule:
+  - Each primitive exposes a set of primitive states as listed in Primitive State Parameters.
+  - In most cases, use the primitive's documented Default Transition Condition. But other Primitive State Parameters can also be used as transition conditions.
+  - Do not assume `reachedTarget` is the only valid transition condition for all primitives.
+- Polling loop pattern:
+  - `all_states = robot.primitive_states()`
+  - `robot.primitive_states()` only contains joint groups that are currently running primitives
+  - check the documented state key(s) from `state.names_and_values` for each target joint group
+  - primitive-state booleans are typically read back as integer `1` or `0`
+  - `time.sleep(0.1-1.0)` in the loop
+
+### 2.1) RDK v2.x Joint-Group API Rule
+- `robot.states()` returns `dict[JointGroup, RobotStates]`, not a single `RobotStates`.
+- `robot.primitive_states()` returns `dict[JointGroup, PrimitiveStates]`, not a flat key-value map.
+- `robot.ExecutePrimitive(...)` takes `dict[JointGroup, PrimitiveArgs]`, not `(name, params)`.
+- `robot.Home(groups=...)` is the blocking convenience wrapper for the Home primitive and accepts a list of single-arm joint groups. If omitted, it homes all applicable single-arm groups.
+- When using fields like `tcp_pose`, first choose a specific joint group, usually one from `robot.info().single_arm_groups`.
+
+## 3) Data Type Mapping for `ExecutePrimitive`
+Map plan-like values to RDK Python objects:
+- `JPOS` -> `flexivrdk.JPos(q_m, q_e)` or `flexivrdk.JPos(q_m)`
+- `COORD` -> `flexivrdk.Coord(position, orientation, ref_frame, ref_q_m, ref_q_e)`
+- `ARRAY_COORD` -> Python list of `flexivrdk.Coord(...)`
+- Scalar types:
+  - `BOOL` -> `bool`
+  - `INT` -> `int`
+  - `DOUBLE` -> `float`
+  - `TYPE` -> `str`
+  - `VEC_3d` -> Python list/array length 3
+
+### 3.1) Fixed-Size Argument Checklist (Pybind)
+- Treat constructor/type hints with fixed sizes as strict runtime requirements.
+- `flexivrdk.JPos(q_m)` requires `q_m` length 7. `flexivrdk.JPos(q_m, q_e)` requires `q_e` length 6.
+- `flexivrdk.Coord(position, orientation, ref_frame)` requires lengths 3, 3, and 2 respectively.
+- Optional `Coord` arguments still have fixed sizes when provided: `ref_q_m` length 7 and `ref_q_e` length 6.
+- If `ref_q_e` is provided, also provide `ref_q_m`.
+- Prefer omitting optional constructor arguments entirely over passing empty or wrong-sized placeholders.
+
+## 4) Critical `Coord` Rule (Do Not Drop Provided Fields)
+From `include/flexiv/rdk/data.hpp`, the current `Coord` signature is:
+
+`flexivrdk.Coord(position, orientation, ref_frame, ref_q_m, ref_q_e)`
+
+Its members are, in order:
+1. `position`
+2. `orientation`
+3. `ref_frame`
+4. `ref_q_m`
+5. `ref_q_e`
+
+Generation rule:
+- `ref_q_m` and `ref_q_e` are optional in the constructor, but if source data provides them, populate them.
+- Do not silently drop `ref_q_m` or `ref_q_e`.
+- If specifying `ref_q_e`, also specify `ref_q_m`.
+
+## 5) Units Guidance
+- RDK `JPos` and `Coord` objects use SI length unit `[m]` on the API side.
+- Header note: conversion between RDK `[m]` and Flexiv Elements custom-type `[mm]` is handled automatically for `JPOS` and `COORD` exchanged through APIs such as `ExecutePrimitive()`.
+- Primitive-documentation caveat: primitive input parameters do not universally use SI units.
+  - For scalar primitive parameters, follow the primitive page exactly.
+  - For `flexivrdk.Coord(...)` and `flexivrdk.JPos(...)`, supply values in the RDK object units defined by the headers.
+
+## 5.1) Upright Posture Convention
+- The robot upright posture is defined as all joint positions being 0.
+
+## 5.2) Euler Readback Normalization
+- Robot Euler orientation readback is normalized to the range [-180, 180] degrees.
+- If an Euler angle increases past 180, it wraps to -180. If it decreases past -180, it wraps to 180.
+- This wraparound does not indicate a different physical orientation; it is only a different representation of the same rotation.
+- When comparing Euler angles from commanded values versus robot readback, compare with wraparound awareness instead of raw numeric equality.
+
+## 6) Safety and Robustness Defaults
+- Always include fault clear path before enabling.
+- Always wait for operational state before switching mode or sending primitives.
+- Use try/except and log exceptions with `spdlog`.
+- Avoid busy-spin loops; include `time.sleep(...)` in polling loops.
+- Prefer timeout-backed wait helpers over unbounded polling loops when generating new scripts.
+- Keep command sequence deterministic and human-readable.
+
+## 7) Output Style for Generated Scripts
+- Prefer one self-contained runnable script per task.
+- Include short comments only for non-obvious behavior.
+- Keep numeric trajectory data hardcoded only when user explicitly asks for hardcoding.
+- Do not parse external plan files if the user asks for hardcoded primitive calls.
+
+## 8) Repository Anchors
+- Python examples: `example_py/`
+- Agent primitive-only reference: `example_py/AGENT_REFERENCE_primitives_only.py`
+- Primitive tutorial reference: `example_py/basics3_primitive_execution.py`
+- Primitive wait helper reference: `example_py/utility.py`
+- Core robot API surface: `include/flexiv/rdk/robot.hpp`
+- Core data type definitions: `include/flexiv/rdk/data.hpp`
+- Repo readme and setup guide: `README.md`
+
+## 9) Minimal Skeleton
+```python
+import argparse
+import time
+import spdlog
+import flexivrdk
+import utility
+
+
+def wait_primitive_transition(robot, transition_keys_by_group, dt=0.2, timeout_s=60.0):
+    """Block until primitive_states() for all groups satisfy their transition key.
+
+    Use the primitive's default transition key unless a custom transition condition
+    is intended. Check primitive docs to confirm available primitive state keys.
+    """
+    t0 = time.time()
+    while True:
+        if utility.primitive_state_true_for_groups(
+            robot.primitive_states(),
+            transition_keys_by_group,
+        ):
+            return
+
+        if time.time() - t0 > timeout_s:
+            raise TimeoutError(
+                "Timed out waiting for primitive transition states: "
+                f"{ {flexivrdk.kJointGroupNames[g]: k for g, k in transition_keys_by_group.items()} }"
+            )
+        time.sleep(dt)
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("robot_sn")
+    args = parser.parse_args()
+
+    logger = spdlog.ConsoleLogger("LLMGenerated")
+    try:
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        if robot.fault():
+            logger.warn("Fault detected, clearing")
+            if not robot.ClearFault():
+                raise RuntimeError("Cannot clear fault")
+
+        robot.ServoOn()
+        while not robot.operational():
+            time.sleep(1)
+
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
+
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs("Home", {})
+                for group in single_arm_groups
+            }
+        )
+        wait_primitive_transition(robot, {group: "reachedTarget" for group in single_arm_groups})
+
+        # Add other primitive calls here.
+
+        robot.Stop()
+
+    except Exception as e:
+        logger.error(f"Unhandled exception: {e}")
+        raise
+
+
+if __name__ == "__main__":
+    main()
+```

thirdparty/flexiv_rdk/test/CMakeLists.txt

@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.22.1)
+project(flexiv_rdk-tests)
+
+# CMake >= 3.30 warns for FindBoost behavior unless CMP0167 is explicitly set.
+if(POLICY CMP0167)
+  cmake_policy(SET CMP0167 OLD)
+endif()
+
+# Show verbose build info
+SET(CMAKE_VERBOSE_MAKEFILE ON)
+
+message("OS: ${CMAKE_SYSTEM_NAME}")
+message("Processor: ${CMAKE_SYSTEM_PROCESSOR}")
+
+# Configure build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
+endif()
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
+
+find_package(flexiv_rdk REQUIRED)
+
+# List of functionality tests
+set(FUNCTIONALITY_TESTS
+  test_dynamics_engine
+  test_repeated_instantiation
+)
+if(CMAKE_HOST_UNIX)
+  list(APPEND FUNCTIONALITY_TESTS
+    test_endurance
+    test_loop_latency
+    test_scheduler
+    test_timeliness_monitor
+  )
+endif()
+
+# Build functionality tests
+foreach(test ${FUNCTIONALITY_TESTS})
+  add_executable(${test} ${test}.cpp)
+  target_link_libraries(${test} flexiv::flexiv_rdk)
+  # C++20 required
+  set_target_properties(${test} PROPERTIES
+    CXX_STANDARD 20
+    CXX_STANDARD_REQUIRED ON)
+endforeach()
+
+
+# List of unit tests
+set(UNIT_TESTS
+  test_robot_constructor
+)
+
+# Download GTest if not found
+include(FetchContent)
+find_package(GTest CONFIG QUIET)
+if(NOT GTest_FOUND)
+  if(MSVC)
+    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+  endif()
+
+  FetchContent_Declare(
+    googletest
+    URL https://github.com/google/googletest/archive/refs/tags/v1.14.0.zip
+  )
+  FetchContent_MakeAvailable(googletest)
+endif()
+
+# Build unit tests
+foreach(test ${UNIT_TESTS})
+  add_executable(${test} ${test}.cpp)
+  target_link_libraries(${test} flexiv::flexiv_rdk GTest::gtest_main)
+  add_test(NAME ${test} COMMAND ${test})
+  # C++20 required
+  set_target_properties(${test} PROPERTIES
+    CXX_STANDARD 20
+    CXX_STANDARD_REQUIRED ON)
+endforeach()

thirdparty/flexiv_rdk/test/test_dynamics_engine.cpp

@@ -0,0 +1,256 @@
+/**
+ * @test test_dynamics_engine.cpp
+ * A test to evaluate the dynamics engine (J, M, G), with and without end-effector tool.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/model.hpp>
+#include <flexiv/rdk/tool.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+#include <Eigen/Eigen>
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <chrono>
+
+namespace {
+/** J, M, G ground truth from MATLAB */
+struct GroundTruth
+{
+    /** Jacobian matrix */
+    Eigen::Matrix<double, 6, 7> J;
+    /** Mass matrix */
+    Eigen::Matrix<double, 7, 7> M;
+    /** Gravity torques */
+    Eigen::Matrix<double, 7, 1> G;
+};
+}
+
+/** Step the dynamics engine once */
+void StepDynamics(flexiv::rdk::Robot& robot, flexiv::rdk::Model& model, const GroundTruth& ref)
+{
+    // Mark timer start point
+    auto tic = std::chrono::high_resolution_clock::now();
+
+    // Update robot model in dynamics engine
+    const auto robot_states = robot.states();
+    model.Update(robot_states.at(flexiv::rdk::JointGroup::ALL).q,
+        robot_states.at(flexiv::rdk::JointGroup::ALL).dtheta);
+
+    // Get J, M, G from dynamic engine
+    Eigen::MatrixXd J = model.J("flange");
+    Eigen::MatrixXd M = model.M();
+    Eigen::VectorXd G = model.g();
+
+    // Get and print computation time
+    auto toc = std::chrono::high_resolution_clock::now();
+    auto computation_time
+        = std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
+    std::cout << "===================================================================" << std::endl;
+    std::cout << "Computation time = " << computation_time << " us" << std::endl;
+
+    // Evaluate J, M, G with true value
+    auto delta_J = J - ref.J;
+    auto delta_M = M - ref.M;
+    auto delta_G = G - ref.G;
+
+    std::cout << std::fixed << std::setprecision(5);
+    std::cout << "Difference of J between ground truth (MATLAB) and integrated dynamics engine = "
+              << std::endl
+              << delta_J << std::endl;
+    std::cout << "Norm of delta J: " << delta_J.norm() << '\n' << std::endl;
+
+    std::cout << "Difference of M between ground truth (MATLAB) and integrated dynamics engine = "
+              << std::endl
+              << delta_M << std::endl;
+    std::cout << "Norm of delta M: " << delta_M.norm() << '\n' << std::endl;
+
+    std::cout << "Difference of G between ground truth (MATLAB) and integrated dynamics engine = "
+              << std::endl
+              << delta_G.transpose() << std::endl;
+    std::cout << "Norm of delta G: " << delta_G.norm() << '\n' << std::endl;
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //==============================================================================================
+    if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    try {
+        // RDK Initialization
+        //==========================================================================================
+        // Instantiate robot interface
+        flexiv::rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        //==========================================================================================
+        robot.Home();
+
+        // Test Dynamics Engine without Tool
+        //==========================================================================================
+        spdlog::info(">>>>> Test 1: no end-effector tool <<<<<");
+
+        // Instantiate dynamics engine
+        flexiv::rdk::Model model(robot);
+
+        // Ground truth from MATLAB without robot tool
+        GroundTruth ref;
+        // clang-format off
+        ref.J <<
+        0.110000000000000,  0.078420538028673,  0.034471999940354, -0.368152340866938, -0.064278760968654,  0.136000000000000, -0.000000000000000,
+        0.687004857483100, -0.000000000000000,  0.576684003660457,  0.000000000000000,  0.033475407198662, -0.000000000000000, -0.000000000000000,
+        0.000000000000000,  0.687004857483100, -0.028925442435894, -0.417782862794537, -0.076604444311898,  0.110000000000000, -0.000000000000000,
+        0.000000000000000, -0.000000000000000,  0.642787609686539,  0.000000000000000,  0.766044443118978, -0.000000000000000,  0.000000000000000,
+                        0, -1.000000000000000, -0.000000000000000,  1.000000000000000,  0.000000000000000, -1.000000000000000,  0.000000000000000,
+        1.000000000000000, -0.000000000000000,  0.766044443118978,  0.000000000000000, -0.642787609686539, -0.000000000000000, -1.000000000000000;
+        ref.M << 
+        2.480084579964625, -0.066276150278304,  1.520475428405090, -0.082258763257690, -0.082884472612488,  0.008542255000000, -0.000900000000000,
+        -0.066276150278304,  2.778187401738267,  0.067350373889147, -1.100953424157635, -0.129191018084721,  0.165182543869134, -0.000000000000000,
+        1.520475428405090,  0.067350373889147,  1.074177611590570, -0.000410055889147, -0.043572229972025, -0.001968185110853, -0.000689439998807,
+        -0.082258763257690, -1.100953424157635, -0.000410055889147,  0.964760218577003,  0.123748338084721, -0.125985653288502,  0.000000000000000,
+        -0.082884472612488, -0.129191018084721, -0.043572229972025,  0.123748338084721,  0.038006882479315, -0.020080821084721,  0.000578508848718,
+        0.008542255000000,  0.165182543869134, -0.001968185110853, -0.125985653288502, -0.020080821084721,  0.034608170000000,                  0,
+        -0.000900000000000, -0.000000000000000, -0.000689439998807,  0.000000000000000,  0.000578508848718,                  0,  0.000900000000000;
+        ref.G << 
+        0.000000000000000, 46.598931189891374, 1.086347692836129, -19.279904969860052, -2.045058704525489,  2.300886450000000,                  0;
+        // clang-format on
+
+        // Step the dynamics engine
+        int i = 0;
+        while (++i <= 3) {
+            StepDynamics(robot, model, ref);
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+
+        // Test Dynamics Engine with Tool
+        //==========================================================================================
+        spdlog::info(">>>>> Test 2: with end-effector tool <<<<<");
+
+        // Ground truth from MATLAB with robot tool
+        // clang-format off
+        ref.M << 
+        2.916316686749461, -0.052869517013466,  1.903540434220357, -0.124348845003517, -0.041914639740668,  0.027649255000000, -0.001464000000000,
+        -0.052869517013466,  3.222619358431081,  0.053667041477633, -1.414236317529289, -0.184390078851855,  0.259867572215541, -0.000000000000000,
+        1.903540434220357,  0.053667041477633,  1.416023230140298, -0.002724223477633,  0.000093550780077,  0.001155982477633, -0.001121489064726,
+        -0.124348845003517, -1.414236317529289, -0.002724223477633,  1.287676548627496,  0.177147398851855, -0.244344118313748,  0.000000000000000,
+        -0.041914639740668, -0.184390078851855,  0.000093550780077,  0.177147398851855,  0.054219756143365, -0.038829881851855,  0.000941041060581,
+        0.027649255000000,  0.259867572215541,  0.001155982477633, -0.244344118313748, -0.038829881851855,  0.082171270000000,                  0,
+        -0.001464000000000, -0.000000000000000, -0.001121489064726,  0.000000000000000,  0.000941041060581,                  0,  0.001464000000000;
+
+        // Matlab value is the joint torque to resist gravity
+        ref.G << 
+        -0.000000000000001,  52.664497076609663,  0.830964961569619, -22.968509865473024, -2.721399343355234, 3.272076450000000,                 0;
+        // clang-format on
+
+        // Instantiate tool interface
+        flexiv::rdk::Tool tool(robot);
+
+        // Tools can only be assigned to single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Set name and parameters for the test tool
+        std::string tool_name = "DynamicsTestTool";
+        flexiv::rdk::ToolParams tool_params;
+        tool_params.mass = 0.9;
+        tool_params.CoM = {0.0, 0.0, 0.057};
+        tool_params.inertia = {2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0};
+        tool_params.tcp_location = {0.0, -0.207, 0.09, 0.7071068, 0.7071068, 0.0, 0.0};
+
+        // Remove any existing tool with the same name
+        if (tool.exist(tool_name)) {
+            spdlog::warn("Tool with the same name [{}] already exists, removing it now", tool_name);
+            // Switch to other tool or no tool (Flange) before removing the current tool
+            for (const auto& [group, _] : single_arm_groups) {
+                tool.Switch(group, "Flange");
+            }
+            tool.Remove(tool_name);
+        }
+
+        // Add the test tool
+        spdlog::info("Adding test tool [{}] to the robot", tool_name);
+        tool.Add(tool_name, tool_params);
+
+        // Switch to the newly added test tool, i.e. set it as the active tool
+        spdlog::info("Switching to test tool [{}]", tool_name);
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, tool_name);
+        }
+
+        // Get and print the current active tool for each joint group, should be the test tool
+        for (const auto& [group, _] : single_arm_groups) {
+            spdlog::info("[{}] Current active tool: {}", flexiv::rdk::kJointGroupNames.at(group),
+                tool.name(group));
+        }
+
+        // Reload robot + tool model using the latest data synced from the connected robot
+        model.Reload();
+
+        // Step the dynamics engine again
+        i = 0;
+        while (++i <= 3) {
+            StepDynamics(robot, model, ref);
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+
+        // Switch to other tool or no tool (Flange) before removing the current tool
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, "Flange");
+        }
+
+        // Clean up by removing the test tool
+        spdlog::info("Removing tool [{}]", tool_name);
+        tool.Remove(tool_name);
+
+        spdlog::info("Program finished");
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/test/test_endurance.cpp

@@ -0,0 +1,278 @@
+/**
+ * @test test_endurance.cpp
+ * Endurance test running Cartesian impedance control to slowly sine-sweep near home for a duration
+ * of user-specified hours. Raw data will be logged to CSV files continuously.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+const double kLoopPeriod = 0.001;
+
+/** TCP sine-sweep amplitude [m] */
+const double kSwingAmp = 0.1;
+
+/** TCP sine-sweep frequency [Hz] */
+const double kSwingFreq = 0.025; // = 10mm/s linear velocity
+
+/** Current Cartesian-space pose (position + rotation) of robot TCP */
+std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> g_curr_tcp_pose;
+
+/** Test duration converted from user-specified hours to loop counts */
+uint64_t g_test_duration_loop_counts = 0;
+
+/** Time duration for each log file [loop counts] */
+const unsigned int kLogDurationLoopCounts = 10 * 60 * 1000; // = 10 min/file
+
+/** Data to be logged in low-priority thread */
+struct LogData
+{
+    std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> tcp_pose;
+    std::map<rdk::JointGroup, std::array<double, rdk::kCartDoF>> tcp_force;
+} g_log_data;
+
+/** Atomic signal to stop the test */
+std::atomic<bool> g_stop = {false};
+}
+
+void HighPriorityTask(rdk::Robot& robot,
+    const std::map<rdk::JointGroup, std::string>& single_arm_groups,
+    const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_poses)
+{
+    // Local periodic loop counter
+    static uint64_t loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "HighPriorityTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        // Swing along Z direction
+        std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
+        for (const auto& [group, _] : single_arm_groups) {
+            auto target_pose = g_curr_tcp_pose.at(group);
+            target_pose[2] = all_init_poses.at(group)[2]
+                             + kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
+            g_curr_tcp_pose[group] = target_pose;
+            rt_cmds[group] = rdk::RtCartesianCmd(target_pose);
+        }
+        robot.StreamCartesianMotionForce(rt_cmds);
+
+        // Save data to global buffer, not using mutex to avoid interruption on RT loop from
+        // potential priority inversion
+        for (const auto& [group, states] : robot.states()) {
+            g_log_data.tcp_pose[group] = states.tcp_pose;
+            g_log_data.tcp_force[group] = states.tcp_wrench;
+        }
+
+        // Stop after test duration has elapsed
+        if (++loop_counter > g_test_duration_loop_counts) {
+            g_stop = true;
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop = true;
+    }
+}
+
+void LowPriorityTask()
+{
+    // Low-priority task loop counter
+    uint64_t loop_counter = 0;
+
+    // Data logging CSV file
+    std::ofstream csv_file;
+
+    // CSV file name
+    std::string csv_file_name;
+
+    // CSV file counter (data during the test is divided to multiple files)
+    unsigned int file_counter = 0;
+
+    // Wait for a while for the robot states data to be available
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    // Use while loop to prevent this thread from return
+    while (true) {
+        // Log data at 1kHz
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        loop_counter++;
+
+        // Close existing log file and create a new one periodically
+        if (loop_counter % kLogDurationLoopCounts == 0) {
+            if (csv_file.is_open()) {
+                csv_file.close();
+                spdlog::info("Saved log file: {}", csv_file_name);
+            }
+
+            // Increment log file counter
+            file_counter++;
+
+            // Create new file name using the updated counter as suffix
+            csv_file_name = "endurance_test_data_" + std::to_string(file_counter) + ".csv";
+
+            // Open new log file
+            csv_file.open(csv_file_name);
+            if (csv_file.is_open()) {
+                spdlog::info("Created new log file: {}", csv_file_name);
+            } else {
+                spdlog::error("Failed to create log file: {}", csv_file_name);
+            }
+        }
+
+        // Log data to file in CSV format
+        if (csv_file.is_open()) {
+            // Loop counter x1, then grouped TCP pose x7 and TCP wrench x6
+            csv_file << loop_counter << ",";
+            for (const auto& [group, pose] : g_log_data.tcp_pose) {
+                csv_file << rdk::kJointGroupNames.at(group) << ",";
+                for (const auto& i : pose) {
+                    csv_file << i << ",";
+                }
+                for (const auto& i : g_log_data.tcp_force.at(group)) {
+                    csv_file << i << ",";
+                }
+            }
+            // End of line
+            csv_file << '\n';
+        }
+
+        // Check if the test duration has elapsed
+        if (g_stop) {
+            spdlog::info("Test duration has elapsed, saving any open log file ...");
+            // Close log file
+            if (csv_file.is_open()) {
+                csv_file.close();
+                spdlog::info("Saved log file: {}", csv_file_name);
+            }
+            // Exit thread
+            return;
+        }
+    }
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn] [test_hours]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "    test_hours: Duration of the test, can have decimals" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //==============================================================================================
+    if (argc < 3 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // test duration in hours
+    double test_hours = std::stof(argv[2]);
+    // convert duration in hours to loop counts
+    g_test_duration_loop_counts = (uint64_t)(test_hours * 3600.0 * 1000.0);
+    spdlog::info("Test duration: {} hours = {} cycles", test_hours, g_test_duration_loop_counts);
+
+    try {
+        // RDK Initialization
+        //==========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        //==========================================================================================
+        // Direct Cartesian control can only be executed by single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        robot.Home();
+
+        // Switch mode after robot is at home
+        robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
+
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetForceControlAxis(
+                group, std::array<bool, rdk::kCartDoF> {false, false, false, false, false, false});
+        }
+
+        // Set initial pose to current TCP pose
+        std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : single_arm_groups) {
+            all_init_pose[group] = robot_states.at(group).tcp_pose;
+            g_curr_tcp_pose[group] = robot_states.at(group).tcp_pose;
+            spdlog::info("[{}] Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: {}",
+                rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(all_init_pose.at(group)));
+        }
+
+        // Periodic Tasks
+        //==========================================================================================
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(HighPriorityTask, std::ref(robot), std::cref(single_arm_groups),
+                              std::cref(all_init_pose)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Use std::thread for logging task without strict chronology
+        std::thread low_priority_thread(LowPriorityTask);
+
+        // low_priority_thread is responsible to release blocking
+        low_priority_thread.join();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/test/test_loop_latency.cpp

@@ -0,0 +1,175 @@
+/**
+ * @test test_loop_latency.cpp
+ * A test to benchmark RDK's loop latency, including communication, computation, etc. The
+ * workstation PC's serial port is used as reference, and the robot server's digital out port is
+ * used as test target.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+#include <string.h>
+#include <atomic>
+
+// Standard input/output definitions
+#include <stdio.h>
+// UNIX standard function definitions
+#include <unistd.h>
+// File control definitions
+#include <fcntl.h>
+// Error number definitions
+#include <errno.h>
+// POSIX terminal control definitions
+#include <termios.h>
+
+namespace {
+/** File descriptor of the serial port */
+int g_fd = 0;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+// callback function for realtime periodic task
+void PeriodicTask(flexiv::rdk::Robot& robot)
+{
+    // Loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        // send signal at 1Hz
+        switch (loop_counter % 1000) {
+            case 0: {
+                spdlog::info(
+                    "Sending benchmark signal to both workstation PC's serial "
+                    "port and robot server's digital out port[0]");
+                break;
+            }
+            case 1: {
+                // signal robot server's digital out port
+                robot.SetDigitalOutputs({{0, true}});
+
+                // signal workstation PC's serial port
+                auto n = write(g_fd, "0", 1);
+                if (n < 0) {
+                    spdlog::error("Failed to write to serial port");
+                }
+
+                break;
+            }
+            case 900: {
+                // reset digital out after a few seconds
+                robot.SetDigitalOutputs({{0, false}});
+                break;
+            }
+            default:
+                break;
+        }
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn] [serial_port_name]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "    serial_port_name: /dev/ttyS0 for COM1, /dev/ttyS1 for COM2, /dev/ttyUSB0 for USB-serial converter" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //=============================================================================
+    if (argc < 3 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // serial port name
+    std::string serial_port = argv[2];
+
+    try {
+        // RDK Initialization
+        //=============================================================================
+        // Instantiate robot interface
+        flexiv::rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Benchmark Signal
+        //=============================================================================
+        // get workstation PC's serial port ready,
+        g_fd = open(serial_port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL | O_CLOEXEC);
+
+        if (g_fd == -1) {
+            spdlog::error("Unable to open serial port [{}]", serial_port);
+        }
+
+        // print messages
+        spdlog::warn("Benchmark signal will be sent every 1 second");
+
+        // Periodic Tasks
+        //=============================================================================
+        flexiv::rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(
+            std::bind(PeriodicTask, std::ref(robot)), "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/test/test_repeated_instantiation.cpp

@@ -0,0 +1,68 @@
+/**
+ * @test test_repeated_instantiation.cpp
+ * A test to repeatedly construct and destruct rdk::Robot instance.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn] [test_cycles]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "    test_cycles: Number of cycles to repeat the instantiation" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 3 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Number of cycles
+    unsigned int test_cycles = std::stoi(argv[2]);
+
+    try {
+        for (size_t i = 1; i <= test_cycles; i++) {
+            spdlog::info("Test cycle: {}/{}", i, test_cycles);
+
+            // Instantiate robot interface
+            rdk::Robot robot(robot_sn);
+
+            // Servo on the robot, make sure the E-stop is released
+            spdlog::info("Servo on the robot ...");
+            robot.ServoOn();
+
+            // Wait for the robot to become operational
+            while (!robot.operational()) {
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+            }
+            spdlog::info("Robot is now operational");
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/test/test_robot_constructor.cpp

@@ -0,0 +1,56 @@
+/**
+ * @test test_robot_constructor.cpp
+ * @brief Test offline construction of flexivrdk.Robot.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#include <flexiv/rdk/robot.hpp>
+
+#include <gtest/gtest.h>
+
+#include <exception>
+#include <string>
+#include <string_view>
+
+namespace {
+
+template <typename ExceptionT>
+void ExpectRobotConstructorFailure(
+    const std::string& robot_sn, std::string_view expected_message_fragment)
+{
+    try {
+        flexiv::rdk::Robot robot(robot_sn, false);
+        FAIL() << "Expected constructor to fail for serial [" << robot_sn << "], but it succeeded";
+    } catch (const ExceptionT& e) {
+        const std::string_view message(e.what());
+        EXPECT_NE(message.find(expected_message_fragment), std::string_view::npos)
+            << "Constructor for serial [" << robot_sn
+            << "] threw the expected exception type but with unexpected message: " << e.what();
+    } catch (const std::exception& e) {
+        FAIL() << "Constructor for serial [" << robot_sn
+               << "] threw an unexpected exception type: " << e.what();
+    }
+}
+
+} // namespace
+
+TEST(RobotConstructor, InvalidSerialNumberFormat)
+{
+    ExpectRobotConstructorFailure<std::invalid_argument>("Enlight412345", "serial number is invalid");
+}
+
+TEST(RobotConstructor, UnsupportedProductModel)
+{
+    for (const std::string robot_sn : {"Enlight-X-123456", "Enlight X-123456"}) {
+        SCOPED_TRACE(robot_sn);
+        ExpectRobotConstructorFailure<std::logic_error>(robot_sn, "is not supported");
+    }
+}
+
+TEST(RobotConstructor, SupportedModelWithoutServer)
+{
+    for (const std::string robot_sn : {"Enlight-L-123456", "Enlight L-123456"}) {
+        SCOPED_TRACE(robot_sn);
+        ExpectRobotConstructorFailure<std::runtime_error>(robot_sn, "Failed to connect");
+    }
+}

thirdparty/flexiv_rdk/test/test_robot_constructor.py

@@ -0,0 +1,37 @@
+"""Test offline construction of flexivrdk.Robot"""
+
+import unittest
+import flexivrdk
+
+
+class RobotConstructorTest(unittest.TestCase):
+    def assert_robot_constructor_failure(
+        self, robot_sn, expected_exception, message_fragment
+    ):
+        with self.assertRaises(expected_exception) as context:
+            flexivrdk.Robot(robot_sn, False)
+
+        self.assertIn(message_fragment, str(context.exception))
+
+    def test_invalid_format(self):
+        self.assert_robot_constructor_failure(
+            "Enlight412345", ValueError, "serial number is invalid"
+        )
+
+    def test_unsupported_model(self):
+        for robot_sn in ("Enlight-X-123456", "Enlight X-123456"):
+            with self.subTest(robot_sn=robot_sn):
+                self.assert_robot_constructor_failure(
+                    robot_sn, RuntimeError, "is not supported"
+                )
+
+    def test_supported_model_without_server(self):
+        for robot_sn in ("Enlight-L-123456", "Enlight L-123456"):
+            with self.subTest(robot_sn=robot_sn):
+                self.assert_robot_constructor_failure(
+                    robot_sn, RuntimeError, "Failed to connect"
+                )
+
+
+if __name__ == "__main__":
+    unittest.main()

thirdparty/flexiv_rdk/test/test_scheduler.cpp

@@ -0,0 +1,148 @@
+/**
+ * @test test_scheduler.cpp
+ * A test to evaluate RDK's internal real-time scheduler.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <mutex>
+#include <atomic>
+
+namespace {
+/** Data shared between threads */
+struct SharedData
+{
+    int64_t measured_interval = 0;
+} g_data;
+
+/** Mutex on the shared data */
+std::mutex g_mutex;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** User-defined high-priority periodic task @ 1kHz */
+void HighPriorityTask()
+{
+    static unsigned int loop_counter = 0;
+
+    // Scheduler loop interval start time point
+    static std::chrono::high_resolution_clock::time_point tic;
+
+    try {
+        // Mark loop interval end point
+        auto toc = std::chrono::high_resolution_clock::now();
+
+        // Calculate scheduler's interrupt interval and print
+        auto measured_interval
+            = std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
+
+        // Safely write shared data
+        {
+            std::lock_guard<std::mutex> lock(g_mutex);
+            g_data.measured_interval = measured_interval;
+        }
+
+        // Stop scheduler after 5 seconds
+        if (++loop_counter > 5000) {
+            loop_counter = 0;
+            g_stop_sched = true;
+        }
+
+        // Mark loop interval start point
+        tic = std::chrono::high_resolution_clock::now();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+/** User-defined low-priority periodic task @1Hz */
+void LowPriorityTask()
+{
+    static uint64_t accumulated_time = 0;
+    static uint64_t num_measures = 0;
+    static float avg_interval = 0.0;
+    int measured_interval = 0;
+
+    // Safely read shared data
+    {
+        std::lock_guard<std::mutex> lock(g_mutex);
+        measured_interval = g_data.measured_interval;
+    }
+
+    // calculate average time interval
+    accumulated_time += measured_interval;
+    num_measures++;
+    avg_interval = (float)accumulated_time / (float)num_measures;
+
+    // print time interval of high-priority periodic task
+    spdlog::info(
+        "High-priority task interval (curr | avg) = {} | {} us", measured_interval, avg_interval);
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: None" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //==============================================================================================
+    if (flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    try {
+        // Periodic Tasks
+        //==========================================================================================
+        flexiv::rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(HighPriorityTask), "HP periodic", 1, scheduler.max_priority());
+        // Add periodic task with 1s interval and lowest applicable priority
+        scheduler.AddTask(std::bind(LowPriorityTask), "LP periodic", 1000, 0);
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+        // Restart scheduler after 2 seconds
+        spdlog::warn("Scheduler will restart in 2 seconds");
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        g_stop_sched = false;
+        scheduler.Start();
+
+        // Wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks, flexiv::rdk::Scheduler's destructor can also do
+        // the thread exit and resources cleanup
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/test/test_timeliness_monitor.cpp

@@ -0,0 +1,160 @@
+/**
+ * @test test_timeliness_monitor.cpp
+ * A test to evaluate RDK's internal timeliness monitor on real-time modes. Bad communication or
+ * insufficient real-time performance of the workstation PC will cause the monitor's timeliness
+ * check to fail. A warning will be issued first, then if the check has failed too many times, the
+ * RDK connection with the server will be closed. During this test, the robot will hold its position
+ * using joint torque streaming mode.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+namespace {
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+// callback function for realtime periodic task
+void PeriodicTask(flexiv::rdk::Robot& robot,
+    const std::map<flexiv::rdk::JointGroup, std::string>& single_arm_groups,
+    const std::map<flexiv::rdk::JointGroup, std::vector<double>>& all_init_pos)
+{
+    // Loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+        // Hold position
+        std::map<flexiv::rdk::JointGroup, flexiv::rdk::RtJointPositionCmd> rt_cmds;
+        for (const auto& [group, _] : single_arm_groups) {
+            const auto& init_pos = all_init_pos.at(group);
+            std::vector<double> target_vel(init_pos.size());
+            std::vector<double> target_acc(init_pos.size());
+            rt_cmds[group] = flexiv::rdk::RtJointPositionCmd(init_pos, target_vel, target_acc);
+        }
+        robot.StreamJointPosition(rt_cmds);
+
+        if (loop_counter == 5000) {
+            spdlog::warn(">>>>> Adding simulated loop delay <<<<<");
+        }
+        // simulate prolonged loop time after 5 seconds
+        else if (loop_counter > 5000) {
+            std::this_thread::sleep_for(std::chrono::microseconds(995));
+        }
+
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //==============================================================================================
+    if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    try {
+        // RDK Initialization
+        //==========================================================================================
+        // Instantiate robot interface
+        flexiv::rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // set mode after robot is operational
+        robot.SwitchMode(flexiv::rdk::Mode::RT_JOINT_POSITION);
+
+        // Direct joint control can only be executed by single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Set initial joint positions
+        std::map<flexiv::rdk::JointGroup, std::vector<double>> all_init_pos;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : single_arm_groups) {
+            all_init_pos[group] = robot_states.at(group).q;
+            spdlog::info("[{}] Initial joint positions: {}",
+                flexiv::rdk::kJointGroupNames.at(group),
+                flexiv::rdk::utility::Vec2Str(all_init_pos.at(group)));
+        }
+        spdlog::warn(">>>>> Simulated loop delay will be added after 5 seconds <<<<<");
+
+        // Periodic Tasks
+        //==========================================================================================
+        flexiv::rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::cref(single_arm_groups),
+                              std::cref(all_init_pos)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}

thirdparty/flexiv_rdk/thirdparty/build_and_install_dependencies.sh

@@ -0,0 +1,77 @@
+#!/usr/bin/env bash
+# Build and install all dependencies of flexiv_rdk.
+echo ">>>>> Start: flexiv_rdk/thirdparty/build_and_install_dependencies.sh <<<<<"
+
+# Absolute path of this script
+script_dir="$(dirname $(readlink -f $0))"
+set -e
+
+# Check script arguments
+if [ "$#" -lt 1 ]; then
+    echo "Error: invalid script argument"
+    echo "Required argument: [install_directory_path]"
+    echo "    install_directory_path: directory to install all dependencies, should be the same as the install directory of flexiv_rdk"
+    echo "Optional arguments: [num_parallel_jobs] [qnx_toolchain_path]"
+    echo "    num_parallel_jobs: number of parallel jobs used to build, use 4 if not specified"
+    echo "    qnx_toolchain_path: path to the QNX toolchain file under cmake/qnx/, required if building for QNX"
+    exit
+fi
+
+# Get dependencies install directory from script argument, should be the same as the install directory of flexiv_rdk
+export INSTALL_DIR=$1
+echo "Dependencies will be installed to: $INSTALL_DIR"
+
+# Use specified number for parallel build jobs, otherwise use 4
+if [ -n "$2" ] ;then
+    export NUM_JOBS=$2
+else
+    export NUM_JOBS=4
+fi
+echo "Number of parallel build jobs: $NUM_JOBS"
+
+# Set shared cmake arguments
+SHARED_CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Release \
+                   -DBUILD_SHARED_LIBS=ON \
+                   -DCMAKE_POSITION_INDEPENDENT_CODE=ON \
+                   -DCMAKE_PREFIX_PATH=$INSTALL_DIR \
+                   -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \
+                   -DBUILD_TESTING=OFF"
+
+# Building for QNX
+if [ -n "$QNX_TARGET" ]; then
+    # Path to the toolchain file must be set
+    if [ -n "$3" ]; then
+        QNX_TOOLCHAIN=$3
+    else
+        echo "Error: path to QNX toolchain file is not provided"
+        exit 1
+    fi
+    # Append toolchain file to cmake arguments
+    SHARED_CMAKE_ARGS="$SHARED_CMAKE_ARGS -DCMAKE_TOOLCHAIN_FILE=$QNX_TOOLCHAIN"
+    # Extract QNX target architecture from toolchain file name, and set it as an environment variable for later use
+    if [[ "$QNX_TOOLCHAIN" == *"x86_64"* ]]; then
+        export QNX_ARCH="x86_64"
+    elif [[ "$QNX_TOOLCHAIN" == *"aarch64"* ]]; then
+        export QNX_ARCH="aarch64"
+    else
+        echo "Error: cannot determine QNX target architecture"
+        exit 1
+    fi
+    echo "Building for QNX $QNX_ARCH target with toolchain file [$QNX_TOOLCHAIN]"
+fi
+export SHARED_CMAKE_ARGS
+
+# Clone all dependencies in a subfolder
+mkdir -p cloned && cd cloned
+
+# Build and install all dependencies to INSTALL_DIR
+bash $script_dir/scripts/install_eigen.sh
+bash $script_dir/scripts/install_spdlog.sh
+bash $script_dir/scripts/install_tinyxml2.sh
+bash $script_dir/scripts/install_yaml-cpp.sh
+bash $script_dir/scripts/install_zenoh.sh
+bash $script_dir/scripts/install_boost.sh
+bash $script_dir/scripts/install_SpaceVecAlg.sh
+bash $script_dir/scripts/install_RBDyn.sh
+
+echo ">>>>> Finished: flexiv_rdk/thirdparty/build_and_install_dependencies.sh <<<<<"

thirdparty/flexiv_rdk/thirdparty/patches/eigen_qnx802.patch

@@ -0,0 +1,23 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index f3e69b845..fafcdc3a4 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -76,18 +76,6 @@ endmacro()
+ 
+ check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11)
+ 
+-if(EIGEN_TEST_CXX11)
+-  set(CMAKE_CXX_STANDARD 11)
+-  set(CMAKE_CXX_EXTENSIONS OFF)
+-  if(EIGEN_COMPILER_SUPPORT_CPP11)
+-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+-  endif()
+-else()
+-  #set(CMAKE_CXX_STANDARD 03)
+-  #set(CMAKE_CXX_EXTENSIONS OFF)
+-  ei_add_cxx_compiler_flag("-std=c++03")
+-endif()
+-
+ # Determine if we should build shared libraries on this platform.
+ get_cmake_property(EIGEN_BUILD_SHARED_LIBS TARGET_SUPPORTS_SHARED_LIBS)
+ 

thirdparty/flexiv_rdk/thirdparty/scripts/install_RBDyn.sh

@@ -0,0 +1,29 @@
+#!/usr/bin/env bash
+# Depends on: tinyxml2, yaml-cpp, Eigen3, SpaceVecAlg
+set -e
+repo="RBDyn"
+echo "Installing $repo"
+
+# https://github.com/jrl-umi3218/RBDyn/releases/tag/v1.9.3
+ver_tag=v1.9.3
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone --recurse-submodules https://github.com/jrl-umi3218/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS \
+         -DPYTHON_BINDING=OFF \
+         -DBoost_USE_STATIC_LIBS=OFF \
+         -DCXX_DISABLE_WERROR=ON
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"

thirdparty/flexiv_rdk/thirdparty/scripts/install_SpaceVecAlg.sh

@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+set -e
+repo="SpaceVecAlg"
+echo "Installing $repo"
+
+# https://github.com/jrl-umi3218/SpaceVecAlg/releases/tag/v1.2.8
+ver_tag=v1.2.8
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone --recurse-submodules https://github.com/jrl-umi3218/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS -DPYTHON_BINDING=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"

thirdparty/flexiv_rdk/thirdparty/scripts/install_boost.sh

@@ -0,0 +1,66 @@
+#!/usr/bin/env bash
+set -e
+repo="boost"
+echo "Installing $repo"
+
+# Function to check if running on Windows
+is_windows() {
+  [[ "$OSTYPE" == "msys"* || "$OSTYPE" == "cygwin"* || "$OSTYPE" == "mingw"* || "$OSTYPE" == "win32"* ]]
+}
+
+# Default version on Ubuntu 24.04
+# Also the minimum version compatible with MSVC v143
+ver_tag=1.83.0
+ver_str=1_83_0
+
+# Download source code, 1.83 
+if [ ! -d ${repo}_${ver_str} ] ; then
+  # download is faster than clone
+  URL="https://archives.boost.io/release/$ver_tag/source/${repo}_${ver_str}.tar.bz2"
+  echo "-- Downloading: $URL"
+  if is_windows; then
+    curl -L -o ${repo}_${ver_str}.tar.bz2 $URL
+  else
+    wget $URL --no-clobber --quiet --show-progress --progress=bar:force 2>&1
+  fi
+  # Unzip
+  echo "-- Extracting: ${repo}_${ver_str}.tar.bz2"
+  tar --bzip2 -xf "${repo}_${ver_str}.tar.bz2"
+  cd ${repo}_${ver_str}
+else
+  cd ${repo}_${ver_str}
+fi
+
+# Bootstrap differently on Unix and Windows
+if is_windows; then
+  # Windows
+  ./bootstrap.bat
+else
+  # Unix
+  ./bootstrap.sh
+fi
+
+# Build differently on QNX and non-QNX
+if [ -n "$QNX_TARGET" ]; then
+  if [ $QNX_ARCH == "x86_64" ]; then
+    # Build for x86_64 target
+    echo "Building $repo for QNX x86_64"
+    ./b2 -j$NUM_JOBS --prefix=$INSTALL_DIR --with-system --with-filesystem variant=release link=shared \
+    toolset=qcc target-os=qnx architecture=x86 address-model=64 \
+    cxxflags="-Vgcc_ntox86_64" linkflags="-Vgcc_ntox86_64" install
+  elif [ $QNX_ARCH == "aarch64" ]; then
+    # Build for aarch64 target
+    echo "Building $repo for QNX aarch64"
+    ./b2 -j$NUM_JOBS --prefix=$INSTALL_DIR --with-system --with-filesystem variant=release link=shared \
+    toolset=qcc target-os=qnx architecture=arm address-model=64 \
+    cxxflags="-Vgcc_ntoaarch64le" linkflags="-Vgcc_ntoaarch64le" install
+  else
+    echo "Error: unknown QNX platform architecture"
+    exit 1
+  fi
+else
+  # Build for non-QNX
+  ./b2 -j$NUM_JOBS --prefix=$INSTALL_DIR --with-system --with-filesystem variant=release link=shared install
+fi
+
+echo "Installed $repo"

thirdparty/flexiv_rdk/thirdparty/scripts/install_eigen.sh

@@ -0,0 +1,32 @@
+#!/usr/bin/env bash
+set -e
+repo="eigen"
+echo "Installing $repo"
+script_dir="$(dirname $(readlink -f $0))"
+
+# Default version on Ubuntu 22.04
+ver_tag=3.4.0
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone https://gitlab.com/libeigen/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Apply patch if building for QNX
+git reset --hard
+if [ -n "$QNX_TARGET" ]; then
+  git apply $script_dir/../patches/eigen_qnx802.patch
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"

thirdparty/flexiv_rdk/thirdparty/scripts/install_spdlog.sh

@@ -0,0 +1,27 @@
+#!/usr/bin/env bash
+set -e
+repo="spdlog"
+echo "Installing $repo"
+
+# Default version on Ubuntu 22.04
+ver_tag=v1.9.2
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone https://github.com/gabime/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS \
+         -DSPDLOG_BUILD_EXAMPLE=OFF \
+         -DSPDLOG_BUILD_TESTS=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"

thirdparty/flexiv_rdk/thirdparty/scripts/install_tinyxml2.sh

@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+set -e
+repo="tinyxml2"
+echo "Installing $repo"
+
+# Default version on Ubuntu 22.04
+ver_tag=9.0.0
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone https://github.com/leethomason/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"

thirdparty/flexiv_rdk/thirdparty/scripts/install_yaml-cpp.sh

@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+set -e
+repo="yaml-cpp"
+echo "Installing $repo"
+
+# Default version on Ubuntu 22.04
+ver_tag=yaml-cpp-0.7.0
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone https://github.com/jbeder/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS -DYAML_CPP_BUILD_TESTS=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"

thirdparty/flexiv_rdk/thirdparty/scripts/install_zenoh.sh

@@ -0,0 +1,141 @@
+#!/usr/bin/env bash
+set -e
+
+# Version for both zenoh-c and zenoh-cpp
+ver_tag="1.9.0"
+
+# Root directory of the script
+root_dir="$(pwd)"
+
+is_windows() {
+  [[ "$OSTYPE" == "msys"* || "$OSTYPE" == "cygwin"* || "$OSTYPE" == "mingw"* || "$OSTYPE" == "win32"* ]]
+}
+
+# Install rust toolchain if cargo is missing.
+ensure_cargo() {
+  # 1) In Windows shells, cargo may be installed under USERPROFILE.
+  # Prepend that location so command lookup can find cargo/rustup immediately.
+  if is_windows && [ -n "$USERPROFILE" ] && command -v cygpath >/dev/null 2>&1; then
+    export PATH="$(cygpath "$USERPROFILE")/.cargo/bin:$PATH"
+  fi
+
+  # 2) Load rustup's env script when present so current shell gets updated PATH.
+  if [ -f "$HOME/.cargo/env" ]; then
+    # shellcheck disable=SC1090
+    source "$HOME/.cargo/env"
+  fi
+
+  # 3) Fast path: cargo already available, nothing else to do.
+  if command -v cargo >/dev/null 2>&1; then
+    echo "Found cargo: $(command -v cargo)"
+    return
+  fi
+
+  echo "cargo is not found, installing rustup and cargo"
+
+  if ! command -v curl >/dev/null 2>&1; then
+    echo "Error: curl is required to install cargo via rustup"
+    exit 1
+  fi
+
+  # 4) Install rustup/cargo non-interactively.
+  curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y
+
+  # 5) Re-load shell env generated by rustup in this same process.
+  if [ -f "$HOME/.cargo/env" ]; then
+    # shellcheck disable=SC1090
+    source "$HOME/.cargo/env"
+  fi
+
+  # 6) Add common cargo bin paths explicitly to handle shell/env variations.
+  export PATH="$HOME/.cargo/bin:$PATH"
+  if is_windows && [ -n "$USERPROFILE" ] && command -v cygpath >/dev/null 2>&1; then
+    export PATH="$(cygpath "$USERPROFILE")/.cargo/bin:$PATH"
+  fi
+
+  # 7) Validate installation before continuing build steps.
+  if ! command -v cargo >/dev/null 2>&1; then
+    echo "Error: cargo installation failed"
+    exit 1
+  fi
+
+  echo "Installed cargo: $(command -v cargo)"
+}
+
+# For Windows builds, force rustup/cargo to use the MSVC toolchain/target.
+# This avoids GNU-target requirements (for example dlltool.exe) during zenoh build.
+configure_rust_windows_toolchain() {
+  if ! is_windows; then
+    return
+  fi
+
+  local rust_msvc_toolchain="stable-x86_64-pc-windows-msvc"
+  export RUSTUP_TOOLCHAIN="$rust_msvc_toolchain"
+  export CARGO_BUILD_TARGET="x86_64-pc-windows-msvc"
+
+  if command -v rustup >/dev/null 2>&1; then
+    rustup toolchain install "$rust_msvc_toolchain" >/dev/null
+    if command -v rustup.exe >/dev/null 2>&1; then
+      rustup.exe toolchain install "$rust_msvc_toolchain" >/dev/null
+    fi
+  else
+    echo "Warning: rustup not found in PATH; cannot enforce MSVC Rust toolchain"
+  fi
+}
+
+# Install zenoh-c backend
+install_zenoh_c() {
+  local repo="zenoh-c"
+  echo "Installing $repo ($ver_tag)"
+  cd "$root_dir"
+
+  if [ ! -d "$repo" ]; then
+    git clone https://github.com/eclipse-zenoh/$repo.git --branch $ver_tag
+    cd "$repo"
+  else
+    cd "$repo"
+    git fetch --all
+    git checkout $ver_tag
+  fi
+
+  mkdir -p build && cd build
+  cmake .. $SHARED_CMAKE_ARGS \
+           -DBUILD_SHARED_LIBS=ON \
+           -DZENOHC_BUILD_WITH_UNSTABLE_API=ON
+
+  cmake --build . --target install --config Release -j $NUM_JOBS
+
+  echo "Installed $repo"
+}
+
+# Install zenoh-cpp aligned to the same version as zenoh-c
+install_zenoh_cpp() {
+  local repo="zenoh-cpp"
+  echo "Installing $repo ($ver_tag)"
+  cd "$root_dir"
+
+  if [ ! -d "$repo" ]; then
+    git clone https://github.com/eclipse-zenoh/$repo.git --branch $ver_tag
+    cd "$repo"
+  else
+    cd "$repo"
+    git fetch --all
+    git checkout $ver_tag
+  fi
+
+  mkdir -p build && cd build
+  cmake .. $SHARED_CMAKE_ARGS \
+           -DBUILD_SHARED_LIBS=ON \
+           -DZENOHCXX_ENABLE_TESTS=OFF \
+           -DZENOHCXX_ENABLE_EXAMPLES=OFF \
+           -DZENOHCXX_EXAMPLES_PROTOBUF=OFF
+
+  cmake --build . --target install --config Release -j $NUM_JOBS
+
+  echo "Installed $repo"
+}
+
+ensure_cargo
+configure_rust_windows_toolchain
+install_zenoh_c
+install_zenoh_cpp