orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

克隆下来必须带上正在用的视觉权重和本机 site.yaml，否则感知只能指到一台机器上的路径，别人起不来。

Browse Source 
顶视迁臂改走厂家 MoveL，D405 框心对准后再对孔；夹爪停在 caught 时 open 不再把相同目标当成已经张开，init 会真的回零。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-12 17:08:43 +08:00
parent b903161113
commit f882c4309c
187 changed files with 22898 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
thirdparty/flexiv_rdk/.clang-format
Vendored
+65
View File
@@ -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
+6
View File
@@ -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
Vendored
+39
View File
@@ -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
Vendored
+29
View File
@@ -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
Vendored
+35
View File
@@ -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
Vendored
+34
View File
@@ -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
Vendored
+37
View File
@@ -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
Vendored
+50
View File
@@ -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
Vendored
+41
View File
@@ -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
+13
View File
@@ -0,0 +1,13 @@
.vscode
.vs
.venv
.DS_Store
*.a
*.so
*.lib
*.dll
config.h
build*/
cloned/
html/
__pycache__
thirdparty/flexiv_rdk/CMakeLists.txt
Vendored
+140
View File
@@ -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/COLCON_IGNORE
Vendored
View File
thirdparty/flexiv_rdk/LICENSE
Vendored
+211
View File
@@ -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
Vendored
+213
View File
@@ -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
Vendored
+103
View File
@@ -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
Vendored
+14
View File
@@ -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
Vendored
+70
View File
@@ -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
Vendored
+70
View File
@@ -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/doc/Doxyfile.in
Vendored
+2665
View File
File diff suppressed because it is too large Load Diff
thirdparty/flexiv_rdk/doc/flexiv_logo.png
Vendored
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.8 KiB

thirdparty/flexiv_rdk/example/CMakeLists.txt
Vendored
+59
View File
@@ -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
Vendored
+120
View File
@@ -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
Vendored
+72
View File
@@ -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
Vendored
+207
View File
@@ -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
Vendored
+152
View File
@@ -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
Vendored
+123
View File
@@ -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
Vendored
+250
View File
@@ -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
Vendored
+168
View File
@@ -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
Vendored
+129
View File
@@ -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
Vendored
+97
View File
@@ -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
Vendored
+199
View File
@@ -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
Vendored
+226
View File
@@ -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
Vendored
+220
View File
@@ -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
Vendored
+167
View File
@@ -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
Vendored
+346
View File
@@ -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
Vendored
+335
View File
@@ -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
Vendored
+158
View File
@@ -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
Vendored
+409
View File
@@ -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
Vendored
+156
View File
@@ -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
Vendored
+62
View File
@@ -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
Vendored
+223
View File
@@ -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
Vendored
+147
View File
@@ -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
Vendored
+112
View File
@@ -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
Vendored
+230
View File
@@ -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
Vendored
+159
View File
@@ -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
Vendored
+129
View File
@@ -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
Vendored
+154
View File
@@ -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
Vendored
+177
View File
@@ -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
Vendored
+280
View File
@@ -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
Vendored
+300
View File
@@ -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
Vendored
+132
View File
@@ -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
Vendored
+109
View File
@@ -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
Vendored
+896
View File
@@ -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
Vendored
+114
View File
@@ -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
Vendored
+128
View File
@@ -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
Vendored
+182
View File
@@ -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
Vendored
+64
View File
@@ -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
Vendored
+126
View File
@@ -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
Vendored
+254
View File
@@ -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
Vendored
+896
View File
@@ -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
Vendored
+168
View File
@@ -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
Vendored
+113
View File
@@ -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
Vendored
+187
View File
@@ -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
Vendored
+245
View File
@@ -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
Vendored
+105
View File
@@ -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
Vendored
+5
View File
@@ -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
Vendored
+1
View File
@@ -0,0 +1 @@
8159b1304cd25f4cbde0aafc37d8e41ea8edf77804b0680cee5abf9442ba7557
thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a.sha256
Vendored
+1
View File
@@ -0,0 +1 @@
d1267bfa2f54b30e044c92c5468bdfdeb80126fc99e22d1a9623e73315b070a7
thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-qnx.a.sha256
Vendored
+1
View File
@@ -0,0 +1 @@
temporarily_suspended
thirdparty/flexiv_rdk/lib/libflexiv_rdk.arm64-darwin.a.sha256
Vendored
+1
View File
@@ -0,0 +1 @@
24b0f511b6679d935c6d26e7dab512e534b80da32583699e4b1f2872f10eabde
thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a.sha256
Vendored
+1
View File
@@ -0,0 +1 @@
f5a0aab1fe2195ac28fd10764c6a3deb81d25789d7b04acb1e87e029f6924aea
thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-qnx.a.sha256
Vendored
+1
View File
@@ -0,0 +1 @@
temporarily_suspended
thirdparty/flexiv_rdk/llms.txt
Vendored
+215
View File
@@ -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
Vendored
+77
View File
@@ -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
Vendored
+256
View File
@@ -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
Vendored
+278
View File
@@ -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
Vendored
+175
View File
@@ -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
Vendored
+68
View File
@@ -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
Vendored
+56
View File
@@ -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
Vendored
+37
View File
@@ -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
Vendored
+148
View File
@@ -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
Vendored
+160
View File
@@ -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
Vendored
+77
View File
@@ -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
Vendored
+23
View File
@@ -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
Vendored
+29
View File
@@ -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
Vendored
+25
View File
@@ -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
Vendored
+66
View File
@@ -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
Vendored
+32
View File
@@ -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
Vendored
+27
View File
@@ -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
Vendored
+25
View File
@@ -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
Vendored
+25
View File
@@ -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
Vendored
+141
View File
@@ -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