From 917bafd1b63d89e9baddea4c3ded4f36126a0f57 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Sat, 9 Nov 2024 17:19:01 -0700 Subject: [PATCH] Fix CMake for installing the package (#3) --- .github/workflows/cmake-single-platform.yml | 21 +- CMakeLists.txt | 58 +- cmake/fast_forward_kinematics-extras.cmake | 67 - .../get_num_joints.py | 0 .../robot_config.cpp.template | 2 +- .../robot_config.cu.template | 0 {scripts => code_generation}/robot_gen.py | 0 fast_forward_kinematics-config.cmake.in | 11 + fast_forward_kinematics.cmake | 114 ++ include/fast_inverse_kinematics.hpp | 46 - .../fast_inverse_kinematics.hpp | 74 + .../fast_kinematics.hpp | 113 ++ .../kinematics_interface.hpp | 2 - include/fast_kinematics.hpp | 51 - src/fast_inverse_kinematics.cpp | 52 - src/fast_kinematics.cpp | 103 -- test/CMakeLists.txt | 13 +- test/fetch_content_test/CMakeLists.txt | 25 + test/fetch_content_test/urdf/robot.urdf | 1356 +++++++++++++++++ test/forward_kinematics_test.cpp | 2 +- .../fast_kinematics_joint_data_length.hpp | 0 {include => test/include}/kdl_kinematics.hpp | 0 test/inverse_kinematics_test.cpp | 2 +- 23 files changed, 1769 insertions(+), 343 deletions(-) delete mode 100644 cmake/fast_forward_kinematics-extras.cmake rename {scripts => code_generation}/get_num_joints.py (100%) rename {scripts => code_generation}/robot_config.cpp.template (99%) rename {scripts => code_generation}/robot_config.cu.template (100%) rename {scripts => code_generation}/robot_gen.py (100%) create mode 100644 fast_forward_kinematics-config.cmake.in create mode 100644 fast_forward_kinematics.cmake delete mode 100644 include/fast_inverse_kinematics.hpp create mode 100644 include/fast_inverse_kinematics/fast_inverse_kinematics.hpp create mode 100644 include/fast_inverse_kinematics/fast_kinematics.hpp rename include/{ => fast_inverse_kinematics}/kinematics_interface.hpp (98%) delete mode 100644 include/fast_kinematics.hpp delete mode 100644 src/fast_inverse_kinematics.cpp delete mode 100644 src/fast_kinematics.cpp create mode 100644 test/fetch_content_test/CMakeLists.txt create mode 100644 test/fetch_content_test/urdf/robot.urdf rename {include => test/include}/fast_kinematics_joint_data_length.hpp (100%) rename {include => test/include}/kdl_kinematics.hpp (100%) diff --git a/.github/workflows/cmake-single-platform.yml b/.github/workflows/cmake-single-platform.yml index 0ac33d9..889f4f7 100644 --- a/.github/workflows/cmake-single-platform.yml +++ b/.github/workflows/cmake-single-platform.yml @@ -21,15 +21,22 @@ jobs: steps: - uses: actions/checkout@v4 - + - name: Set up Python 3.8 + uses: actions/setup-python@v2 + with: + python-version: 3.8 + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install urdf-parser-py jinja2 numpy - name: Configure CMake # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type - run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBUILD_TESTING=ON -DCMAKE_INSTALL_PREFIX=${{github.workspace}}/install - name: Build # Build your program with the given configuration - run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} + run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} --target install - name: Test working-directory: ${{github.workspace}}/build @@ -37,3 +44,11 @@ jobs: # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail run: ctest -C ${{env.BUILD_TYPE}} + - name: Configure CMake For Fetch Content + # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. + # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type + run: cmake -B ${{github.workspace}}/build_fetch -S ${{github.workspace}}/test/fetch_content_test -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} + + - name: Build For Fetch Content + # Build your program with the given configuration + run: cmake --build ${{github.workspace}}/build_fetch --config ${{env.BUILD_TYPE}} diff --git a/CMakeLists.txt b/CMakeLists.txt index 6da7c6f..d123030 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,17 +1,57 @@ cmake_minimum_required(VERSION 3.22) project(fast_forward_kinematics) -# Set C++ standard to C++20 -set(CMAKE_CXX_STANDARD 20) +set(FFK_VERSION_MAJOR 0) +set(FFK_VERSION_MINOR 1) +set(FFK_VERSION ${FFK_VERSION_MAJOR}.${FFK_VERSION_MINOR}) -# Generate the module for the python -include(cmake/fast_forward_kinematics-extras.cmake) +option(BUILD_TESTING "Build fast_forward_kinematics tests" OFF) -configure_file( - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/fast_forward_kinematics-extras.cmake - "${PROJECT_BINARY_DIR}/${CMAKE_FILES_DIRECTORY}/fast_forward_kinematics-extras.cmake" @ONLY +# Include GNUInstallDirs to get canonical paths +include(GNUInstallDirs) +include(CTest) + +# Add cmake macro to project +include(fast_forward_kinematics.cmake) + +add_library(fast_forward_kinematics_header INTERFACE) +target_include_directories(fast_forward_kinematics_header INTERFACE + $ + $ ) +if (BUILD_TESTING) + add_subdirectory(test) +endif () + + +include(CMakePackageConfigHelpers) -install(DIRECTORY include DESTINATION include/) +configure_package_config_file(fast_forward_kinematics-config.cmake.in fast_forward_kinematics-config.cmake + INSTALL_DESTINATION "${CMAKE_INSTALL_LIBDIR}/fast_forward_kinematics" + PATH_VARS CMAKE_INSTALL_INCLUDEDIR CMAKE_INSTALL_LIBDIR + NO_CHECK_REQUIRED_COMPONENTS_MACRO) -add_subdirectory(test) +write_basic_package_version_file(fast_forward_kinematics-config-version.cmake + VERSION ${FFK_VERSION} COMPATIBILITY AnyNewerVersion) + +install(FILES + "${CMAKE_BINARY_DIR}/fast_forward_kinematics-config.cmake" + "${CMAKE_BINARY_DIR}/fast_forward_kinematics-config-version.cmake" + "${CMAKE_SOURCE_DIR}/fast_forward_kinematics.cmake" + DESTINATION "${CMAKE_INSTALL_DATADIR}/fast_forward_kinematics/cmake") + +install(DIRECTORY "${CMAKE_SOURCE_DIR}/code_generation" DESTINATION "${CMAKE_INSTALL_DATADIR}/fast_forward_kinematics/cmake") +install(DIRECTORY include/ DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/fast_forward_kinematics") + +# Create an export set +install(TARGETS fast_forward_kinematics_header EXPORT fast_forward_kinematicsTargets) +# Targets files +export( + EXPORT fast_forward_kinematicsTargets + FILE ${CMAKE_CURRENT_BINARY_DIR}/fast_forward_kinematics-targets.cmake +) +install( + EXPORT fast_forward_kinematicsTargets + FILE fast_forward_kinematics-targets.cmake + DESTINATION lib/cmake/fast_forward_kinematics +) \ No newline at end of file diff --git a/cmake/fast_forward_kinematics-extras.cmake b/cmake/fast_forward_kinematics-extras.cmake deleted file mode 100644 index 5eabda1..0000000 --- a/cmake/fast_forward_kinematics-extras.cmake +++ /dev/null @@ -1,67 +0,0 @@ -function(generate_fast_forward_kinematics_library_common URDF_FILE ROOT_LINK TIP_LINK EXT) - - find_package(Python REQUIRED COMPONENTS Interpreter) - if (Python_Interpreter_FOUND) - message(STATUS "Python executable: ${Python_EXECUTABLE}") - else () - message(FATAL_ERROR "Python interpreter not found.") - endif () - - execute_process( - COMMAND ${Python_EXECUTABLE} ${CMAKE_SOURCE_DIR}/scripts/get_num_joints.py ${URDF_FILE} ${ROOT_LINK} ${TIP_LINK} - OUTPUT_VARIABLE FAST_FK_NUMBER_OF_JOINTS - OUTPUT_STRIP_TRAILING_WHITESPACE - COMMAND_ECHO STDOUT - ) - - add_custom_command( - OUTPUT forward_kinematics_lib.${EXT} - COMMAND ${Python_EXECUTABLE} ${CMAKE_SOURCE_DIR}/scripts/robot_gen.py ${URDF_FILE} ${CMAKE_SOURCE_DIR}/scripts/robot_config.${EXT}.template - ${CMAKE_CURRENT_BINARY_DIR}/forward_kinematics_lib.${EXT} ${ROOT_LINK} ${TIP_LINK} - DEPENDS ${URDF_FILE} ${CMAKE_SOURCE_DIR}/scripts/robot_config.${EXT}.template - COMMENT - "Running `${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/scripts/robot_gen.py - ${URDF_FILE} - ${CMAKE_SOURCE_DIR}/scripts/robot_config.cpp.template - ${CMAKE_CURRENT_BINARY_DIR}/forward_kinematics_test.cpp - ${ROOT_LINK} - ${TIP_LINK}`" - VERBATIM - ) - add_custom_target(code_generation DEPENDS forward_kinematics_lib.${EXT}) - - set(sources ${CMAKE_SOURCE_DIR}/src/fast_kinematics.cpp ${CMAKE_SOURCE_DIR}/src/fast_inverse_kinematics.cpp) - - add_library(fast_forward_kinematics_library SHARED forward_kinematics_lib.${EXT} ${sources}) - add_dependencies(fast_forward_kinematics_library code_generation) - - target_include_directories(fast_forward_kinematics_library PUBLIC ${CMAKE_SOURCE_DIR}/include) - target_compile_definitions(fast_forward_kinematics_library PUBLIC "${FAST_FK_NUMBER_OF_JOINTS}") - find_package(Eigen3 3.3 NO_MODULE) - target_link_libraries(fast_forward_kinematics_library PUBLIC Eigen3::Eigen) - -endfunction() - -function(generate_fast_forward_kinematics_library URDF_FILE ROOT_LINK TIP_LINK) - include(ExternalProject) - ExternalProject_Add( - LBFGSpp - PREFIX ${CMAKE_BINARY_DIR}/LBFGSpp - GIT_REPOSITORY https://github.com/yixuan/LBFGSpp.git - GIT_TAG v0.3.0 - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_BINARY_DIR} - ) - ExternalProject_Get_Property(LBFGSpp source_dir) - set(LBFGSppIncludeDir ${source_dir}/include) - - generate_fast_forward_kinematics_library_common(${URDF_FILE} ${ROOT_LINK} ${TIP_LINK} "cpp") - - add_dependencies(fast_forward_kinematics_library LBFGSpp) - target_compile_definitions(fast_forward_kinematics_library PUBLIC FAST_FK_USE_IK) - target_include_directories(fast_forward_kinematics_library PUBLIC ${LBFGSppIncludeDir}) - -endfunction() - -function(generate_fast_forward_kinematics_library_cuda URDF_FILE ROOT_LINK TIP_LINK) - generate_fast_forward_kinematics_library_common(${URDF_FILE} ${ROOT_LINK} ${TIP_LINK} "cu") -endfunction() \ No newline at end of file diff --git a/scripts/get_num_joints.py b/code_generation/get_num_joints.py similarity index 100% rename from scripts/get_num_joints.py rename to code_generation/get_num_joints.py diff --git a/scripts/robot_config.cpp.template b/code_generation/robot_config.cpp.template similarity index 99% rename from scripts/robot_config.cpp.template rename to code_generation/robot_config.cpp.template index cc8e508..408c790 100644 --- a/scripts/robot_config.cpp.template +++ b/code_generation/robot_config.cpp.template @@ -219,7 +219,7 @@ namespace fast_fk::internal { } #ifdef FAST_FK_USE_IK -#include "fast_inverse_kinematics.hpp" +#include "fast_inverse_kinematics/fast_inverse_kinematics.hpp" namespace fast_fk::internal { constexpr float axis_scale = 1; diff --git a/scripts/robot_config.cu.template b/code_generation/robot_config.cu.template similarity index 100% rename from scripts/robot_config.cu.template rename to code_generation/robot_config.cu.template diff --git a/scripts/robot_gen.py b/code_generation/robot_gen.py similarity index 100% rename from scripts/robot_gen.py rename to code_generation/robot_gen.py diff --git a/fast_forward_kinematics-config.cmake.in b/fast_forward_kinematics-config.cmake.in new file mode 100644 index 0000000..b85508a --- /dev/null +++ b/fast_forward_kinematics-config.cmake.in @@ -0,0 +1,11 @@ +@PACKAGE_INIT@ + +set(FFK_VERSION_MAJOR @FFK_VERSION_MAJOR@) +set(FFK_VERSION_MINOR @FFK_VERSION_MINOR@) +set(FFK_VERSION @FFK_VERSION@) + +set(fast_forward_kinematics_FOUND ON) +set_and_check(fast_forward_kinematics_INCLUDE_DIRS "@PACKAGE_CMAKE_INSTALL_INCLUDEDIR@") + +include("${CMAKE_CURRENT_LIST_DIR}/fast_forward_kinematics-targets.cmake") +include("${CMAKE_CURRENT_LIST_DIR}/cmake/fast_forward_kinematics.cmake") \ No newline at end of file diff --git a/fast_forward_kinematics.cmake b/fast_forward_kinematics.cmake new file mode 100644 index 0000000..1c48ef9 --- /dev/null +++ b/fast_forward_kinematics.cmake @@ -0,0 +1,114 @@ +include(CMakeParseArguments) +function(generate_fast_forward_kinematics_library_common target_name) + cmake_parse_arguments(ARG "" "URDF_FILE;ROOT_LINK;TIP_LINK;EXT" "" ${ARGN}) + if (ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "generate_fast_forward_kinematics_library_common() called with invalid arguments.") + endif () + + # Set cmake build type to Release if not specified + if (NOT DEFINED CMAKE_BUILD_TYPE OR "${CMAKE_BUILD_TYPE}" STREQUAL "") + set(CMAKE_BUILD_TYPE Release) + endif () + + find_package(Python REQUIRED COMPONENTS Interpreter) + if (Python_Interpreter_FOUND) + message(STATUS "Python executable: ${Python_EXECUTABLE}") + else () + message(FATAL_ERROR "Python interpreter not found.") + endif () + + execute_process( + COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/code_generation/get_num_joints.py ${ARG_URDF_FILE} ${ARG_ROOT_LINK} ${ARG_TIP_LINK} + OUTPUT_VARIABLE FAST_FK_NUMBER_OF_JOINTS + OUTPUT_STRIP_TRAILING_WHITESPACE + COMMAND_ECHO STDOUT + ) + + add_custom_command( + OUTPUT forward_kinematics_lib.${ARG_EXT} + COMMAND ${Python_EXECUTABLE} ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/code_generation/robot_gen.py ${ARG_URDF_FILE} ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/code_generation/robot_config.${ARG_EXT}.template + ${CMAKE_CURRENT_BINARY_DIR}/forward_kinematics_lib.${ARG_EXT} ${ARG_ROOT_LINK} ${ARG_TIP_LINK} + DEPENDS ${ARG_URDF_FILE} ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/code_generation/robot_config.${ARG_EXT}.template + COMMENT + "Running `${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/code_generation/robot_gen.py + ${ARG_URDF_FILE} + ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/code_generation/robot_config.cpp.template + ${CMAKE_CURRENT_BINARY_DIR}/forward_kinematics_test.cpp + ${ROOT_LINK} + ${TIP_LINK}`" + VERBATIM + ) + add_custom_target(code_generation DEPENDS forward_kinematics_lib.${ARG_EXT}) + add_library(${target_name} SHARED forward_kinematics_lib.${ARG_EXT}) + add_dependencies(${target_name} code_generation) + + target_link_libraries(${target_name} PUBLIC fast_forward_kinematics_header) + target_compile_definitions(${target_name} PUBLIC "${FAST_FK_NUMBER_OF_JOINTS}") + + +endfunction() + + +macro(ffk_failed_arg_parse) + message(FATAL_ERROR "generate_fast_forward_kinematics_library() called with unused arguments: ${ARG_UNPARSED_ARGUMENTS}. " + "Expected arguments: generate_fast_forward_kinematics_library(target_name URDF_FILE val1 ROOT_LINK val2 TIP_LINK val3)") +endmacro() + +function(generate_fast_forward_kinematics_library target_name) + cmake_parse_arguments(ARG "" "URDF_FILE;ROOT_LINK;TIP_LINK" "" ${ARGN}) + if (ARG_UNPARSED_ARGUMENTS) + ffk_failed_arg_parse() + endif () + + include(ExternalProject) + ExternalProject_Add( + LBFGSpp + PREFIX ${CMAKE_BINARY_DIR}/LBFGSpp + GIT_REPOSITORY https://github.com/yixuan/LBFGSpp.git + GIT_TAG v0.3.0 + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_BINARY_DIR} -DBUILD_TESTING=OFF + ) + ExternalProject_Get_Property(LBFGSpp source_dir) + set(LBFGSppIncludeDir ${source_dir}/include) + + generate_fast_forward_kinematics_library_common(fast_forward_kinematics_library + URDF_FILE "${ARG_URDF_FILE}" + ROOT_LINK "${ARG_ROOT_LINK}" + TIP_LINK "${ARG_TIP_LINK}" + EXT "cpp") + + add_dependencies(fast_forward_kinematics_library LBFGSpp) + target_compile_definitions(fast_forward_kinematics_library PUBLIC FAST_FK_USE_IK) + target_include_directories(fast_forward_kinematics_library PUBLIC ${LBFGSppIncludeDir}) + find_package(Eigen3 3.3 NO_MODULE) + if(NOT Eigen3_FOUND) + include(FetchContent) + set(PROJECT_BUILD_TESTING ${BUILD_TESTING}) + set(BUILD_TESTING OFF) + FetchContent_Declare( + eigen + GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git + GIT_TAG 3.4.0 + ) + FetchContent_MakeAvailable(eigen) + set(BUILD_TESTING ${PROJECT_BUILD_TESTING}) + + endif () + target_link_libraries(fast_forward_kinematics_library PUBLIC Eigen3::Eigen) + + target_compile_options(fast_forward_kinematics_library PUBLIC -Ofast -march=native) + +endfunction() + +function(generate_fast_forward_kinematics_library_cuda target_name) + cmake_parse_arguments(ARG "" "URDF_FILE;ROOT_LINK;TIP_LINK" "" ${ARGN}) + if (ARG_UNPARSED_ARGUMENTS) + ffk_failed_arg_parse() + endif () + enable_language(CUDA) + generate_fast_forward_kinematics_library_common(fast_forward_kinematics_library + URDF_FILE "${ARG_URDF_FILE}" + ROOT_LINK "${ARG_ROOT_LINK}" + TIP_LINK "${ARG_TIP_LINK}" + EXT "cu") +endfunction() \ No newline at end of file diff --git a/include/fast_inverse_kinematics.hpp b/include/fast_inverse_kinematics.hpp deleted file mode 100644 index 3f8584d..0000000 --- a/include/fast_inverse_kinematics.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#pragma once - -#include "memory" - -#include -#include - -#include "fast_kinematics_joint_data_length.hpp" -#include "kinematics_interface.hpp" - -#ifdef FAST_FK_USE_IK -#include "LBFGS.h" - -namespace fast_fk::internal { - - class InverseKinematics { - public: - Eigen::Matrix target_rot_; - Eigen::Vector target_pose_; - std::array, FAST_FK_NUMBER_OF_JOINTS> joint_data = {0}; - std::unique_ptr> solver; - LBFGSpp::LBFGSParam param; - - InverseKinematics(const Eigen::Matrix &target_rot, - const Eigen::Vector &target_pose); - - float operator()(const Eigen::VectorX &q, Eigen::VectorX &grad); - - fk_interface::IKSolverStats inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess); - - }; - } -#else -namespace fast_fk::internal { - class InverseKinematics { - public: - InverseKinematics(const Eigen::Matrix &target_rot, - const Eigen::Vector &target_pose); - - float operator()(const Eigen::VectorX &q, Eigen::VectorX &grad); - - fk_interface::IKSolverStats - inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess); - }; -} -#endif diff --git a/include/fast_inverse_kinematics/fast_inverse_kinematics.hpp b/include/fast_inverse_kinematics/fast_inverse_kinematics.hpp new file mode 100644 index 0000000..efd7c55 --- /dev/null +++ b/include/fast_inverse_kinematics/fast_inverse_kinematics.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include "memory" + +#include +#include + +#ifdef FAST_FK_USE_IK + +#include "LBFGS.h" + +#include "kinematics_interface.hpp" + +namespace fast_fk::internal { + class InverseKinematics { + public: + Eigen::Matrix target_rot_; + Eigen::Vector target_pose_; + std::array, FAST_FK_NUMBER_OF_JOINTS> joint_data = {0}; + std::unique_ptr> solver; + LBFGSpp::LBFGSParam param; + + InverseKinematics(const Eigen::Matrix &target_rot, + const Eigen::Vector &target_pose) : target_rot_{target_rot}, + target_pose_{target_pose} { + param.epsilon = 1E-3; + param.epsilon_rel = 1E-3; + param.max_iterations = 30; + + solver = std::make_unique>(param); + } + + fk_interface::IKSolverStats + inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess) { + target_rot_ = transform.block<3, 3>(0, 0); + target_pose_(0) = transform(0, 3); + target_pose_(1) = transform(1, 3); + target_pose_(2) = transform(2, 3); + + float fx = 1E10; + int niter; + + try { + niter = solver->minimize(*this, q_guess, fx); + } catch (const std::runtime_error &e) { + return {fx, niter, solver->final_grad_norm(), false, e.what()}; + } + + return {fx, niter, solver->final_grad_norm(), true, ""}; + } + + float operator()(const Eigen::VectorX &q, Eigen::VectorX &grad); + + + }; +} +#else +namespace fast_fk::internal { + class InverseKinematics { + public: + InverseKinematics(const Eigen::Matrix &target_rot, + const Eigen::Vector &target_pose){} + + fk_interface::IKSolverStats + inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess) { + throw std::logic_error("Function not implemented."); + return {}; + } + + float operator()(const Eigen::VectorX &q, Eigen::VectorX &grad); + + }; +} +#endif diff --git a/include/fast_inverse_kinematics/fast_kinematics.hpp b/include/fast_inverse_kinematics/fast_kinematics.hpp new file mode 100644 index 0000000..a231372 --- /dev/null +++ b/include/fast_inverse_kinematics/fast_kinematics.hpp @@ -0,0 +1,113 @@ +#pragma once + +#include "memory" + +#include +#include +#include "fast_inverse_kinematics.hpp" + +namespace fast_fk { + namespace internal { + // input_data: sin(t) cos(t) px py pz R11, R12, R13... + void forward_kinematics_internal(float *input_data, size_t size); + } + + struct JointData { + + static constexpr size_t get_num_joints() { + return FAST_FK_NUMBER_OF_JOINTS; + } + + JointData() : target_pose{Eigen::Vector::Zero()}, + target_rot{Eigen::Matrix::Zero()}, + fun{std::make_unique(target_rot, target_pose)} { + + } + + void set_joint(size_t ind, float value) { + joint_data[ind][0] = std::sin(value); + joint_data[ind][1] = std::cos(value); + } + + void set_joints(const Eigen::Vector &values) { +#pragma unroll + for (auto ind = 0; ind < FAST_FK_NUMBER_OF_JOINTS; ++ind) { + joint_data[ind][0] = std::sin(values[ind]); + joint_data[ind][1] = std::cos(values[ind]); + } + } + + void set_joint(size_t ind, float sin_t, float cos_t) { + joint_data[ind][0] = sin_t; + joint_data[ind][1] = cos_t; + } + + void set_joints(const float *sin_values, const float *cos_values) { +#pragma unroll + for (auto ind = 0; ind < FAST_FK_NUMBER_OF_JOINTS; ++ind) { + joint_data[ind][0] = sin_values[ind]; + joint_data[ind][1] = cos_values[ind]; + } + } + + void set_joints(const Eigen::Vector &sin_values, + const Eigen::Vector &cos_values) { +#pragma unroll + for (auto ind = 0; ind < FAST_FK_NUMBER_OF_JOINTS; ++ind) { + joint_data[ind][0] = sin_values[ind]; + joint_data[ind][1] = cos_values[ind]; + } + } + + + [[nodiscard]] float get_joint(size_t ind) const { + return atan2f(joint_data[ind][0], joint_data[ind][1]); + } + + void get_joints(Eigen::Vector &values) const { +#pragma unroll + for (auto ind = 0; ind < FAST_FK_NUMBER_OF_JOINTS; ++ind) { + values[ind] = atan2f(joint_data[ind][0], joint_data[ind][1]); + } + } + + void get_frame(size_t index, Eigen::Matrix &transform) const { + transform(0, 3) = joint_data[index][2]; + transform(1, 3) = joint_data[index][3]; + transform(2, 3) = joint_data[index][4]; + + transform(0, 0) = joint_data[index][5]; + transform(0, 1) = joint_data[index][6]; + transform(0, 2) = joint_data[index][7]; + + transform(1, 0) = joint_data[index][8]; + transform(1, 1) = joint_data[index][9]; + transform(1, 2) = joint_data[index][10]; + + transform(2, 0) = joint_data[index][11]; + transform(2, 1) = joint_data[index][12]; + transform(2, 2) = joint_data[index][13]; + + transform(3, 0) = 0.0; + transform(3, 1) = 0.0; + transform(3, 2) = 0.0; + transform(3, 3) = 1.0; + } + + void forward_kinematics() { + internal::forward_kinematics_internal(joint_data.data()->data(), + joint_data.size() * 16); + } + + fk_interface::IKSolverStats + inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess) { + return fun->inverse_kinematics(transform, q_guess); + } + + Eigen::Matrix target_rot; + Eigen::Vector target_pose; + std::array, FAST_FK_NUMBER_OF_JOINTS> joint_data = {0}; + std::unique_ptr fun; + }; + +} diff --git a/include/kinematics_interface.hpp b/include/fast_inverse_kinematics/kinematics_interface.hpp similarity index 98% rename from include/kinematics_interface.hpp rename to include/fast_inverse_kinematics/kinematics_interface.hpp index 6d60915..340eae5 100644 --- a/include/kinematics_interface.hpp +++ b/include/fast_inverse_kinematics/kinematics_interface.hpp @@ -1,8 +1,6 @@ #pragma once #include -#include - #include namespace fk_interface { diff --git a/include/fast_kinematics.hpp b/include/fast_kinematics.hpp deleted file mode 100644 index cd56538..0000000 --- a/include/fast_kinematics.hpp +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include "memory" - -#include -#include - -#include "kinematics_interface.hpp" -#include "fast_kinematics_joint_data_length.hpp" -#include "fast_inverse_kinematics.hpp" - -namespace fast_fk { - struct JointData { - - static constexpr size_t get_num_joints() { - return FAST_FK_NUMBER_OF_JOINTS; - } - - JointData(); - - void set_joint(size_t ind, float value); - - void set_joints(const Eigen::Vector &values); - - void set_joint(size_t ind, float sin_t, float cos_t); - - void set_joints(const float *sin_values, const float *cos_values); - - void set_joints(const Eigen::Vector &sin_values, - const Eigen::Vector &cos_values); - - - [[nodiscard]] float get_joint(size_t ind) const; - - void get_joints(Eigen::Vector &values) const; - - - void get_frame(size_t index, Eigen::Matrix &transform) const; - - void forward_kinematics(); - - fk_interface::IKSolverStats - inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess); - - Eigen::Matrix target_rot; - Eigen::Vector target_pose; - std::array, FAST_FK_NUMBER_OF_JOINTS> joint_data = {0}; - std::unique_ptr fun; - }; - -} diff --git a/src/fast_inverse_kinematics.cpp b/src/fast_inverse_kinematics.cpp deleted file mode 100644 index 231594b..0000000 --- a/src/fast_inverse_kinematics.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "fast_inverse_kinematics.hpp" - -#ifdef FAST_FK_USE_IK - -namespace fast_fk::internal { - // input_data: sin(t) cos(t) px py pz R11, R12, R13... - void forward_kinematics_internal(float *input_data, size_t size); - - InverseKinematics::InverseKinematics(const Eigen::Matrix &target_rot, - const Eigen::Vector &target_pose) : target_rot_{target_rot}, - target_pose_{target_pose} { - param.epsilon = 1E-3; - param.epsilon_rel = 1E-3; - param.max_iterations = 30; - - solver = std::make_unique>(param); - } - - fk_interface::IKSolverStats - InverseKinematics::inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess) { - target_rot_ = transform.block<3, 3>(0, 0); - target_pose_(0) = transform(0, 3); - target_pose_(1) = transform(1, 3); - target_pose_(2) = transform(2, 3); - - float fx = 1E10; - int niter; - - try { - niter = solver->minimize(*this, q_guess, fx); - } catch (const std::runtime_error &e) { - return {fx, niter, solver->final_grad_norm(), false, e.what()}; - } - - return {fx, niter, solver->final_grad_norm(), true, ""}; - } -} - -#else - -namespace fast_fk::internal { - InverseKinematics::InverseKinematics(const Eigen::Matrix &target_rot, - const Eigen::Vector &target_pose){} - - fk_interface::IKSolverStats - InverseKinematics::inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess) { - throw std::logic_error("Function not implemented."); - return {}; - } - -}; -#endif \ No newline at end of file diff --git a/src/fast_kinematics.cpp b/src/fast_kinematics.cpp deleted file mode 100644 index 6e719bd..0000000 --- a/src/fast_kinematics.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#pragma once - -#include "memory" - -#include - -#include "fast_kinematics.hpp" -#include "fast_inverse_kinematics.hpp" - -namespace fast_fk { - namespace internal { - // input_data: sin(t) cos(t) px py pz R11, R12, R13... - void forward_kinematics_internal(float *input_data, size_t size); - } - - JointData::JointData() : target_pose{Eigen::Vector::Zero()}, - target_rot{Eigen::Matrix::Zero()}, - fun{std::make_unique(target_rot, target_pose)} { - - } - - - void JointData::set_joint(size_t ind, float value) { - joint_data[ind][0] = std::sin(value); - joint_data[ind][1] = std::cos(value); - } - - void JointData::set_joints(const Eigen::Vector &values) { -#pragma unroll - for (auto ind = 0; ind < FAST_FK_NUMBER_OF_JOINTS; ++ind) { - joint_data[ind][0] = std::sin(values[ind]); - joint_data[ind][1] = std::cos(values[ind]); - } - } - - void JointData::set_joint(size_t ind, float sin_t, float cos_t) { - joint_data[ind][0] = sin_t; - joint_data[ind][1] = cos_t; - } - - void JointData::set_joints(const float *sin_values, const float *cos_values) { -#pragma unroll - for (auto ind = 0; ind < FAST_FK_NUMBER_OF_JOINTS; ++ind) { - joint_data[ind][0] = sin_values[ind]; - joint_data[ind][1] = cos_values[ind]; - } - } - - void JointData::set_joints(const Eigen::Vector &sin_values, - const Eigen::Vector &cos_values) { -#pragma unroll - for (auto ind = 0; ind < FAST_FK_NUMBER_OF_JOINTS; ++ind) { - joint_data[ind][0] = sin_values[ind]; - joint_data[ind][1] = cos_values[ind]; - } - } - - - [[nodiscard]] float JointData::get_joint(size_t ind) const { - return atan2f(joint_data[ind][0], joint_data[ind][1]); - } - - void JointData::get_joints(Eigen::Vector &values) const { -#pragma unroll - for (auto ind = 0; ind < FAST_FK_NUMBER_OF_JOINTS; ++ind) { - values[ind] = atan2f(joint_data[ind][0], joint_data[ind][1]); - } - } - - - void JointData::get_frame(size_t index, Eigen::Matrix &transform) const { - transform(0, 3) = joint_data[index][2]; - transform(1, 3) = joint_data[index][3]; - transform(2, 3) = joint_data[index][4]; - - transform(0, 0) = joint_data[index][5]; - transform(0, 1) = joint_data[index][6]; - transform(0, 2) = joint_data[index][7]; - - transform(1, 0) = joint_data[index][8]; - transform(1, 1) = joint_data[index][9]; - transform(1, 2) = joint_data[index][10]; - - transform(2, 0) = joint_data[index][11]; - transform(2, 1) = joint_data[index][12]; - transform(2, 2) = joint_data[index][13]; - - transform(3, 0) = 0.0; - transform(3, 1) = 0.0; - transform(3, 2) = 0.0; - transform(3, 3) = 1.0; - } - - void JointData::forward_kinematics() { - internal::forward_kinematics_internal(joint_data.data()->data(), - joint_data.size() * internal::joint_data_length); - } - - fk_interface::IKSolverStats - JointData::inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess) { - return fun->inverse_kinematics(transform, q_guess); - } -} \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index de72df0..92d8446 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,8 +1,3 @@ -cmake_minimum_required(VERSION 3.22) -project(fast_forward_kinematics) -set(CMAKE_CUDA_ARCHITECTURES "native") -enable_language(CUDA) - set(CMAKE_CXX_STANDARD 20) if (NOT DEFINED CMAKE_BUILD_TYPE OR "${CMAKE_BUILD_TYPE}" STREQUAL "") set(CMAKE_BUILD_TYPE Release) @@ -24,7 +19,9 @@ set(FK_IMPL "FFK") if (${FK_IMPL} STREQUAL "FFK") target_compile_definitions(forward_kinematics_test PUBLIC USE_FAST_KINEMATICS) target_compile_definitions(inverse_kinematics_test PUBLIC USE_FAST_KINEMATICS) - generate_fast_forward_kinematics_library(${URDF_FILE} ${ROOT} ${TIP}) + target_include_directories(forward_kinematics_test PUBLIC include) + target_include_directories(inverse_kinematics_test PUBLIC include) + generate_fast_forward_kinematics_library(fast_forward_kinematics_library URDF_FILE ${URDF_FILE} ROOT_LINK ${ROOT} TIP_LINK ${TIP}) if (${CMAKE_BUILD_TYPE} STREQUAL "Release") set_target_properties(fast_forward_kinematics_library PROPERTIES CMAKE_BUILD_TYPE Release) target_compile_options(fast_forward_kinematics_library PUBLIC -Ofast -march=native) @@ -34,9 +31,10 @@ if (${FK_IMPL} STREQUAL "FFK") target_link_libraries(inverse_kinematics_test PRIVATE fast_forward_kinematics_library) target_compile_definitions(inverse_kinematics_test PUBLIC MULTIPLIER=32) elseif (${FK_IMPL} STREQUAL "FFK_CUDA") + set(CMAKE_CUDA_ARCHITECTURES "native") target_compile_definitions(forward_kinematics_test PUBLIC USE_FAST_KINEMATICS) target_compile_definitions(inverse_kinematics_test PUBLIC USE_FAST_KINEMATICS) - generate_fast_forward_kinematics_library_cuda(${URDF_FILE} ${ROOT} ${TIP}) + generate_fast_forward_kinematics_library_cuda(fast_forward_kinematics_library URDF_FILE ${URDF_FILE} ROOT_LINK ${ROOT} TIP_LINK ${TIP}) if (${CMAKE_BUILD_TYPE} STREQUAL "Release") set_target_properties(fast_forward_kinematics_library PROPERTIES CMAKE_BUILD_TYPE Release) target_compile_options(fast_forward_kinematics_library PUBLIC -O3) @@ -85,6 +83,7 @@ else () find_package(Eigen3 3.3 NO_MODULE) add_library(kdl_implementation SHARED kdl/kdl_implementation.cpp) target_include_directories(kdl_implementation PUBLIC ../include) + target_include_directories(kdl_implementation PUBLIC include) target_compile_definitions(kdl_implementation PUBLIC NUMBER_OF_JOINTS=6) target_compile_definitions(kdl_implementation PUBLIC URDF_FILE=${URDF_FILE}) target_compile_definitions(kdl_implementation PUBLIC ROOT=${ROOT}) diff --git a/test/fetch_content_test/CMakeLists.txt b/test/fetch_content_test/CMakeLists.txt new file mode 100644 index 0000000..7fea2ee --- /dev/null +++ b/test/fetch_content_test/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.22) +project(fast_forward_kinematics_fetch_content_test) + +set(CMAKE_CXX_STANDARD 20) +if (NOT DEFINED CMAKE_BUILD_TYPE OR "${CMAKE_BUILD_TYPE}" STREQUAL "") + set(CMAKE_BUILD_TYPE Release) +endif () + +# do not build fast_forward_kinematics test for this test project +set(BUILD_TESTING OFF) + +include(FetchContent) +FetchContent_Declare( + fast_forward_kinematics + GIT_REPOSITORY https://github.com/pac48/fast_robot_kinematics.git + GIT_TAG fix-cmake +) +FetchContent_MakeAvailable(fast_forward_kinematics) + + +#TODO use ZIP_LISTS to test all robots +set(URDF_FILE ${CMAKE_SOURCE_DIR}/urdf/robot.urdf) +set(ROOT base_link) +set(TIP grasp_link) +generate_fast_forward_kinematics_library(fast_forward_kinematics_library_{robots} URDF_FILE ${URDF_FILE} ROOT_LINK ${ROOT} TIP_LINK ${TIP}) diff --git a/test/fetch_content_test/urdf/robot.urdf b/test/fetch_content_test/urdf/robot.urdf new file mode 100644 index 0000000..91a1b69 --- /dev/null +++ b/test/fetch_content_test/urdf/robot.urdf @@ -0,0 +1,1356 @@ + + + + + + + + + + + + + + + + + + ur_robot_driver/URPositionHardwareInterface + 192.10.0.11 + /home/demo/ws_studio/install/ur_robot_driver/share/ur_robot_driver/resources/ros_control.urscript + /home/demo/ws_studio/install/ur_robot_driver/share/ur_robot_driver/resources/rtde_output_recipe.txt + /home/demo/ws_studio/install/ur_robot_driver/share/ur_robot_driver/resources/rtde_input_recipe.txt + True + 50001 + 50002 + "" + 0 + 2000 + 0.03 + False + calib_5972249097379105770 + 24 + 0 + 115200 + 1 + 1.5 + 3.5 + /tmp/ttyUR + 54321 + + + + {-2*pi} + {2*pi} + + + -3.15 + 3.15 + + + + 0.0 + + + + + + + {-2*pi} + {2*pi} + + + -3.15 + 3.15 + + + + -1.57 + + + + + + + {-pi} + {pi} + + + -3.15 + 3.15 + + + + 0.0 + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + -1.57 + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + 0.0 + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + 0.0 + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6 + + 1.13446 + + 640 + 480 + R8G8B8 + + + 0.01 + 300 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + d415_camera/image_raw:=wrist_mounted_camera/color/image_raw + d415_camera/camera_info:=wrist_mounted_camera/color/camera_info + + d415_camera + wrist_mounted_camera_depth_optical_frame + 0.0 + + + + 6 + + 1.3997540600994522 + + B8G8R8 + 640 + 480 + + + 0.05 + 8.0 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + d415_depth/depth/image_raw:=wrist_mounted_camera/depth/image_rect_raw + d415_depth/depth/camera_info:=wrist_mounted_camera/depth/camera_info + d415_depth/points:=wrist_mounted_camera/depth/color/points + d415_depth/camera_info:=wrist_mounted_camera/unused1 + d415_depth/image_raw:=wrist_mounted_camera/unused2 + + d415_depth + wrist_mounted_camera_depth_optical_frame + 0.0 + 0.15 + 4.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1000000.0 + 100.0 + 30.0 + 30.0 + 1.0 + 0.001 + Gazebo/Grey + + + + + + + + + + + + 1000000.0 + 100.0 + 30.0 + 30.0 + 1.0 + 0.001 + Gazebo/Grey + + + + robotiq_85_left_knuckle_joint + robotiq_85_right_knuckle_joint + 1.0 + 0 + 0.0 + + 10.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + robotiq_85_left_knuckle_joint + robotiq_85_left_inner_knuckle_joint + 1.0 + 0 + 0.0 + + 10.0 + + + + + + + + + + + + + robotiq_85_left_knuckle_joint + robotiq_85_right_inner_knuckle_joint + 1.0 + 0 + 0.0 + + 10.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + robotiq_85_left_knuckle_joint + robotiq_85_left_finger_tip_joint + 1.0 + 0 + 0.0 + + 10.0 + + + + + + + + + + + + + robotiq_85_left_knuckle_joint + robotiq_85_right_finger_tip_joint + 1.0 + 0 + 0.0 + + 10.0 + + + + + + + + + + + + + + + + + + + + + 1000000.0 + 100.0 + 30.0 + 30.0 + 1.0 + 0.001 + Gazebo/Grey + + + + + + + + + + + + + + + + + + + + 1000000.0 + 100.0 + 30.0 + 30.0 + 1.0 + 0.001 + Gazebo/Grey + + + transmission_interface/SimpleTransmission + + hardware_interface/PositionJointInterface + + + 1 + hardware_interface/PositionJointInterface + + + + + + robotiq_85_left_knuckle_joint + robotiq_85_right_knuckle_joint + + + + robotiq_85_left_knuckle_joint + robotiq_85_left_inner_knuckle_joint + + + + robotiq_85_left_knuckle_joint + robotiq_85_right_inner_knuckle_joint + + + + robotiq_85_left_knuckle_joint + robotiq_85_left_finger_tip_joint + -1.0 + 0.0 + + + robotiq_85_left_knuckle_joint + robotiq_85_right_finger_tip_joint + -1.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 6 + + 1.13446 + + 640 + 480 + R8G8B8 + + + 0.01 + 300 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + d435_camera/image_raw:=scene_camera/color/image_raw + d435_camera/camera_info:=scene_camera/color/camera_info + + d435_camera + scene_camera_depth_optical_frame + 0.0 + + + + 6 + + 1.3997540600994522 + + B8G8R8 + 640 + 480 + + + 0.05 + 8.0 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + d435_depth/depth/image_raw:=scene_camera/depth/image_rect_raw + d435_depth/depth/camera_info:=scene_camera/depth/camera_info + d435_depth/points:=scene_camera/depth/color/points + d435_depth/camera_info:=scene_camera/unused1 + d435_depth/image_raw:=scene_camera/unused2 + + d435_depth + scene_camera_depth_optical_frame + 0.0 + 0.15 + 4.0 + + + + diff --git a/test/forward_kinematics_test.cpp b/test/forward_kinematics_test.cpp index 161dde7..bb228b5 100644 --- a/test/forward_kinematics_test.cpp +++ b/test/forward_kinematics_test.cpp @@ -3,7 +3,7 @@ #ifdef USE_FAST_KINEMATICS -#include "fast_kinematics.hpp" +#include "fast_inverse_kinematics/fast_kinematics.hpp" using KI = fast_fk::JointData; #else diff --git a/include/fast_kinematics_joint_data_length.hpp b/test/include/fast_kinematics_joint_data_length.hpp similarity index 100% rename from include/fast_kinematics_joint_data_length.hpp rename to test/include/fast_kinematics_joint_data_length.hpp diff --git a/include/kdl_kinematics.hpp b/test/include/kdl_kinematics.hpp similarity index 100% rename from include/kdl_kinematics.hpp rename to test/include/kdl_kinematics.hpp diff --git a/test/inverse_kinematics_test.cpp b/test/inverse_kinematics_test.cpp index 4fc67a8..e8b6da2 100644 --- a/test/inverse_kinematics_test.cpp +++ b/test/inverse_kinematics_test.cpp @@ -2,7 +2,7 @@ #include "iostream" #ifdef USE_FAST_KINEMATICS -#include "fast_kinematics.hpp" +#include "fast_inverse_kinematics/fast_kinematics.hpp" using IK = fast_fk::JointData; #else