├── codecov.yml ├── devcontainer_distro ├── docs ├── migration_notes.rst ├── assets │ ├── control.png │ ├── rt-loop.png │ ├── pop_up_fci.png │ ├── rt-interfaces.png │ ├── edit-connections.png │ ├── static-ip-ubuntu.png │ ├── FrankaUI_Settings_Bar.png │ ├── libfranka-architecture.png │ ├── FrankaUI_Settings_Network.png │ ├── FrankaUI_System_ActivateFCI.png │ ├── fci-architecture-non-realtime.png │ ├── FrankaUI_System_ActivateFCI_Done.png │ ├── FrankaUI_System_ActivateFCI_Confirm.png │ ├── example_cmake_lists.txt │ └── example_main.cpp ├── libfranka_architecture.rst ├── usage_examples.rst ├── index.rst ├── api_references.rst ├── compatibility_with_images.rst ├── network_requirements.rst └── system_requirements.rst ├── pylibfranka ├── docs │ ├── requirements.txt │ ├── build.sh │ ├── Makefile │ ├── api │ │ └── gripper.rst │ ├── index.rst │ ├── class_hierarchy.rst │ ├── modules.rst │ ├── class_list.rst │ └── conf.py ├── scripts │ ├── lint_code.sh │ ├── build_package.sh │ ├── get_version.sh │ ├── setup_dependencies.sh │ ├── repair_wheels.sh │ └── test_installation.sh ├── CMakeLists.txt ├── include │ └── pygripper.h ├── __init__.py └── examples │ ├── move_gripper.py │ ├── cartesian_pose_example.py │ ├── joint_velocity_example.py │ └── joint_position_example.py ├── .clang-format ├── .gitmodules ├── doc ├── Logo_FRANKA_ROBOTICS_dark.png ├── main.md └── CMakeLists.txt ├── scripts ├── fail-on-output.sh ├── format-check.sh └── mimic-jenkins.bash ├── requirements.txt ├── include └── franka │ ├── commands │ └── get_robot_model_command.hpp │ ├── async_control │ └── target_status.hpp │ ├── logging │ ├── cout_logging_sink.hpp │ ├── logging_sink_interface.hpp │ ├── robot_state_log.hpp │ └── logger.hpp │ ├── gripper_state.h │ ├── active_torque_control.h │ ├── vacuum_gripper_state.h │ ├── active_motion_generator.h │ ├── lowpass_filter.h │ ├── exception.h │ └── active_control.h ├── .ci ├── libonly.sh ├── lint.sh ├── debug.sh ├── coverage.sh ├── release.sh ├── urdfdom.patch ├── pinocchio.patch └── checkgithistory.sh ├── .gitignore ├── test ├── mock_robot.h ├── test_utils.h ├── gripper_tests.cpp ├── vacuum_gripper_tests.cpp ├── errors_tests.cpp ├── mock_robot_control.h ├── mock_robot_impl.h ├── lowpass_filter_tests.cpp └── duration_tests.cpp ├── examples ├── utility_examples │ ├── CMakeLists.txt │ └── logging_example.cpp ├── echo_robot_state.cpp ├── print_joint_poses.cpp ├── vacuum_object.cpp ├── grasp_object.cpp ├── joint_point_to_point_motion.cpp ├── CMakeLists.txt ├── examples_common.h ├── generate_joint_velocity_motion.cpp ├── generate_elbow_motion.cpp ├── generate_cartesian_pose_motion.cpp ├── generate_joint_position_motion.cpp ├── generate_consecutive_motions.cpp ├── generate_joint_velocity_motion_external_control_loop.cpp └── generate_cartesian_pose_motion_external_control_loop.cpp ├── cmake ├── FindEigen3.cmake ├── FrankaConfig.cmake.in ├── FetchFMT.cmake ├── SetupGoogleTest.cmake ├── FindPoco.cmake ├── SetVersionFromGit.cmake └── FindTinyXML2.cmake ├── NOTICE ├── .devcontainer ├── docker-compose.yml └── devcontainer.json ├── src ├── gripper_state.cpp ├── platform.h ├── active_torque_control.cpp ├── logging │ ├── cout_logging_sink.cpp │ ├── robot_state_logger.hpp │ ├── logger.cpp │ ├── robot_state_logger.cpp │ └── robot_state_log.cpp ├── load_calculations.h ├── motion_generator_traits.h ├── active_control.cpp ├── vacuum_gripper_state.cpp ├── exception.cpp ├── active_motion_generator.cpp ├── control_tools.cpp ├── lowpass_filter.cpp ├── network.cpp ├── duration.cpp ├── gripper.cpp └── load_calculations.cpp ├── setup.cfg ├── package.xml ├── .pre-commit-config.yaml ├── .github └── workflows │ ├── doxygen.yml │ ├── pylibfranka-docs.yml │ └── libfranka-build.yml ├── pyproject.toml └── .clang-tidy /codecov.yml: -------------------------------------------------------------------------------- 1 | comment: off 2 | -------------------------------------------------------------------------------- /devcontainer_distro: -------------------------------------------------------------------------------- 1 | 22.04 2 | -------------------------------------------------------------------------------- /docs/migration_notes.rst: -------------------------------------------------------------------------------- 1 | .. _migration_notes: 2 | Migration Notes 3 | --------------- 4 | -------------------------------------------------------------------------------- /pylibfranka/docs/requirements.txt: -------------------------------------------------------------------------------- 1 | sphinx>=5.0 2 | pydata-sphinx-theme>=0.14.0 3 | numpy 4 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Chromium 2 | Standard: Cpp11 3 | SortIncludes: true 4 | ColumnLimit: 100 5 | -------------------------------------------------------------------------------- /docs/assets/control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/control.png -------------------------------------------------------------------------------- /docs/assets/rt-loop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/rt-loop.png -------------------------------------------------------------------------------- /docs/assets/pop_up_fci.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/pop_up_fci.png -------------------------------------------------------------------------------- /docs/assets/rt-interfaces.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/rt-interfaces.png -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "common"] 2 | path = common 3 | url = https://github.com/frankarobotics/libfranka-common.git 4 | -------------------------------------------------------------------------------- /doc/Logo_FRANKA_ROBOTICS_dark.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/doc/Logo_FRANKA_ROBOTICS_dark.png -------------------------------------------------------------------------------- /docs/assets/edit-connections.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/edit-connections.png -------------------------------------------------------------------------------- /docs/assets/static-ip-ubuntu.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/static-ip-ubuntu.png -------------------------------------------------------------------------------- /docs/assets/FrankaUI_Settings_Bar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/FrankaUI_Settings_Bar.png -------------------------------------------------------------------------------- /docs/assets/libfranka-architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/libfranka-architecture.png -------------------------------------------------------------------------------- /docs/libfranka_architecture.rst: -------------------------------------------------------------------------------- 1 | .. _libfranka_architecture: 2 | Library Architecture 3 | -------------------- 4 | 5 | Coming soon ... 6 | -------------------------------------------------------------------------------- /scripts/fail-on-output.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | output="$($@)" 3 | if [[ "$output" ]]; then 4 | echo "$output" 5 | exit 1 6 | fi 7 | -------------------------------------------------------------------------------- /docs/assets/FrankaUI_Settings_Network.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/FrankaUI_Settings_Network.png -------------------------------------------------------------------------------- /docs/assets/FrankaUI_System_ActivateFCI.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/FrankaUI_System_ActivateFCI.png -------------------------------------------------------------------------------- /docs/assets/fci-architecture-non-realtime.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/fci-architecture-non-realtime.png -------------------------------------------------------------------------------- /docs/assets/FrankaUI_System_ActivateFCI_Done.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/FrankaUI_System_ActivateFCI_Done.png -------------------------------------------------------------------------------- /docs/assets/FrankaUI_System_ActivateFCI_Confirm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/frankarobotics/libfranka/HEAD/docs/assets/FrankaUI_System_ActivateFCI_Confirm.png -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy>=1.19.0 2 | setuptools>=61.0 3 | wheel 4 | pybind11>=2.6.0 5 | numpy>=1.19.0 6 | patchelf>=0.10 7 | auditwheel>=6.0.0 8 | build>=0.10 9 | -------------------------------------------------------------------------------- /include/franka/commands/get_robot_model_command.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace franka { 6 | 7 | struct GetRobotModelResult { 8 | std::string robot_model_urdf; 9 | }; 10 | 11 | } // namespace franka 12 | -------------------------------------------------------------------------------- /.ci/libonly.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | set -e 4 | set -x 5 | 6 | rm -rf build-libonly 7 | mkdir build-libonly 8 | cd build-libonly 9 | cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_COVERAGE=OFF -DBUILD_DOCUMENTATION=OFF \ 10 | -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF .. 11 | cmake --build . 12 | -------------------------------------------------------------------------------- /.ci/lint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | set -e 4 | set -x 5 | 6 | rm -rf build-lint 7 | mkdir build-lint 8 | cd build-lint 9 | cmake -DBUILD_COVERAGE=OFF -DBUILD_DOCUMENTATION=OFF -DBUILD_EXAMPLES=ON -DBUILD_TESTS=OFF .. 10 | cmake --build . --target check-format 11 | cmake --build . --target check-tidy 12 | -------------------------------------------------------------------------------- /.ci/debug.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | set -e 4 | set -x 5 | 6 | rm -rf build-debug 7 | mkdir build-debug 8 | cd build-debug 9 | cmake -DCMAKE_BUILD_TYPE=Debug -DSTRICT=ON -DBUILD_COVERAGE=OFF -DBUILD_DOCUMENTATION=OFF \ 10 | -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. 11 | cmake --build . 12 | ctest -V 13 | -------------------------------------------------------------------------------- /docs/assets/example_cmake_lists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(MyRobotProject) 3 | 4 | # Find libfranka 5 | find_package(Franka REQUIRED) 6 | 7 | # Create executable 8 | add_executable(${PROJECT_NAME} src/main.cpp) 9 | 10 | # Link libfranka 11 | target_link_libraries(${PROJECT_NAME} Franka::Franka) 12 | -------------------------------------------------------------------------------- /.ci/coverage.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | set -e 4 | set -x 5 | 6 | rm -rf build-coverage 7 | mkdir build-coverage 8 | cd build-coverage 9 | cmake -DCMAKE_BUILD_TYPE=Debug -DBUILD_COVERAGE=ON -DBUILD_DOCUMENTATION=OFF \ 10 | -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=ON .. 11 | cmake --build . 12 | cmake --build . --target coverage 13 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build*/ 2 | cmake-build-*/ 3 | /3rdparty/* 4 | .vs*/ 5 | install*/ 6 | *.user 7 | .cache 8 | .env 9 | *.egg-info 10 | __pycache__/ 11 | *.so 12 | *.dylib 13 | *.a 14 | *.lib 15 | *.exp 16 | *.def 17 | *.manifest 18 | *.res 19 | *.egg-info 20 | *.whl 21 | .mypy_cache/ 22 | version_info.properties 23 | pylibfranka/docs/_build/ 24 | pylibfranka/_version.py 25 | -------------------------------------------------------------------------------- /test/mock_robot.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | 7 | using namespace franka; 8 | 9 | class RobotMock : public Robot { 10 | public: 11 | ~RobotMock() = default; 12 | RobotMock(std::shared_ptr robot_impl) : Robot(robot_impl) {}; 13 | }; 14 | -------------------------------------------------------------------------------- /include/franka/async_control/target_status.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #pragma once 5 | 6 | namespace franka { 7 | 8 | /** 9 | * Describes the current status of the asynchronous control. 10 | */ 11 | enum class TargetStatus { kIdle, kExecuting, kTargetReached, kAborted }; 12 | 13 | } // namespace franka 14 | -------------------------------------------------------------------------------- /docs/usage_examples.rst: -------------------------------------------------------------------------------- 1 | .. _libfranka_usage_examples: 2 | Usage Examples 3 | -------------------- 4 | 5 | The following examples demonstrate how to use libfranka in both C++ and Python: 6 | 7 | - **C++ Example Programs:** `C++ Example Programs `_ 8 | - **Python Example Programs:** `Python Example Programs `_ 9 | -------------------------------------------------------------------------------- /.ci/release.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | set -e 4 | set -x 5 | 6 | rm -rf build-release 7 | mkdir build-release 8 | cd build-release 9 | cmake -DCMAKE_BUILD_TYPE=Release -DSTRICT=ON -DBUILD_COVERAGE=OFF -DBUILD_DOCUMENTATION=ON \ 10 | -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. 11 | cmake --build . 12 | ctest -V 13 | cpack 14 | 15 | cd .. 16 | rm -rf build-examples 17 | mkdir build-examples 18 | cd build-examples 19 | cmake -DFranka_DIR:PATH=../build-release ../examples 20 | cmake --build . 21 | -------------------------------------------------------------------------------- /doc/main.md: -------------------------------------------------------------------------------- 1 | # libfranka: C++ library for Franka Robotics research robots 2 | 3 | With this library, you can control research versions of Franka Robotics robots. See the [Franka Control Interface (FCI) documentation][fci-docs] for more information about what `libfranka` can do and how to set it up. 4 | 5 | ## License 6 | 7 | `libfranka` is licensed under the [Apache 2.0 license][apache-2.0]. 8 | 9 | [apache-2.0]: https://www.apache.org/licenses/LICENSE-2.0.html 10 | [fci-docs]: https://frankarobotics.github.io/docs 11 | -------------------------------------------------------------------------------- /examples/utility_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(EXAMPLES 2 | logging_example 3 | ) 4 | 5 | foreach(example ${EXAMPLES}) 6 | add_executable(${example} ${example}.cpp) 7 | target_include_directories(${example} PUBLIC 8 | $ 9 | $ 10 | ) 11 | target_link_libraries(${example} Franka::Franka examples_common fmt::fmt) 12 | 13 | install(TARGETS ${example} 14 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 15 | ) 16 | endforeach() 17 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | find_package(Eigen3 CONFIG) 2 | mark_as_advanced(FORCE Eigen3_DIR) 3 | 4 | include(FindPackageHandleStandardArgs) 5 | find_package_handle_standard_args(Eigen3 6 | FOUND_VAR Eigen3_FOUND 7 | REQUIRED_VARS EIGEN3_INCLUDE_DIRS 8 | ) 9 | 10 | if(NOT TARGET Eigen3::Eigen3) 11 | add_library(Eigen3::Eigen3 INTERFACE IMPORTED) 12 | set_target_properties(Eigen3::Eigen3 PROPERTIES 13 | INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIRS} 14 | INTERFACE_COMPILE_DEFINITIONS "${EIGEN3_DEFINITIONS}" 15 | ) 16 | endif() 17 | -------------------------------------------------------------------------------- /cmake/FrankaConfig.cmake.in: -------------------------------------------------------------------------------- 1 | get_filename_component(Franka_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 2 | 3 | set(THREADS_PREFER_PTHREAD_FLAG ON) 4 | find_package(Threads REQUIRED) 5 | 6 | find_package(fmt REQUIRED) 7 | 8 | if(NOT TARGET Franka::Franka) 9 | include("${Franka_CMAKE_DIR}/FrankaTargets.cmake") 10 | endif() 11 | 12 | # It is recommended to use target_link_libraries( Franka::Franka) instead. 13 | set(Franka_LIBRARIES Franka::Franka) 14 | get_target_property(Franka_INCLUDE_DIRS Franka::Franka INTERFACE_INCLUDE_DIRECTORIES) 15 | -------------------------------------------------------------------------------- /pylibfranka/scripts/lint_code.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | echo "Running code linting..." 5 | 6 | # Ensure user-level Python packages are in PATH 7 | export PATH=$HOME/.local/bin:$PATH 8 | 9 | # Install flake8 if not already installed 10 | if ! command -v flake8 &> /dev/null; then 11 | echo "Installing flake8..." 12 | python3 -m pip install --user flake8 13 | fi 14 | 15 | # Run linting (allow it to fail without stopping the build) 16 | echo "Running flake8 on pylibfranka and examples..." 17 | flake8 pylibfranka examples || echo "Linting issues found but continuing" 18 | 19 | echo "Linting complete!" 20 | -------------------------------------------------------------------------------- /pylibfranka/scripts/build_package.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | echo "Building package..." 5 | 6 | # Ensure user-level Python packages are in PATH 7 | export PATH=$HOME/.local/bin:$PATH 8 | 9 | # Set up CMake for pybind11 10 | export pybind11_DIR=/usr/lib/cmake/pybind11 11 | 12 | # Install the package in development mode 13 | echo "Installing package..." 14 | pip install . 15 | 16 | # Build the wheel 17 | echo "Building wheel..." 18 | python3 -m build --wheel 19 | 20 | # Create wheelhouse directory 21 | mkdir -p wheelhouse 22 | 23 | echo "Package build complete!" 24 | echo "Built wheels:" 25 | ls -la dist/*.whl 26 | -------------------------------------------------------------------------------- /NOTICE: -------------------------------------------------------------------------------- 1 | libfranka 2 | 3 | Copyright 2023 Franka Robotics GmbH 4 | 5 | Licensed under the Apache License, Version 2.0 (the "License"); 6 | you may not use this file except in compliance with the License. 7 | You may obtain a copy of the License at 8 | 9 | http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | Unless required by applicable law or agreed to in writing, software 12 | distributed under the License is distributed on an "AS IS" BASIS, 13 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | See the License for the specific language governing permissions and 15 | limitations under the License. 16 | -------------------------------------------------------------------------------- /pylibfranka/scripts/get_version.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # Get Package Version from CMakeLists.txt (single source of truth) 5 | echo "Getting package version from CMakeLists.txt..." 6 | PACKAGE_VERSION=$(grep -oP 'set\(libfranka_VERSION\s+\K[\d.]+' CMakeLists.txt) 7 | 8 | if [ -z "$PACKAGE_VERSION" ]; then 9 | echo "Error: Could not extract version from CMakeLists.txt" 10 | exit 1 11 | fi 12 | 13 | echo "Package version: $PACKAGE_VERSION" 14 | 15 | # Export for use in pipelines 16 | echo "PACKAGE_VERSION=$PACKAGE_VERSION" >> version_info.properties 17 | export PACKAGE_VERSION 18 | echo "PACKAGE_VERSION=$PACKAGE_VERSION" 19 | -------------------------------------------------------------------------------- /.devcontainer/docker-compose.yml: -------------------------------------------------------------------------------- 1 | services: 2 | libfranka_project: 3 | build: 4 | context: ../.ci/ 5 | dockerfile: Dockerfile 6 | args: 7 | USER_UID: ${USER_UID:-1000} 8 | USER_GID: ${USER_GID:-1000} 9 | UBUNTU_VERSION: ${UBUNTU_VERSION:-22.04} 10 | container_name: libfranka-${UBUNTU_VERSION:-22.04} 11 | network_mode: "host" 12 | shm_size: 512m 13 | privileged: true 14 | command: /bin/bash 15 | tty: true 16 | stdin_open: true 17 | volumes: 18 | - ../:/workspaces 19 | cap_add: 20 | - SYS_NICE 21 | ulimits: 22 | rtprio: 99 23 | rttime: -1 24 | memlock: 8428281856 25 | -------------------------------------------------------------------------------- /.ci/urdfdom.patch: -------------------------------------------------------------------------------- 1 | diff --git a/urdf_parser/CMakeLists.txt b/urdf_parser/CMakeLists.txt 2 | index 20a6a05..5577e99 100644 3 | --- a/urdf_parser/CMakeLists.txt 4 | +++ b/urdf_parser/CMakeLists.txt 5 | @@ -3,7 +3,7 @@ macro(add_urdfdom_library) 6 | set(multiValueArgs SOURCES LINK) 7 | cmake_parse_arguments(add_urdfdom_library "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) 8 | 9 | - add_library(${add_urdfdom_library_LIBNAME} SHARED 10 | + add_library(${add_urdfdom_library_LIBNAME} STATIC 11 | ${add_urdfdom_library_SOURCES}) 12 | target_include_directories(${add_urdfdom_library_LIBNAME} PUBLIC 13 | "$" 14 | -------------------------------------------------------------------------------- /src/gripper_state.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include "franka/gripper_state.h" 4 | 5 | #include 6 | 7 | namespace franka { 8 | 9 | std::ostream& operator<<(std::ostream& ostream, const franka::GripperState& gripper_state) { 10 | ostream << "{\"width\": " << gripper_state.width << ", \"max_width\": " << gripper_state.max_width 11 | << ", \"is_grasped\": " << gripper_state.is_grasped 12 | << ", \"temperature\": " << gripper_state.temperature 13 | << ", \"time\": " << gripper_state.time.toSec() << "}"; 14 | return ostream; 15 | } 16 | 17 | } // namespace franka 18 | -------------------------------------------------------------------------------- /pylibfranka/docs/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | DOCS_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" 6 | 7 | # Set locale to avoid locale errors 8 | export LC_ALL=C.UTF-8 9 | export LANG=C.UTF-8 10 | 11 | # Add sphinx to PATH 12 | export PATH="$HOME/.local/bin:$PATH" 13 | 14 | # Navigate to docs directory 15 | cd "$DOCS_DIR" 16 | 17 | # Build documentation 18 | echo "Building documentation..." 19 | make clean 20 | make html 21 | 22 | echo "" 23 | echo "✓ Documentation built successfully!" 24 | echo " Location: $DOCS_DIR/_build/html/index.html" 25 | echo "" 26 | echo "To view:" 27 | echo " ./view_docs.sh" 28 | echo " or open file://$DOCS_DIR/_build/html/index.html in your browser" 29 | 30 | -------------------------------------------------------------------------------- /pylibfranka/docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | 3 | # You can set these variables from the command line, and also 4 | # from the environment for the first two. 5 | SPHINXOPTS ?= 6 | SPHINXBUILD ?= sphinx-build 7 | SOURCEDIR = . 8 | BUILDDIR = _build 9 | 10 | # Put it first so that "make" without argument is like "make help". 11 | help: 12 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 13 | 14 | .PHONY: help Makefile 15 | 16 | # Catch-all target: route all unknown targets to Sphinx using the new 17 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 18 | %: Makefile 19 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 20 | -------------------------------------------------------------------------------- /include/franka/logging/cout_logging_sink.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | 6 | namespace franka { 7 | 8 | /** 9 | * A example logging sink that logs messages to the console. 10 | */ 11 | class CoutLoggingSink : public LoggingSinkInterface { 12 | public: 13 | virtual ~CoutLoggingSink() = default; 14 | 15 | // Inherited via LoggingSinkInterface 16 | auto getName() const -> std::string override; 17 | auto logInfo(const std::string& message) -> void override; 18 | auto logWarn(const std::string& message) -> void override; 19 | auto logError(const std::string& message) -> void override; 20 | }; 21 | 22 | } // namespace franka 23 | -------------------------------------------------------------------------------- /src/platform.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #undef LIBFRANKA_X64 6 | #undef LIBFRANKA_X86 7 | #undef LIBFRANKA_ARM64 8 | #undef LIBFRANKA_ARM 9 | 10 | #if defined(__amd64__) || defined(_M_AMD64) 11 | #define LIBFRANKA_X64 12 | #elif defined(__X86__) || defined(_M_IX86) 13 | #define LIBFRANKA_X86 14 | #elif defined(__aarch64__) || defined(_M_ARM64) 15 | #define LIBFRANKA_ARM64 16 | #elif defined(__arm__) || defined(_M_ARM) 17 | #define LIBFRANKA_ARM 18 | #endif 19 | 20 | #undef LIBFRANKA_WINDOWS 21 | #undef LIBFRANKA_LINUX 22 | 23 | #if defined(_WIN32) || defined(_WIN64) 24 | #define LIBFRANKA_WINDOWS 25 | #elif defined(__unix) || defined(__unix__) 26 | #define LIBFRANKA_LINUX 27 | #endif 28 | -------------------------------------------------------------------------------- /src/active_torque_control.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "robot_impl.h" 9 | 10 | namespace franka { 11 | 12 | void ActiveTorqueControl::writeOnce(const Torques& control_input) { 13 | if (control_finished) { 14 | throw franka::ControlException("writeOnce must not be called after the motion has finished."); 15 | } 16 | 17 | if (control_input.motion_finished) { 18 | robot_impl->finishMotion(motion_id, control_input); 19 | control_finished = true; 20 | control_lock.unlock(); 21 | return; 22 | } 23 | 24 | robot_impl->writeOnce(control_input); 25 | } 26 | 27 | } // namespace franka 28 | -------------------------------------------------------------------------------- /cmake/FetchFMT.cmake: -------------------------------------------------------------------------------- 1 | # Try to find fmt package. If available, take it. Otherwise, try to download. 2 | find_package(fmt QUIET) 3 | if(NOT fmt_FOUND) 4 | message(STATUS "fmt not found. Fetching fmt...") 5 | include(FetchContent) 6 | 7 | FetchContent_Declare( 8 | fmt 9 | GIT_REPOSITORY https://github.com/fmtlib/fmt 10 | GIT_TAG 11.0.2) 11 | FetchContent_GetProperties(fmt) 12 | if(NOT fmt_POPULATED) 13 | FetchContent_Populate(fmt) 14 | add_subdirectory(${fmt_SOURCE_DIR} ${fmt_BINARY_DIR} EXCLUDE_FROM_ALL) 15 | endif() 16 | 17 | set_target_properties(fmt PROPERTIES POSITION_INDEPENDENT_CODE ON) 18 | 19 | if(NOT fmt_POPULATED) 20 | message(FATAL_ERROR "Failed to fetch fmt. Please install via 'apt install libfmt-dev' or visit https://fmt.dev/11.0/get-started/") 21 | endif() 22 | else() 23 | message(STATUS "Found fmt: ${fmt_VERSION}") 24 | endif() 25 | -------------------------------------------------------------------------------- /src/logging/cout_logging_sink.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | 6 | #include 7 | 8 | const std::string kCoutLoggingSinkName = "CoutLoggingSink"; 9 | 10 | namespace franka { 11 | 12 | auto CoutLoggingSink::getName() const -> std::string { 13 | return kCoutLoggingSinkName; 14 | } 15 | 16 | auto CoutLoggingSink::logInfo(const std::string& message) -> void { 17 | std::cout << "INFO: " << message << std::endl; 18 | } 19 | 20 | auto CoutLoggingSink::logWarn(const std::string& message) -> void { 21 | std::cout << "WARNING: " << message << std::endl; 22 | } 23 | 24 | auto CoutLoggingSink::logError(const std::string& message) -> void { 25 | std::cout << "ERROR: " << message << std::endl; 26 | } 27 | 28 | } // namespace franka 29 | -------------------------------------------------------------------------------- /pylibfranka/scripts/setup_dependencies.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | echo "Setting up dependencies..." 5 | 6 | # Update package lists (allow failure) 7 | sudo apt-get update || true 8 | 9 | # Install system dependencies 10 | sudo apt-get install -y libeigen3-dev libpoco-dev pybind11-dev || true 11 | 12 | # Install Python dependencies 13 | echo "Installing Python dependencies..." 14 | python3 -m pip install --user auditwheel setuptools build wheel patchelf pybind11 numpy cmake 15 | 16 | # Add user-level Python bin to PATH 17 | export PATH=$HOME/.local/bin:$PATH 18 | 19 | # Verify auditwheel is available 20 | if command -v auditwheel &> /dev/null; then 21 | echo "auditwheel found at: $(which auditwheel)" 22 | else 23 | echo "WARNING: auditwheel not found in PATH" 24 | fi 25 | 26 | # Set up CMake for pybind11 27 | export pybind11_DIR=/usr/lib/cmake/pybind11 28 | 29 | echo "Dependencies setup complete!" 30 | -------------------------------------------------------------------------------- /.ci/pinocchio.patch: -------------------------------------------------------------------------------- 1 | diff --git a/CMakeLists.txt b/CMakeLists.txt 2 | index df25fb61..2c777f5c 100644 3 | --- a/CMakeLists.txt 4 | +++ b/CMakeLists.txt 5 | @@ -58,6 +58,10 @@ else() 6 | endif() 7 | endif() 8 | 9 | +FIND_PACKAGE(assimp REQUIRED) 10 | +FIND_PACKAGE(console_bridge REQUIRED) 11 | +FIND_PACKAGE(tinyxml2 REQUIRED) 12 | + 13 | set(DOXYGEN_USE_MATHJAX YES) 14 | 15 | # ---------------------------------------------------- 16 | diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt 17 | index 500d657f..4ed70b82 100644 18 | --- a/src/CMakeLists.txt 19 | +++ b/src/CMakeLists.txt 20 | @@ -69,7 +69,7 @@ function(PINOCCHIO_TARGET target_name) 21 | set(LIB_NAME "${target_name}") 22 | 23 | # Manage different scope type if building an interface or shared library 24 | - set(LIBRARY_TYPE SHARED) 25 | + set(LIBRARY_TYPE STATIC) 26 | set(LIBRARY_PUBLIC_SCOPE PUBLIC) 27 | if(ARGS_INTERFACE) 28 | set(LIBRARY_TYPE INTERFACE) 29 | -------------------------------------------------------------------------------- /docs/assets/example_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | int main(int argc, char** argv) { 7 | if (argc != 2) { 8 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 9 | return -1; 10 | } 11 | 12 | try { 13 | // Connect to robot 14 | franka::Robot robot(argv[1]); 15 | 16 | // Read robot state 17 | franka::RobotState state = robot.readOnce(); 18 | 19 | // Print robot information 20 | std::cout << "Successfully connected to robot!" << std::endl; 21 | 22 | // Print end-effector position 23 | std::cout << "End-effector position (x, y, z): " << state.O_T_EE[12] << ", " << state.O_T_EE[13] 24 | << ", " << state.O_T_EE[14] << std::endl; 25 | 26 | return 0; 27 | } catch (const franka::Exception& e) { 28 | std::cerr << "Error: " << e.what() << std::endl; 29 | return -1; 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /src/logging/robot_state_logger.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace franka { 14 | 15 | class RobotStateLogger { 16 | public: 17 | explicit RobotStateLogger(size_t log_size); 18 | 19 | void log(const RobotState& state, const research_interface::robot::RobotCommand& command); 20 | 21 | std::vector flush(); 22 | 23 | private: 24 | std::vector states_; 25 | std::vector commands_; 26 | size_t ring_front_{0}; 27 | size_t ring_size_{0}; 28 | 29 | const size_t log_size_; // NOLINT(readability-identifier-naming) 30 | }; 31 | 32 | } // namespace franka 33 | -------------------------------------------------------------------------------- /docs/index.rst: -------------------------------------------------------------------------------- 1 | libfranka client library 2 | ======================== 3 | 4 | This documentation should help you to get started with libfranka. libfranka is a C++ library that allows you to control a Franka robot from a remote computer. The library is open-source and can be found on `GitHub `_. 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents: 9 | :titlesonly: 10 | 11 | overview 12 | libfranka_architecture 13 | system_requirements 14 | compatibility_with_images 15 | installation 16 | real_time_kernel 17 | getting_started 18 | usage_examples 19 | api_references 20 | migration_notes 21 | 22 | 23 | ChangeLog 24 | --------- 25 | 26 | .. literalinclude:: ../CHANGELOG.md 27 | :language: markdown 28 | :lines: 1-40 29 | 30 | `Full Libfranka CHANGELOG `__ is available on `GitHub `__. 31 | -------------------------------------------------------------------------------- /scripts/format-check.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | # grep the result of clang-format with -output-replacements-xml flag 3 | # if any "replacement" tags are found, then code is incorrectly formatted 4 | CLANG_FORMAT=$1 5 | shift 6 | # skip the -output-replacements parameter 7 | shift 8 | FILES="$@" 9 | 10 | # Array to hold files with formatting issues 11 | FILES_WITH_ISSUES=() 12 | 13 | # Check each file 14 | for file in $FILES; do 15 | # Run clang-format and check if replacements are needed 16 | OUTPUT=$($CLANG_FORMAT -output-replacements-xml "$file") 17 | 18 | # If there are replacements, add the file to the issues array 19 | if [[ $OUTPUT == *" 6 | #include 7 | #include 8 | #include 9 | 10 | namespace franka_test_utils { 11 | 12 | inline std::string readFileToString(const std::string& filename) { 13 | std::ifstream file(filename); 14 | if (!file.is_open()) { 15 | std::cerr << "Error opening file: " << filename << std::endl; 16 | return ""; 17 | } 18 | 19 | std::ostringstream oss; 20 | oss << file.rdbuf(); 21 | file.close(); 22 | 23 | return oss.str(); 24 | } 25 | 26 | // Common constants 27 | constexpr double kEps = 1e-5; 28 | constexpr double kForwardKinematicsEps = 1e-3; 29 | 30 | // Helper to get URDF path from current file 31 | inline std::string getUrdfPath(const std::string& file_path) { 32 | return file_path.substr(0, file_path.find_last_of("/\\") + 1) + "fr3.urdf"; 33 | } 34 | 35 | } // namespace franka_test_utils 36 | -------------------------------------------------------------------------------- /include/franka/logging/logging_sink_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #pragma once 5 | 6 | #include 7 | 8 | namespace franka { 9 | 10 | /** 11 | * Interface for a logging sink. Implement this interface to create a custom logger. 12 | */ 13 | class LoggingSinkInterface { 14 | public: 15 | virtual ~LoggingSinkInterface() = default; 16 | 17 | virtual auto getName() const -> std::string = 0; 18 | 19 | /** 20 | * Logs an info message. 21 | * @param message The message to log. 22 | */ 23 | virtual auto logInfo(const std::string& message) -> void = 0; 24 | 25 | /** 26 | * Logs a warning message. 27 | * @param message The message to log. 28 | */ 29 | virtual auto logWarn(const std::string& message) -> void = 0; 30 | 31 | /** 32 | * Logs an error message. 33 | * @param message The message to log. 34 | */ 35 | virtual auto logError(const std::string& message) -> void = 0; 36 | }; 37 | 38 | } // namespace franka 39 | -------------------------------------------------------------------------------- /cmake/SetupGoogleTest.cmake: -------------------------------------------------------------------------------- 1 | # Try to find gtest package. If available, take it. Otherwise, try to download. 2 | find_package(googletest QUIET) 3 | if(NOT googletest_FOUND) 4 | message(STATUS "googletest not found. Fetching googletest...") 5 | include(FetchContent) 6 | 7 | FetchContent_Declare( 8 | gtest 9 | GIT_REPOSITORY https://github.com/google/googletest 10 | GIT_TAG v1.15.2) 11 | FetchContent_GetProperties(gtest) 12 | if(NOT gtest_POPULATED) 13 | FetchContent_Populate(gtest) 14 | add_subdirectory(${gtest_SOURCE_DIR} ${gtest_BINARY_DIR} EXCLUDE_FROM_ALL) 15 | endif() 16 | 17 | set_target_properties(gtest PROPERTIES POSITION_INDEPENDENT_CODE ON) 18 | 19 | if(NOT gtest_POPULATED) 20 | message(FATAL_ERROR "Failed to fetch googletest. Please install googletest or visit https://github.com/google/googletest") 21 | endif() 22 | else() 23 | message(STATUS "Found googletest: ${gtest_VERSION}") 24 | endif() 25 | 26 | # Prevent overriding the parent project's compiler/linker 27 | # settings on Windows 28 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 29 | -------------------------------------------------------------------------------- /cmake/FindPoco.cmake: -------------------------------------------------------------------------------- 1 | find_package(Poco COMPONENTS ${Poco_FIND_COMPONENTS} CONFIG QUIET) 2 | if(Poco_FOUND) 3 | return() 4 | endif() 5 | 6 | find_path(Poco_INCLUDE_DIR Poco/Poco.h) 7 | mark_as_advanced(FORCE Poco_INCLUDE_DIR) 8 | 9 | foreach(component ${Poco_FIND_COMPONENTS}) 10 | set(component_var "Poco_${component}_LIBRARY") 11 | find_library(${component_var} Poco${component}) 12 | mark_as_advanced(FORCE ${component_var}) 13 | if(${component_var}) 14 | set(Poco_${component}_FOUND TRUE) 15 | list(APPEND Poco_LIBRARIES ${component}) 16 | if(NOT TARGET Poco::${component}) 17 | add_library(Poco::${component} SHARED IMPORTED) 18 | set_target_properties(Poco::${component} PROPERTIES 19 | INTERFACE_INCLUDE_DIRECTORIES ${Poco_INCLUDE_DIR} 20 | IMPORTED_LOCATION ${${component_var}} 21 | ) 22 | endif() 23 | endif() 24 | endforeach() 25 | 26 | include(FindPackageHandleStandardArgs) 27 | find_package_handle_standard_args(Poco 28 | FOUND_VAR Poco_FOUND 29 | REQUIRED_VARS Poco_INCLUDE_DIR Poco_LIBRARIES 30 | VERSION_VAR Poco_VERSION 31 | HANDLE_COMPONENTS 32 | ) 33 | -------------------------------------------------------------------------------- /docs/api_references.rst: -------------------------------------------------------------------------------- 1 | .. _libfranka_api_references: 2 | API References 3 | -------------- 4 | 5 | This section provides comprehensive API documentation for libfranka in both C++ and Python. 6 | 7 | .. _libfranka_cpp_api: 8 | C++ API 9 | ~~~~~~~ 10 | 11 | The C++ API documentation contains detailed information about classes, methods, and functions available in the libfranka C++ library. 12 | 13 | **Base URL:** https://frankarobotics.github.io/libfranka 14 | 15 | **Version-Specific URLs:** 16 | 17 | :: 18 | 19 | https://frankarobotics.github.io/libfranka/{version}/ 20 | 21 | Replace ``{version}`` with your desired version number. 22 | 23 | .. _libfranka_python_api: 24 | Python API 25 | ~~~~~~~~~~ 26 | 27 | The Python API documentation provides detailed information about modules, classes, and functions available in the pylibfranka Python package. 28 | 29 | **Base URL:** https://frankarobotics.github.io/libfranka/pylibfranka/latest/ 30 | 31 | **Version-Specific URLs:** 32 | 33 | :: 34 | 35 | https://frankarobotics.github.io/libfranka/pylibfranka/{version}/ 36 | 37 | Replace ``{version}`` with your desired version number. 38 | -------------------------------------------------------------------------------- /pylibfranka/docs/api/gripper.rst: -------------------------------------------------------------------------------- 1 | Gripper 2 | ======= 3 | 4 | .. currentmodule:: pylibfranka 5 | 6 | Interface for controlling the Franka Hand gripper. 7 | 8 | Gripper Class 9 | ------------- 10 | 11 | .. autoclass:: Gripper 12 | :members: 13 | :undoc-members: 14 | :show-inheritance: 15 | 16 | The :class:`Gripper` class provides control of the Franka Hand parallel-jaw gripper. 17 | 18 | Methods 19 | ------- 20 | 21 | .. automethod:: Gripper.__init__ 22 | :noindex: 23 | 24 | .. automethod:: Gripper.homing 25 | :noindex: 26 | 27 | .. automethod:: Gripper.move 28 | :noindex: 29 | 30 | .. automethod:: Gripper.grasp 31 | :noindex: 32 | 33 | .. automethod:: Gripper.stop 34 | :noindex: 35 | 36 | .. automethod:: Gripper.read_once 37 | :noindex: 38 | 39 | 40 | GripperState Class 41 | ------------------ 42 | 43 | .. autoclass:: GripperState 44 | :members: 45 | :undoc-members: 46 | :show-inheritance: 47 | 48 | Current state of the Franka Hand gripper. 49 | 50 | See Also 51 | -------- 52 | 53 | * :class:`Robot` -- Main robot control interface 54 | * :class:`GripperState` -- Gripper state information 55 | -------------------------------------------------------------------------------- /pylibfranka/scripts/repair_wheels.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | echo "Repairing wheels with auditwheel..." 5 | 6 | # Ensure user-level Python packages are in PATH 7 | export PATH=$HOME/.local/bin:$PATH 8 | 9 | # Create wheelhouse directory if it doesn't exist 10 | mkdir -p wheelhouse 11 | 12 | # Try different approaches to run auditwheel 13 | if command -v auditwheel &> /dev/null; then 14 | echo "Using system auditwheel" 15 | auditwheel repair dist/*.whl -w wheelhouse/ 16 | elif [ -f $HOME/.local/bin/auditwheel ]; then 17 | echo "Using user-local auditwheel" 18 | $HOME/.local/bin/auditwheel repair dist/*.whl -w wheelhouse/ 19 | else 20 | echo "Installing auditwheel and running repair" 21 | python3 -m pip install --user auditwheel 22 | $HOME/.local/bin/auditwheel repair dist/*.whl -w wheelhouse/ 23 | fi 24 | 25 | # If auditwheel fails, copy the wheel directly as fallback 26 | if [ ! -f wheelhouse/*.whl ]; then 27 | echo "auditwheel failed, copying wheel directly" 28 | cp dist/*.whl wheelhouse/ 29 | fi 30 | 31 | # List the final wheels 32 | echo "Final wheels in wheelhouse:" 33 | ls -la wheelhouse/ 34 | -------------------------------------------------------------------------------- /examples/echo_robot_state.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | /** 9 | * @example echo_robot_state.cpp 10 | * An example showing how to continuously read the robot state. 11 | */ 12 | 13 | int main(int argc, char** argv) { 14 | if (argc != 2) { 15 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 16 | return -1; 17 | } 18 | 19 | try { 20 | franka::Robot robot(argv[1]); 21 | 22 | size_t count = 0; 23 | robot.read([&count](const franka::RobotState& robot_state) { 24 | // Printing to std::cout adds a delay. This is acceptable for a read loop such as this, but 25 | // should not be done in a control loop. 26 | std::cout << robot_state << std::endl; 27 | return count++ < 100; 28 | }); 29 | 30 | std::cout << "Done." << std::endl; 31 | } catch (franka::Exception const& e) { 32 | std::cout << e.what() << std::endl; 33 | return -1; 34 | } 35 | 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_program(DOXYGEN_PROG doxygen) 2 | if(NOT DOXYGEN_PROG) 3 | message(FATAL_ERROR "Doxygen not found.") 4 | endif() 5 | 6 | set(DOXYFILE ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) 7 | set(DOXYGEN_EXAMPLE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/../examples) 8 | set(DOXYGEN_SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../include ${DOXYGEN_EXAMPLE_PATH}) 9 | string(REPLACE ";" " " DOXYGEN_SOURCE_DIRS "${DOXYGEN_SOURCE_DIRS}") 10 | if(STRICT) 11 | set(DOXYGEN_WARN_AS_ERROR "YES") 12 | else() 13 | set(DOXYGEN_WARN_AS_ERROR "NO") 14 | endif() 15 | 16 | # Create the directory for the output and configure the Doxyfile 17 | set(DOXYGEN_OUTPUT_PATH "docs/${PROJECT_VERSION}") 18 | file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/${DOXYGEN_OUTPUT_PATH}) 19 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in ${DOXYFILE} @ONLY) 20 | 21 | add_custom_target(doc ALL 22 | COMMAND ${DOXYGEN_PROG} ${DOXYFILE} 23 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR} 24 | COMMENT "Generating documentation with Doxygen" 25 | VERBATIM 26 | ) 27 | 28 | include(GNUInstallDirs) 29 | install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/docs/${PROJECT_VERSION}/html DESTINATION ${CMAKE_INSTALL_DOCDIR}) 30 | -------------------------------------------------------------------------------- /pylibfranka/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(franka_python_bindings VERSION ${libfranka_VERSION}) 3 | 4 | # Set C++ standard 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 8 | 9 | find_package(Python3 COMPONENTS Interpreter Development NumPy REQUIRED) 10 | find_package(pybind11 CONFIG REQUIRED) 11 | 12 | if(NOT pybind11_FOUND) 13 | find_package(pybind11 MODULE REQUIRED) 14 | endif() 15 | 16 | find_package(Eigen3 REQUIRED) 17 | find_package(Poco REQUIRED COMPONENTS Foundation Net) 18 | find_package(pinocchio REQUIRED) 19 | 20 | # Create Python module 21 | pybind11_add_module(_pylibfranka 22 | src/pylibfranka.cpp 23 | ) 24 | 25 | # Include directories 26 | target_include_directories(_pylibfranka PRIVATE 27 | ${CMAKE_CURRENT_SOURCE_DIR}/include 28 | ) 29 | 30 | # Link against the shared libfranka library 31 | target_link_libraries(_pylibfranka PRIVATE 32 | franka 33 | 34 | Eigen3::Eigen 35 | Poco::Foundation 36 | Poco::Net 37 | pinocchio::pinocchio 38 | ) 39 | 40 | # Install the Python module 41 | install(TARGETS _pylibfranka DESTINATION pylibfranka) 42 | -------------------------------------------------------------------------------- /src/load_calculations.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace franka { 10 | 11 | std::array combineCenterOfMass( 12 | double m_ee, 13 | const std::array& F_x_Cee, // NOLINT(readability-identifier-naming) 14 | double m_load, 15 | const std::array& F_x_Cload); // NOLINT(readability-identifier-naming) 16 | 17 | Eigen::Matrix3d skewSymmetricMatrixFromVector(const Eigen::Vector3d& input); 18 | 19 | std::array combineInertiaTensor( 20 | double m_ee, 21 | const std::array& F_x_Cee, // NOLINT(readability-identifier-naming) 22 | const std::array& I_ee, // NOLINT(readability-identifier-naming) 23 | double m_load, 24 | const std::array& F_x_Cload, // NOLINT(readability-identifier-naming) 25 | const std::array& I_load, // NOLINT(readability-identifier-naming) 26 | double m_total, 27 | const std::array& F_x_Ctotal); // NOLINT(readability-identifier-naming) 28 | 29 | } // namespace franka 30 | -------------------------------------------------------------------------------- /pylibfranka/scripts/test_installation.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | # Accept wheel pattern as first argument, default to all wheels 5 | WHEEL_PATTERN=${1:-"*.whl"} 6 | 7 | echo "Installing wheel with pattern: $WHEEL_PATTERN" 8 | 9 | # Find wheels matching the pattern 10 | if [[ "$WHEEL_PATTERN" == "*.whl" ]]; then 11 | # Install all wheels (default behavior) 12 | pip install wheelhouse/*.whl 13 | else 14 | # Find specific wheel matching pattern 15 | echo "Available wheels:" 16 | ls -la wheelhouse/ 17 | echo "Filtering for wheels matching: $WHEEL_PATTERN" 18 | MATCHING_WHEEL=$(find wheelhouse/ -name "*${WHEEL_PATTERN}" | head -1) 19 | if [ -z "$MATCHING_WHEEL" ]; then 20 | echo "No wheel found matching pattern: $WHEEL_PATTERN" 21 | exit 1 22 | fi 23 | echo "Found wheel: $MATCHING_WHEEL" 24 | pip install "$MATCHING_WHEEL" 25 | fi 26 | 27 | # Test the installation 28 | echo "Testing pylibfranka import..." 29 | cd / 30 | python -c " 31 | import pylibfranka 32 | print('pylibfranka imported successfully') 33 | try: 34 | print(f'Version: {pylibfranka.__version__}') 35 | except AttributeError: 36 | print('No version attribute found') 37 | " 38 | 39 | echo "Installation test complete!" 40 | -------------------------------------------------------------------------------- /.ci/checkgithistory.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # the script checks if the public repo and the local repo commit history are sync, otherwise it fails 3 | set -e 4 | 5 | PUBLIC_URL=$1 6 | BRANCH=${2:-master} # if no second arg supplied, then "master" is used 7 | 8 | if [ $# -lt 1 ]; then 9 | >&2 echo "Not enough argument supplied. Usage: checkgithistory.sh PUBLIC_URL [BRANCH]" 10 | exit 1 11 | fi 12 | 13 | if [ ! -d ".git/" ] ; then 14 | echo "Current directory `pwd` does not appear to be a Git repository. Change directory before calling this script" 15 | exit 1 16 | fi 17 | 18 | public_remote_name=$(git remote -v | grep ${PUBLIC_URL} | head -n 1 | sed -e 's/\s.*$//') 19 | 20 | if [ -z "$public_remote_name" ]; then 21 | public_remote_name="public" 22 | git remote add $public_remote_name $PUBLIC_URL 23 | fi 24 | 25 | git fetch $public_remote_name 26 | 27 | local_commit_hash=$(git rev-parse origin/${BRANCH}) 28 | public_commit_hash=$(git rev-parse ${public_remote_name}/${BRANCH}) 29 | 30 | if [ "$local_commit_hash" != "$public_commit_hash" ]; then 31 | >&2 echo "Local and public commit hashes do not match. Please update local to the latest public commit" 32 | exit 1 33 | else 34 | echo "Local and public commit hashes do match." 35 | fi 36 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | libfranka 7 | 0.19.0 8 | libfranka is a C++ library for Franka Robotics research robots 9 | Franka Robotics GmbH 10 | Apache 2.0 11 | 12 | http://wiki.ros.org/libfranka 13 | https://github.com/frankarobotics/libfranka 14 | https://github.com/frankarobotics/libfranka/issues 15 | Franka Robotics GmbH 16 | 17 | cmake 18 | 19 | eigen 20 | fmt 21 | libpoco-dev 22 | pinocchio 23 | 24 | fmt 25 | libpoco-dev 26 | pinocchio 27 | 28 | doxygen 29 | graphviz 30 | 31 | 32 | cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/logging/logger.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace franka::logging { 9 | 10 | std::map> 11 | loggers; // NOLINT(cppcoreguidelines-avoid-non-const-global-variables) 12 | 13 | auto addLogger(const std::shared_ptr& logger) -> void { 14 | loggers.emplace(logger->getName(), logger); 15 | } 16 | 17 | auto removeLogger(const std::string& logger_name) -> void { 18 | loggers.erase(logger_name); 19 | } 20 | 21 | auto removeAllLoggers() -> void { 22 | loggers.clear(); 23 | } 24 | 25 | auto logInfo(const std::string& message) -> void { 26 | for (const auto& [logger_name, logger] : loggers) { 27 | logger->logInfo(message); 28 | } 29 | } 30 | 31 | auto logWarn(const std::string& message) -> void { 32 | for (const auto& [logger_name, logger] : loggers) { 33 | logger->logWarn(message); 34 | } 35 | } 36 | 37 | auto logError(const std::string& message) -> void { 38 | for (const auto& [logger_name, logger] : loggers) { 39 | logger->logError(message); 40 | } 41 | } 42 | 43 | } // namespace franka::logging 44 | -------------------------------------------------------------------------------- /pylibfranka/include/pygripper.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | /** 5 | * @file pygripper.h 6 | * @brief Python bindings for the Franka Robotics Gripper Control 7 | * 8 | * This header file provides C++ class that wraps the Franka Robotics Gripper Control 9 | * for use in Python through pybind11. It offers: 10 | * - Gripper homing and movement control 11 | * - Grasping functionality with configurable parameters 12 | * - State reading and control methods 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace pylibfranka { 23 | 24 | class PyGripper { 25 | public: 26 | explicit PyGripper(const std::string& franka_address); 27 | ~PyGripper() = default; 28 | 29 | bool homing(); 30 | bool grasp(double width, 31 | double speed, 32 | double force, 33 | double epsilon_inner = 0.005, 34 | double epsilon_outer = 0.005); 35 | 36 | franka::GripperState readOnce(); 37 | 38 | bool stop(); 39 | bool move(double width, double speed); 40 | franka::Gripper::ServerVersion serverVersion(); 41 | 42 | private: 43 | std::unique_ptr gripper_; 44 | }; 45 | 46 | } // namespace pylibfranka 47 | -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "libfranka", 3 | "dockerComposeFile": "docker-compose.yml", 4 | "service": "libfranka_project", 5 | "workspaceFolder": "/workspaces", 6 | "remoteUser": "user", 7 | "initializeCommand": "bash -c 'ENV_FILE=\".devcontainer/.env\"; UBUNTU_VERSION=$(cat devcontainer_distro 2>/dev/null || echo 22.04); if [ ! -f \"$ENV_FILE\" ]; then echo -e \"UBUNTU_VERSION=${UBUNTU_VERSION}\\nUSER_UID=$(id -u)\\nUSER_GID=$(id -g)\" > \"$ENV_FILE\"; else sed -i.bak \"s/^UBUNTU_VERSION=.*/UBUNTU_VERSION=${UBUNTU_VERSION}/\" \"$ENV_FILE\"; sed -i.bak \"s/^USER_UID=.*/USER_UID=$(id -u)/\" \"$ENV_FILE\"; sed -i.bak \"s/^USER_GID=.*/USER_GID=$(id -g)/\" \"$ENV_FILE\"; rm -f \"$ENV_FILE.bak\"; fi'", 8 | "customizations": { 9 | "vscode": { 10 | "extensions": [ 11 | "ms-python.python", 12 | "tamasfe.even-better-toml", 13 | "ms-vscode.cpptools", 14 | "xaver.clang-format", 15 | "twxs.cmake", 16 | "cheshirekow.cmake-format", 17 | "ms-azuretools.vscode-docker" 18 | ], 19 | "settings": { 20 | "terminal.integrated.defaultProfile.linux": "bash", 21 | "terminal.integrated.profiles.linux": { "bash": { "path": "/bin/bash" } } 22 | } 23 | } 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /test/gripper_tests.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "helpers.h" 13 | #include "mock_server.h" 14 | 15 | using ::testing::_; 16 | using ::testing::Return; 17 | 18 | using franka::Gripper; 19 | using franka::IncompatibleVersionException; 20 | using franka::NetworkException; 21 | 22 | using research_interface::gripper::Connect; 23 | using research_interface::gripper::GripperState; 24 | 25 | TEST(Gripper, CannotConnectIfNoServerRunning) { 26 | EXPECT_THROW(Gripper gripper("127.0.0.1"), NetworkException) 27 | << "Shut down local gripper service to run tests."; 28 | } 29 | 30 | TEST(Gripper, CanPerformHandshake) { 31 | GripperMockServer server; 32 | 33 | Gripper gripper("127.0.0.1"); 34 | EXPECT_EQ(research_interface::gripper::kVersion, gripper.serverVersion()); 35 | } 36 | 37 | TEST(Gripper, ThrowsOnIncompatibleLibraryVersion) { 38 | GripperMockServer server([](const Connect::Request&) { 39 | return Connect::Response(Connect::Status::kIncompatibleLibraryVersion); 40 | }); 41 | 42 | EXPECT_THROW(Gripper("127.0.0.1"), IncompatibleVersionException); 43 | } 44 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | # Python formatting with black 3 | - repo: https://github.com/psf/black 4 | rev: 23.3.0 5 | hooks: 6 | - id: black 7 | language_version: python3 8 | types: [python] 9 | 10 | # Python import sorting with isort 11 | - repo: https://github.com/pycqa/isort 12 | rev: 5.12.0 13 | hooks: 14 | - id: isort 15 | args: ["--profile", "black"] 16 | 17 | # Python linting with flake8 18 | - repo: https://github.com/pycqa/flake8 19 | rev: 6.0.0 20 | hooks: 21 | - id: flake8 22 | additional_dependencies: [flake8-docstrings] 23 | 24 | # Python type checking with mypy 25 | - repo: https://github.com/pre-commit/mirrors-mypy 26 | rev: v1.3.0 27 | hooks: 28 | - id: mypy 29 | args: [--ignore-missing-imports] 30 | 31 | # C++ formatting with clang-format 32 | - repo: https://github.com/pre-commit/mirrors-clang-format 33 | rev: v18.1.8 34 | hooks: 35 | - id: clang-format 36 | types_or: [c++, c, cuda] 37 | args: ["-style=file"] 38 | 39 | # General hooks 40 | - repo: https://github.com/pre-commit/pre-commit-hooks 41 | rev: v4.4.0 42 | hooks: 43 | - id: trailing-whitespace 44 | - id: end-of-file-fixer 45 | - id: check-yaml 46 | - id: check-added-large-files 47 | - id: check-ast 48 | - id: check-merge-conflict 49 | -------------------------------------------------------------------------------- /pylibfranka/__init__.py: -------------------------------------------------------------------------------- 1 | """Python bindings for Franka Robotics control library. 2 | 3 | pylibfranka provides high-level Python access to Franka Robotics robots, 4 | enabling real-time control with torque, position, and velocity commands. 5 | 6 | """ 7 | 8 | from ._pylibfranka import ( 9 | ActiveControlBase, 10 | CartesianPose, 11 | CartesianVelocities, 12 | CommandException, 13 | ControlException, 14 | ControllerMode, 15 | Duration, 16 | Errors, 17 | FrankaException, 18 | Gripper, 19 | GripperState, 20 | InvalidOperationException, 21 | JointPositions, 22 | JointVelocities, 23 | Model, 24 | NetworkException, 25 | RealtimeConfig, 26 | RealtimeException, 27 | Robot, 28 | RobotMode, 29 | RobotState, 30 | Torques, 31 | ) 32 | from ._version import __version__ 33 | 34 | __all__ = [ 35 | "ActiveControlBase", 36 | "CartesianPose", 37 | "CartesianVelocities", 38 | "CommandException", 39 | "ControlException", 40 | "ControllerMode", 41 | "CartesianPose", 42 | "Duration", 43 | "Errors", 44 | "FrankaException", 45 | "Gripper", 46 | "GripperState", 47 | "InvalidOperationException", 48 | "JointPositions", 49 | "JointVelocities", 50 | "Model", 51 | "NetworkException", 52 | "RealtimeConfig", 53 | "RealtimeException", 54 | "Robot", 55 | "RobotMode", 56 | "RobotState", 57 | "Torques", 58 | ] 59 | -------------------------------------------------------------------------------- /src/motion_generator_traits.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | namespace franka { 9 | 10 | template 11 | struct MotionGeneratorTraits {}; 12 | 13 | template <> 14 | struct MotionGeneratorTraits { 15 | static constexpr auto kMotionGeneratorMode = 16 | research_interface::robot::Move::MotionGeneratorMode::kJointPosition; 17 | }; 18 | 19 | template <> 20 | struct MotionGeneratorTraits { 21 | static constexpr auto kMotionGeneratorMode = 22 | research_interface::robot::Move::MotionGeneratorMode::kJointVelocity; 23 | }; 24 | 25 | template <> 26 | struct MotionGeneratorTraits { 27 | static constexpr auto kMotionGeneratorMode = 28 | research_interface::robot::Move::MotionGeneratorMode::kCartesianPosition; 29 | }; 30 | 31 | template <> 32 | struct MotionGeneratorTraits { 33 | static constexpr auto kMotionGeneratorMode = 34 | research_interface::robot::Move::MotionGeneratorMode::kCartesianVelocity; 35 | }; 36 | 37 | template <> 38 | struct MotionGeneratorTraits { 39 | static constexpr auto kMotionGeneratorMode = 40 | research_interface::robot::Move::MotionGeneratorMode::kNone; 41 | }; 42 | 43 | } // namespace franka 44 | -------------------------------------------------------------------------------- /src/active_control.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "robot_impl.h" 13 | 14 | namespace franka { 15 | 16 | ActiveControl::ActiveControl(std::shared_ptr robot_impl, 17 | uint32_t motion_id, 18 | std::unique_lock control_lock) 19 | : robot_impl(std::move(robot_impl)), 20 | motion_id(motion_id), 21 | control_lock(std::move(control_lock)) {} 22 | 23 | ActiveControl::~ActiveControl() { 24 | if (!control_finished) { 25 | robot_impl->cancelMotion(motion_id); 26 | } 27 | } 28 | 29 | std::pair ActiveControl::readOnce() { 30 | auto robot_state = robot_impl->readOnce(); 31 | robot_impl->throwOnMotionError(robot_state, motion_id); 32 | 33 | if (!last_read_access.has_value()) { 34 | last_read_access = robot_state.time; 35 | } 36 | 37 | auto time_since_last_read = robot_state.time - last_read_access.value(); 38 | last_read_access = robot_state.time; 39 | 40 | return std::make_pair(robot_state, time_since_last_read); 41 | } 42 | 43 | } // namespace franka 44 | -------------------------------------------------------------------------------- /src/vacuum_gripper_state.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include "franka/vacuum_gripper_state.h" 4 | 5 | #include 6 | 7 | namespace franka { 8 | 9 | std::ostream& operator<<(std::ostream& ostream, 10 | const franka::VacuumGripperState& vacuum_gripper_state) { 11 | std::string device_status; 12 | switch (vacuum_gripper_state.device_status) { 13 | case VacuumGripperDeviceStatus::kGreen: 14 | device_status = "Green"; 15 | break; 16 | case VacuumGripperDeviceStatus::kYellow: 17 | device_status = "Yellow"; 18 | break; 19 | case VacuumGripperDeviceStatus::kOrange: 20 | device_status = "Orange"; 21 | break; 22 | case VacuumGripperDeviceStatus::kRed: 23 | device_status = "Red"; 24 | break; 25 | } 26 | 27 | ostream << "{\"in_control_range\": " << vacuum_gripper_state.in_control_range 28 | << ", \"part_detached\": " << vacuum_gripper_state.part_detached 29 | << ", \"part_present\": " << vacuum_gripper_state.part_present << ", \"device_status\": " 30 | << "\"" << device_status << "\"" 31 | << ", \"actual_power\": " << vacuum_gripper_state.actual_power 32 | << ", \"vacuum\": " << vacuum_gripper_state.vacuum 33 | << ", \"time\": " << vacuum_gripper_state.time.toSec() << "}"; 34 | return ostream; 35 | } 36 | 37 | } // namespace franka 38 | -------------------------------------------------------------------------------- /pylibfranka/docs/index.rst: -------------------------------------------------------------------------------- 1 | pylibfranka Documentation 2 | ========================== 3 | 4 | Overview 5 | -------- 6 | 7 | **pylibfranka** is a Python library that provides high-level access to Franka Robotics robots, 8 | enabling real-time control with torque, position, and velocity commands. 9 | 10 | Key Features 11 | ~~~~~~~~~~~~ 12 | 13 | * **Real-time Control**: Send torque, position, and velocity commands at 1 kHz 14 | * **Multiple Control Modes**: Joint/Cartesian position, velocity, and torque control 15 | * **Robot State Access**: Read all robot state information including joint positions, velocities, torques 16 | * **Dynamics Model**: Compute forward/inverse kinematics, mass matrix, Coriolis, and gravity 17 | * **Gripper Control**: Full control of Franka Hand gripper 18 | * **NumPy Integration**: All arrays are NumPy arrays for seamless integration 19 | 20 | Documentation Structure 21 | ----------------------- 22 | 23 | .. toctree:: 24 | :maxdepth: 1 25 | :caption: Quick Reference 26 | 27 | api_overview 28 | class_list 29 | class_hierarchy 30 | modules 31 | 32 | .. toctree:: 33 | :maxdepth: 2 34 | :caption: Detailed API Reference 35 | 36 | api/robot 37 | api/control 38 | api/model 39 | api/gripper 40 | api/state 41 | api/exceptions 42 | 43 | Indices and Tables 44 | ================== 45 | 46 | * :ref:`genindex` - Complete index of all classes and methods 47 | * :ref:`modindex` - Module index 48 | * :ref:`search` - Search the documentation -------------------------------------------------------------------------------- /test/vacuum_gripper_tests.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "helpers.h" 13 | #include "mock_server.h" 14 | 15 | using ::testing::_; 16 | using ::testing::Return; 17 | 18 | using franka::IncompatibleVersionException; 19 | using franka::NetworkException; 20 | using franka::VacuumGripper; 21 | 22 | using research_interface::vacuum_gripper::Connect; 23 | using research_interface::vacuum_gripper::VacuumGripperState; 24 | 25 | TEST(VacuumGripper, CannotConnectIfNoServerRunning) { 26 | EXPECT_THROW(VacuumGripper vacuum_gripper("127.0.0.1"), NetworkException) 27 | << "Shut down local vacuum gripper service to run tests."; 28 | } 29 | 30 | TEST(VacuumGripper, CanPerformHandshake) { 31 | VacuumGripperMockServer server; 32 | 33 | VacuumGripper vacuum_gripper("127.0.0.1"); 34 | EXPECT_EQ(research_interface::vacuum_gripper::kVersion, vacuum_gripper.serverVersion()); 35 | } 36 | 37 | TEST(VacuumGripper, ThrowsOnIncompatibleLibraryVersion) { 38 | VacuumGripperMockServer server([](const Connect::Request&) { 39 | return Connect::Response(Connect::Status::kIncompatibleLibraryVersion); 40 | }); 41 | 42 | EXPECT_THROW(VacuumGripper("127.0.0.1"), IncompatibleVersionException); 43 | } 44 | -------------------------------------------------------------------------------- /examples/print_joint_poses.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | /** 10 | * @example print_joint_poses.cpp 11 | * An example showing how to use the model library that prints the transformation 12 | * matrix of each joint with respect to the base frame. 13 | */ 14 | 15 | template 16 | std::ostream& operator<<(std::ostream& ostream, const std::array& array) { 17 | ostream << "["; 18 | std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator(ostream, ",")); 19 | std::copy(array.cend() - 1, array.cend(), std::ostream_iterator(ostream)); 20 | ostream << "]"; 21 | return ostream; 22 | } 23 | 24 | int main(int argc, char** argv) { 25 | if (argc != 2) { 26 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 27 | return -1; 28 | } 29 | 30 | try { 31 | franka::Robot robot(argv[1]); 32 | 33 | franka::RobotState state = robot.readOnce(); 34 | 35 | franka::Model model(robot.loadModel()); 36 | for (franka::Frame frame = franka::Frame::kJoint1; frame <= franka::Frame::kEndEffector; 37 | frame++) { 38 | std::cout << model.pose(frame, state) << std::endl; 39 | } 40 | } catch (franka::Exception const& e) { 41 | std::cout << e.what() << std::endl; 42 | return -1; 43 | } 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /src/exception.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | // `using std::string_literals::operator""s` produces a GCC warning that cannot be disabled, so we 9 | // have to use `using namespace ...`. 10 | // See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=65923#c0 11 | using namespace std::string_literals; // NOLINT(google-build-using-namespace) 12 | 13 | namespace franka { 14 | 15 | ControlException::ControlException(const std::string& what, 16 | std::vector log) noexcept 17 | : Exception(what), log(std::move(log)) {} 18 | 19 | IncompatibleVersionException::IncompatibleVersionException(uint16_t server_version, 20 | uint16_t library_version) noexcept 21 | : Exception("libfranka: Incompatible library version (server version: "s + 22 | std::to_string(server_version) + ", library version: "s + 23 | std::to_string(library_version) + 24 | "). Please check https://frankarobotics.github.io for system updates " 25 | "or choose a libfranka version that uses the server version " + 26 | std::to_string(server_version) + 27 | " from the table at https://frankarobotics.github.io/docs/compatibility.html ."s), 28 | server_version(server_version), 29 | library_version(library_version) {} 30 | 31 | } // namespace franka 32 | -------------------------------------------------------------------------------- /.github/workflows/doxygen.yml: -------------------------------------------------------------------------------- 1 | name: Generate Doxygen Documentation for libfranka 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | workflow_dispatch: 8 | 9 | jobs: 10 | doxygen: 11 | runs-on: ubuntu-latest 12 | 13 | steps: 14 | - name: Checkout repository 15 | uses: actions/checkout@v3 16 | 17 | - name: Install Doxygen 18 | run: sudo apt-get install doxygen graphviz -y 19 | 20 | - name: Generate Doxygen configuration and build it 21 | run: | 22 | TAG_NAME=$(echo $GITHUB_REF | sed 's/refs\/tags\///') 23 | mkdir -p build/doc/docs/$TAG_NAME && cd build && cmake -D BUILD_DOCUMENTATION=ON -D SKIP_CXX_BUILD=ON .. && make doc 24 | echo "Created folder for documentation:" 25 | ls doc/docs 26 | 27 | - name: Move contents of html folder one level up 28 | run: | 29 | TAG_NAME=$(echo $GITHUB_REF | sed 's/refs\/tags\///') 30 | mv build/doc/docs/$TAG_NAME/html/* build/doc/docs/$TAG_NAME/ 31 | 32 | - name: Create index.html for redirect 33 | run: | 34 | TAG_NAME=$(echo $GITHUB_REF | sed 's/refs\/tags\///') 35 | URL="https://frankarobotics.github.io/libfranka/${TAG_NAME}/" 36 | echo $URL 37 | echo "" > build/doc/docs/index.html 38 | 39 | - name: Deploy to GitHub Pages 40 | uses: peaceiris/actions-gh-pages@v3 41 | with: 42 | github_token: ${{ secrets.GITHUB_TOKEN }} 43 | publish_branch: gh-pages 44 | publish_dir: build/doc/docs 45 | keep_files: true 46 | -------------------------------------------------------------------------------- /include/franka/gripper_state.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | /** 11 | * @file gripper_state.h 12 | * Contains the franka::GripperState type. 13 | */ 14 | 15 | namespace franka { 16 | 17 | /** 18 | * Describes the gripper state. 19 | */ 20 | struct GripperState { 21 | /** 22 | * Current gripper opening width. Unit: \f$[m]\f$. 23 | */ 24 | double width{}; 25 | 26 | /** 27 | * Maximum gripper opening width. 28 | * This parameter is estimated by homing the gripper. 29 | * After changing the gripper fingers, a homing needs to be done. Unit: \f$[m]\f$. 30 | * 31 | * @see Gripper::homing. 32 | */ 33 | double max_width{}; 34 | 35 | /** 36 | * Indicates whether an object is currently grasped. 37 | */ 38 | bool is_grasped{}; 39 | 40 | /** 41 | * Current gripper temperature. Unit: \f$[°C]\f$. 42 | */ 43 | uint16_t temperature{}; 44 | 45 | /** 46 | * Strictly monotonically increasing timestamp since robot start. 47 | */ 48 | Duration time{}; 49 | }; 50 | 51 | /** 52 | * Streams the gripper state as JSON object: {"field_name_1": value, "field_name_2": value, ...} 53 | * 54 | * @param[in] ostream Ostream instance 55 | * @param[in] gripper_state GripperState struct instance to stream 56 | * 57 | * @return Ostream instance 58 | */ 59 | std::ostream& operator<<(std::ostream& ostream, const franka::GripperState& gripper_state); 60 | 61 | } // namespace franka 62 | -------------------------------------------------------------------------------- /test/errors_tests.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "helpers.h" 10 | 11 | TEST(Errors, IsInitializedToNoErrors) { 12 | franka::Errors errors; 13 | 14 | EXPECT_FALSE(errors); 15 | } 16 | 17 | TEST(Errors, EvaluatedToTrueOnError) { 18 | std::array error_flags{}; 19 | error_flags[rand() % error_flags.size()] = true; 20 | 21 | franka::Errors errors(error_flags); 22 | 23 | EXPECT_TRUE(errors); 24 | } 25 | 26 | TEST(Errors, CanGetNamesOfErrors) { 27 | std::array error_flags{}; 28 | std::fill(error_flags.begin(), error_flags.end(), false); 29 | error_flags[static_cast( 30 | research_interface::robot::Error::kJointPositionLimitsViolation)] = true; 31 | error_flags[static_cast( 32 | research_interface::robot::Error::kSelfcollisionAvoidanceViolation)] = true; 33 | 34 | franka::Errors errors(error_flags); 35 | 36 | std::string expected = 37 | R"(["joint_position_limits_violation", "self_collision_avoidance_violation"])"; 38 | EXPECT_EQ(expected, static_cast(errors)); 39 | } 40 | 41 | TEST(Errors, CanBeStreamed) { 42 | franka::Errors errors; 43 | 44 | std::stringstream ss; 45 | ss << errors; 46 | std::string output(ss.str()); 47 | 48 | EXPECT_EQ("[]", output); 49 | } 50 | -------------------------------------------------------------------------------- /src/logging/robot_state_logger.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include "robot_state_logger.hpp" 4 | 5 | namespace franka { 6 | 7 | RobotStateLogger::RobotStateLogger(size_t log_size) : log_size_(log_size) { 8 | states_.resize(log_size); 9 | commands_.resize(log_size); 10 | } 11 | 12 | void RobotStateLogger::log(const RobotState& state, 13 | const research_interface::robot::RobotCommand& command) { 14 | if (log_size_ == 0) { 15 | return; 16 | } 17 | 18 | commands_[ring_front_] = command; 19 | states_[ring_front_] = state; 20 | 21 | ring_front_ = (ring_front_ + 1) % log_size_; 22 | ring_size_ = std::min(log_size_, ring_size_ + 1); 23 | } 24 | 25 | std::vector RobotStateLogger::flush() { 26 | std::vector log; 27 | 28 | for (size_t i = 0; i < ring_size_; i++) { 29 | size_t wrapped_index = (ring_front_ + i) % ring_size_; 30 | 31 | RobotCommand command; 32 | command.joint_positions = commands_[wrapped_index].motion.q_c; 33 | command.joint_velocities = commands_[wrapped_index].motion.dq_c; 34 | command.cartesian_pose.O_T_EE = commands_[wrapped_index].motion.O_T_EE_c; 35 | command.cartesian_velocities.O_dP_EE = commands_[wrapped_index].motion.O_dP_EE_c; 36 | command.torques.tau_J = commands_[wrapped_index].control.tau_J_d; 37 | 38 | Record record; 39 | record.state = states_[wrapped_index]; 40 | record.command = command; 41 | log.push_back(record); 42 | } 43 | 44 | ring_front_ = 0; 45 | ring_size_ = 0; 46 | return log; 47 | } 48 | 49 | } // namespace franka 50 | -------------------------------------------------------------------------------- /examples/utility_examples/logging_example.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | /* 5 | * This example demonstrates how to create a custom logger for libfranka which is using the custom 6 | * logger to forward messages. Those messages could be logged to a file, a database, or any other 7 | * intended tool. 8 | * 9 | * Note: A pre-defined std::cout logger already exists in libfranka, see 10 | * franka/logging/cout_logging_sink.hpp. 11 | */ 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | class ExampleLogger : public franka::LoggingSinkInterface { 19 | public: 20 | ~ExampleLogger() override = default; 21 | 22 | auto getName() const -> std::string override { return name_; } 23 | 24 | auto logInfo(const std::string& message) -> void override { 25 | std::cout << "INFO: " << message << std::endl; 26 | } 27 | 28 | auto logWarn(const std::string& message) -> void override { 29 | std::cout << "WARNING: " << message << std::endl; 30 | } 31 | 32 | auto logError(const std::string& message) -> void override { 33 | std::cout << "ERROR: " << message << std::endl; 34 | } 35 | 36 | private: 37 | std::string name_ = "ExampleLogger"; 38 | }; 39 | 40 | int main() { 41 | auto example_logger = std::make_shared(); 42 | franka::logging::addLogger(example_logger); 43 | 44 | // libfranka will internally use these log lines to forward messages to the custom logger. 45 | franka::logging::logInfo("This is an info message."); 46 | franka::logging::logWarn("This is a warning message."); 47 | franka::logging::logError("This is an error message."); 48 | 49 | return 0; 50 | } 51 | -------------------------------------------------------------------------------- /docs/compatibility_with_images.rst: -------------------------------------------------------------------------------- 1 | .. _libfranka_compatibility: 2 | Robot System Version Compatibility 3 | ----------------------------------- 4 | 5 | Several components in the Franka ecosystem evolve independently, and versions may not always be compatible. It is recommended to use the most recent versions of all components. 6 | 7 | The symbol ``>=`` indicates that newer Robot System Versions have **not** been explicitly tested with the corresponding ``libfranka`` release. Compatibility *may* work but is **not guaranteed** (e.g., ``libfranka 0.2.0`` may not be compatible with Robot System version ``4.0.0``). 8 | 9 | The table below shows tested compatibility between libfranka versions, Robot System versions, and Robot/Gripper servers. All versions use ``>=`` and ``<`` where applicable to indicate tested ranges. 10 | 11 | .. list-table:: 12 | :header-rows: 1 13 | :widths: 25 25 20 14 | :stub-columns: 0 15 | 16 | * - libfranka Version 17 | - Robot System Version 18 | - Robot/Gripper Server 19 | 20 | * - >= 0.18.0 21 | - >= 5.9.0 22 | - 10 / 3 23 | 24 | * - >= 0.15.0 < 0.18.0 25 | - >= 5.7.2 < 5.9.0 26 | - 9 / 3 27 | 28 | * - >= 0.14.1 < 0.15.0 29 | - >= 5.7.0 < 5.7.2 30 | - 8 / 3 31 | 32 | * - >= 0.13.3 < 0.14.1 33 | - >= 5.5.0 < 5.7.0 34 | - 7 / 3 35 | 36 | * - >= 0.10.0 < 0.13.3 37 | - >= 5.2.0 < 5.5.0 38 | - 6 / 3 39 | 40 | * - >= 0.9.1 < 0.10.0 41 | - >= 4.2.1 < 5.2.0 42 | - 5 / 3 43 | 44 | * - >= 0.8.0 < 0.9.1 45 | - >= 4.0.0 < 4.2.1 46 | - 4 / 3 47 | 48 | * - >= 0.7.1 < 0.8.0 49 | - >= 3.0.0 < 4.0.0 50 | - 3 / 3 51 | 52 | * - >= 0.5.0 < 0.7.1 53 | - >= 1.3.0 < 3.0.0 54 | - 3 / 2 55 | 56 | * - >= 0.3.0 < 0.5.0 57 | - >= 1.2.0 < 1.3.0 58 | - 2 / 2 59 | 60 | * - >= 0.2.0 < 0.3.0 61 | - >= 1.1.0 < 1.2.0 62 | - 2 / 1 63 | -------------------------------------------------------------------------------- /examples/vacuum_object.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | /** 10 | * @example vacuum_object.cpp 11 | * An example showing how to control FRANKA's vacuum gripper. 12 | */ 13 | 14 | int main(int argc, char** argv) { 15 | if (argc != 2) { 16 | std::cerr << "Usage: ./vacuum_object " << std::endl; 17 | return -1; 18 | } 19 | 20 | franka::VacuumGripper vacuum_gripper(argv[1]); 21 | try { 22 | // Print a vacuum gripper state. 23 | franka::VacuumGripperState vacuum_gripper_state = vacuum_gripper.readOnce(); 24 | std::cout << "Initial vacuum gripper state: " << vacuum_gripper_state << std::endl; 25 | 26 | // Vacuum the object. 27 | if (!vacuum_gripper.vacuum(100, std::chrono::milliseconds(1000))) { 28 | std::cout << "Failed to vacuum the object." << std::endl; 29 | return -1; 30 | } 31 | 32 | vacuum_gripper_state = vacuum_gripper.readOnce(); 33 | std::cout << "Vacuum gripper state after applying vacuum: " << vacuum_gripper_state 34 | << std::endl; 35 | 36 | // Wait 3s and check afterwards, if the object is still grasped. 37 | std::this_thread::sleep_for(std::chrono::duration(3000)); 38 | 39 | vacuum_gripper_state = vacuum_gripper.readOnce(); 40 | if (!vacuum_gripper_state.in_control_range) { 41 | std::cout << "Object lost." << std::endl; 42 | return -1; 43 | } 44 | 45 | std::cout << "Vacuumed object, will release it now." << std::endl; 46 | vacuum_gripper.dropOff(std::chrono::milliseconds(1000)); 47 | } catch (franka::Exception const& e) { 48 | vacuum_gripper.stop(); 49 | std::cout << e.what() << std::endl; 50 | return -1; 51 | } 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /include/franka/active_torque_control.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include "active_control.h" 6 | 7 | /** 8 | * @file active_torque_control.h 9 | * Contains the `franka::ActiveTorqueControl` type. 10 | */ 11 | 12 | namespace franka { 13 | 14 | /** 15 | * Allows the user to read the state of a Robot and to send new torque control commands after 16 | * starting a control process of a Robot. 17 | * 18 | * hint: To create an ActiveTorqueControl, see franka::Robot 19 | * 20 | */ 21 | class ActiveTorqueControl : public ActiveControl { 22 | public: 23 | ~ActiveTorqueControl() override = default; 24 | 25 | /** 26 | * Updates the joint-level based torque commands of an active joint effort control 27 | * 28 | * @param control_input the new joint-level based torques 29 | * 30 | * @throw ControlException if an error related to torque control or motion generation occurred, or 31 | * the motion was already finished. 32 | * @throw NetworkException if the connection is lost, e.g. after a timeout. 33 | */ 34 | void writeOnce(const Torques& control_input) override; 35 | 36 | /** 37 | * franka::Robot as friend to allow construction of ActiveTorqueControl in 38 | * startTorqueControl methods 39 | * 40 | */ 41 | friend class Robot; 42 | 43 | private: 44 | /** 45 | * Construct a new ActiveTorqueControl object 46 | * 47 | * @param robot shared_ptr to the Robot::Impl in the Robot 48 | * @param motion_id id of the managed motion 49 | * @param control_lock of the Robot, preventing other read and write accesses during the active 50 | * control 51 | */ 52 | ActiveTorqueControl(std::shared_ptr robot_impl, 53 | uint32_t motion_id, 54 | std::unique_lock control_lock) 55 | : ActiveControl(std::move(robot_impl), motion_id, std::move(control_lock)) {}; 56 | }; 57 | 58 | } // namespace franka 59 | -------------------------------------------------------------------------------- /cmake/SetVersionFromGit.cmake: -------------------------------------------------------------------------------- 1 | # Use set_version_from_git to get the current version and tag from git. 2 | # If the current commit is tagged the git tag is returned as version. 3 | # If not, the latest tag is returned. If no tag exists in the commits history, 0.1.0 is returned. 4 | # If a tag has never been set, it will return 0.1.0 as VERSION and the short git commit hash as TAG. 5 | # ${MAJOR}.${MINOR}.${PATCH} 6 | # 7 | # USAGE: set_version_from_git( ) 8 | # 9 | # ARGUMENTS: output_version variable that will contain the version 10 | # output_tag variable that will contain the tag 11 | # 12 | # EXAMPLE: 13 | # set_version_from_git(PROJECT_VERSION PROJECT_TAG) 14 | # project( VERSION ${PROJECT_VERSION}) 15 | # 16 | function(set_version_from_git VERSION TAG) 17 | find_program(GIT_EXEC git REQUIRED) 18 | if(NOT GIT_EXEC) 19 | message(FATAL_ERROR "cannot execute set_version_from_git: git not found.\napt install git") 20 | endif() 21 | 22 | 23 | set(ENV{GIT_DIR} ${CMAKE_SOURCE_DIR}/.git) 24 | execute_process(COMMAND ${GIT_EXEC} describe --tags 25 | OUTPUT_VARIABLE LOCAL_SCOPE_TAG 26 | RESULT_VARIABLE GIT_NOT_FOUND 27 | ERROR_QUIET) 28 | if(GIT_NOT_FOUND) 29 | message(FATAL_ERROR "cannot find a tag in git") 30 | endif() 31 | 32 | # separate the version from the commit short hash 33 | if(LOCAL_SCOPE_TAG) 34 | string(STRIP ${LOCAL_SCOPE_TAG} LOCAL_SCOPE_TAG) 35 | string(FIND ${LOCAL_SCOPE_TAG} "-" FIND_INDEX) 36 | string(SUBSTRING ${LOCAL_SCOPE_TAG} 0 ${FIND_INDEX} LOCAL_SCOPE_VERSION) 37 | else() 38 | set(LOCAL_SCOPE_VERSION "0.1.0") 39 | execute_process(COMMAND ${GIT_EXEC} describe --tags --always 40 | OUTPUT_VARIABLE LOCAL_SCOPE_TAG 41 | ERROR_QUIET) 42 | string(STRIP ${LOCAL_SCOPE_TAG} LOCAL_SCOPE_TAG) 43 | endif() 44 | 45 | set(${VERSION} ${LOCAL_SCOPE_VERSION} PARENT_SCOPE) 46 | set(${TAG} ${LOCAL_SCOPE_TAG} PARENT_SCOPE) 47 | endfunction() 48 | -------------------------------------------------------------------------------- /test/mock_robot_control.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "robot_control.h" 12 | 13 | class MockRobotControl : public franka::RobotControl { 14 | public: 15 | virtual ~MockRobotControl() = default; 16 | 17 | MOCK_METHOD(uint32_t, 18 | startMotion, 19 | (research_interface::robot::Move::ControllerMode, 20 | research_interface::robot::Move::MotionGeneratorMode, 21 | const research_interface::robot::Move::Deviation&, 22 | const research_interface::robot::Move::Deviation&, 23 | bool, 24 | const std::optional>&), 25 | (override)); 26 | MOCK_METHOD(franka::RobotState, 27 | updateMotion, 28 | (const std::optional&, 29 | const std::optional&), 30 | (override)); 31 | MOCK_METHOD(void, 32 | finishMotion, 33 | (uint32_t, 34 | const std::optional&, 35 | const std::optional&), 36 | (override)); 37 | MOCK_METHOD(void, cancelMotion, (uint32_t), (override)); 38 | MOCK_METHOD(void, throwOnMotionError, (const franka::RobotState&, uint32_t), (override)); 39 | MOCK_METHOD((std::array), 40 | getUpperJointVelocityLimits, 41 | ((const std::array&)), 42 | (const, override)); 43 | MOCK_METHOD((std::array), 44 | getLowerJointVelocityLimits, 45 | ((const std::array&)), 46 | (const, override)); 47 | 48 | franka::RealtimeConfig realtimeConfig() const noexcept override { 49 | return franka::RealtimeConfig::kIgnore; 50 | } 51 | }; 52 | -------------------------------------------------------------------------------- /include/franka/logging/robot_state_log.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | /** 11 | * @file log.h 12 | * Contains helper types for logging sent commands and received robot states. 13 | */ 14 | 15 | namespace franka { 16 | 17 | /** 18 | * Command sent to the robot. Structure used only for logging purposes. 19 | */ 20 | struct RobotCommand { 21 | /** 22 | * \f$q_d\f$ sent to the robot. 23 | */ 24 | JointPositions joint_positions{0, 0, 0, 0, 0, 0, 0}; 25 | /** 26 | * \f$\dot{q}_d\f$ sent to the robot. 27 | */ 28 | JointVelocities joint_velocities{0, 0, 0, 0, 0, 0, 0}; 29 | /** 30 | * \f$^O{\mathbf{T}_{EE}}_{d}\f$ sent to the robot. 31 | */ 32 | CartesianPose cartesian_pose{1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; 33 | /** 34 | * \f$^O\dot{P}_{EE}\f$ sent to the robot. 35 | */ 36 | CartesianVelocities cartesian_velocities{0, 0, 0, 0, 0, 0}; 37 | /** 38 | * \f${\tau_J}_d\f$ sent to the robot. 39 | */ 40 | Torques torques{0, 0, 0, 0, 0, 0, 0}; 41 | }; 42 | 43 | /** 44 | * One row of the log contains a robot command of timestamp n and a 45 | * corresponding robot state of timestamp n+1. 46 | * Provided by the ControlException. 47 | */ 48 | struct Record { 49 | /** 50 | * Robot state of timestamp n+1. 51 | */ 52 | RobotState state; 53 | /** 54 | * Robot command of timestamp n, after rate limiting (if activated). 55 | */ 56 | RobotCommand command; 57 | }; 58 | 59 | /** 60 | * Writes the log to a string in CSV format. If the string is not empty, the first row contains the 61 | * header with names of columns. The following lines contain rows of values separated by commas. 62 | * 63 | * If the log is empty, the function returns an empty string. 64 | * 65 | * @param[in] log Log provided by the ControlException. 66 | * 67 | * @return a string in CSV format, or empty string. 68 | */ 69 | std::string logToCSV(const std::vector& log); 70 | } // namespace franka 71 | -------------------------------------------------------------------------------- /examples/grasp_object.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | /** 12 | * @example grasp_object.cpp 13 | * An example showing how to control FRANKA's gripper. 14 | */ 15 | 16 | int main(int argc, char** argv) { 17 | if (argc != 4) { 18 | std::cerr << "Usage: ./grasp_object " << std::endl; 19 | return -1; 20 | } 21 | 22 | try { 23 | franka::Gripper gripper(argv[1]); 24 | double grasping_width = std::stod(argv[3]); 25 | 26 | std::stringstream string_stream(argv[2]); 27 | bool homing{}; 28 | if (!(string_stream >> homing)) { 29 | std::cerr << " can be 0 or 1." << std::endl; 30 | return -1; 31 | } 32 | 33 | if (homing) { 34 | // Do a homing in order to estimate the maximum grasping width with the current fingers. 35 | gripper.homing(); 36 | } 37 | 38 | // Check for the maximum grasping width. 39 | franka::GripperState gripper_state = gripper.readOnce(); 40 | if (gripper_state.max_width < grasping_width) { 41 | std::cout << "Object is too large for the current fingers on the gripper." << std::endl; 42 | return -1; 43 | } 44 | 45 | // Grasp the object. 46 | if (!gripper.grasp(grasping_width, 0.1, 60)) { 47 | std::cout << "Failed to grasp object." << std::endl; 48 | return -1; 49 | } 50 | 51 | // Wait 3s and check afterwards, if the object is still grasped. 52 | std::this_thread::sleep_for(std::chrono::duration(3000)); 53 | 54 | gripper_state = gripper.readOnce(); 55 | if (!gripper_state.is_grasped) { 56 | std::cout << "Object lost." << std::endl; 57 | return -1; 58 | } 59 | 60 | std::cout << "Grasped object, will release it now." << std::endl; 61 | gripper.stop(); 62 | } catch (franka::Exception const& e) { 63 | std::cout << e.what() << std::endl; 64 | return -1; 65 | } 66 | 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /include/franka/vacuum_gripper_state.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | /** 12 | * @file vacuum_gripper_state.h 13 | * Contains the franka::VacuumGripperState type. 14 | */ 15 | 16 | namespace franka { 17 | 18 | /** 19 | * Vacuum gripper device status. 20 | */ 21 | enum class VacuumGripperDeviceStatus : uint8_t { 22 | kGreen, /**< Device is working optimally */ 23 | kYellow, /**< Device is working but there are warnings */ 24 | kOrange, /**< Device is working but there are severe warnings */ 25 | kRed /**< Device is not working properly */ 26 | }; 27 | 28 | /** 29 | * Describes the vacuum gripper state. For more information check the cobot-pump manual. 30 | */ 31 | struct VacuumGripperState { 32 | /** 33 | * Vacuum value within in setpoint area. 34 | */ 35 | bool in_control_range{}; 36 | 37 | /** 38 | * The part has been detached after a suction cycle 39 | */ 40 | bool part_detached{}; 41 | 42 | /** 43 | * Vacuum is over H2 and not yet under H2-h2. For more information check the cobot-pump manual. 44 | */ 45 | bool part_present{}; 46 | 47 | /** 48 | * Current vacuum gripper device status. 49 | */ 50 | VacuumGripperDeviceStatus device_status{}; 51 | 52 | /** 53 | * Current vacuum gripper actual power. Unit: \f$[%]\f$. 54 | */ 55 | uint16_t actual_power{}; 56 | 57 | /** 58 | * Current system vacuum. Unit: \f$[mbar]\f$. 59 | */ 60 | uint16_t vacuum{}; 61 | 62 | /** 63 | * Strictly monotonically increasing timestamp since robot start. 64 | */ 65 | Duration time{}; 66 | }; 67 | 68 | /** 69 | * Streams the vacuum gripper state as JSON object: 70 | * {"field_name_1": value, "field_name_2": value, ...} 71 | * 72 | * @param[in] ostream Ostream instance 73 | * @param[in] vacuum_gripper_state VacuumGripperState struct instance to stream 74 | * 75 | * @return Ostream instance 76 | */ 77 | std::ostream& operator<<(std::ostream& ostream, 78 | const franka::VacuumGripperState& vacuum_gripper_state); 79 | 80 | } // namespace franka 81 | -------------------------------------------------------------------------------- /test/mock_robot_impl.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | using namespace franka; 10 | using namespace research_interface; 11 | 12 | class RobotImplMock : public Robot::Impl { 13 | public: 14 | virtual ~RobotImplMock() = default; 15 | RobotImplMock(std::unique_ptr network, size_t log_size, RealtimeConfig realtime_config) 16 | : Robot::Impl(std::move(network), log_size, realtime_config) {}; 17 | MOCK_METHOD(uint32_t, 18 | startMotion, 19 | (research_interface::robot::Move::ControllerMode, 20 | research_interface::robot::Move::MotionGeneratorMode, 21 | const research_interface::robot::Move::Deviation&, 22 | const research_interface::robot::Move::Deviation&, 23 | bool, 24 | const std::optional>&), 25 | (override)); 26 | MOCK_METHOD(void, 27 | finishMotion, 28 | (uint32_t, 29 | const std::optional&, 30 | const std::optional&), 31 | (override)); 32 | MOCK_METHOD(void, cancelMotion, (uint32_t), (override)); 33 | MOCK_METHOD1(writeOnce, void(const Torques& control_input)); 34 | MOCK_METHOD1(writeOnce, void(const JointPositions& motion_input)); 35 | MOCK_METHOD1(writeOnce, void(const JointVelocities& motion_input)); 36 | MOCK_METHOD1(writeOnce, void(const CartesianPose& motion_input)); 37 | MOCK_METHOD1(writeOnce, void(const CartesianVelocities& motion_input)); 38 | MOCK_METHOD2(writeOnce, void(const JointPositions& motion_input, const Torques& control_input)); 39 | MOCK_METHOD2(writeOnce, void(const JointVelocities& motion_input, const Torques& control_input)); 40 | MOCK_METHOD2(writeOnce, void(const CartesianPose& motion_input, const Torques& control_input)); 41 | MOCK_METHOD2(writeOnce, 42 | void(const CartesianVelocities& motion_input, const Torques& control_input)); 43 | MOCK_METHOD0(readOnce, RobotState()); 44 | MOCK_METHOD2(throwOnMotionError, void(const RobotState& robot_state, uint32_t motion_id)); 45 | }; 46 | -------------------------------------------------------------------------------- /pylibfranka/examples/move_gripper.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2025 Franka Robotics GmbH 4 | # Use of this source code is governed by the Apache-2.0 license, see LICENSE 5 | 6 | import argparse 7 | import time 8 | 9 | from pylibfranka import Gripper 10 | 11 | 12 | def main(): 13 | # Parse command line arguments 14 | parser = argparse.ArgumentParser() 15 | parser.add_argument("--ip", type=str, required=True, help="Robot IP address") 16 | parser.add_argument("--width", type=float, default=0.005, help="Object width to grasp") 17 | parser.add_argument( 18 | "--homing", type=int, default=1, choices=[0, 1], help="Perform homing (0 or 1)" 19 | ) 20 | parser.add_argument("--speed", type=float, default=0.1, help="Gripper speed") 21 | parser.add_argument("--force", type=float, default=60, help="Gripper force") 22 | args = parser.parse_args() 23 | 24 | try: 25 | # Connect to gripper 26 | gripper = Gripper(args.ip) 27 | grasping_width = args.width 28 | 29 | if args.homing: 30 | # Do a homing in order to estimate the maximum grasping width with the current fingers 31 | print("Homing gripper") 32 | gripper.homing() 33 | 34 | time.sleep(2.0) 35 | 36 | # Check for the maximum grasping width 37 | gripper_state = gripper.read_once() 38 | print(f"Gripper width: {gripper_state.width}") 39 | print(f"Gripper is grasped: {gripper_state.is_grasped}") 40 | print(f"Gripper temperature: {gripper_state.temperature}") 41 | print(f"Gripper time: {gripper_state.time.to_sec()}") 42 | 43 | # Grasp the object 44 | if not gripper.grasp(grasping_width, args.speed, args.force): 45 | print("Failed to grasp object.") 46 | return -1 47 | 48 | # Wait 3s and check afterwards, if the object is still grasped 49 | time.sleep(3.0) 50 | 51 | gripper_state = gripper.read_once() 52 | if not gripper_state.is_grasped: 53 | print("Object lost.") 54 | return -1 55 | 56 | print("Grasped object, will release it now.") 57 | gripper.stop() 58 | 59 | except Exception as e: 60 | print(f"Error occurred: {e}") 61 | return -1 62 | 63 | return 0 64 | 65 | 66 | if __name__ == "__main__": 67 | main() 68 | -------------------------------------------------------------------------------- /examples/joint_point_to_point_motion.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "examples_common.h" 10 | 11 | /** 12 | * @example joint_point_to_point_motion.cpp 13 | * An example that moves the robot to a target position by commanding joint positions. 14 | * 15 | * @warning Before executing this example, make sure there is enough space in front of the robot. 16 | */ 17 | 18 | int main(int argc, char** argv) { 19 | if (argc != 10) { 20 | std::cerr << "Usage: " << argv[0] << " " 21 | << " " 22 | << "" << std::endl 23 | << "joint0 to joint6 are joint angles in [rad]." << std::endl 24 | << "speed-factor must be between zero and one." << std::endl; 25 | return -1; 26 | } 27 | try { 28 | franka::Robot robot(argv[1]); 29 | setDefaultBehavior(robot); 30 | 31 | std::array q_goal; 32 | for (size_t i = 0; i < 7; i++) { 33 | q_goal[i] = std::stod(argv[i + 2]); 34 | } 35 | double speed_factor = std::stod(argv[9]); 36 | 37 | // Set additional parameters always before the control loop, NEVER in the control loop! 38 | // Set collision behavior. 39 | robot.setCollisionBehavior( 40 | {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 41 | {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, 42 | {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 43 | {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); 44 | 45 | MotionGenerator motion_generator(speed_factor, q_goal); 46 | std::cout << "WARNING: This example will move the robot! " 47 | << "Please make sure to have the user stop button at hand!" << std::endl 48 | << "Press Enter to continue..." << std::endl; 49 | std::cin.ignore(); 50 | robot.control(motion_generator); 51 | std::cout << "Motion finished" << std::endl; 52 | } catch (const franka::Exception& e) { 53 | std::cout << e.what() << std::endl; 54 | return -1; 55 | } 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.4) 2 | 3 | project(libfranka-examples CXX) 4 | 5 | list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_CURRENT_LIST_DIR}/../cmake) 6 | 7 | set(CMAKE_CXX_STANDARD 17) 8 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 9 | 10 | ## Load dependencies 11 | include(FetchFMT) 12 | 13 | if(NOT FRANKA_IS_FOUND) 14 | find_package(Franka REQUIRED) 15 | endif() 16 | find_package(Eigen3 REQUIRED) 17 | find_package(Poco REQUIRED COMPONENTS Foundation) 18 | 19 | set(THREADS_PREFER_PTHREAD_FLAG ON) 20 | find_package(Threads REQUIRED) 21 | 22 | add_library(examples_common STATIC 23 | examples_common.cpp 24 | ) 25 | 26 | target_link_libraries(examples_common PUBLIC Franka::Franka Eigen3::Eigen3) 27 | target_include_directories(examples_common PUBLIC 28 | $ 29 | $ 30 | ) 31 | set(EXAMPLES 32 | async_position_control 33 | cartesian_impedance_control 34 | communication_test 35 | echo_robot_state 36 | force_control 37 | generate_cartesian_pose_motion 38 | generate_cartesian_pose_motion_external_control_loop 39 | generate_cartesian_velocity_motion 40 | generate_cartesian_velocity_motion_external_control_loop 41 | generate_consecutive_motions 42 | generate_elbow_motion 43 | generate_joint_position_motion 44 | generate_joint_position_motion_external_control_loop 45 | generate_joint_velocity_motion 46 | generate_joint_velocity_motion_external_control_loop 47 | grasp_object 48 | joint_impedance_control 49 | joint_point_to_point_motion 50 | motion_with_control 51 | motion_with_control_external_control_loop 52 | print_joint_poses 53 | vacuum_object 54 | ) 55 | 56 | foreach(example ${EXAMPLES}) 57 | add_executable(${example} ${example}.cpp) 58 | target_include_directories(${example} PUBLIC 59 | $ 60 | $ 61 | ) 62 | target_link_libraries(${example} Franka::Franka examples_common Eigen3::Eigen3) 63 | endforeach() 64 | 65 | target_link_libraries(joint_impedance_control Threads::Threads) 66 | target_link_libraries(motion_with_control Poco::Foundation) 67 | target_link_libraries(motion_with_control_external_control_loop Poco::Foundation) 68 | 69 | include(GNUInstallDirs) 70 | install(TARGETS ${EXAMPLES} 71 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 72 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} 73 | ) 74 | 75 | # Further examples 76 | add_subdirectory(utility_examples) 77 | -------------------------------------------------------------------------------- /examples/examples_common.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | /** 15 | * @file examples_common.h 16 | * Contains common types and functions for the examples. 17 | */ 18 | 19 | /** 20 | * Sets a default collision behavior, joint impedance and Cartesian impedance. 21 | * 22 | * @param[in] robot Robot instance to set behavior on. 23 | */ 24 | void setDefaultBehavior(franka::Robot& robot); 25 | 26 | /** 27 | * An example showing how to generate a joint pose motion to a goal position. Adapted from: 28 | * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots 29 | * (Kogan Page Science Paper edition). 30 | */ 31 | class MotionGenerator { 32 | public: 33 | /** 34 | * Creates a new MotionGenerator instance for a target q. 35 | * 36 | * @param[in] speed_factor General speed factor in range [0, 1]. 37 | * @param[in] q_goal Target joint positions. 38 | */ 39 | MotionGenerator(double speed_factor, const std::array& q_goal); 40 | 41 | /** 42 | * Sends joint position calculations 43 | * 44 | * @param[in] robot_state Current state of the robot. 45 | * @param[in] period Duration of execution. 46 | * 47 | * @return Joint positions for use inside a control loop. 48 | */ 49 | franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period); 50 | 51 | private: 52 | using Vector7d = Eigen::Matrix; 53 | using Vector7i = Eigen::Matrix; 54 | 55 | bool calculateDesiredValues(double time, Vector7d* delta_q_d) const; 56 | void calculateSynchronizedValues(); 57 | 58 | static constexpr double kDeltaQMotionFinished = 1e-6; 59 | Vector7d q_goal_; 60 | 61 | Vector7d q_start_; 62 | Vector7d delta_q_; 63 | 64 | Vector7d dq_max_sync_; 65 | Vector7d t_1_sync_; 66 | Vector7d t_2_sync_; 67 | Vector7d t_f_sync_; 68 | Vector7d q_1_; 69 | 70 | double time_ = 0.0; 71 | 72 | Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished(); 73 | Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); 74 | Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); 75 | }; 76 | -------------------------------------------------------------------------------- /include/franka/active_motion_generator.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include "active_control.h" 6 | 7 | /** 8 | * @file active_motion_generator.h 9 | * Contains the `franka::ActiveMotionGenerator` type. 10 | */ 11 | 12 | namespace franka { 13 | 14 | /** 15 | * Allows the user to read the state of a Robot and to send new motion generator commands after 16 | * starting a control process of a Robot. 17 | * 18 | * hint: To create an ActiveMotionGenerator, see franka::Robot 19 | * 20 | */ 21 | template 22 | class ActiveMotionGenerator : public ActiveControl { 23 | public: 24 | ~ActiveMotionGenerator() override = default; 25 | 26 | /** 27 | * Updates the motion generator commands of an active control 28 | * 29 | * @param motion_generator_input the new motion generator input 30 | * @param control_input optional: the external control input for each joint, if an external 31 | * controller is used 32 | * 33 | * @throw ControlException if an error related to torque control or motion generation occurred, 34 | or 35 | * the motion was already finished. 36 | * @throw NetworkException if the connection is lost, e.g. after a timeout. 37 | */ 38 | void writeOnce(const MotionGeneratorType& motion_generator_input, 39 | const std::optional& control_input) override; 40 | /** 41 | * franka::Robot as friend to allow construction of ActiveMotionGenerator in 42 | * startControl methods 43 | * 44 | */ 45 | friend class Robot; 46 | 47 | private: 48 | /** 49 | * Construct a new ActiveMotionGenerator object 50 | * 51 | * @param robot shared_ptr to the Robot::Impl in the Robot 52 | * @param motion_id id of the managed motion 53 | * @param control_lock of the Robot, preventing other read and write accesses during the active 54 | * control 55 | * @param controller_type defining which controller shall be used 56 | */ 57 | ActiveMotionGenerator(std::shared_ptr robot_impl, 58 | uint32_t motion_id, 59 | std::unique_lock control_lock, 60 | research_interface::robot::Move::ControllerMode controller_type) 61 | : ActiveControl(robot_impl, motion_id, std::move(control_lock)), 62 | controller_type_(controller_type) {}; 63 | 64 | bool isTorqueControlFinished(const std::optional& control_input); 65 | 66 | research_interface::robot::Move::ControllerMode controller_type_; 67 | }; 68 | } // namespace franka 69 | -------------------------------------------------------------------------------- /.github/workflows/pylibfranka-docs.yml: -------------------------------------------------------------------------------- 1 | name: Generate Sphinx Documentation for pylibfranka 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | 8 | jobs: 9 | sphinx-docs: 10 | runs-on: ubuntu-latest 11 | 12 | steps: 13 | - name: Checkout repository 14 | uses: actions/checkout@v4 15 | with: 16 | submodules: 'recursive' 17 | fetch-depth: 0 # Fetch all history including tags for version detection 18 | 19 | - name: Build Docker Image 20 | uses: docker/build-push-action@v4 21 | with: 22 | context: .ci/ 23 | file: .ci/Dockerfile 24 | tags: pylibfranka:latest 25 | build-args: | 26 | UBUNTU_VERSION=20.04 27 | push: false 28 | load: true 29 | 30 | - name: Setup Dependencies 31 | uses: addnab/docker-run-action@v3 32 | with: 33 | image: pylibfranka:latest 34 | options: -v ${{ github.workspace }}:/workspace 35 | run: | 36 | cd /workspace/pylibfranka 37 | ./scripts/setup_dependencies.sh 38 | 39 | - name: Build and Install pylibfranka and Generate Docs 40 | uses: addnab/docker-run-action@v3 41 | with: 42 | image: pylibfranka:latest 43 | options: -v ${{ github.workspace }}:/workspace 44 | run: | 45 | cd /workspace 46 | export PATH="$HOME/.local/bin:$PATH" 47 | export LC_ALL=C.UTF-8 48 | export LANG=C.UTF-8 49 | 50 | # Install pylibfranka 51 | pip3 install . 52 | 53 | # Move to docs directory 54 | cd pylibfranka/docs 55 | 56 | # Install Sphinx requirements 57 | pip3 install -r requirements.txt --user 58 | 59 | # Get version from installed package (synced from CMakeLists.txt) 60 | VERSION=$(python3 -c "import pylibfranka; print(pylibfranka.__version__)") 61 | echo "Building documentation for version: $VERSION" 62 | 63 | # Build documentation 64 | make clean 65 | make html 66 | 67 | # Prepare output directory with pylibfranka subfolder 68 | mkdir -p /workspace/docs_output/pylibfranka/${VERSION} 69 | cp -r _build/html/* /workspace/docs_output/pylibfranka/${VERSION}/ 70 | 71 | # Also copy to 'latest' directory for easy access 72 | mkdir -p /workspace/docs_output/pylibfranka/latest 73 | cp -r _build/html/* /workspace/docs_output/pylibfranka/latest/ 74 | 75 | - name: Deploy to GitHub Pages 76 | uses: peaceiris/actions-gh-pages@v3 77 | with: 78 | github_token: ${{ secrets.GITHUB_TOKEN }} 79 | publish_branch: gh-pages 80 | publish_dir: docs_output 81 | keep_files: true 82 | -------------------------------------------------------------------------------- /cmake/FindTinyXML2.cmake: -------------------------------------------------------------------------------- 1 | # FindTinyXML2.cmake - Find TinyXML2 library 2 | # 3 | # This module defines: 4 | # TinyXML2_FOUND - True if TinyXML2 is found 5 | # TinyXML2_INCLUDE_DIRS - Include directories for TinyXML2 6 | # TinyXML2_LIBRARIES - Libraries to link against TinyXML2 7 | # TinyXML2::TinyXML2 - Imported target for TinyXML2 8 | 9 | # First, try a CMake config package (common when built from source) 10 | set(_tinyxml2_config_found FALSE) 11 | find_package(tinyxml2 QUIET CONFIG) 12 | if(tinyxml2_FOUND) 13 | # Try common target names 14 | if(TARGET tinyxml2::tinyxml2) 15 | add_library(TinyXML2::TinyXML2 INTERFACE IMPORTED) 16 | target_link_libraries(TinyXML2::TinyXML2 INTERFACE tinyxml2::tinyxml2) 17 | set(_tinyxml2_config_found TRUE) 18 | elseif(TARGET tinyxml2) 19 | add_library(TinyXML2::TinyXML2 INTERFACE IMPORTED) 20 | target_link_libraries(TinyXML2::TinyXML2 INTERFACE tinyxml2) 21 | set(_tinyxml2_config_found TRUE) 22 | endif() 23 | endif() 24 | 25 | if(NOT _tinyxml2_config_found) 26 | # Fallback to pkg-config and manual discovery (typical on Ubuntu/Debian) 27 | find_package(PkgConfig QUIET) 28 | if(PkgConfig_FOUND) 29 | pkg_check_modules(PC_TinyXML2 QUIET tinyxml2) 30 | endif() 31 | 32 | find_path(TinyXML2_INCLUDE_DIR 33 | NAMES tinyxml2.h 34 | PATHS ${PC_TinyXML2_INCLUDE_DIRS} 35 | PATH_SUFFIXES include 36 | ) 37 | 38 | find_library(TinyXML2_LIBRARY 39 | NAMES tinyxml2 40 | PATHS ${PC_TinyXML2_LIBRARY_DIRS} 41 | PATH_SUFFIXES lib lib64 42 | ) 43 | 44 | include(FindPackageHandleStandardArgs) 45 | find_package_handle_standard_args(TinyXML2 46 | REQUIRED_VARS TinyXML2_LIBRARY TinyXML2_INCLUDE_DIR 47 | VERSION_VAR PC_TinyXML2_VERSION 48 | ) 49 | 50 | if(TinyXML2_FOUND) 51 | set(TinyXML2_LIBRARIES ${TinyXML2_LIBRARY}) 52 | set(TinyXML2_INCLUDE_DIRS ${TinyXML2_INCLUDE_DIR}) 53 | 54 | if(NOT TARGET TinyXML2::TinyXML2) 55 | add_library(TinyXML2::TinyXML2 UNKNOWN IMPORTED) 56 | set_target_properties(TinyXML2::TinyXML2 PROPERTIES 57 | IMPORTED_LOCATION "${TinyXML2_LIBRARY}" 58 | INTERFACE_INCLUDE_DIRECTORIES "${TinyXML2_INCLUDE_DIR}" 59 | ) 60 | endif() 61 | endif() 62 | 63 | mark_as_advanced(TinyXML2_INCLUDE_DIR TinyXML2_LIBRARY) 64 | else() 65 | # Config-based discovery succeeded 66 | set(TinyXML2_FOUND TRUE) 67 | # TinyXML2_INCLUDE_DIRS and TinyXML2_LIBRARIES are not strictly needed when using targets, 68 | # but set minimal placeholders for compatibility. 69 | set(TinyXML2_INCLUDE_DIRS "") 70 | set(TinyXML2_LIBRARIES "") 71 | endif() 72 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = [ 3 | "setuptools>=61.0", 4 | "wheel", 5 | "pybind11>=2.6.0", 6 | "numpy>=1.19.0", 7 | "cmake>=3.16", 8 | "patchelf>=0.10", 9 | "auditwheel>=6.0.0", 10 | "build>=0.10", 11 | ] 12 | build-backend = "setuptools.build_meta" 13 | 14 | [project] 15 | name = "pylibfranka" 16 | dynamic = ["version"] 17 | description = "Python bindings for libfranka with automatic dependency bundling" 18 | authors = [{name = "Franka Robotics GmbH", email = "info@franka.de"}] 19 | license = {text = "Apache-2.0"} 20 | requires-python = ">=3.8" 21 | dependencies = [ 22 | "numpy>=1.19.0", 23 | ] 24 | classifiers = [ 25 | "Development Status :: 4 - Beta", 26 | "Intended Audience :: Developers", 27 | "License :: OSI Approved :: Apache Software License", 28 | "Programming Language :: Python :: 3.8", 29 | "Programming Language :: Python :: 3.9", 30 | "Programming Language :: Python :: 3.10", 31 | "Programming Language :: Python :: 3.11", 32 | "Topic :: Scientific/Engineering :: Robotics", 33 | ] 34 | 35 | [project.urls] 36 | Homepage = "https://github.com/frankarobotics/libfranka" 37 | Repository = "https://github.com/frankarobotics/libfranka" 38 | Issues = "https://github.com/frankarobotics/libfranka/issues" 39 | 40 | [project.optional-dependencies] 41 | dev = [ 42 | "pre-commit>=3.3.2", 43 | "black>=23.3.0", 44 | "isort>=5.12.0", 45 | "flake8>=6.0.0", 46 | "flake8-docstrings>=1.7.0", 47 | "mypy>=1.3.0", 48 | "pytest>=7.3.1", 49 | ] 50 | 51 | [tool.setuptools] 52 | packages = ["pylibfranka"] 53 | zip-safe = false 54 | 55 | [tool.setuptools.dynamic] 56 | version = {attr = "pylibfranka._version.__version__"} 57 | 58 | [tool.setuptools.package-data] 59 | pylibfranka = ["*.so", "*.pyd", "_pylibfranka*"] 60 | 61 | [tool.cibuildwheel] 62 | # Build only on modern Python versions 63 | build = "cp38-* cp39-* cp310-* cp311-*" 64 | skip = "pp* *-manylinux_i686" 65 | 66 | # Install system dependencies before building 67 | before-all = [ 68 | "yum install -y eigen3-devel poco-devel || true", 69 | "apt-get update && apt-get install -y libeigen3-dev libpoco-dev || true", 70 | "pip install pybind11 cmake" 71 | ] 72 | 73 | # Build requirements 74 | build-verbosity = 1 75 | 76 | [tool.cibuildwheel.linux] 77 | # Use manylinux2014 for better compatibility 78 | manylinux-x86_64-image = "manylinux2014" 79 | manylinux-aarch64-image = "manylinux2014" 80 | 81 | # Let auditwheel bundle dependencies automatically 82 | repair-wheel-command = "auditwheel repair -w {dest_dir} {wheel} --lib-sdir ." 83 | 84 | [tool.black] 85 | line-length = 100 86 | target-version = ["py38", "py39", "py310", "py311"] 87 | 88 | [tool.isort] 89 | profile = "black" 90 | line_length = 100 91 | -------------------------------------------------------------------------------- /src/active_motion_generator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "robot_impl.h" 10 | 11 | namespace franka { 12 | 13 | template 14 | bool ActiveMotionGenerator::isTorqueControlFinished( 15 | const std::optional& control_input) { 16 | if (!control_input.has_value()) { 17 | return false; 18 | } 19 | return control_input.value().motion_finished; 20 | } 21 | 22 | template 23 | void ActiveMotionGenerator::writeOnce( 24 | const MotionGeneratorType& motion_generator_input, 25 | const std::optional& control_input) { 26 | if (control_finished) { 27 | throw franka::ControlException("writeOnce must not be called after the motion has finished."); 28 | } 29 | 30 | if (control_input.has_value() && 31 | controller_type_ != research_interface::robot::Move::ControllerMode::kExternalController) { 32 | throw franka::ControlException("Torques can only be commanded in kExternalController mode."); 33 | } 34 | 35 | if (!control_input.has_value() && 36 | controller_type_ == research_interface::robot::Move::ControllerMode::kExternalController) { 37 | throw franka::ControlException( 38 | "Torque command missing, please use writeOnce(const MotionGeneratorType& " 39 | "motion_generator_input, const Torques& control_input) for external controllers."); 40 | } 41 | 42 | if (motion_generator_input.motion_finished || isTorqueControlFinished(control_input)) { 43 | auto motion_command = robot_impl->createMotionCommand(motion_generator_input); 44 | if (!control_input.has_value()) { 45 | robot_impl->finishMotion(motion_id, motion_command, std::nullopt); 46 | } else { 47 | auto control_command = robot_impl->createControllerCommand(control_input.value()); 48 | robot_impl->finishMotion(motion_id, motion_command, control_command); 49 | } 50 | 51 | control_finished = true; 52 | control_lock.unlock(); 53 | return; 54 | } 55 | 56 | if (!control_input.has_value()) { 57 | robot_impl->writeOnce(motion_generator_input); 58 | return; 59 | } 60 | 61 | robot_impl->writeOnce(motion_generator_input, control_input.value()); 62 | } 63 | 64 | template class ActiveMotionGenerator; 65 | template class ActiveMotionGenerator; 66 | template class ActiveMotionGenerator; 67 | template class ActiveMotionGenerator; 68 | 69 | } // namespace franka 70 | -------------------------------------------------------------------------------- /include/franka/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | /** 9 | * @file lowpass_filter.h 10 | * Contains functions for filtering signals with a low-pass filter. 11 | */ 12 | 13 | namespace franka { 14 | /** 15 | * Maximum cutoff frequency 16 | */ 17 | constexpr double kMaxCutoffFrequency = 1000.0; 18 | /** 19 | * Default cutoff frequency 20 | */ 21 | constexpr double kDefaultCutoffFrequency = 100.0; 22 | /** 23 | * Applies a first-order low-pass filter 24 | * 25 | * @param[in] sample_time Sample time constant 26 | * @param[in] current_signal_value Current value of the signal to be filtered 27 | * @param[in] last_signal_value Value of the signal to be filtered in the previous time step 28 | * @param[in] cutoff_frequency Cutoff frequency of the low-pass filter 29 | * 30 | * @throw std::invalid_argument if current_signal_value is infinite or NaN. 31 | * @throw std::invalid_argument if last_signal_value is infinite or NaN. 32 | * @throw std::invalid_argument if cutoff_frequency is zero, negative, infinite or NaN. 33 | * @throw std::invalid_argument if sample_time is negative, infinite or NaN. 34 | * 35 | * @return Filtered value. 36 | */ 37 | double lowpassFilter(double sample_time, 38 | double current_signal_value, 39 | double last_signal_value, 40 | double cutoff_frequency); 41 | 42 | /** 43 | * Applies a first-order low-pass filter to the translation and spherical linear interpolation 44 | * to the rotation of a transformation matrix which represents a Cartesian Motion. 45 | * 46 | * @param[in] sample_time Sample time constant 47 | * @param[in] current_signal_value Current Cartesian transformation matrix to be filtered 48 | * @param[in] last_signal_value Cartesian transformation matrix from the previous time step 49 | * @param[in] cutoff_frequency Cutoff frequency of the low-pass filter 50 | * 51 | * @throw std::invalid_argument if elements of current_signal_value is infinite or NaN. 52 | * @throw std::invalid_argument if elements of last_signal_value is infinite or NaN. 53 | * @throw std::invalid_argument if cutoff_frequency is zero, negative, infinite or NaN. 54 | * @throw std::invalid_argument if sample_time is negative, infinite or NaN. 55 | * 56 | * @return Filtered Cartesian transformation matrix. 57 | */ 58 | 59 | std::array cartesianLowpassFilter(double sample_time, 60 | std::array current_signal_value, 61 | std::array last_signal_value, 62 | double cutoff_frequency); 63 | } // namespace franka 64 | -------------------------------------------------------------------------------- /include/franka/logging/logger.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | /** 10 | * Provides logging functionality. A user can implement a custom logger by inheriting from the 11 | * LoggingSinkInterface and adding it the stored loggers. 12 | * Now, whenever libfranka wants to log anything, it will call the logInfo, logWarn, or logError 13 | * methods of all stored loggers. 14 | */ 15 | namespace franka::logging { 16 | 17 | /** 18 | * Adds a logger to the list of loggers. 19 | * @param logger The logger to add. 20 | */ 21 | auto addLogger(const std::shared_ptr& logger) -> void; 22 | 23 | /** 24 | * Removes a logger from the list of loggers. 25 | * @param logger_name The name of the logger to remove. 26 | */ 27 | auto removeLogger(const std::string& logger_name) -> void; 28 | 29 | /** 30 | * Removes all loggers from the list of loggers. 31 | */ 32 | auto removeAllLoggers() -> void; 33 | 34 | /** 35 | * Logs an info message. 36 | * @param message The message to log. 37 | */ 38 | auto logInfo(const std::string& message) -> void; 39 | 40 | /** 41 | * Logs an info message. 42 | * @tparam S The type of the format string. 43 | * @tparam Args The types of the arguments. 44 | * @param format_str The format string. 45 | * @param args The arguments. 46 | */ 47 | template 48 | auto logInfo(const S& format_str, Args&&... args) -> void { 49 | logInfo(fmt::format(format_str, std::forward(args)...)); 50 | } 51 | 52 | /** 53 | * Logs a warning message. 54 | * @param message The message to log. 55 | */ 56 | auto logWarn(const std::string& message) -> void; 57 | 58 | /** 59 | * Logs a warning message. 60 | * @tparam S The type of the format string. 61 | * @tparam Args The types of the arguments. 62 | * @param format_str The format string. 63 | * @param args The arguments. 64 | */ 65 | template 66 | auto logWarn(const S& format_str, Args&&... args) -> void { 67 | logWarn(fmt::format(format_str, std::forward(args)...)); 68 | } 69 | 70 | /** 71 | * Logs an error message. 72 | * @param message The message to log. 73 | */ 74 | auto logError(const std::string& message) -> void; 75 | 76 | /** 77 | * Logs an error message. 78 | * @tparam S The type of the format string. 79 | * @tparam Args The types of the arguments. 80 | * @param format_str The format string. 81 | * @param args The arguments. 82 | */ 83 | template 84 | auto logError(const S& format_str, Args&&... args) -> void { 85 | logError(fmt::format(format_str, std::forward(args)...)); 86 | } 87 | 88 | } // namespace franka::logging 89 | -------------------------------------------------------------------------------- /scripts/mimic-jenkins.bash: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | set -x # Print commands and their arguments as they are executed 3 | set -e # Exit immediately on error 4 | trap 'echo "Error: Script failed at line $LINENO" >&2; exit 1' ERR 5 | 6 | # This script performs the same actions that the Jenkins job will perform. 7 | # EXCEPT for publishing 8 | # It's done in a script so that you don't have to do this manually. 9 | 10 | # IMPORTANT ! 11 | # In this script STRICT is set to OFF (-werror is not on), 12 | # so that the build does not fail on warnings. 13 | # The real Jenkins Job has STRICT=ON and will fail on warnings 14 | 15 | echo 'Clean Workspace build dirs' 16 | rm -rf build-* 17 | 18 | echo 'Build' 19 | ## Building libfranka will use all your cores, virtual or otherwise 20 | ## Also, will consume about 16GB RAM - if available. 21 | ## This helps you maintain some control while the build progresses. 22 | TASKS=$(( $(nproc) > 1 ? $(nproc) - 1 : 1 )) 23 | echo "Using $TASKS build tasks" 24 | 25 | echo 'Build debug' 26 | mkdir build-debug && pushd build-debug 27 | cmake -DCMAKE_BUILD_TYPE=Debug -DSTRICT=OFF -DBUILD_COVERAGE=OFF \ 28 | -DBUILD_DOCUMENTATION=OFF -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. 29 | make -j${TASKS} 30 | popd 31 | 32 | echo 'Build release' 33 | mkdir build-release && pushd build-release 34 | cmake -DCMAKE_BUILD_TYPE=Release -DSTRICT=OFF -DBUILD_COVERAGE=OFF \ 35 | -DBUILD_DOCUMENTATION=ON -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. 36 | make -j${TASKS} 37 | popd 38 | 39 | echo 'Build examples (debug)' 40 | mkdir build-debug-examples && pushd build-debug-examples 41 | cmake -DFranka_DIR:PATH=../build-debug.${DISTRO} ../examples 42 | make -j${TASKS} 43 | popd 44 | 45 | echo 'Build examples (release)' 46 | mkdir build-release-examples && pushd build-release-examples 47 | cmake -DFranka_DIR:PATH=../build-release.${DISTRO} ../examples 48 | make -j${TASKS} 49 | popd 50 | 51 | echo 'Build coverage' 52 | mkdir build-coverage && pushd build-coverage 53 | cmake -DCMAKE_BUILD_TYPE=Debug -DBUILD_COVERAGE=ON \ 54 | -DBUILD_DOCUMENTATION=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=ON .. 55 | make -j${TASKS} 56 | popd 57 | 58 | echo 'Lint' 59 | mkdir build-lint && pushd build-lint 60 | cmake -DBUILD_COVERAGE=OFF -DBUILD_DOCUMENTATION=OFF -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. 61 | make check-tidy -j${TASKS} 62 | popd 63 | 64 | echo 'Format' 65 | mkdir build-format && pushd build-format 66 | cmake -DBUILD_COVERAGE=OFF -DBUILD_DOCUMENTATION=OFF -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. 67 | make check-format -j${TASKS} 68 | popd 69 | 70 | echo 'Coverage' 71 | mkdir build-coverage && pushd build-coverage 72 | cmake -DBUILD_COVERAGE=ON -DBUILD_DOCUMENTATION=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=ON .. 73 | make coverage -j${TASKS} 74 | popd 75 | 76 | echo 'Test Debug' 77 | mkdir build-debug && pushd build-debug 78 | ctest -V 79 | popd 80 | 81 | pushd build-release 82 | ctest -V 83 | popd 84 | -------------------------------------------------------------------------------- /src/control_tools.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "platform.h" 11 | 12 | #ifdef LIBFRANKA_WINDOWS 13 | #include 14 | #else 15 | #include 16 | #endif 17 | 18 | // `using std::string_literals::operator""s` produces a GCC warning that cannot be disabled, so we 19 | // have to use `using namespace ...`. 20 | // See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=65923#c0 21 | using namespace std::string_literals; // NOLINT(google-build-using-namespace) 22 | 23 | namespace franka { 24 | 25 | bool hasRealtimeKernel() { 26 | #ifdef LIBFRANKA_WINDOWS 27 | return true; 28 | #else 29 | std::ifstream realtime("/sys/kernel/realtime", std::ios_base::in); 30 | bool is_realtime{}; 31 | realtime >> is_realtime; 32 | return is_realtime; 33 | #endif 34 | } 35 | 36 | bool setCurrentThreadToHighestSchedulerPriority(std::string* error_message) { 37 | #ifdef LIBFRANKA_WINDOWS 38 | auto get_last_windows_error = []() -> std::string { 39 | DWORD error_id = GetLastError(); 40 | LPSTR buffer = nullptr; 41 | size_t size = FormatMessageA( 42 | FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, 43 | nullptr, error_id, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPSTR)(&buffer), 0, nullptr); 44 | return std::string(buffer, size); 45 | }; 46 | 47 | if (!SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS)) { 48 | if (error_message != nullptr) { 49 | *error_message = 50 | "libfranka: unable to set priority for the process: "s + get_last_windows_error(); 51 | } 52 | return false; 53 | } 54 | 55 | if (!SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL)) { 56 | if (error_message != nullptr) { 57 | *error_message = 58 | "libfranka: unable to set priority for the thread: "s + get_last_windows_error(); 59 | } 60 | return false; 61 | } 62 | 63 | return true; 64 | #else 65 | const int thread_priority = sched_get_priority_max(SCHED_FIFO); 66 | if (thread_priority == -1) { 67 | if (error_message != nullptr) { 68 | *error_message = 69 | "libfranka: unable to get maximum possible thread priority: "s + std::strerror(errno); 70 | } 71 | return false; 72 | } 73 | 74 | sched_param thread_param{}; 75 | thread_param.sched_priority = thread_priority; 76 | if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &thread_param) != 0) { 77 | if (error_message != nullptr) { 78 | *error_message = "libfranka: unable to set realtime scheduling: "s + std::strerror(errno); 79 | } 80 | return false; 81 | } 82 | return true; 83 | #endif 84 | } 85 | 86 | } // namespace franka 87 | -------------------------------------------------------------------------------- /examples/generate_joint_velocity_motion.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "examples_common.h" 10 | 11 | /** 12 | * @example generate_joint_velocity_motion.cpp 13 | * An example showing how to generate a joint velocity motion. 14 | * 15 | * @warning Before executing this example, make sure there is enough space in front of the robot. 16 | */ 17 | 18 | int main(int argc, char** argv) { 19 | if (argc != 2) { 20 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 21 | return -1; 22 | } 23 | try { 24 | franka::Robot robot(argv[1]); 25 | setDefaultBehavior(robot); 26 | 27 | // First move the robot to a suitable joint configuration 28 | std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; 29 | MotionGenerator motion_generator(0.5, q_goal); 30 | std::cout << "WARNING: This example will move the robot! " 31 | << "Please make sure to have the user stop button at hand!" << std::endl 32 | << "Press Enter to continue..." << std::endl; 33 | std::cin.ignore(); 34 | robot.control(motion_generator); 35 | std::cout << "Finished moving to initial joint configuration." << std::endl; 36 | 37 | // Set additional parameters always before the control loop, NEVER in the control loop! 38 | // Set collision behavior. 39 | robot.setCollisionBehavior( 40 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 41 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 42 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, 43 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); 44 | 45 | double time_max = 1.0; 46 | double omega_max = 1.0; 47 | double time = 0.0; 48 | robot.control( 49 | [=, &time](const franka::RobotState&, franka::Duration period) -> franka::JointVelocities { 50 | time += period.toSec(); 51 | 52 | double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); 53 | double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); 54 | 55 | franka::JointVelocities velocities = {{0.0, 0.0, 0.0, omega, omega, omega, omega}}; 56 | 57 | if (time >= 2 * time_max) { 58 | std::cout << std::endl << "Finished motion, shutting down example" << std::endl; 59 | return franka::MotionFinished(velocities); 60 | } 61 | return velocities; 62 | }); 63 | } catch (const franka::Exception& e) { 64 | std::cout << e.what() << std::endl; 65 | return -1; 66 | } 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /.github/workflows/libfranka-build.yml: -------------------------------------------------------------------------------- 1 | name: Build libfranka Debian Packages 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | workflow_dispatch: 8 | 9 | jobs: 10 | build-deb: 11 | runs-on: ubuntu-latest 12 | strategy: 13 | fail-fast: false 14 | matrix: 15 | ubuntu_version: ["20.04", "22.04", "24.04"] 16 | 17 | steps: 18 | - name: Checkout repository 19 | uses: actions/checkout@v4 20 | with: 21 | submodules: 'recursive' 22 | fetch-depth: 0 23 | 24 | - name: Set up Docker Buildx 25 | uses: docker/setup-buildx-action@v3 26 | 27 | - name: Build Docker image 28 | uses: docker/build-push-action@v4 29 | with: 30 | context: .ci/ 31 | file: .ci/Dockerfile 32 | build-args: | 33 | UBUNTU_VERSION=${{ matrix.ubuntu_version }} 34 | tags: libfranka-build:${{ matrix.ubuntu_version }} 35 | push: false 36 | load: true 37 | cache-from: type=gha,scope=${{ matrix.ubuntu_version }} 38 | cache-to: type=gha,mode=max,scope=${{ matrix.ubuntu_version }} 39 | 40 | - name: Build and package in container 41 | uses: addnab/docker-run-action@v3 42 | with: 43 | image: libfranka-build:${{ matrix.ubuntu_version }} 44 | options: -v ${{ github.workspace }}:/workspaces 45 | run: | 46 | cd /workspaces 47 | cmake -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=OFF -B build -S . 48 | cmake --build build -- -j$(nproc) 49 | cd build 50 | cpack -G DEB 51 | 52 | - name: Generate SHA256 checksums 53 | run: | 54 | cd build 55 | for deb in *.deb; do 56 | if [ -f "$deb" ]; then 57 | sha256sum "$deb" > "${deb}.sha256" 58 | echo "Generated checksum for $deb:" 59 | cat "${deb}.sha256" 60 | fi 61 | done 62 | 63 | - name: List generated files 64 | run: | 65 | echo "Generated packages and checksums:" 66 | ls -lh build/*.deb build/*.sha256 67 | 68 | - name: Upload Debian package and checksum (manual runs) 69 | if: github.event_name == 'workflow_dispatch' 70 | uses: actions/upload-artifact@v4 71 | with: 72 | name: libfranka-${{ matrix.ubuntu_version }} 73 | path: | 74 | build/*.deb 75 | build/*.sha256 76 | retention-days: 30 77 | 78 | - name: Upload to GitHub Release 79 | if: startsWith(github.ref, 'refs/tags/') && matrix.ubuntu_version == '20.04' 80 | uses: softprops/action-gh-release@v2 81 | with: 82 | files: | 83 | build/*.deb 84 | build/*.sha256 85 | generate_release_notes: true 86 | draft: false 87 | prerelease: false 88 | fail_on_unmatched_files: false 89 | env: 90 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 91 | -------------------------------------------------------------------------------- /examples/generate_elbow_motion.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "examples_common.h" 10 | 11 | /** 12 | * @example generate_elbow_motion.cpp 13 | * An example showing how to move the robot's elbow. 14 | * 15 | * @warning Before executing this example, make sure that the elbow has enough space to move. 16 | */ 17 | 18 | int main(int argc, char** argv) { 19 | if (argc != 2) { 20 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 21 | return -1; 22 | } 23 | try { 24 | franka::Robot robot(argv[1]); 25 | setDefaultBehavior(robot); 26 | 27 | // First move the robot to a suitable joint configuration 28 | std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; 29 | MotionGenerator motion_generator(0.5, q_goal); 30 | std::cout << "WARNING: This example will move the robot! " 31 | << "Please make sure to have the user stop button at hand!" << std::endl 32 | << "Press Enter to continue..." << std::endl; 33 | std::cin.ignore(); 34 | robot.control(motion_generator); 35 | std::cout << "Finished moving to initial joint configuration." << std::endl; 36 | 37 | // Set additional parameters always before the control loop, NEVER in the control loop! 38 | // Set collision behavior. 39 | robot.setCollisionBehavior( 40 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 41 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 42 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, 43 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); 44 | 45 | std::array initial_pose; 46 | std::array initial_elbow; 47 | double time = 0.0; 48 | robot.control( 49 | [&time, &initial_pose, &initial_elbow](const franka::RobotState& robot_state, 50 | franka::Duration period) -> franka::CartesianPose { 51 | time += period.toSec(); 52 | 53 | if (time == 0.0) { 54 | initial_pose = robot_state.O_T_EE; 55 | initial_elbow = robot_state.elbow; 56 | } 57 | 58 | double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * time)); 59 | 60 | auto elbow = initial_elbow; 61 | elbow[0] += angle; 62 | 63 | if (time >= 10.0) { 64 | std::cout << std::endl << "Finished motion, shutting down example" << std::endl; 65 | return franka::MotionFinished({initial_pose, elbow}); 66 | } 67 | 68 | return {initial_pose, elbow}; 69 | }); 70 | } catch (const franka::Exception& e) { 71 | std::cout << e.what() << std::endl; 72 | return -1; 73 | } 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /test/lowpass_filter_tests.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "helpers.h" 10 | 11 | using namespace franka; 12 | 13 | const double kNoLimit{std::numeric_limits::max()}; 14 | 15 | TEST(LowpassFilter, KeepsValueIfNoChange) { 16 | EXPECT_NEAR(lowpassFilter(0.001, 1.0, 1.0, 100.0), 1.0, 1e-6); 17 | EXPECT_NEAR(lowpassFilter(0.001, 1.0, 1.0, 500.0), 1.0, 1e-6); 18 | EXPECT_NEAR(lowpassFilter(0.001, 1.0, 1.0, 1000.0), 1.0, 1e-6); 19 | } 20 | 21 | TEST(LowpassFilter, DoesFilter) { 22 | EXPECT_NEAR(lowpassFilter(0.001, 1.0, 0.0, 100.0), 0.3859, 1e-4); 23 | EXPECT_NEAR(lowpassFilter(0.001, 1.0, 0.0, 500.0), 0.7585, 1e-4); 24 | EXPECT_NEAR(lowpassFilter(0.001, 1.0, 0.0, 900.0), 0.8497, 1e-4); 25 | } 26 | 27 | TEST(CartesianLowpassFilter, CanFixNonOrthonormalRotation) { 28 | // These three poses are all only barely orthonormal, such that the cartesianLowpassFilter will 29 | // generate a jerky movement when it does not orthonormalize these first before applying the 30 | // filter. 31 | Eigen::Affine3d pose1; 32 | Eigen::Affine3d pose2; 33 | Eigen::Affine3d pose3; 34 | pose1.translation() << 1, 1, 1; 35 | pose2.translation() << 1, 1, 1; 36 | pose3.translation() << 1, 1, 1; 37 | // clang-format off 38 | pose1.linear() << 0.00462567, 0.999974, 0.00335239, 39 | 0.0145489, -0.00341934, 0.999888, 40 | 0.999874, -0.00457638, -0.0145646; 41 | pose2.linear() << 0.00463526, 0.999984, 0.00335239, 42 | 0.014549, -0.00341951, 0.999888, 43 | 0.999883, -0.00458597, -0.0145646; 44 | pose3.linear() << 0.00465436, 0.999984, 0.00335239, 45 | 0.0145489, -0.00341979, 0.999888, 46 | 0.999883, -0.00460507, -0.0145646; 47 | // clang-format on 48 | std::array pose_array1{}; 49 | std::array pose_array2{}; 50 | std::array pose_array3{}; 51 | Eigen::Map(&pose_array1[0], 4, 4) = pose1.matrix(); 52 | Eigen::Map(&pose_array2[0], 4, 4) = pose2.matrix(); 53 | Eigen::Map(&pose_array3[0], 4, 4) = pose3.matrix(); 54 | 55 | auto output_array1 = cartesianLowpassFilter(0.001, pose_array2, pose_array1, 100.); 56 | auto output_array2 = cartesianLowpassFilter(0.001, pose_array3, pose_array2, 100.); 57 | auto velocity1 = differentiateOneSample(output_array1, pose_array1, 0.001); 58 | auto velocity2 = differentiateOneSample(output_array2, pose_array2, 0.001); 59 | 60 | std::array jerk; 61 | for (int i = 0; i < 6; i++) { 62 | double acceleration1 = velocity1[i] / 0.001; 63 | double acceleration2 = (velocity2[i] - velocity1[i]) / 0.001; 64 | jerk[i] = (acceleration2 - acceleration1) / 0.001; 65 | } 66 | double total_jerk = std::sqrt(std::pow(jerk[3], 2) + std::pow(jerk[4], 2) + std::pow(jerk[5], 2)); 67 | EXPECT_LT(total_jerk, 1000); 68 | } 69 | -------------------------------------------------------------------------------- /examples/generate_cartesian_pose_motion.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "examples_common.h" 10 | 11 | /** 12 | * @example generate_cartesian_pose_motion.cpp 13 | * An example showing how to generate a Cartesian motion. 14 | * 15 | * @warning Before executing this example, make sure there is enough space in front of the robot. 16 | */ 17 | 18 | int main(int argc, char** argv) { 19 | if (argc != 2) { 20 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 21 | return -1; 22 | } 23 | try { 24 | franka::Robot robot(argv[1]); 25 | setDefaultBehavior(robot); 26 | 27 | // First move the robot to a suitable joint configuration 28 | std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; 29 | MotionGenerator motion_generator(0.5, q_goal); 30 | std::cout << "WARNING: This example will move the robot! " 31 | << "Please make sure to have the user stop button at hand!" << std::endl 32 | << "Press Enter to continue..." << std::endl; 33 | std::cin.ignore(); 34 | robot.control(motion_generator); 35 | std::cout << "Finished moving to initial joint configuration." << std::endl; 36 | 37 | // Set additional parameters always before the control loop, NEVER in the control loop! 38 | // Set collision behavior. 39 | robot.setCollisionBehavior( 40 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 41 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 42 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, 43 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); 44 | 45 | std::array initial_pose; 46 | double time = 0.0; 47 | robot.control([&time, &initial_pose](const franka::RobotState& robot_state, 48 | franka::Duration period) -> franka::CartesianPose { 49 | time += period.toSec(); 50 | 51 | if (time == 0.0) { 52 | initial_pose = robot_state.O_T_EE; 53 | } 54 | 55 | constexpr double kRadius = 0.3; 56 | double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time)); 57 | double delta_x = kRadius * std::sin(angle); 58 | double delta_z = kRadius * (std::cos(angle) - 1); 59 | 60 | std::array new_pose = initial_pose; 61 | new_pose[12] += delta_x; 62 | new_pose[14] += delta_z; 63 | 64 | if (time >= 10.0) { 65 | std::cout << std::endl << "Finished motion, shutting down example" << std::endl; 66 | return franka::MotionFinished(new_pose); 67 | } 68 | return new_pose; 69 | }); 70 | } catch (const franka::Exception& e) { 71 | std::cout << e.what() << std::endl; 72 | return -1; 73 | } 74 | 75 | return 0; 76 | } 77 | -------------------------------------------------------------------------------- /examples/generate_joint_position_motion.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "examples_common.h" 10 | 11 | /** 12 | * @example generate_joint_position_motion.cpp 13 | * An example showing how to generate a joint position motion. 14 | * 15 | * @warning Before executing this example, make sure there is enough space in front of the robot. 16 | */ 17 | 18 | int main(int argc, char** argv) { 19 | if (argc != 2) { 20 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 21 | return -1; 22 | } 23 | try { 24 | franka::Robot robot(argv[1]); 25 | setDefaultBehavior(robot); 26 | 27 | // First move the robot to a suitable joint configuration 28 | std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; 29 | MotionGenerator motion_generator(0.5, q_goal); 30 | std::cout << "WARNING: This example will move the robot! " 31 | << "Please make sure to have the user stop button at hand!" << std::endl 32 | << "Press Enter to continue..." << std::endl; 33 | std::cin.ignore(); 34 | robot.control(motion_generator); 35 | std::cout << "Finished moving to initial joint configuration." << std::endl; 36 | 37 | // Set additional parameters always before the control loop, NEVER in the control loop! 38 | // Set collision behavior. 39 | robot.setCollisionBehavior( 40 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 41 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 42 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, 43 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); 44 | 45 | std::array initial_position; 46 | double time = 0.0; 47 | robot.control([&initial_position, &time](const franka::RobotState& robot_state, 48 | franka::Duration period) -> franka::JointPositions { 49 | time += period.toSec(); 50 | 51 | if (time == 0.0) { 52 | initial_position = robot_state.q; 53 | } 54 | 55 | double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time)); 56 | 57 | franka::JointPositions output = {{initial_position[0], initial_position[1], 58 | initial_position[2], initial_position[3] + delta_angle, 59 | initial_position[4] + delta_angle, initial_position[5], 60 | initial_position[6] + delta_angle}}; 61 | 62 | if (time >= 5.0) { 63 | std::cout << std::endl << "Finished motion, shutting down example" << std::endl; 64 | return franka::MotionFinished(output); 65 | } 66 | return output; 67 | }); 68 | } catch (const franka::Exception& e) { 69 | std::cout << e.what() << std::endl; 70 | return -1; 71 | } 72 | 73 | return 0; 74 | } 75 | -------------------------------------------------------------------------------- /src/lowpass_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace franka { 10 | 11 | double lowpassFilter(double sample_time, 12 | double current_signal_value, 13 | double last_signal_value, 14 | double cutoff_frequency) { 15 | if (sample_time < 0 || !std::isfinite(sample_time)) { 16 | throw std::invalid_argument("lowpass-filter: sample_time is negative, infinite or NaN."); 17 | } 18 | if (cutoff_frequency <= 0 || !std::isfinite(cutoff_frequency)) { 19 | throw std::invalid_argument( 20 | "lowpass-filter: cutoff_frequency is zero, negative, infinite or NaN."); 21 | } 22 | if (!std::isfinite(current_signal_value) || !std::isfinite(last_signal_value)) { 23 | throw std::invalid_argument( 24 | "lowpass-filter: current or past input value of the signal to be filtered is infinite or " 25 | "NaN."); 26 | } 27 | double gain = sample_time / (sample_time + (1.0 / (2.0 * M_PI * cutoff_frequency))); 28 | return gain * current_signal_value + (1 - gain) * last_signal_value; 29 | } 30 | 31 | std::array cartesianLowpassFilter(double sample_time, 32 | std::array current_signal_value, 33 | std::array last_signal_value, 34 | double cutoff_frequency) { 35 | if (sample_time < 0 || !std::isfinite(sample_time)) { 36 | throw std::invalid_argument( 37 | "Cartesian lowpass-filter: sample_time is negative, infinite or NaN."); 38 | } 39 | if (cutoff_frequency <= 0 || !std::isfinite(cutoff_frequency)) { 40 | throw std::invalid_argument( 41 | "Cartesian lowpass-filter: cutoff_frequency is zero, negative, infinite or NaN."); 42 | } 43 | for (size_t i = 0; i < current_signal_value.size(); i++) { 44 | if (!std::isfinite(current_signal_value[i]) || !std::isfinite(last_signal_value[i])) { 45 | throw std::invalid_argument( 46 | "Cartesian lowpass-filter: current or past input value of the signal to be filtered is " 47 | "infinite or NaN."); 48 | } 49 | } 50 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(current_signal_value.data())); 51 | Eigen::Affine3d transform_last(Eigen::Matrix4d::Map(last_signal_value.data())); 52 | Eigen::Quaterniond orientation(transform.rotation()); 53 | Eigen::Quaterniond orientation_last(transform_last.rotation()); 54 | 55 | double gain = sample_time / (sample_time + (1.0 / (2.0 * M_PI * cutoff_frequency))); 56 | transform.translation() = 57 | gain * transform.translation() + (1.0 - gain) * transform_last.translation(); 58 | orientation = orientation_last.slerp(gain, orientation); 59 | 60 | transform.linear() << orientation.normalized().toRotationMatrix(); 61 | std::array filtered_values{}; 62 | Eigen::Map(filtered_values.data(), 4, 4) = transform.matrix(); 63 | 64 | return filtered_values; 65 | } 66 | } // namespace franka 67 | -------------------------------------------------------------------------------- /src/network.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include "network.h" 4 | 5 | #include 6 | #include 7 | 8 | using namespace std::string_literals; // NOLINT(google-build-using-namespace) 9 | 10 | namespace franka { 11 | 12 | Network::Network(const std::string& franka_address, 13 | uint16_t franka_port, 14 | std::chrono::milliseconds tcp_timeout, 15 | std::chrono::milliseconds udp_timeout, 16 | std::tuple tcp_keepalive) { 17 | try { 18 | Poco::Timespan poco_timeout(1000L * tcp_timeout.count()); 19 | Poco::Net::SocketAddress address(franka_address, franka_port); 20 | 21 | tcp_socket_.connect(address, poco_timeout); 22 | tcp_socket_.setBlocking(true); 23 | tcp_socket_.setSendTimeout(poco_timeout); 24 | tcp_socket_.setReceiveTimeout(poco_timeout); 25 | 26 | if (std::get<0>(tcp_keepalive)) { 27 | tcp_socket_.setKeepAlive(true); 28 | try { 29 | tcp_socket_.setOption(IPPROTO_TCP, TCP_KEEPIDLE, std::get<1>(tcp_keepalive)); 30 | tcp_socket_.setOption(IPPROTO_TCP, TCP_KEEPCNT, std::get<2>(tcp_keepalive)); 31 | tcp_socket_.setOption(IPPROTO_TCP, TCP_KEEPINTVL, std::get<3>(tcp_keepalive)); 32 | } catch (...) { 33 | } 34 | } 35 | 36 | udp_socket_.bind({"0.0.0.0", 0}); 37 | udp_socket_.setReceiveTimeout(Poco::Timespan{1000L * udp_timeout.count()}); 38 | udp_port_ = udp_socket_.address().port(); 39 | } catch (const Poco::Net::ConnectionRefusedException& e) { 40 | throw NetworkException( 41 | "libfranka: Connection to FCI refused. Please install FCI feature or enable FCI mode in Desk."s); 42 | } catch (const Poco::Net::NetException& e) { 43 | throw NetworkException("libfranka: Connection error: "s + e.what()); 44 | } catch (const Poco::TimeoutException& e) { 45 | throw NetworkException( 46 | "libfranka: Connection timeout. Please check your network connection or settings."s); 47 | } catch (const Poco::Exception& e) { 48 | throw NetworkException("libfranka: "s + e.what()); 49 | } 50 | } 51 | 52 | Network::~Network() { 53 | try { 54 | tcp_socket_.shutdown(); 55 | } catch (...) { 56 | } 57 | } 58 | 59 | uint16_t Network::udpPort() const noexcept { 60 | return udp_port_; 61 | } 62 | 63 | bool Network::isTcpSocketAlive() const noexcept { 64 | return !tcp_socket_.poll(Poco::Timespan(0), Poco::Net::Socket::SELECT_ERROR); 65 | } 66 | 67 | void Network::tcpThrowIfConnectionClosed() try { 68 | std::unique_lock lock(tcp_mutex_, std::try_to_lock); 69 | if (!lock.owns_lock()) { 70 | return; 71 | } 72 | if (tcp_socket_.poll(0, Poco::Net::Socket::SELECT_READ)) { 73 | std::array buffer; 74 | int received_bytes = 75 | tcp_socket_.receiveBytes(buffer.data(), static_cast(buffer.size()), MSG_PEEK); 76 | 77 | if (received_bytes == 0) { 78 | throw NetworkException("libfranka: server closed connection"); 79 | } 80 | } 81 | } catch (const Poco::Exception& e) { 82 | throw NetworkException("libfranka: "s + e.what()); 83 | } 84 | 85 | } // namespace franka 86 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | Checks: | 2 | clang-diagnostic-*, 3 | clang-analyzer-*, 4 | -clang-analyzer-alpha*, 5 | google-*, 6 | misc-*, 7 | readability-*, 8 | modernize-*, 9 | performance-*, 10 | cppcoreguidelines-*, 11 | -cppcoreguidelines-pro-type-reinterpret-cast, 12 | -cppcoreguidelines-pro-bounds-pointer-arithmetic, 13 | -cppcoreguidelines-pro-bounds-constant-array-index, 14 | -google-explicit-constructor, 15 | -google-runtime-references, 16 | -cppcoreguidelines-pro-type-member-init, 17 | -cppcoreguidelines-special-member-functions, 18 | -modernize-use-trailing-return-type, 19 | -readability-magic-numbers, 20 | -cppcoreguidelines-avoid-magic-numbers, 21 | -modernize-use-nodiscard, 22 | -cppcoreguidelines-non-private-member-variables-in-classes, 23 | -misc-non-private-member-variables-in-classes, 24 | -readability-suspicious-call-argument 25 | -cppcoreguidelines-prefer-member-initializer 26 | HeaderFilterRegex: '/(src|include/franka)/' 27 | CheckOptions: 28 | # Classes, structs, ... 29 | - key: readability-identifier-naming.NamespaceCase 30 | value: lower_case 31 | - key: readability-identifier-naming.ClassCase 32 | value: CamelCase 33 | - key: readability-identifier-naming.StructCase 34 | value: CamelCase 35 | - key: readability-identifier-naming.EnumCase 36 | value: CamelCase 37 | - key: readability-identifier-naming.UnionCase 38 | value: CamelCase 39 | - key: readability-identifier-naming.TypedefCase 40 | value: CamelCase 41 | 42 | # Variables, member variables, ... 43 | - key: readability-identifier-naming.ParameterCase 44 | value: lower_case 45 | - key: readability-identifier-naming.VariableCase 46 | value: lower_case 47 | - key: readability-identifier-naming.MemberCase 48 | value: lower_case 49 | - key: readability-identifier-naming.PublicMemberCase 50 | value: lower_case 51 | - key: readability-identifier-naming.ProtectedMemberCase 52 | value: lower_case 53 | - key: readability-identifier-naming.PrivateMemberCase 54 | value: lower_case 55 | - key: readability-identifier-naming.PrivateMemberSuffix 56 | value: _ 57 | 58 | # Functions, methods, ... 59 | - key: readability-identifier-naming.FunctionCase 60 | value: camelBack 61 | - key: readability-identifier-naming.MethodCase 62 | value: camelBack 63 | 64 | # Constants 65 | - key: readability-identifier-naming.ConstantPrefix 66 | value: k 67 | - key: readability-identifier-naming.ConstantCase 68 | value: CamelCase 69 | - key: readability-identifier-naming.ConstantMemberPrefix 70 | value: '' 71 | - key: readability-identifier-naming.ConstantMemberCase 72 | value: lower_case 73 | - key: readability-identifier-naming.ConstantParameterPrefix 74 | value: '' 75 | - key: readability-identifier-naming.ConstantParameterCase 76 | value: lower_case 77 | - key: readability-identifier-naming.LocalConstantParameterPrefix 78 | value: '' 79 | - key: readability-identifier-naming.LocalConstantCase 80 | value: lower_case 81 | - key: readability-identifier-naming.ConstexprVariablePrefix 82 | value: k 83 | - key: readability-identifier-naming.ConstexprVariableCase 84 | value: CamelCase 85 | -------------------------------------------------------------------------------- /pylibfranka/docs/class_hierarchy.rst: -------------------------------------------------------------------------------- 1 | Class Hierarchy 2 | =============== 3 | 4 | .. currentmodule:: pylibfranka 5 | 6 | This page shows the class hierarchy for all classes in pylibfranka. 7 | 8 | Exception Hierarchy 9 | ------------------- 10 | 11 | All exceptions inherit from the base :class:`FrankaException`: 12 | 13 | .. inheritance-diagram:: FrankaException CommandException ControlException NetworkException InvalidOperationException RealtimeException 14 | :parts: 1 15 | :top-classes: Exception 16 | 17 | Text Representation 18 | ~~~~~~~~~~~~~~~~~~~ 19 | 20 | .. code-block:: text 21 | 22 | Exception (Python built-in) 23 | └── FrankaException 24 | ├── CommandException 25 | ├── ControlException 26 | ├── NetworkException 27 | ├── InvalidOperationException 28 | └── RealtimeException 29 | 30 | Control and State Classes 31 | -------------------------- 32 | 33 | The main classes for robot control and state access: 34 | 35 | .. list-table:: 36 | :widths: 30 70 37 | :header-rows: 1 38 | 39 | * - Class 40 | - Description 41 | * - :class:`Robot` 42 | - Main robot control interface 43 | * - :class:`ActiveControlBase` 44 | - Active control session for reading state and writing commands 45 | * - :class:`RobotState` 46 | - Complete robot state information 47 | * - :class:`Model` 48 | - Robot dynamics model 49 | 50 | Gripper Classes 51 | --------------- 52 | 53 | .. list-table:: 54 | :widths: 30 70 55 | :header-rows: 1 56 | 57 | * - Class 58 | - Description 59 | * - :class:`Gripper` 60 | - Gripper control interface 61 | * - :class:`GripperState` 62 | - Gripper state information 63 | 64 | Motion Command Types 65 | --------------------- 66 | 67 | All motion command types are independent data structures: 68 | 69 | .. list-table:: 70 | :widths: 30 70 71 | :header-rows: 1 72 | 73 | * - Class 74 | - Description 75 | * - :class:`JointPositions` 76 | - Joint space position commands 77 | * - :class:`JointVelocities` 78 | - Joint space velocity commands 79 | * - :class:`CartesianPose` 80 | - Cartesian space pose commands 81 | * - :class:`CartesianVelocities` 82 | - Cartesian space velocity commands 83 | * - :class:`Torques` 84 | - Joint torque commands 85 | 86 | Enumeration Types 87 | ----------------- 88 | 89 | .. list-table:: 90 | :widths: 30 70 91 | :header-rows: 1 92 | 93 | * - Enum 94 | - Description 95 | * - :class:`RealtimeConfig` 96 | - Real-time enforcement mode (kEnforce, kIgnore) 97 | * - :class:`ControllerMode` 98 | - Controller type (JointImpedance, CartesianImpedance) 99 | * - :class:`RobotMode` 100 | - Current robot operation mode 101 | 102 | Other Types 103 | ----------- 104 | 105 | .. list-table:: 106 | :widths: 30 70 107 | :header-rows: 1 108 | 109 | * - Class 110 | - Description 111 | * - :class:`Duration` 112 | - Time duration representation 113 | * - :class:`Errors` 114 | - Robot error state flags 115 | 116 | See Also 117 | -------- 118 | 119 | * :doc:`class_list` - Alphabetical class list 120 | * :doc:`api/exceptions` - Detailed exception documentation 121 | 122 | -------------------------------------------------------------------------------- /docs/network_requirements.rst: -------------------------------------------------------------------------------- 1 | Minimum System and Network Requirements 2 | ======================================= 3 | 4 | This page specifies the requirements for running the Franka Control Interface (FCI). 5 | Additional requirements may be documented in the materials provided with your robot. 6 | 7 | Workstation PC 8 | -------------- 9 | 10 | +-------------------+--------------------------------------+ 11 | | Minimum Requirement| Specification | 12 | +===================+======================================+ 13 | | Operating System | Linux with PREEMPT_RT patched kernel | 14 | +-------------------+--------------------------------------+ 15 | | Network card | 100BASE-TX | 16 | +-------------------+--------------------------------------+ 17 | 18 | Since the robot sends data at a 1 kHz frequency, it is important that the workstation PC is configured to minimize latencies. For example, we recommend to :ref:`disable CPU frequency scaling `. Other optimizations may depend on your particular system. 19 | 20 | Network 21 | ------- 22 | 23 | .. _requirement-network: 24 | 25 | If possible, directly connect your workstation PC to the LAN port of **Control**, avoiding any intermediate devices such as switches. 26 | 27 | .. important:: 28 | The workstation PC controlling your robot using the FCI **must** be connected to the LAN port of Control (shop floor network) and **not** to the LAN port of the Arm (robot network). 29 | 30 | Relays or switches in between may introduce delays, jitter, or packet loss, which can decrease controller performance or render it unusable. 31 | 32 | .. hint:: 33 | The best performance is achieved when connecting directly to the LAN port of Control. 34 | This requires setting up a static IP for the shop floor network in the administrator interface beforehand. 35 | See :ref:`setting-up-the-network`. 36 | 37 | Time Constraints 38 | ---------------- 39 | 40 | To control the robot reliably, the sum of the following time measurements must be **less than 1 ms**: 41 | 42 | * Round-trip time (RTT) between the workstation PC and FCI. 43 | * Execution time of your motion generator or control loop. 44 | * Time needed by the robot to process the data and update the internal controller. 45 | 46 | .. caution:: 47 | If the **<1 ms constraint** is violated for a cycle, the packet is dropped by FCI. After 20 consecutively dropped packets, the robot `will stop` with the ``communication_constraints_violation`` error. 48 | Communication quality can be monitored via the ``RobotState::control_command_success_rate`` field. 49 | 50 | Packet Handling 51 | --------------- 52 | 53 | * **Motion generator packet dropped**: 54 | The robot uses previous waypoints and performs a linear extrapolation (constant acceleration integration) for the missed time step. More than 20 consecutive dropped packets will cause the robot to `stop`. 55 | 56 | * **Controller command packet dropped**: 57 | FCI reuses the torques from the last successfully received packet. Again, more than 20 consecutive dropped packets will cause the robot to `stop`. 58 | 59 | .. hint:: 60 | Measure network performance (see :ref:`network-bandwidth-delay-test`) and the control or motion generator loop beforehand to ensure compliance with timing requirements. 61 | -------------------------------------------------------------------------------- /test/duration_tests.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | using franka::Duration; 10 | 11 | TEST(Duration, CanDefaultConstruct) { 12 | Duration d; 13 | EXPECT_EQ(0u, d.toMSec()); 14 | EXPECT_EQ(0.0, d.toSec()); 15 | } 16 | 17 | TEST(Duration, CanConstructFromMilliseconds) { 18 | Duration d(12345u); 19 | EXPECT_EQ(12345u, d.toMSec()); 20 | EXPECT_EQ(12.345, d.toSec()); 21 | } 22 | 23 | TEST(Duration, CanConstructFromChrono) { 24 | std::chrono::duration chrono_duration(12345u); 25 | Duration d(chrono_duration); 26 | EXPECT_EQ(12345u, d.toMSec()); 27 | EXPECT_EQ(12.345, d.toSec()); 28 | } 29 | 30 | TEST(Duration, CanConvertToChrono) { 31 | Duration d(12345u); 32 | std::chrono::duration chrono_duration = d; 33 | EXPECT_EQ(12345u, chrono_duration.count()); 34 | } 35 | 36 | TEST(Duration, CanCopy) { 37 | Duration d(12345u); 38 | Duration d2(d); 39 | EXPECT_EQ(12345u, d2.toMSec()); 40 | 41 | Duration d3; 42 | d3 = d; 43 | EXPECT_EQ(12345u, d2.toMSec()); 44 | } 45 | 46 | TEST(Duration, CanMove) { 47 | Duration d(12345u); 48 | Duration d2(std::move(d)); 49 | EXPECT_EQ(12345u, d2.toMSec()); 50 | 51 | Duration d3; 52 | d3 = std::move(d); 53 | EXPECT_EQ(12345u, d2.toMSec()); 54 | } 55 | 56 | TEST(Duration, CanUseArithmeticOperations) { 57 | Duration d1(4u); 58 | Duration d2(3u); 59 | 60 | EXPECT_EQ(7u, (d1 + d2).toMSec()); 61 | EXPECT_EQ(7u, (d2 + d1).toMSec()); 62 | 63 | EXPECT_EQ(1u, (d1 - d2).toMSec()); 64 | 65 | EXPECT_EQ(8u, (d1 * 2).toMSec()); 66 | EXPECT_EQ(8u, (2 * d1).toMSec()); 67 | 68 | EXPECT_EQ(2u, (d1 / 2).toMSec()); 69 | EXPECT_EQ(1u, d1 / d2); 70 | 71 | EXPECT_EQ(1u, (d1 % d2).toMSec()); 72 | EXPECT_EQ(1u, (d1 % 3u).toMSec()); 73 | 74 | d1 += d2; 75 | EXPECT_EQ(7u, d1.toMSec()); 76 | 77 | d1 -= d2; 78 | EXPECT_EQ(4u, d1.toMSec()); 79 | 80 | d1 *= 2; 81 | EXPECT_EQ(8u, d1.toMSec()); 82 | 83 | d1 /= 2; 84 | EXPECT_EQ(4u, d1.toMSec()); 85 | 86 | d1 %= 3; 87 | EXPECT_EQ(1u, d1.toMSec()); 88 | 89 | d1 *= 4; 90 | d1 %= d2; 91 | EXPECT_EQ(1u, d1.toMSec()); 92 | } 93 | 94 | TEST(Duration, CanCompare) { 95 | EXPECT_TRUE(Duration(1) == Duration(1)); 96 | EXPECT_FALSE(Duration(1) == Duration(2)); 97 | EXPECT_FALSE(Duration(2) == Duration(1)); 98 | 99 | EXPECT_FALSE(Duration(1) != Duration(1)); 100 | EXPECT_TRUE(Duration(1) != Duration(2)); 101 | EXPECT_TRUE(Duration(2) != Duration(1)); 102 | 103 | EXPECT_TRUE(Duration(1) < Duration(2)); 104 | EXPECT_FALSE(Duration(2) < Duration(1)); 105 | EXPECT_FALSE(Duration(2) < Duration(2)); 106 | 107 | EXPECT_FALSE(Duration(1) > Duration(2)); 108 | EXPECT_TRUE(Duration(2) > Duration(1)); 109 | EXPECT_FALSE(Duration(2) > Duration(2)); 110 | 111 | EXPECT_TRUE(Duration(1) <= Duration(2)); 112 | EXPECT_FALSE(Duration(2) <= Duration(1)); 113 | EXPECT_TRUE(Duration(2) <= Duration(2)); 114 | 115 | EXPECT_FALSE(Duration(1) >= Duration(2)); 116 | EXPECT_TRUE(Duration(2) >= Duration(1)); 117 | EXPECT_TRUE(Duration(2) >= Duration(2)); 118 | } 119 | -------------------------------------------------------------------------------- /pylibfranka/examples/cartesian_pose_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2025 Franka Robotics GmbH 4 | # Use of this source code is governed by the Apache-2.0 license, see LICENSE 5 | 6 | import argparse 7 | import time 8 | 9 | import numpy as np 10 | 11 | from pylibfranka import CartesianPose, ControllerMode, RealtimeConfig, Robot 12 | 13 | 14 | def main(): 15 | # Parse command line arguments 16 | parser = argparse.ArgumentParser() 17 | parser.add_argument("--ip", type=str, default="localhost", help="Robot IP address") 18 | args = parser.parse_args() 19 | 20 | # Connect to robot 21 | robot = Robot(args.ip, RealtimeConfig.kIgnore) 22 | 23 | try: 24 | # Set collision behavior 25 | lower_torque_thresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] 26 | upper_torque_thresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] 27 | lower_force_thresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] 28 | upper_force_thresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] 29 | 30 | robot.set_collision_behavior( 31 | lower_torque_thresholds, 32 | upper_torque_thresholds, 33 | lower_force_thresholds, 34 | upper_force_thresholds, 35 | ) 36 | 37 | # First move the robot to a suitable joint configuration 38 | print("WARNING: This example will move the robot!") 39 | print("Please make sure to have the user stop button at hand!") 40 | input("Press Enter to continue...") 41 | 42 | # Start cartesian pose control with external control loop 43 | active_control = robot.start_cartesian_pose_control(ControllerMode.JointImpedance) 44 | 45 | time_elapsed = 0.0 46 | motion_finished = False 47 | 48 | robot_state, duration = active_control.readOnce() 49 | initial_cartesian_pose = robot_state.O_T_EE 50 | 51 | # External control loop 52 | while not motion_finished: 53 | # Read robot state and duration 54 | robot_state, duration = active_control.readOnce() 55 | 56 | kRadius = 0.3 57 | angle = np.pi / 4 * (1 - np.cos(np.pi / 5.0 * time_elapsed)) 58 | delta_x = kRadius * np.sin(angle) 59 | delta_z = kRadius * (np.cos(angle) - 1) 60 | 61 | # Update time 62 | time_elapsed += duration.to_sec() 63 | 64 | # Update joint positions 65 | new_cartesian_pose = initial_cartesian_pose.copy() 66 | new_cartesian_pose[12] += delta_x # x position 67 | new_cartesian_pose[14] += delta_z # z position 68 | 69 | # Set joint positions 70 | cartesian_pose = CartesianPose(new_cartesian_pose) 71 | 72 | # Set motion_finished flag to True on the last update 73 | if time_elapsed >= 5.0: 74 | cartesian_pose.motion_finished = True 75 | motion_finished = True 76 | print("Finished motion, shutting down example") 77 | 78 | # Send command to robot 79 | active_control.writeOnce(cartesian_pose) 80 | 81 | except Exception as e: 82 | print(f"Error occurred: {e}") 83 | if robot is not None: 84 | robot.stop() 85 | return -1 86 | 87 | 88 | if __name__ == "__main__": 89 | main() 90 | -------------------------------------------------------------------------------- /docs/system_requirements.rst: -------------------------------------------------------------------------------- 1 | Minimum system and network requirements 2 | ======================================= 3 | 4 | This page only specifies the requirements for running the Franka Control Interface (FCI). 5 | Additional requirements are specified in the documents that you have received with your robot. 6 | 7 | Workstation PC 8 | -------------- 9 | 10 | +---------------------------------------------------------+ 11 | | Minimum System Requirements | 12 | +===================+=====================================+ 13 | | Operating System | Linux with PREEMPT_RT patched kernel| 14 | +-------------------+-------------------------------------+ 15 | | Network card | 100BASE-TX | 16 | +-------------------+-------------------------------------+ 17 | 18 | Since the robot sends data at 1 kHz frequency, it is important that the workstation PC is configured 19 | to minimize latencies. For example, we recommend to 20 | :ref:`disable CPU frequency scaling `. Other possible optimizations 21 | will depend on your particular system. 22 | 23 | .. _requirement-network: 24 | 25 | Network 26 | ------- 27 | If possible, directly connect your workstation PC to the LAN port of Control, i.e. avoid any 28 | intermediate devices such as switches. 29 | 30 | .. important:: 31 | The workstation PC which commands your robot using the FCI must always be connected to the LAN 32 | port of Control (shop floor network) and **not** to the LAN port of the Arm (robot network). 33 | 34 | Having relays in between could lead to delay, jitter or packet loss. This will decrease the 35 | performance of your controller or make it unusable. 36 | 37 | .. hint:: 38 | The best performance can be achieved when connecting directly to the LAN port of Control. 39 | This requires setting up a static IP for the shop floor network in the administrator's interface 40 | beforehand. See :ref:`setting-up-the-network`. 41 | 42 | To control the robot, it must be guaranteed that the sum of the following time measurements is 43 | less than 1 ms: 44 | 45 | * Round trip time (RTT) between the workstation PC and FCI. 46 | * Execution time of your motion generator or control loop. 47 | * Time needed by the robot to process your data and step the internal controller. 48 | 49 | .. caution:: 50 | If the **<1 ms constraint** is violated for a cycle, the received packet is dropped by 51 | FCI. After 20 consecutively dropped packets, your robot `will stop` with the 52 | ``communication_constraints_violation`` error. Current measure of communication quality 53 | can be read from the ``RobotState::control_command_success_rate`` field. 54 | 55 | If a **motion generator command packet is dropped**, the robot takes the previous waypoints and 56 | performs a linear extrapolation (keep acceleration constant and integrate) for the missed 57 | time step. If more than 20 packets are lost or dropped in a row, your robot `will stop`. 58 | 59 | If a **controller command packet is dropped**, FCI will reuse the torques of the last successful 60 | received packet. Again, more than 20 consecutive lost or dropped packets will cause your robot to 61 | `stop`. 62 | 63 | .. hint:: 64 | Measure the performance of your network (see :ref:`network-bandwidth-delay-test`) and the 65 | control or motion generator loop beforehand. 66 | -------------------------------------------------------------------------------- /src/duration.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | namespace franka { 6 | 7 | Duration::Duration() noexcept : duration_{0U} {} 8 | 9 | Duration::Duration(std::chrono::duration duration) noexcept 10 | : duration_{duration} {} 11 | 12 | Duration::Duration(uint64_t milliseconds) noexcept : duration_{milliseconds} {} 13 | 14 | Duration::operator std::chrono::duration() const noexcept { 15 | return duration_; 16 | } 17 | 18 | double Duration::toSec() const noexcept { 19 | return std::chrono::duration_cast>(duration_).count(); 20 | } 21 | 22 | uint64_t Duration::toMSec() const noexcept { 23 | return duration_.count(); 24 | } 25 | 26 | Duration Duration::operator+(const Duration& rhs) const noexcept { 27 | return duration_ + rhs.duration_; 28 | } 29 | 30 | Duration& Duration::operator+=(const Duration& rhs) noexcept { 31 | duration_ += rhs.duration_; 32 | return *this; 33 | } 34 | 35 | Duration Duration::operator-(const Duration& rhs) const noexcept { 36 | return duration_ - rhs.duration_; 37 | } 38 | 39 | Duration& Duration::operator-=(const Duration& rhs) noexcept { 40 | duration_ -= rhs.duration_; 41 | return *this; 42 | } 43 | 44 | Duration Duration::operator*(uint64_t rhs) const noexcept { 45 | return duration_ * rhs; 46 | } 47 | 48 | Duration& Duration::operator*=(uint64_t rhs) noexcept { 49 | duration_ *= rhs; 50 | return *this; 51 | } 52 | 53 | uint64_t Duration::operator/(const Duration& rhs) const noexcept { 54 | return duration_ / rhs.duration_; 55 | } 56 | 57 | Duration Duration::operator/(uint64_t rhs) const noexcept { 58 | return duration_ / rhs; 59 | } 60 | 61 | Duration& Duration::operator/=(uint64_t rhs) noexcept { 62 | duration_ /= rhs; 63 | return *this; 64 | } 65 | 66 | Duration Duration::operator%(const Duration& rhs) const noexcept { 67 | return duration_ % rhs.duration_; 68 | } 69 | 70 | Duration Duration::operator%(uint64_t rhs) const noexcept { 71 | return duration_ % rhs; 72 | } 73 | 74 | Duration& Duration::operator%=(const Duration& rhs) noexcept { 75 | duration_ %= rhs.duration_; 76 | return *this; 77 | } 78 | 79 | Duration& Duration::operator%=(uint64_t rhs) noexcept { 80 | duration_ %= rhs; 81 | return *this; 82 | } 83 | 84 | bool Duration::operator==(const Duration& rhs) const noexcept { 85 | return duration_ == rhs.duration_; 86 | } 87 | 88 | bool Duration::operator!=(const Duration& rhs) const noexcept { 89 | return duration_ != rhs.duration_; 90 | } 91 | 92 | bool Duration::operator<(const Duration& rhs) const noexcept { 93 | return duration_ < rhs.duration_; 94 | } 95 | 96 | bool Duration::operator<=(const Duration& rhs) const noexcept { 97 | return duration_ <= rhs.duration_; 98 | } 99 | 100 | bool Duration::operator>(const Duration& rhs) const noexcept { 101 | return duration_ > rhs.duration_; 102 | } 103 | 104 | bool Duration::operator>=(const Duration& rhs) const noexcept { 105 | return duration_ >= rhs.duration_; 106 | } 107 | 108 | Duration operator*(uint64_t lhs, const Duration& rhs) noexcept { 109 | return rhs * lhs; 110 | } 111 | 112 | } // namespace franka 113 | -------------------------------------------------------------------------------- /examples/generate_consecutive_motions.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "examples_common.h" 10 | 11 | /** 12 | * @example generate_consecutive_motions.cpp 13 | * An example showing how to execute consecutive motions with error recovery. 14 | * 15 | * @warning Before executing this example, make sure there is enough space in front and to the side 16 | * of the robot. 17 | */ 18 | 19 | int main(int argc, char** argv) { 20 | if (argc != 2) { 21 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 22 | return -1; 23 | } 24 | try { 25 | franka::Robot robot(argv[1]); 26 | setDefaultBehavior(robot); 27 | 28 | // First move the robot to a suitable joint configuration 29 | std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; 30 | MotionGenerator motion_generator(0.5, q_goal); 31 | std::cout << "WARNING: This example will move the robot! " 32 | << "Please make sure to have the user stop button at hand!" << std::endl 33 | << "Press Enter to continue..." << std::endl; 34 | std::cin.ignore(); 35 | robot.control(motion_generator); 36 | std::cout << "Finished moving to initial joint configuration." << std::endl; 37 | 38 | // Set additional parameters always before the control loop, NEVER in the control loop! 39 | // Set collision behavior. 40 | robot.setCollisionBehavior( 41 | {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, 42 | {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, 43 | {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, 44 | {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}); 45 | 46 | for (size_t i = 0; i < 5; i++) { 47 | std::cout << "Executing motion." << std::endl; 48 | try { 49 | double time_max = 4.0; 50 | double omega_max = 0.2; 51 | double time = 0.0; 52 | robot.control([=, &time](const franka::RobotState&, 53 | franka::Duration period) -> franka::JointVelocities { 54 | time += period.toSec(); 55 | 56 | double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); 57 | double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); 58 | 59 | franka::JointVelocities velocities = {{0.0, 0.0, omega, 0.0, 0.0, 0.0, 0.0}}; 60 | if (time >= 2 * time_max) { 61 | std::cout << std::endl << "Finished motion." << std::endl; 62 | return franka::MotionFinished(velocities); 63 | } 64 | return velocities; 65 | }); 66 | } catch (const franka::ControlException& e) { 67 | std::cout << e.what() << std::endl; 68 | std::cout << "Running error recovery..." << std::endl; 69 | robot.automaticErrorRecovery(); 70 | } 71 | } 72 | } catch (const franka::Exception& e) { 73 | std::cout << e.what() << std::endl; 74 | return -1; 75 | } 76 | 77 | std::cout << "Finished." << std::endl; 78 | 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /pylibfranka/examples/joint_velocity_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2025 Franka Robotics GmbH 4 | # Use of this source code is governed by the Apache-2.0 license, see LICENSE 5 | 6 | import argparse 7 | 8 | import numpy as np 9 | 10 | from pylibfranka import ControllerMode, JointVelocities, Robot 11 | 12 | 13 | def main(): 14 | # Parse command line arguments 15 | parser = argparse.ArgumentParser() 16 | parser.add_argument("--ip", type=str, default="localhost", help="Robot IP address") 17 | args = parser.parse_args() 18 | 19 | # Connect to robot 20 | robot = Robot(args.ip) 21 | 22 | try: 23 | # Set collision behavior 24 | lower_torque_thresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] 25 | upper_torque_thresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] 26 | lower_force_thresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] 27 | upper_force_thresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] 28 | 29 | robot.set_collision_behavior( 30 | lower_torque_thresholds, 31 | upper_torque_thresholds, 32 | lower_force_thresholds, 33 | upper_force_thresholds, 34 | ) 35 | 36 | # First move the robot to a suitable joint configuration 37 | print("WARNING: This example will move the robot!") 38 | print("Please make sure to have the user stop button at hand!") 39 | input("Press Enter to continue...") 40 | 41 | # Start joint velocity control with external control loop 42 | active_control = robot.start_joint_velocity_control(ControllerMode.CartesianImpedance) 43 | 44 | time_max = 1.0 45 | omega_max = 1.0 46 | time_elapsed = 0.0 47 | motion_finished = False 48 | 49 | # External control loop 50 | while not motion_finished: 51 | # Read robot state and duration 52 | robot_state, duration = active_control.readOnce() 53 | 54 | # Update time 55 | time_elapsed += duration.to_sec() 56 | 57 | # Calculate omega using the same formula as in C++ example 58 | cycle = np.floor( 59 | np.power(-1.0, (time_elapsed - np.fmod(time_elapsed, time_max)) / time_max) 60 | ) 61 | omega = cycle * omega_max / 2.0 * (1.0 - np.cos(2.0 * np.pi / time_max * time_elapsed)) 62 | 63 | # Update joint positions 64 | velocities = [ 65 | 0.0, 66 | 0.0, 67 | 0.0, 68 | omega, 69 | omega, 70 | omega, 71 | omega, 72 | ] 73 | 74 | # Set joint positions 75 | joint_velocities = JointVelocities(velocities) 76 | 77 | # Set motion_finished flag to True on the last update 78 | if time_elapsed >= 2.0 * time_max: 79 | joint_velocities.motion_finished = True 80 | motion_finished = True 81 | print("Finished motion, shutting down example") 82 | 83 | # Send command to robot 84 | active_control.writeOnce(joint_velocities) 85 | 86 | except Exception as e: 87 | print(f"Error occurred: {e}") 88 | if robot is not None: 89 | robot.stop() 90 | return -1 91 | 92 | 93 | if __name__ == "__main__": 94 | main() 95 | -------------------------------------------------------------------------------- /pylibfranka/docs/modules.rst: -------------------------------------------------------------------------------- 1 | Module Reference 2 | ================ 3 | 4 | .. currentmodule:: pylibfranka 5 | 6 | This page provides an overview of the module structure in pylibfranka. 7 | 8 | Main Module 9 | ----------- 10 | 11 | All classes and types are available directly from the top-level :mod:`pylibfranka` module: 12 | 13 | .. code-block:: python 14 | 15 | import pylibfranka 16 | 17 | robot = pylibfranka.Robot("172.16.0.2") 18 | gripper = pylibfranka.Gripper("172.16.0.2") 19 | 20 | Module Contents 21 | ~~~~~~~~~~~~~~~ 22 | 23 | .. autosummary:: 24 | :nosignatures: 25 | 26 | Robot 27 | ActiveControlBase 28 | Model 29 | Gripper 30 | GripperState 31 | RobotState 32 | RobotMode 33 | Errors 34 | JointPositions 35 | JointVelocities 36 | CartesianPose 37 | CartesianVelocities 38 | Torques 39 | Duration 40 | RealtimeConfig 41 | ControllerMode 42 | FrankaException 43 | CommandException 44 | ControlException 45 | NetworkException 46 | InvalidOperationException 47 | RealtimeException 48 | 49 | Detailed API Reference by Category 50 | ----------------------------------- 51 | 52 | For detailed documentation organized by functionality, see: 53 | 54 | .. toctree:: 55 | :maxdepth: 1 56 | 57 | api/robot 58 | api/control 59 | api/model 60 | api/gripper 61 | api/state 62 | api/exceptions 63 | 64 | Organization by Functionality 65 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 66 | 67 | Robot Control 68 | ^^^^^^^^^^^^^ 69 | 70 | Classes for establishing connection and controlling the robot: 71 | 72 | * :class:`Robot` - Main robot interface 73 | * :class:`ActiveControlBase` - Active control session 74 | * :class:`RealtimeConfig` - Real-time configuration 75 | * :class:`ControllerMode` - Controller mode selection 76 | 77 | Motion Commands 78 | ^^^^^^^^^^^^^^^ 79 | 80 | Data structures for sending motion commands: 81 | 82 | * :class:`JointPositions` - Joint position commands 83 | * :class:`JointVelocities` - Joint velocity commands 84 | * :class:`CartesianPose` - Cartesian pose commands 85 | * :class:`CartesianVelocities` - Cartesian velocity commands 86 | * :class:`Torques` - Torque commands 87 | 88 | Robot State and Model 89 | ^^^^^^^^^^^^^^^^^^^^^ 90 | 91 | Classes for reading robot state and computing dynamics: 92 | 93 | * :class:`RobotState` - Complete robot state 94 | * :class:`Model` - Dynamics model (mass matrix, Coriolis, gravity, etc.) 95 | * :class:`RobotMode` - Operation mode 96 | * :class:`Errors` - Error state 97 | 98 | Gripper 99 | ^^^^^^^ 100 | 101 | Classes for gripper control: 102 | 103 | * :class:`Gripper` - Gripper interface 104 | * :class:`GripperState` - Gripper state 105 | 106 | Exceptions 107 | ^^^^^^^^^^ 108 | 109 | Exception hierarchy for error handling: 110 | 111 | * :class:`FrankaException` - Base exception 112 | * :class:`CommandException` - Command errors 113 | * :class:`ControlException` - Control/motion errors 114 | * :class:`NetworkException` - Network errors 115 | * :class:`InvalidOperationException` - Invalid operation errors 116 | * :class:`RealtimeException` - Real-time scheduling errors 117 | 118 | See Also 119 | -------- 120 | 121 | * :doc:`class_list` - Complete class list with descriptions 122 | * :doc:`class_hierarchy` - Class inheritance structure 123 | * :ref:`genindex` - Complete index 124 | 125 | -------------------------------------------------------------------------------- /src/logging/robot_state_log.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2024 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | using namespace std::string_literals; // NOLINT(google-build-using-namespace) 9 | 10 | namespace franka { 11 | 12 | namespace { 13 | 14 | template 15 | std::string csvName(const std::array& /*unused*/, const std::string& name) { 16 | std::ostringstream ostream; 17 | for (size_t i = 0; i < N - 1; i++) { 18 | ostream << name << "[" << i << "],"; 19 | } 20 | ostream << name << "[" << N - 1 << "]"; 21 | return ostream.str(); 22 | } 23 | 24 | template 25 | std::ostream& operator<<(std::ostream& ostream /*unused*/, const std::array& array) { 26 | std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator(ostream, ",")); 27 | std::copy(array.cend() - 1, array.cend(), std::ostream_iterator(ostream)); 28 | return ostream; 29 | } 30 | 31 | std::string csvRobotStateHeader() { 32 | RobotState robot_state; 33 | std::ostringstream ostream; 34 | ostream << "time,success_rate," << csvName(robot_state.q, "state.q") << "," 35 | << csvName(robot_state.q_d, "state.q_d") << "," << csvName(robot_state.dq, "state.dq") 36 | << "," << csvName(robot_state.dq_d, "state.dq_d") << "," 37 | << csvName(robot_state.tau_J, "state.tau_J") << "," 38 | << csvName(robot_state.tau_ext_hat_filtered, "state.tau_ext_hat_filtered"); 39 | return ostream.str(); 40 | } 41 | 42 | std::string csvRobotCommandHeader() { 43 | franka::RobotCommand command; 44 | std::ostringstream ostream; 45 | ostream << csvName(command.joint_positions.q, "cmd.q_d") << "," 46 | << csvName(command.joint_velocities.dq, "cmd.dq_d") << "," 47 | << csvName(command.cartesian_pose.O_T_EE, "cmd.O_T_EE_d") << "," 48 | << csvName(command.cartesian_velocities.O_dP_EE, "cmd.O_dP_EE_d") << "," 49 | << csvName(command.torques.tau_J, "cmd.tau_J_d"); 50 | return ostream.str(); 51 | } 52 | 53 | std::string csvLine(const franka::RobotState& robot_state) { 54 | std::ostringstream ostream; 55 | ostream << robot_state.time.toMSec() << "," << robot_state.control_command_success_rate << "," 56 | << robot_state.q << "," << robot_state.q_d << "," << robot_state.dq << "," 57 | << robot_state.dq_d << "," << robot_state.tau_J << "," 58 | << robot_state.tau_ext_hat_filtered; 59 | return ostream.str(); 60 | } 61 | 62 | std::string csvLine(const franka::RobotCommand& command) { 63 | std::ostringstream ostream; 64 | ostream << command.joint_positions.q << "," << command.joint_velocities.dq << "," 65 | << command.cartesian_pose.O_T_EE << "," << command.cartesian_velocities.O_dP_EE << "," 66 | << command.torques.tau_J; 67 | return ostream.str(); 68 | } 69 | 70 | } // anonymous namespace 71 | 72 | std::string logToCSV(const std::vector& log) { 73 | if (log.empty()) { 74 | return ""; 75 | } 76 | std::ostringstream ostream; 77 | 78 | ostream << csvRobotStateHeader() << "," << csvRobotCommandHeader() << std::endl; 79 | for (const Record& record : log) { 80 | ostream << csvLine(record.state) << "," << csvLine(record.command) << std::endl; 81 | } 82 | 83 | return ostream.str(); 84 | } 85 | 86 | } // namespace franka 87 | -------------------------------------------------------------------------------- /src/gripper.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "network.h" 11 | 12 | namespace franka { 13 | 14 | namespace { 15 | 16 | template 17 | bool executeCommand(Network& network, TArgs&&... args) { 18 | uint32_t command_id = network.tcpSendRequest(std::forward(args)...); 19 | typename T::Response response = network.tcpBlockingReceiveResponse(command_id); 20 | 21 | switch (response.status) { 22 | case T::Status::kSuccess: 23 | return true; 24 | case T::Status::kFail: 25 | throw CommandException("libfranka gripper: Command failed!"); 26 | case T::Status::kUnsuccessful: 27 | return false; 28 | case T::Status::kAborted: 29 | throw CommandException("libfranka gripper: Command aborted!"); 30 | default: 31 | throw ProtocolException("libfranka gripper: Unexpected response while handling command!"); 32 | } 33 | } 34 | 35 | GripperState convertGripperState( 36 | const research_interface::gripper::GripperState& gripper_state) noexcept { 37 | GripperState converted; 38 | converted.width = gripper_state.width; 39 | converted.max_width = gripper_state.max_width; 40 | converted.is_grasped = gripper_state.is_grasped; 41 | converted.temperature = gripper_state.temperature; 42 | converted.time = Duration(gripper_state.message_id); 43 | return converted; 44 | } 45 | 46 | } // anonymous namespace 47 | 48 | Gripper::Gripper(const std::string& franka_address) 49 | : network_{ 50 | std::make_unique(franka_address, research_interface::gripper::kCommandPort)} { 51 | connect( 52 | *network_, &ri_version_); 53 | } 54 | 55 | Gripper::~Gripper() noexcept = default; 56 | Gripper::Gripper(Gripper&&) noexcept = default; 57 | Gripper& Gripper::operator=(Gripper&&) noexcept = default; 58 | 59 | Gripper::ServerVersion Gripper::serverVersion() const noexcept { 60 | return ri_version_; 61 | } 62 | 63 | bool Gripper::homing() const { 64 | return executeCommand(*network_); 65 | } 66 | 67 | bool Gripper::grasp(double width, 68 | double speed, 69 | double force, 70 | double epsilon_inner, 71 | double epsilon_outer) const { 72 | research_interface::gripper::Grasp::GraspEpsilon epsilon(epsilon_inner, epsilon_outer); 73 | return executeCommand(*network_, width, epsilon, speed, 74 | force); 75 | } 76 | 77 | bool Gripper::move(double width, double speed) const { 78 | return executeCommand(*network_, width, speed); 79 | } 80 | 81 | bool Gripper::stop() const { 82 | return executeCommand(*network_); 83 | } 84 | 85 | GripperState Gripper::readOnce() const { 86 | research_interface::gripper::GripperState gripper_state; 87 | // Delete old data from the UDP buffer. 88 | while (network_->udpReceive(&gripper_state)) { 89 | } 90 | 91 | gripper_state = network_->udpBlockingReceive(); 92 | return convertGripperState(gripper_state); 93 | } 94 | 95 | } // namespace franka 96 | -------------------------------------------------------------------------------- /pylibfranka/docs/class_list.rst: -------------------------------------------------------------------------------- 1 | Class List 2 | ========== 3 | 4 | .. currentmodule:: pylibfranka 5 | 6 | Here are all the classes, structs, enums, and types with brief descriptions: 7 | 8 | Core Robot Control 9 | ------------------ 10 | 11 | .. list-table:: 12 | :widths: 30 70 13 | :header-rows: 0 14 | 15 | * - :class:`Robot` 16 | - Maintains a network connection to the robot, provides the current robot state, gives access to the model library and allows to control the robot 17 | * - :class:`ActiveControlBase` 18 | - Allows the user to read the state of a Robot and to send new control commands after starting a control process 19 | * - :class:`Model` 20 | - Calculates poses of joints and dynamic properties of the robot 21 | 22 | Gripper Control 23 | --------------- 24 | 25 | .. list-table:: 26 | :widths: 30 70 27 | :header-rows: 0 28 | 29 | * - :class:`Gripper` 30 | - Maintains a network connection to the gripper, provides the current gripper state, and allows the execution of commands 31 | * - :class:`GripperState` 32 | - Describes the gripper state 33 | 34 | Robot State 35 | ----------- 36 | 37 | .. list-table:: 38 | :widths: 30 70 39 | :header-rows: 0 40 | 41 | * - :class:`RobotState` 42 | - Describes the robot state including joint positions, velocities, torques, and errors 43 | * - :class:`RobotMode` 44 | - Enumerates different robot operation modes 45 | * - :class:`Errors` 46 | - Enumerates errors that can occur while controlling a Robot 47 | 48 | Motion Types 49 | ------------ 50 | 51 | .. list-table:: 52 | :widths: 30 70 53 | :header-rows: 0 54 | 55 | * - :class:`JointPositions` 56 | - Stores values for joint position motion generation 57 | * - :class:`JointVelocities` 58 | - Stores values for joint velocity motion generation 59 | * - :class:`CartesianPose` 60 | - Stores values for Cartesian pose motion generation 61 | * - :class:`CartesianVelocities` 62 | - Stores values for Cartesian velocity motion generation 63 | * - :class:`Torques` 64 | - Stores values for torque control 65 | 66 | Configuration Types 67 | ------------------- 68 | 69 | .. list-table:: 70 | :widths: 30 70 71 | :header-rows: 0 72 | 73 | * - :class:`Duration` 74 | - Represents a duration with millisecond resolution 75 | * - :class:`RealtimeConfig` 76 | - Enumeration for real-time scheduling requirements 77 | * - :class:`ControllerMode` 78 | - Enumeration for controller mode selection (joint or Cartesian impedance) 79 | 80 | Exceptions 81 | ---------- 82 | 83 | .. list-table:: 84 | :widths: 30 70 85 | :header-rows: 0 86 | 87 | * - :class:`FrankaException` 88 | - Base class for all exceptions used by libfranka 89 | * - :class:`CommandException` 90 | - Thrown if an error occurs during command execution 91 | * - :class:`ControlException` 92 | - Thrown if an error occurs during motion generation or torque control 93 | * - :class:`NetworkException` 94 | - Thrown if a connection to the robot cannot be established, or when a timeout occurs 95 | * - :class:`InvalidOperationException` 96 | - Thrown if an operation cannot be performed 97 | * - :class:`RealtimeException` 98 | - Thrown if real-time scheduling requirements cannot be met 99 | 100 | See Also 101 | -------- 102 | 103 | * :doc:`class_hierarchy` - View the inheritance structure 104 | * :doc:`modules` - Browse by module 105 | 106 | -------------------------------------------------------------------------------- /pylibfranka/examples/joint_position_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2025 Franka Robotics GmbH 4 | # Use of this source code is governed by the Apache-2.0 license, see LICENSE 5 | 6 | import argparse 7 | import time 8 | 9 | import numpy as np 10 | 11 | from pylibfranka import ControllerMode, JointPositions, Robot 12 | 13 | 14 | def main(): 15 | # Parse command line arguments 16 | parser = argparse.ArgumentParser() 17 | parser.add_argument("--ip", type=str, default="localhost", help="Robot IP address") 18 | args = parser.parse_args() 19 | 20 | # Connect to robot 21 | robot = Robot(args.ip) 22 | 23 | try: 24 | # Set collision behavior 25 | lower_torque_thresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] 26 | upper_torque_thresholds = [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0] 27 | lower_force_thresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] 28 | upper_force_thresholds = [20.0, 20.0, 20.0, 25.0, 25.0, 25.0] 29 | 30 | robot.set_collision_behavior( 31 | lower_torque_thresholds, 32 | upper_torque_thresholds, 33 | lower_force_thresholds, 34 | upper_force_thresholds, 35 | ) 36 | 37 | # First move the robot to a suitable joint configuration 38 | print("WARNING: This example will move the robot!") 39 | print("Please make sure to have the user stop button at hand!") 40 | input("Press Enter to continue...") 41 | 42 | # Start joint position control with external control loop 43 | active_control = robot.start_joint_position_control(ControllerMode.CartesianImpedance) 44 | 45 | initial_position = [0.0] * 7 46 | time_elapsed = 0.0 47 | motion_finished = False 48 | 49 | # External control loop 50 | while not motion_finished: 51 | # Read robot state and duration 52 | robot_state, duration = active_control.readOnce() 53 | 54 | # Update time 55 | time_elapsed += duration.to_sec() 56 | 57 | # On first iteration, capture initial position 58 | if time_elapsed <= duration.to_sec(): 59 | initial_position = robot_state.q_d if hasattr(robot_state, "q_d") else robot_state.q 60 | 61 | # Calculate delta angle using the same formula as in C++ example 62 | delta_angle = np.pi / 8.0 * (1 - np.cos(np.pi / 2.5 * time_elapsed)) 63 | 64 | # Update joint positions 65 | new_positions = [ 66 | initial_position[0], 67 | initial_position[1], 68 | initial_position[2], 69 | initial_position[3] + delta_angle, 70 | initial_position[4] + delta_angle, 71 | initial_position[5], 72 | initial_position[6] + delta_angle, 73 | ] 74 | 75 | # Set joint positions 76 | joint_positions = JointPositions(new_positions) 77 | 78 | # Set motion_finished flag to True on the last update 79 | if time_elapsed >= 5.0: 80 | joint_positions.motion_finished = True 81 | motion_finished = True 82 | print("Finished motion, shutting down example") 83 | 84 | # Send command to robot 85 | active_control.writeOnce(joint_positions) 86 | 87 | except Exception as e: 88 | print(f"Error occurred: {e}") 89 | if robot is not None: 90 | robot.stop() 91 | return -1 92 | 93 | 94 | if __name__ == "__main__": 95 | main() 96 | -------------------------------------------------------------------------------- /include/franka/exception.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | /** 11 | * @file exception.h 12 | * Contains exception definitions. 13 | */ 14 | 15 | namespace franka { 16 | 17 | /** 18 | * Base class for all exceptions used by `libfranka`. 19 | */ 20 | struct Exception : public std::runtime_error { 21 | using std::runtime_error::runtime_error; 22 | }; 23 | 24 | /** 25 | * ModelException is thrown if an error occurs when loading the model library. 26 | */ 27 | struct ModelException : public Exception { 28 | using Exception::Exception; 29 | }; 30 | 31 | /** 32 | * NetworkException is thrown if a connection to the robot cannot be established, or when a timeout 33 | * occurs. 34 | */ 35 | struct NetworkException : public Exception { 36 | using Exception::Exception; 37 | }; 38 | 39 | /** 40 | * ProtocolException is thrown if the robot returns an incorrect message. 41 | */ 42 | struct ProtocolException : public Exception { 43 | using Exception::Exception; 44 | }; 45 | 46 | /** 47 | * IncompatibleVersionException is thrown if the robot does not support this version of libfranka. 48 | */ 49 | struct IncompatibleVersionException : public Exception { 50 | /** 51 | * Creates the exception using the two different protocol versions. 52 | * @param[in] server_version Protocol version on the Control side. 53 | * @param[in] library_version Protocol version of libfranka. 54 | */ 55 | IncompatibleVersionException(uint16_t server_version, uint16_t library_version) noexcept; 56 | 57 | /** 58 | * Control's protocol version. 59 | */ 60 | const uint16_t server_version; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) 61 | /** 62 | * libfranka protocol version. 63 | */ 64 | const uint16_t library_version; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) 65 | }; 66 | 67 | /** 68 | * ControlException is thrown if an error occurs during motion generation or torque control. 69 | * The exception holds a vector with the last received robot states. The number of recorded 70 | * states can be configured in the Robot constructor. 71 | * 72 | */ 73 | struct ControlException : public Exception { 74 | /** 75 | * Creates the exception with an explanatory string and a Log object. 76 | * 77 | * @param[in] what Explanatory string. 78 | * @param[in] log Vector of last received states and commands. 79 | */ 80 | explicit ControlException(const std::string& what, std::vector log = {}) noexcept; 81 | 82 | /** 83 | * Vector of states and commands logged just before the exception occurred. 84 | */ 85 | const std::vector 86 | log; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) 87 | }; 88 | 89 | /** 90 | * CommandException is thrown if an error occurs during command execution. 91 | */ 92 | struct CommandException : public Exception { 93 | using Exception::Exception; 94 | }; 95 | 96 | /** 97 | * RealtimeException is thrown if realtime priority cannot be set. 98 | */ 99 | struct RealtimeException : public Exception { 100 | using Exception::Exception; 101 | }; 102 | 103 | /** 104 | * InvalidOperationException is thrown if an operation cannot be performed. 105 | */ 106 | struct InvalidOperationException : public Exception { 107 | using Exception::Exception; 108 | }; 109 | 110 | } // namespace franka 111 | -------------------------------------------------------------------------------- /src/load_calculations.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include "load_calculations.h" 4 | 5 | namespace franka { 6 | 7 | std::array combineCenterOfMass( 8 | double m_ee, 9 | const std::array& F_x_Cee, // NOLINT(readability-identifier-naming) 10 | double m_load, 11 | const std::array& F_x_Cload) { // NOLINT(readability-identifier-naming) 12 | std::array F_x_Ctotal{}; // NOLINT(readability-identifier-naming) 13 | if ((m_ee + m_load) > 0) { 14 | for (size_t i = 0; i < F_x_Ctotal.size(); i++) { 15 | F_x_Ctotal[i] = (m_ee * F_x_Cee[i] + m_load * F_x_Cload[i]) / (m_ee + m_load); 16 | } 17 | } 18 | 19 | return F_x_Ctotal; 20 | } 21 | 22 | Eigen::Matrix3d skewSymmetricMatrixFromVector(const Eigen::Vector3d& input) { 23 | Eigen::Matrix3d input_hat; 24 | input_hat << 0, -input(2), input(1), input(2), 0, -input(0), -input(1), input(0), 0; 25 | return input_hat; 26 | } 27 | 28 | std::array combineInertiaTensor( 29 | double m_ee, 30 | const std::array& F_x_Cee, // NOLINT(readability-identifier-naming) 31 | const std::array& I_ee, // NOLINT(readability-identifier-naming) 32 | double m_load, 33 | const std::array& F_x_Cload, // NOLINT(readability-identifier-naming) 34 | const std::array& I_load, // NOLINT(readability-identifier-naming) 35 | double m_total, 36 | const std::array& F_x_Ctotal) { // NOLINT(readability-identifier-naming) 37 | // If the combined mass equals to zero, the combined inertia is also zero. 38 | if (m_total == 0) { 39 | return std::array{}; 40 | } 41 | 42 | Eigen::Vector3d center_of_mass_ee(F_x_Cee.data()); 43 | Eigen::Vector3d center_of_mass_load(F_x_Cload.data()); 44 | Eigen::Vector3d center_of_mass_total(F_x_Ctotal.data()); 45 | 46 | Eigen::Matrix3d inertia_ee(I_ee.data()); 47 | Eigen::Matrix3d inertia_ee_flange = Eigen::Matrix3d::Zero(); 48 | Eigen::Matrix3d inertia_load(I_load.data()); 49 | Eigen::Matrix3d inertia_load_flange = Eigen::Matrix3d::Zero(); 50 | Eigen::Matrix3d inertia_total_flange = Eigen::Matrix3d::Zero(); 51 | 52 | // Check if the mass equals zero, the inertia should then be zero as well. 53 | if (m_ee == 0) { 54 | inertia_ee = Eigen::Matrix3d::Zero(); 55 | } 56 | if (m_load == 0) { 57 | inertia_load = Eigen::Matrix3d::Zero(); 58 | } 59 | 60 | // Calculate inertia tensor of EE and load in flange coordinates. 61 | inertia_ee_flange = inertia_ee - m_ee * (skewSymmetricMatrixFromVector(center_of_mass_ee) * 62 | skewSymmetricMatrixFromVector(center_of_mass_ee)); 63 | inertia_load_flange = 64 | inertia_load - m_load * (skewSymmetricMatrixFromVector(center_of_mass_load) * 65 | skewSymmetricMatrixFromVector(center_of_mass_load)); 66 | 67 | // Calculate combined inertia tensor in flange coordinate. 68 | inertia_total_flange = inertia_ee_flange + inertia_load_flange; 69 | 70 | // Calculate combined inertia tensor in combined body center of mass coordinate. 71 | std::array I_total; // NOLINT(readability-identifier-naming) 72 | Eigen::Map inertia_total(I_total.data(), 3, 3); 73 | inertia_total = 74 | inertia_total_flange + m_total * (skewSymmetricMatrixFromVector(center_of_mass_total) * 75 | skewSymmetricMatrixFromVector(center_of_mass_total)); 76 | 77 | return I_total; 78 | } 79 | 80 | } // namespace franka 81 | -------------------------------------------------------------------------------- /examples/generate_joint_velocity_motion_external_control_loop.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "examples_common.h" 12 | 13 | /** 14 | * @example generate_joint_velocity_motion_external_control_loop.cpp 15 | * An example showing how to generate a joint velocity motion with an external control loop. 16 | * 17 | * @warning Before executing this example, make sure there is enough space in front of the robot. 18 | */ 19 | 20 | int main(int argc, char** argv) { 21 | // Check whether the required arguments were passed 22 | if (argc != 2) { 23 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 24 | return -1; 25 | } 26 | 27 | try { 28 | franka::Robot robot(argv[1]); 29 | setDefaultBehavior(robot); 30 | 31 | // First move the robot to a suitable joint configuration 32 | std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; 33 | MotionGenerator motion_generator(0.5, q_goal); 34 | std::cout << "WARNING: This example will move the robot! " 35 | << "Please make sure to have the user stop button at hand!" << std::endl 36 | << "Press Enter to continue..." << std::endl; 37 | std::cin.ignore(); 38 | robot.control(motion_generator); 39 | std::cout << "Finished moving to initial joint configuration." << std::endl; 40 | 41 | // Set additional parameters always before the control loop, NEVER in the control loop! 42 | // Set collision behavior. 43 | robot.setCollisionBehavior( 44 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 45 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 46 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, 47 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); 48 | 49 | double time_max = 1.0; 50 | double omega_max = 1.0; 51 | double time = 0.0; 52 | 53 | auto callback_control = [=, &time](const franka::RobotState&, 54 | franka::Duration period) -> franka::JointVelocities { 55 | time += period.toSec(); 56 | 57 | double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); 58 | double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); 59 | 60 | franka::JointVelocities velocities = {{0.0, 0.0, 0.0, omega, omega, omega, omega}}; 61 | 62 | if (time >= 2 * time_max) { 63 | std::cout << std::endl << "Finished motion, shutting down example" << std::endl; 64 | return franka::MotionFinished(velocities); 65 | } 66 | return velocities; 67 | }; 68 | 69 | bool motion_finished = false; 70 | auto active_control = robot.startJointVelocityControl( 71 | research_interface::robot::Move::ControllerMode::kJointImpedance); 72 | while (!motion_finished) { 73 | auto read_once_return = active_control->readOnce(); 74 | auto robot_state = read_once_return.first; 75 | auto duration = read_once_return.second; 76 | auto joint_velocities = callback_control(robot_state, duration); 77 | motion_finished = joint_velocities.motion_finished; 78 | active_control->writeOnce(joint_velocities); 79 | } 80 | 81 | } catch (const franka::Exception& e) { 82 | std::cout << e.what() << std::endl; 83 | return -1; 84 | } 85 | 86 | return 0; 87 | } 88 | -------------------------------------------------------------------------------- /include/franka/active_control.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #pragma once 4 | 5 | #include 6 | #include 7 | 8 | #include "robot.h" 9 | 10 | /** 11 | * @file active_control.h 12 | * Implements the ActiveControlBase abstract class. Contains the `franka::ActiveControl`, 13 | * `franka::ActiveTorqueControl` and `franka::ActiveMotionGenerator` type. 14 | */ 15 | 16 | namespace franka { 17 | 18 | /** 19 | * Documented in ActiveControlBase 20 | */ 21 | class ActiveControl : public ActiveControlBase { 22 | public: 23 | ~ActiveControl() override; 24 | 25 | std::pair readOnce() override; 26 | 27 | void writeOnce(const Torques& /* control_input */) override { 28 | throw franka::ControlException(wrong_write_once_method_called); 29 | }; 30 | 31 | void writeOnce(const JointPositions& /* motion_generator_input */, 32 | const std::optional& /*control_input*/) override { 33 | throw franka::ControlException(wrong_write_once_method_called); 34 | }; 35 | 36 | void writeOnce(const JointVelocities& /* motion_generator_input */, 37 | const std::optional& /* control_input */) override { 38 | throw franka::ControlException(wrong_write_once_method_called); 39 | }; 40 | 41 | void writeOnce(const CartesianPose& /* motion_generator_input */, 42 | const std::optional& /* control_input */) override { 43 | throw franka::ControlException(wrong_write_once_method_called); 44 | }; 45 | 46 | void writeOnce(const CartesianVelocities& /* motion_generator_input */, 47 | const std::optional& /* control_input */) override { 48 | throw franka::ControlException(wrong_write_once_method_called); 49 | }; 50 | 51 | void writeOnce(const JointPositions& motion_generator_input) override { 52 | writeOnce(motion_generator_input, std::optional()); 53 | }; 54 | 55 | void writeOnce(const JointVelocities& motion_generator_input) override { 56 | writeOnce(motion_generator_input, std::optional()); 57 | }; 58 | 59 | void writeOnce(const CartesianPose& motion_generator_input) override { 60 | writeOnce(motion_generator_input, std::optional()); 61 | }; 62 | 63 | void writeOnce(const CartesianVelocities& motion_generator_input) override { 64 | writeOnce(motion_generator_input, std::optional()); 65 | }; 66 | 67 | protected: 68 | /** 69 | * Construct a new ActiveControl object 70 | * 71 | * @param robot_impl shared_ptr to the Robot::Impl in the Robot 72 | * @param motion_id id of the managed motion 73 | * @param control_lock of the Robot, preventing other read and write accesses during the active 74 | * control 75 | */ 76 | ActiveControl(std::shared_ptr robot_impl, 77 | uint32_t motion_id, 78 | std::unique_lock control_lock); 79 | 80 | /// shared pointer to Robot::Impl instance for read and write accesses 81 | std::shared_ptr robot_impl; 82 | 83 | /// motion id of running motion 84 | uint32_t motion_id; 85 | 86 | /// control-lock preventing parallel control processes 87 | std::unique_lock control_lock; 88 | 89 | /// flag indicating if control process is finished 90 | bool control_finished{false}; 91 | 92 | /// duration to last read access 93 | std::optional last_read_access; 94 | 95 | private: 96 | const std::string wrong_write_once_method_called{ 97 | "Wrong writeOnce method called for currently active control!"}; 98 | }; 99 | 100 | } // namespace franka 101 | -------------------------------------------------------------------------------- /pylibfranka/docs/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # Documentation is extracted from docstrings in the compiled C++ extension module. 4 | 5 | import os 6 | import sys 7 | import locale 8 | 9 | # Set locale to avoid locale errors during build 10 | try: 11 | locale.setlocale(locale.LC_ALL, 'C.UTF-8') 12 | except locale.Error: 13 | try: 14 | locale.setlocale(locale.LC_ALL, 'en_US.UTF-8') 15 | except locale.Error: 16 | pass # Fallback to default 17 | 18 | # Add libfranka to library path so the compiled extension can be imported 19 | libfranka_paths = [ 20 | '/usr/local/lib', 21 | ] 22 | ld_library_path = os.environ.get('LD_LIBRARY_PATH', '') 23 | new_paths = ':'.join([p for p in libfranka_paths if os.path.exists(p)]) 24 | if new_paths: 25 | if ld_library_path: 26 | os.environ['LD_LIBRARY_PATH'] = f"{new_paths}:{ld_library_path}" 27 | else: 28 | os.environ['LD_LIBRARY_PATH'] = new_paths 29 | 30 | # Add the parent directory to sys.path to import the module 31 | sys.path.insert(0, os.path.abspath('..')) 32 | 33 | # -- Project information ----------------------------------------------------- 34 | project = 'pylibfranka' 35 | copyright = '2025, Franka Robotics GmbH' 36 | author = 'Franka Robotics GmbH' 37 | 38 | # Get version from _version.py 39 | import pylibfranka 40 | version = pylibfranka.__version__ 41 | 42 | # -- General configuration --------------------------------------------------- 43 | extensions = [ 44 | 'sphinx.ext.autodoc', 45 | 'sphinx.ext.autosummary', 46 | 'sphinx.ext.intersphinx', 47 | 'sphinx.ext.napoleon', 48 | 'sphinx.ext.viewcode', 49 | 'sphinx.ext.mathjax', 50 | 'sphinx.ext.inheritance_diagram', 51 | 'sphinx.ext.graphviz', 52 | ] 53 | 54 | templates_path = ['_templates'] 55 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 56 | 57 | # -- Options for HTML output ------------------------------------------------- 58 | html_theme = 'pydata_sphinx_theme' 59 | html_static_path = [] 60 | 61 | html_theme_options = { 62 | "logo": { 63 | "text": f"pylibfranka v{version}", 64 | }, 65 | "github_url": "https://github.com/frankarobotics/libfranka", 66 | "navbar_end": ["navbar-icon-links", "search-field"], 67 | "show_nav_level": 2, 68 | "navigation_depth": 3, 69 | "show_toc_level": 2, 70 | "secondary_sidebar_items": ["page-toc"], 71 | } 72 | 73 | html_context = { 74 | "default_mode": "dark", 75 | "display_version": True, 76 | } 77 | 78 | # Add version to HTML title 79 | html_title = f"pylibfranka {version} documentation" 80 | 81 | # -- Extension configuration ------------------------------------------------- 82 | 83 | # Autodoc settings 84 | autodoc_default_options = { 85 | 'members': True, 86 | 'member-order': 'bysource', 87 | 'undoc-members': True, 88 | 'show-inheritance': True, 89 | 'inherited-members': False, 90 | } 91 | 92 | autodoc_typehints = 'description' 93 | autodoc_typehints_description_target = 'documented' 94 | 95 | # Autosummary settings 96 | autosummary_generate = True 97 | 98 | # Napoleon settings to parse @param and @return tags 99 | napoleon_google_docstring = False 100 | napoleon_numpy_docstring = False 101 | napoleon_use_param = True 102 | napoleon_use_rtype = True 103 | napoleon_custom_sections = [('Returns', 'params_style')] 104 | 105 | # Intersphinx mapping 106 | intersphinx_mapping = { 107 | 'python': ('https://docs.python.org/3', None), 108 | 'numpy': ('https://numpy.org/doc/stable/', None), 109 | } 110 | 111 | # Inheritance diagram settings 112 | inheritance_graph_attrs = dict(rankdir="TB", size='"8.0, 10.0"', fontsize=14, ratio='auto') 113 | inheritance_node_attrs = dict(shape='box', fontsize=14, height=0.5, color='dodgerblue', style='filled', fillcolor='lightyellow') 114 | -------------------------------------------------------------------------------- /examples/generate_cartesian_pose_motion_external_control_loop.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Franka Robotics GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "examples_common.h" 11 | 12 | /** 13 | * @example generate_cartesian_pose_motion_external_control_loop.cpp 14 | * An example showing how to generate a Cartesian motion with an external control loop. 15 | * 16 | * @warning Before executing this example, make sure there is enough space in front of the robot. 17 | */ 18 | 19 | int main(int argc, char** argv) { 20 | // Check whether the required arguments were passed 21 | if (argc != 2) { 22 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 23 | return -1; 24 | } 25 | 26 | try { 27 | franka::Robot robot(argv[1]); 28 | setDefaultBehavior(robot); 29 | 30 | // First move the robot to a suitable joint configuration 31 | std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; 32 | MotionGenerator motion_generator(0.5, q_goal); 33 | std::cout << "WARNING: This example will move the robot! " 34 | << "Please make sure to have the user stop button at hand!" << std::endl 35 | << "Press Enter to continue..." << std::endl; 36 | std::cin.ignore(); 37 | robot.control(motion_generator); 38 | std::cout << "Finished moving to initial joint configuration." << std::endl; 39 | 40 | // Set additional parameters always before the control loop, NEVER in the control loop! 41 | // Set collision behavior. 42 | robot.setCollisionBehavior( 43 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 44 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 45 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, 46 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); 47 | 48 | std::array initial_pose; 49 | double time = 0.0; 50 | 51 | auto callback_control = [&time, &initial_pose]( 52 | const franka::RobotState& robot_state, 53 | franka::Duration period) -> franka::CartesianPose { 54 | time += period.toSec(); 55 | 56 | if (time == 0.0) { 57 | initial_pose = robot_state.O_T_EE_c; 58 | } 59 | 60 | constexpr double kRadius = 0.3; 61 | double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time)); 62 | double delta_x = kRadius * std::sin(angle); 63 | double delta_z = kRadius * (std::cos(angle) - 1); 64 | 65 | std::array new_pose = initial_pose; 66 | new_pose[12] += delta_x; 67 | new_pose[14] += delta_z; 68 | 69 | if (time >= 10.0) { 70 | std::cout << std::endl << "Finished motion, shutting down example" << std::endl; 71 | return franka::MotionFinished(new_pose); 72 | } 73 | return new_pose; 74 | }; 75 | 76 | bool motion_finished = false; 77 | auto active_control = robot.startCartesianPoseControl( 78 | research_interface::robot::Move::ControllerMode::kJointImpedance); 79 | while (!motion_finished) { 80 | auto read_once_return = active_control->readOnce(); 81 | auto robot_state = read_once_return.first; 82 | auto duration = read_once_return.second; 83 | auto cartesian_positions = callback_control(robot_state, duration); 84 | motion_finished = cartesian_positions.motion_finished; 85 | active_control->writeOnce(cartesian_positions); 86 | } 87 | 88 | } catch (const franka::Exception& e) { 89 | std::cout << e.what() << std::endl; 90 | return -1; 91 | } 92 | 93 | return 0; 94 | } 95 | --------------------------------------------------------------------------------