diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..80f4f71 --- /dev/null +++ b/.clang-format @@ -0,0 +1,68 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializers: BeforeComma +BinPackParameters: true +ColumnLimit: 90 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: Never +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false +ReflowComments: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true', + AfterControlStatement: 'true', + AfterEnum : 'true', + AfterFunction : 'true', + AfterNamespace : 'true', + AfterStruct : 'true', + AfterUnion : 'true', + BeforeCatch : 'true', + BeforeElse : 'true', + IndentBraces : 'false', + SplitEmptyFunction: 'false' +} +... diff --git a/.github/workflows/cmake-single-platform.yml b/.github/workflows/cmake-single-platform.yml index 889f4f7..3f2ed9e 100644 --- a/.github/workflows/cmake-single-platform.yml +++ b/.github/workflows/cmake-single-platform.yml @@ -4,9 +4,9 @@ name: CMake on a single platform on: push: - branches: [ "main" ] + branches: ["main"] pull_request: - branches: [ "main" ] + branches: ["main"] env: # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) @@ -20,35 +20,35 @@ jobs: runs-on: ubuntu-latest 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}} -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}} --target install - - - name: Test - working-directory: ${{github.workspace}}/build - # Execute tests defined by the CMake configuration. - # 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}} + - 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}} -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}} --target install + + - name: Test + working-directory: ${{github.workspace}}/build + # Execute tests defined by the CMake configuration. + # 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/.github/workflows/format.yml b/.github/workflows/format.yml new file mode 100644 index 0000000..017de18 --- /dev/null +++ b/.github/workflows/format.yml @@ -0,0 +1,15 @@ +name: format + +on: + # Run action on certain pull request events + pull_request: + types: [opened, synchronize, reopened, ready_for_review] + +jobs: + pre-commit: + name: pre-commit + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + - uses: pre-commit/action@v3.0.1 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..2742c2d --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,67 @@ +repos: + + # Runs pre-commit hooks and other file format checks. + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-builtin-literals + - id: check-case-conflict + - id: check-docstring-first + - id: check-executables-have-shebangs + - id: check-json + - id: check-merge-conflict + - id: check-symlinks + - id: check-toml + - id: check-vcs-permalinks + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: fix-byte-order-marker + - id: forbid-new-submodules + - id: mixed-line-ending + - id: name-tests-test + - id: requirements-txt-fixer + - id: sort-simple-yaml + - id: trailing-whitespace + + # Autoformats Python code. + - repo: https://github.com/psf/black.git + rev: 24.10.0 + hooks: + - id: black + + # Finds spelling issues in code. + - repo: https://github.com/codespell-project/codespell + rev: v2.3.0 + hooks: + - id: codespell + + # Finds issues in YAML files. + - repo: https://github.com/adrienverge/yamllint + rev: v1.35.1 + hooks: + - id: yamllint + args: + [ + "--no-warnings", + "--config-data", + "{extends: default, rules: {line-length: disable, braces: {max-spaces-inside: 1}}}", + ] + types: [text] + files: \.(yml|yaml)$ + + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.10 + hooks: + - id: cmake-format + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 + hooks: + - id: clang-format + files: \.(c|cc|cxx|cpp|h|hpp|hxx|)$ + args: ["-fallback-style=none"] diff --git a/CMakeLists.txt b/CMakeLists.txt index d123030..f455711 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,43 +15,45 @@ include(CTest) 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 () - +target_include_directories( + fast_forward_kinematics_header + INTERFACE $ + $) +if(BUILD_TESTING) + add_subdirectory(test) +endif() include(CMakePackageConfigHelpers) -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) +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) -write_basic_package_version_file(fast_forward_kinematics-config-version.cmake - VERSION ${FFK_VERSION} COMPATIBILITY AnyNewerVersion) +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" +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") + 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") +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) +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 -) +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 + EXPORT fast_forward_kinematicsTargets + FILE fast_forward_kinematics-targets.cmake + DESTINATION lib/cmake/fast_forward_kinematics) diff --git a/code_generation/get_num_joints.py b/code_generation/get_num_joints.py index 001cace..3d70f4d 100644 --- a/code_generation/get_num_joints.py +++ b/code_generation/get_num_joints.py @@ -4,9 +4,9 @@ def run(): parser = argparse.ArgumentParser() - parser.add_argument('urdf_file') - parser.add_argument('root_link_name') - parser.add_argument('tip_link_name') + parser.add_argument("urdf_file") + parser.add_argument("root_link_name") + parser.add_argument("tip_link_name") args = parser.parse_args() root_link_name = args.root_link_name @@ -18,7 +18,7 @@ def run(): while tip_link_name != root_link_name: tip_joint_name, tip_link_name = robot.parent_map[tip_link_name] joint = robot.joint_map[tip_joint_name] - if not joint.type == 'fixed': + if not joint.type == "fixed": joint_names.append(tip_joint_name) print(f"FAST_FK_NUMBER_OF_JOINTS={len(joint_names)}", end="") diff --git a/code_generation/robot_config.cpp.template b/code_generation/robot_config.cpp.template index 408c790..b043a86 100644 --- a/code_generation/robot_config.cpp.template +++ b/code_generation/robot_config.cpp.template @@ -362,4 +362,4 @@ namespace fast_fk::internal { } } -#endif \ No newline at end of file +#endif diff --git a/code_generation/robot_config.cu.template b/code_generation/robot_config.cu.template index 7e2f4c5..ad81dae 100644 --- a/code_generation/robot_config.cu.template +++ b/code_generation/robot_config.cu.template @@ -246,4 +246,4 @@ namespace fast_fk::internal { if (cudaerr != cudaSuccess) std::printf("kernel launch failed with error \"%s\".\n", cudaGetErrorString(cudaerr)); } -} \ No newline at end of file +} diff --git a/code_generation/robot_gen.py b/code_generation/robot_gen.py index 0200928..d8c7821 100644 --- a/code_generation/robot_gen.py +++ b/code_generation/robot_gen.py @@ -7,11 +7,11 @@ def run(): parser = argparse.ArgumentParser() - parser.add_argument('urdf_file') - parser.add_argument('fk_template') - parser.add_argument('fk_output_file') - parser.add_argument('root_link_name') - parser.add_argument('tip_link_name') + parser.add_argument("urdf_file") + parser.add_argument("fk_template") + parser.add_argument("fk_output_file") + parser.add_argument("root_link_name") + parser.add_argument("tip_link_name") args = parser.parse_args() root_link_name = args.root_link_name @@ -24,17 +24,29 @@ def get_transform(joint): xyz = joint.origin.xyz T = np.eye(4) - yaw = np.array([[np.cos(rpy[2]), np.sin(rpy[2]), 0], - [-np.sin(rpy[2]), np.cos(rpy[2]), 0], - [0, 0, 1]]) - - pitch = np.array([[np.cos(rpy[1]), 0, np.sin(rpy[1])], - [0, 1, 0], - [-np.sin(rpy[1]), 0, np.cos(rpy[1])]]) - - roll = np.array([[1, 0, 0], - [0, np.cos(rpy[0]), np.sin(rpy[0])], - [0, -np.sin(rpy[0]), np.cos(rpy[0])]]) + yaw = np.array( + [ + [np.cos(rpy[2]), np.sin(rpy[2]), 0], + [-np.sin(rpy[2]), np.cos(rpy[2]), 0], + [0, 0, 1], + ] + ) + + pitch = np.array( + [ + [np.cos(rpy[1]), 0, np.sin(rpy[1])], + [0, 1, 0], + [-np.sin(rpy[1]), 0, np.cos(rpy[1])], + ] + ) + + roll = np.array( + [ + [1, 0, 0], + [0, np.cos(rpy[0]), np.sin(rpy[0])], + [0, -np.sin(rpy[0]), np.cos(rpy[0])], + ] + ) T[:3, :3] = yaw @ pitch @ roll T[:3, 3] = xyz @@ -53,40 +65,43 @@ def get_transform(joint): T_fixed = np.eye(4) for joint_name in joint_names: joint = robot.joint_map[joint_name] - if joint.type == 'fixed': + if joint.type == "fixed": T_fixed = T_fixed * get_transform(joint) - elif joint.type == 'revolute': + elif joint.type == "revolute": # TODO: need to add joint limits? T = get_transform(joint) @ T_fixed T_fixed = np.eye(4) rotations.append(T[:3, :3]) offsets.append(T[:3, 3]) - types.append('revolute') - elif joint.type == 'continuous': + types.append("revolute") + elif joint.type == "continuous": T = get_transform(joint) @ T_fixed T_fixed = np.eye(4) rotations.append(T[:3, :3]) offsets.append(T[:3, 3]) - types.append('revolute') - elif joint.type == 'prismatic': + types.append("revolute") + elif joint.type == "prismatic": # TODO: need to add joint limits? T = get_transform(joint) @ T_fixed T_fixed = np.eye(4) rotations.append(T[:3, :3]) offsets.append(T[:3, 3]) - types.append('prismatic') + types.append("prismatic") else: raise Exception(f"joint type {joint.type} in URDF not supported") # truncate near zero values - offsets = [val * (np.abs(val) > 1E-5) for val in offsets] - rotations = [val * (np.abs(val) > 1E-5) for val in rotations] + offsets = [val * (np.abs(val) > 1e-5) for val in offsets] + rotations = [val * (np.abs(val) > 1e-5) for val in rotations] - with open(args.fk_template, 'r') as f: + with open(args.fk_template, "r") as f: j2_template = Template(f.read()) - code = j2_template.render({'rotations': rotations, 'offsets': offsets, 'types': types}, trim_blocks=True) + code = j2_template.render( + {"rotations": rotations, "offsets": offsets, "types": types}, + trim_blocks=True, + ) - with open(args.fk_output_file, 'w') as f: + with open(args.fk_output_file, "w") as f: f.write(code) print(f"FAST_FK_NUMBER_OF_JOINTS={len(types)}", end="") diff --git a/fast_forward_kinematics-config.cmake.in b/fast_forward_kinematics-config.cmake.in index b85508a..4bedc3f 100644 --- a/fast_forward_kinematics-config.cmake.in +++ b/fast_forward_kinematics-config.cmake.in @@ -8,4 +8,4 @@ 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 +include("${CMAKE_CURRENT_LIST_DIR}/cmake/fast_forward_kinematics.cmake") diff --git a/fast_forward_kinematics.cmake b/fast_forward_kinematics.cmake index 1c48ef9..35db164 100644 --- a/fast_forward_kinematics.cmake +++ b/fast_forward_kinematics.cmake @@ -1,114 +1,139 @@ include(CMakeParseArguments) +# generate_fast_forward_kinematics_library_common common cmake function used by +# the library. This should not be directly called by users. 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 + 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." ) - - 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 + 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}") + 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)") + 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) + 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 + 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() diff --git a/include/fast_inverse_kinematics/fast_inverse_kinematics.hpp b/include/fast_inverse_kinematics/fast_inverse_kinematics.hpp index efd7c55..39e19b8 100644 --- a/include/fast_inverse_kinematics/fast_inverse_kinematics.hpp +++ b/include/fast_inverse_kinematics/fast_inverse_kinematics.hpp @@ -1,74 +1,81 @@ #pragma once -#include "memory" - #include #include +#include "memory" + #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); - - - }; -} +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); +}; +} // namespace fast_fk::internal #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); - - }; -} +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); +}; +} // namespace fast_fk::internal #endif diff --git a/include/fast_inverse_kinematics/fast_kinematics.hpp b/include/fast_inverse_kinematics/fast_kinematics.hpp index a231372..1d90d09 100644 --- a/include/fast_inverse_kinematics/fast_kinematics.hpp +++ b/include/fast_inverse_kinematics/fast_kinematics.hpp @@ -1,113 +1,129 @@ #pragma once -#include "memory" - #include #include + #include "fast_inverse_kinematics.hpp" +#include "memory" -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); +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); +} // namespace internal + +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]); } + } - 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 sin_t, float cos_t) + { + joint_data[ind][0] = sin_t; + joint_data[ind][1] = cos_t; + } - 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) { + 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]; - } - } + 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]); - } + [[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 { + 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; - }; - -} + 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; +}; + +} // namespace fast_fk diff --git a/include/fast_inverse_kinematics/kinematics_interface.hpp b/include/fast_inverse_kinematics/kinematics_interface.hpp index 340eae5..9151f46 100644 --- a/include/fast_inverse_kinematics/kinematics_interface.hpp +++ b/include/fast_inverse_kinematics/kinematics_interface.hpp @@ -1,53 +1,64 @@ #pragma once -#include #include +#include + +namespace fk_interface +{ + +struct IKSolverStats +{ + float fx = 0; + int niter = 0; + float grad_norm = 0; + bool success = false; + std::string what; +}; + +template +concept KinematicInterfaceConstraint = + requires(T obj, size_t ind, float value, + const Eigen::Vector& values, + Eigen::Vector& values_non_const, + Eigen::VectorX& q_guess) +{ + { T::get_num_joints() }; + { obj.set_joint(ind, value) }; + { obj.set_joints(values) }; + { obj.set_joint(ind, value, value) }; + { obj.set_joints(values, values) }; + { obj.get_joint(ind) }; + { obj.get_joints(values_non_const) }; +}; + +template +concept ForwardKinematicInterfaceConstraint = + requires(T obj, size_t ind, Eigen::Matrix& transform) +{ + { obj.get_frame(ind, transform) }; + { obj.forward_kinematics() }; +}; + +template +concept InverseInterfaceConstraint = requires(T obj, Eigen::VectorX& q_guess, + Eigen::Matrix& transform) +{ + { + obj.inverse_kinematics(transform, q_guess) + } -> std::same_as; +}; + +template +requires KinematicInterfaceConstraint && ForwardKinematicInterfaceConstraint +struct ForwardKinematicsInterface : KI +{ +}; + +template +requires KinematicInterfaceConstraint && ForwardKinematicInterfaceConstraint && + InverseInterfaceConstraint +struct InverseKinematicsInterface : IK +{ +}; -namespace fk_interface { - - struct IKSolverStats { - float fx = 0; - int niter = 0; - float grad_norm = 0; - bool success = false; - std::string what; - }; - - template - concept KinematicInterfaceConstraint = requires(T obj, size_t ind, float value, - const Eigen::Vector &values, - Eigen::Vector &values_non_const, - Eigen::VectorX &q_guess) { - { T::get_num_joints() }; - { obj.set_joint(ind, value) }; - { obj.set_joints(values) }; - { obj.set_joint(ind, value, value) }; - { obj.set_joints(values, values) }; - { obj.get_joint(ind) }; - { obj.get_joints(values_non_const) }; - }; - - - template - concept ForwardKinematicInterfaceConstraint = requires(T obj, size_t ind, Eigen::Matrix &transform) { - { obj.get_frame(ind, transform) }; - { obj.forward_kinematics() }; - }; - - template - concept InverseInterfaceConstraint = requires(T obj, Eigen::VectorX &q_guess, - Eigen::Matrix &transform) { - { obj.inverse_kinematics(transform, q_guess) } -> std::same_as; - }; - - template requires KinematicInterfaceConstraint && ForwardKinematicInterfaceConstraint - struct ForwardKinematicsInterface : KI { - }; - - - template requires KinematicInterfaceConstraint && ForwardKinematicInterfaceConstraint && - InverseInterfaceConstraint - struct InverseKinematicsInterface : IK { - }; - -} +} // namespace fk_interface diff --git a/perf_stat b/perf_stat index 3420acc..7c680d1 100644 --- a/perf_stat +++ b/perf_stat @@ -43,4 +43,3 @@ Average: 33.8014 nanoseconds 0.04% forward_kinemat [unknown] [k] 0xffffffffa30b8238 0.03% forward_kinemat [unknown] [k] 0xffffffffa37ad79c 0.03% forward_kinemat [unknown] [k] 0xffffffffa347ab9e - diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 92d8446..e8ae22c 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,7 +1,7 @@ set(CMAKE_CXX_STANDARD 20) -if (NOT DEFINED CMAKE_BUILD_TYPE OR "${CMAKE_BUILD_TYPE}" STREQUAL "") - set(CMAKE_BUILD_TYPE Release) -endif () +if(NOT DEFINED CMAKE_BUILD_TYPE OR "${CMAKE_BUILD_TYPE}" STREQUAL "") + set(CMAKE_BUILD_TYPE Release) +endif() add_executable(forward_kinematics_test forward_kinematics_test.cpp) add_executable(inverse_kinematics_test inverse_kinematics_test.cpp) @@ -10,98 +10,127 @@ set(URDF_FILE ${CMAKE_SOURCE_DIR}/test/urdf/robot.urdf) set(ROOT base_link) set(TIP grasp_link) - # generate FK/IK implementation set(FK_IMPL "FFK") -#set(FK_IMPL "FFK_CUDA") -#set(FK_IMPL "KDL") - -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) - 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) - endif () - target_link_libraries(forward_kinematics_test PUBLIC fast_forward_kinematics_library) - target_compile_definitions(forward_kinematics_test PUBLIC MULTIPLIER=64) - 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(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) - endif () - target_link_libraries(forward_kinematics_test PUBLIC fast_forward_kinematics_library) - target_compile_definitions(forward_kinematics_test PUBLIC MULTIPLIER=64) - target_link_libraries(inverse_kinematics_test PRIVATE fast_forward_kinematics_library) - target_compile_definitions(inverse_kinematics_test PUBLIC MULTIPLIER=32) -else () - set(CXX_FLAGS "-Ofast") +# set(FK_IMPL "FFK_CUDA") set(FK_IMPL "KDL") - set(LIB_KDL_PARSER ${CMAKE_CURRENT_BINARY_DIR}/kdl_parser/src/kdl-parser-build/libkdl_parser.so) - include(ExternalProject) +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) + 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) + endif() + target_link_libraries(forward_kinematics_test + PUBLIC fast_forward_kinematics_library) + target_compile_definitions(forward_kinematics_test PUBLIC MULTIPLIER=64) + 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( + 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) + endif() + target_link_libraries(forward_kinematics_test + PUBLIC fast_forward_kinematics_library) + target_compile_definitions(forward_kinematics_test PUBLIC MULTIPLIER=64) + target_link_libraries(inverse_kinematics_test + PRIVATE fast_forward_kinematics_library) + target_compile_definitions(inverse_kinematics_test PUBLIC MULTIPLIER=32) +else() + set(CXX_FLAGS "-Ofast") - ExternalProject_Add( - kdl-parser - PREFIX ${CMAKE_CURRENT_BINARY_DIR}/kdl_parser - SOURCE_SUBDIR kdl_parser - GIT_REPOSITORY https://github.com/ros/kdl_parser.git - GIT_TAG 2.12.0 - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_BINARY_DIR} -DCMAKE_CXX_FLAGS=${CXX_FLAGS} - BUILD_BYPRODUCTS ${LIB_KDL_PARSER} - ) - # use custom target to ensure kdl-parser is downloaded and installed before building main target - add_library(kdl_parser_lib SHARED IMPORTED) - add_custom_target(kdl_parser_lib-target DEPENDS kdl-parser) - add_dependencies(kdl_parser_lib kdl_parser_lib-target) - set_target_properties(kdl_parser_lib PROPERTIES IMPORTED_LOCATION ${LIB_KDL_PARSER}) + set(LIB_KDL_PARSER + ${CMAKE_CURRENT_BINARY_DIR}/kdl_parser/src/kdl-parser-build/libkdl_parser.so + ) + include(ExternalProject) - set(LIB_KDL ${CMAKE_CURRENT_BINARY_DIR}/kdl/src/kdl-content-build/src/liborocos-kdl.so) - ExternalProject_Add( - kdl-content - PREFIX ${CMAKE_CURRENT_BINARY_DIR}/kdl - SOURCE_SUBDIR orocos_kdl - GIT_REPOSITORY https://github.com/orocos/orocos_kinematics_dynamics.git - GIT_TAG master # v1.5.1 - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_BINARY_DIR} -DCMAKE_CXX_FLAGS=${CXX_FLAGS} - BUILD_BYPRODUCTS ${LIB_KDL} - ) - # use custom target to ensure kdl-content is downloaded and installed before building main target - add_library(kdl_lib SHARED IMPORTED) - add_custom_target(kdl_lib-target DEPENDS kdl-content) - add_dependencies(kdl_lib kdl_lib-target) - set_target_properties(kdl_lib PROPERTIES IMPORTED_LOCATION ${LIB_KDL}) + ExternalProject_Add( + kdl-parser + PREFIX ${CMAKE_CURRENT_BINARY_DIR}/kdl_parser + SOURCE_SUBDIR kdl_parser + GIT_REPOSITORY https://github.com/ros/kdl_parser.git + GIT_TAG 2.12.0 + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_BINARY_DIR} + -DCMAKE_CXX_FLAGS=${CXX_FLAGS} + BUILD_BYPRODUCTS ${LIB_KDL_PARSER}) + # use custom target to ensure kdl-parser is downloaded and installed before + # building main target + add_library(kdl_parser_lib SHARED IMPORTED) + add_custom_target(kdl_parser_lib-target DEPENDS kdl-parser) + add_dependencies(kdl_parser_lib kdl_parser_lib-target) + set_target_properties(kdl_parser_lib PROPERTIES IMPORTED_LOCATION + ${LIB_KDL_PARSER}) - 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}) - target_compile_definitions(kdl_implementation PUBLIC TIP=${TIP}) - if (${CMAKE_BUILD_TYPE} STREQUAL "Release") - set_target_properties(kdl_implementation PROPERTIES CMAKE_BUILD_TYPE Release) - target_compile_options(kdl_implementation PUBLIC ${CXX_FLAGS_ARGS}) - endif () - target_link_libraries(kdl_implementation PUBLIC kdl_lib kdl_parser_lib Eigen3::Eigen) + set(LIB_KDL + ${CMAKE_CURRENT_BINARY_DIR}/kdl/src/kdl-content-build/src/liborocos-kdl.so + ) + ExternalProject_Add( + kdl-content + PREFIX ${CMAKE_CURRENT_BINARY_DIR}/kdl + SOURCE_SUBDIR orocos_kdl + GIT_REPOSITORY https://github.com/orocos/orocos_kinematics_dynamics.git + GIT_TAG master # v1.5.1 + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_BINARY_DIR} + -DCMAKE_CXX_FLAGS=${CXX_FLAGS} + BUILD_BYPRODUCTS ${LIB_KDL}) + # use custom target to ensure kdl-content is downloaded and installed before + # building main target + add_library(kdl_lib SHARED IMPORTED) + add_custom_target(kdl_lib-target DEPENDS kdl-content) + add_dependencies(kdl_lib kdl_lib-target) + set_target_properties(kdl_lib PROPERTIES IMPORTED_LOCATION ${LIB_KDL}) - target_include_directories(kdl_implementation PUBLIC ${CMAKE_BINARY_DIR}/include/kdl) - target_include_directories(kdl_implementation PUBLIC ${CMAKE_BINARY_DIR}/include/kdl_parser) + 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}) + target_compile_definitions(kdl_implementation PUBLIC TIP=${TIP}) + if(${CMAKE_BUILD_TYPE} STREQUAL "Release") + set_target_properties(kdl_implementation PROPERTIES CMAKE_BUILD_TYPE + Release) + target_compile_options(kdl_implementation PUBLIC ${CXX_FLAGS_ARGS}) + endif() + target_link_libraries(kdl_implementation PUBLIC kdl_lib kdl_parser_lib + Eigen3::Eigen) - target_link_libraries(forward_kinematics_test PUBLIC kdl_implementation) - target_link_libraries(inverse_kinematics_test PUBLIC kdl_implementation) + target_include_directories(kdl_implementation + PUBLIC ${CMAKE_BINARY_DIR}/include/kdl) + target_include_directories(kdl_implementation + PUBLIC ${CMAKE_BINARY_DIR}/include/kdl_parser) - target_compile_definitions(forward_kinematics_test PUBLIC MULTIPLIER=2) - target_compile_definitions(inverse_kinematics_test PUBLIC MULTIPLIER=1) + target_link_libraries(forward_kinematics_test PUBLIC kdl_implementation) + target_link_libraries(inverse_kinematics_test PUBLIC kdl_implementation) + target_compile_definitions(forward_kinematics_test PUBLIC MULTIPLIER=2) + target_compile_definitions(inverse_kinematics_test PUBLIC MULTIPLIER=1) -endif () \ No newline at end of file +endif() diff --git a/test/fetch_content_test/CMakeLists.txt b/test/fetch_content_test/CMakeLists.txt index f2cd172..24a67ba 100644 --- a/test/fetch_content_test/CMakeLists.txt +++ b/test/fetch_content_test/CMakeLists.txt @@ -2,27 +2,29 @@ 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 () +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 main -) + fast_forward_kinematics + GIT_REPOSITORY https://github.com/pac48/fast_robot_kinematics.git + GIT_TAG main) FetchContent_MakeAvailable(fast_forward_kinematics) - -#TODO use ZIP_LISTS to test all robots +# TODO use ZIP_LISTS to test all robots set(URDF_FILE ${CMAKE_CURRENT_SOURCE_DIR}/urdf/robot.urdf) set(ROOT base_link) set(TIP grasp_link) -generate_fast_forward_kinematics_library(fast_forward_kinematics_library - URDF_FILE ${URDF_FILE} - ROOT_LINK ${ROOT} - TIP_LINK ${TIP}) +generate_fast_forward_kinematics_library( + fast_forward_kinematics_library + URDF_FILE + ${URDF_FILE} + ROOT_LINK + ${ROOT} + TIP_LINK + ${TIP}) diff --git a/test/forward_kinematics_test.cpp b/test/forward_kinematics_test.cpp index bb228b5..a0a174d 100644 --- a/test/forward_kinematics_test.cpp +++ b/test/forward_kinematics_test.cpp @@ -11,32 +11,37 @@ using KI = fast_fk::JointData; using KI = kdl_impl::JointData; #endif -int main(int arc, char **argv) { - constexpr int iterations = 128 * 128; - constexpr int multiplier = 128 * MULTIPLIER; - std::array, iterations> rand_values; - for (auto &rand_val: rand_values) { - rand_val = Eigen::Vector::Random(); +int main(int arc, char** argv) +{ + constexpr int iterations = 128 * 128; + constexpr int multiplier = 128 * MULTIPLIER; + std::array, iterations> rand_values; + for(auto& rand_val : rand_values) + { + rand_val = Eigen::Vector::Random(); + } + + fk_interface::ForwardKinematicsInterface fk_interface; + Eigen::Matrix tf; + + auto start = std::chrono::high_resolution_clock::now(); + + for(int k = 0; k < multiplier; k++) + { + for(int i = 0; i < iterations; i++) + { + fk_interface.set_joints(rand_values[i]); + fk_interface.forward_kinematics(); } + } - fk_interface::ForwardKinematicsInterface fk_interface; - Eigen::Matrix tf; + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); - auto start = std::chrono::high_resolution_clock::now(); + std::cout << "Time taken by function: " << (float)duration.count() << " nanoseconds" + << std::endl; + std::cout << "Average: " << ((float)duration.count()) / (iterations * multiplier) + << " nanoseconds" << std::endl; - for (int k = 0; k < multiplier; k++) { - for (int i = 0; i < iterations; i++) { - fk_interface.set_joints(rand_values[i]); - fk_interface.forward_kinematics(); - } - } - - auto stop = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop - start); - - std::cout << "Time taken by function: " << (float) duration.count() << " nanoseconds" << std::endl; - std::cout << "Average: " << ((float) duration.count()) / (iterations * multiplier) << " nanoseconds" - << std::endl; - - return 0; + return 0; } diff --git a/test/include/fast_kinematics_joint_data_length.hpp b/test/include/fast_kinematics_joint_data_length.hpp deleted file mode 100644 index a8d65c4..0000000 --- a/test/include/fast_kinematics_joint_data_length.hpp +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include "cstddef" - -namespace fast_fk::internal { - constexpr size_t joint_data_length = 16; -} - diff --git a/test/include/kdl_kinematics.hpp b/test/include/kdl_kinematics.hpp index 1b65f74..0d6ef3d 100644 --- a/test/include/kdl_kinematics.hpp +++ b/test/include/kdl_kinematics.hpp @@ -1,139 +1,147 @@ #pragma once -#include "memory" -#include "fstream" - -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" #include "chainfksolverpos_recursive.hpp" -#include "chainiksolverpos_lma.hpp" #include "chainfksolvervel_recursive.hpp" +#include "chainiksolverpos_lma.hpp" #include "chainjnttojacsolver.hpp" -#include "treejnttojacsolver.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" +#include "fstream" #include "kdl_parser/kdl_parser.hpp" - #include "kinematics_interface.hpp" - +#include "memory" +#include "treejnttojacsolver.hpp" #define STRINGIZE(x) #x #define STRINGIZE_VALUE_OF(x) STRINGIZE(x) -namespace kdl_impl { - - - struct JointData { - static constexpr size_t get_num_joints() { - return NUMBER_OF_JOINTS; - } - - JointData() { - std::string urdf_file = STRINGIZE_VALUE_OF(URDF_FILE); - std::string robot_description; - { - std::fstream f; - f.open(urdf_file); - std::stringstream ss; - ss << f.rdbuf(); - robot_description = ss.str(); - } - // create kinematic chain - kdl_parser::treeFromString(robot_description, robot_tree); - std::string root_name = STRINGIZE_VALUE_OF(ROOT); - std::string tip_name = STRINGIZE_VALUE_OF(TIP); - - if (!robot_tree.getChain(root_name, tip_name, chain)) { - throw std::runtime_error("failed to load robot from URDF"); - } - auto num_joints_ = chain.getNrOfJoints(); - assert(num_joints_ == NUMBER_OF_JOINTS); - q = KDL::JntArray(num_joints_); - q_out = q; - - fk_pos_solver = std::make_shared(chain); - ik_solver_lma = std::make_shared(chain); - - for (auto i = 0; i < chain.getNrOfSegments(); ++i) { - if (chain.getSegment(i).getName() == tip_name) { - ee_ind = i + 1; - break; - } - } - if (ee_ind == -1) { - throw std::runtime_error("The tip `" + tip_name + " is missing from the chain."); - } - - - } - - - 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) { +namespace kdl_impl +{ + +struct JointData +{ + static constexpr size_t get_num_joints() + { + return NUMBER_OF_JOINTS; + } + + JointData() + { + std::string urdf_file = STRINGIZE_VALUE_OF(URDF_FILE); + std::string robot_description; + { + std::fstream f; + f.open(urdf_file); + std::stringstream ss; + ss << f.rdbuf(); + robot_description = ss.str(); + } + // create kinematic chain + kdl_parser::treeFromString(robot_description, robot_tree); + std::string root_name = STRINGIZE_VALUE_OF(ROOT); + std::string tip_name = STRINGIZE_VALUE_OF(TIP); + + if(!robot_tree.getChain(root_name, tip_name, chain)) + { + throw std::runtime_error("failed to load robot from URDF"); + } + auto num_joints_ = chain.getNrOfJoints(); + assert(num_joints_ == NUMBER_OF_JOINTS); + q = KDL::JntArray(num_joints_); + q_out = q; + + fk_pos_solver = std::make_shared(chain); + ik_solver_lma = std::make_shared(chain); + + for(auto i = 0; i < chain.getNrOfSegments(); ++i) + { + if(chain.getSegment(i).getName() == tip_name) + { + ee_ind = i + 1; + break; + } + } + if(ee_ind == -1) + { + throw std::runtime_error("The tip `" + tip_name + " is missing from the chain."); + } + } + + 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 < 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) { + for(auto ind = 0; ind < 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 < 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) { + for(auto ind = 0; ind < 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 < 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 0; -// return atan2f(joint_data[ind][0], joint_data[ind][1]); - } - - void get_joints(Eigen::Vector &values) const { + for(auto ind = 0; ind < 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 0; + // return atan2f(joint_data[ind][0], joint_data[ind][1]); + } + + void get_joints(Eigen::Vector& values) const + { #pragma unroll - for (auto ind = 0; ind < NUMBER_OF_JOINTS; ++ind) { -// values[ind] = atan2f(joint_data[ind][0], joint_data[ind][1]); - } - } - - - void get_frame(size_t index, Eigen::Matrix &tf) const; - - void forward_kinematics(); + for(auto ind = 0; ind < NUMBER_OF_JOINTS; ++ind) + { + // values[ind] = atan2f(joint_data[ind][0], joint_data[ind][1]); + } + } - fk_interface::IKSolverStats - inverse_kinematics(Eigen::Matrix &transform, Eigen::VectorX &q_guess); + void get_frame(size_t index, Eigen::Matrix& tf) const; + void forward_kinematics(); - Eigen::Matrix transform; - KDL::JntArray q; - KDL::JntArray q_out; - int ee_ind = -1; + fk_interface::IKSolverStats inverse_kinematics(Eigen::Matrix& transform, + Eigen::VectorX& q_guess); - KDL::Tree robot_tree; - KDL::Chain chain; - KDL::Frame frame; - std::shared_ptr fk_pos_solver; - std::shared_ptr ik_solver_lma; + Eigen::Matrix transform; + KDL::JntArray q; + KDL::JntArray q_out; + int ee_ind = -1; - }; + KDL::Tree robot_tree; + KDL::Chain chain; + KDL::Frame frame; + std::shared_ptr fk_pos_solver; + std::shared_ptr ik_solver_lma; +}; -} +} // namespace kdl_impl diff --git a/test/inverse_kinematics_test.cpp b/test/inverse_kinematics_test.cpp index e8b6da2..91d660d 100644 --- a/test/inverse_kinematics_test.cpp +++ b/test/inverse_kinematics_test.cpp @@ -11,55 +11,57 @@ using IK = fast_fk::JointData; using IK = kdl_impl::JointData; #endif +int main(int arc, char** argv) +{ + unsigned seed = time(0); + srand(seed); + rand(); -int main(int arc, char **argv) { - unsigned seed = time(0); - srand(seed); - rand(); + constexpr int iterations = 128 * 128 * 5 * MULTIPLIER; - constexpr int iterations = 128 * 128 * 5* MULTIPLIER; + // get target pose + Eigen::VectorX q_in = Eigen::VectorX::Random(IK::get_num_joints()); + fk_interface::InverseKinematicsInterface fk_interface; + fk_interface.set_joints(q_in); + fk_interface.forward_kinematics(); + Eigen::Matrix tf; + fk_interface.get_frame(IK::get_num_joints() - 1, tf); - // get target pose - Eigen::VectorX q_in = Eigen::VectorX::Random(IK::get_num_joints()); - fk_interface::InverseKinematicsInterface fk_interface; - fk_interface.set_joints(q_in); - fk_interface.forward_kinematics(); - Eigen::Matrix tf; - fk_interface.get_frame(IK::get_num_joints() - 1, tf); + auto start = std::chrono::high_resolution_clock::now(); - auto start = std::chrono::high_resolution_clock::now(); + fk_interface::IKSolverStats stats; + size_t failed = 0; + size_t succeeded = 0; - fk_interface::IKSolverStats stats; - size_t failed = 0; - size_t succeeded = 0; + Eigen::VectorX q = 1 * Eigen::VectorX::Random(IK::get_num_joints()); + for(int ind = 0; ind < iterations; ++ind) + { + q = Eigen::VectorX::Random(IK::get_num_joints()); + stats = fk_interface.inverse_kinematics(tf, q); + failed += stats.success == 0; + succeeded += stats.success == 1; + // if (!stats.success) { + // debug info + // std::cout << " failed!!" << std::endl; + // std::cout << "q: " << q.transpose() << std::endl; + // std::cout << "q_in: " << q_in.transpose() << std::endl; + // std::cout << stats.niter << " iterations" << std::endl; + // std::cout << "f(x) = " << stats.fx << std::endl; + // std::cout << "||grad|| = " << stats.grad_norm << std::endl; + // std::cout << "error: " << stats.what << std::endl; + // continue; + // } + } - Eigen::VectorX q = 1 * Eigen::VectorX::Random(IK::get_num_joints()); - for (int ind = 0; ind < iterations; ++ind) { - q = Eigen::VectorX::Random(IK::get_num_joints()); - stats = fk_interface.inverse_kinematics(tf, q); - failed += stats.success == 0; - succeeded += stats.success == 1; -// if (!stats.success) { - // debug info -// std::cout << " failed!!" << std::endl; -// std::cout << "q: " << q.transpose() << std::endl; -// std::cout << "q_in: " << q_in.transpose() << std::endl; -// std::cout << stats.niter << " iterations" << std::endl; -// std::cout << "f(x) = " << stats.fx << std::endl; -// std::cout << "||grad|| = " << stats.grad_norm << std::endl; -// std::cout << "error: " << stats.what << std::endl; -// continue; -// } - } + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); - auto stop = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop - start); + std::cout << "Time taken by function: " << (double)duration.count() << " microseconds" + << std::endl; + std::cout << "Average: " << ((double)duration.count()) / (iterations) << " microseconds" + << std::endl; + std::cout << "Percent success: " << ((double)(100.0 * succeeded) / (failed + succeeded)) + << "%" << std::endl; - std::cout << "Time taken by function: " << (double) duration.count() << " microseconds" << std::endl; - std::cout << "Average: " << ((double) duration.count()) / (iterations) << " microseconds" - << std::endl; - std::cout << "Percent success: " << ((double) (100.0 * succeeded) / (failed + succeeded)) << "%" - << std::endl; - - return 0; + return 0; } diff --git a/test/kdl/kdl_implementation.cpp b/test/kdl/kdl_implementation.cpp index b4b5c8b..85d639f 100644 --- a/test/kdl/kdl_implementation.cpp +++ b/test/kdl/kdl_implementation.cpp @@ -1,51 +1,58 @@ #include "kdl_kinematics.hpp" -namespace kdl_impl { - - void JointData::get_frame(size_t index, Eigen::Matrix &tf) const { - tf = transform; - } - - // taken from https://github.com/ros/geometry/blob/noetic-devel/eigen_conversions/src/eigen_kdl.cpp - void transformKDLToEigen(const KDL::Frame &k, Eigen::Matrix &e) { - // translation - for (unsigned int i = 0; i < 3; ++i) - e(i, 3) = k.p[i]; - - // rotation matrix - for (unsigned int i = 0; i < 9; ++i) - e(i / 3, i % 3) = k.M.data[i]; - - // "identity" row - e(3, 0) = 0.0; - e(3, 1) = 0.0; - e(3, 2) = 0.0; - e(3, 3) = 1.0; - } - - void transformEigenToKDL(Eigen::Matrix &e, KDL::Frame &k) { - for (unsigned int i = 0; i < 3; ++i) - k.p[i] = e(i, 3); - for (unsigned int i = 0; i < 9; ++i) - k.M.data[i] = e(i / 3, i % 3); - } - - void JointData::forward_kinematics() { - // create forward kinematics solver - fk_pos_solver->JntToCart(q, frame, ee_ind); - transformKDLToEigen(frame, transform); - int o = 0; - } - - fk_interface::IKSolverStats - JointData::inverse_kinematics(Eigen::Matrix &tf, Eigen::VectorX &q_guess) { - for (int ind = 0; ind < get_num_joints(); ++ind) { - q.data[ind] = q_guess[ind]; - } - transformEigenToKDL(tf, frame); - - auto ret = ik_solver_lma->CartToJnt(q, frame, q_out); - - return {-1.0, -1, -1, ret == 0, ""}; - } -} \ No newline at end of file +namespace kdl_impl +{ + +void JointData::get_frame(size_t index, Eigen::Matrix& tf) const +{ + tf = transform; +} + +// taken from https://github.com/ros/geometry/blob/noetic-devel/eigen_conversions/src/eigen_kdl.cpp +void transformKDLToEigen(const KDL::Frame& k, Eigen::Matrix& e) +{ + // translation + for(unsigned int i = 0; i < 3; ++i) + e(i, 3) = k.p[i]; + + // rotation matrix + for(unsigned int i = 0; i < 9; ++i) + e(i / 3, i % 3) = k.M.data[i]; + + // "identity" row + e(3, 0) = 0.0; + e(3, 1) = 0.0; + e(3, 2) = 0.0; + e(3, 3) = 1.0; +} + +void transformEigenToKDL(Eigen::Matrix& e, KDL::Frame& k) +{ + for(unsigned int i = 0; i < 3; ++i) + k.p[i] = e(i, 3); + for(unsigned int i = 0; i < 9; ++i) + k.M.data[i] = e(i / 3, i % 3); +} + +void JointData::forward_kinematics() +{ + // create forward kinematics solver + fk_pos_solver->JntToCart(q, frame, ee_ind); + transformKDLToEigen(frame, transform); + int o = 0; +} + +fk_interface::IKSolverStats JointData::inverse_kinematics(Eigen::Matrix& tf, + Eigen::VectorX& q_guess) +{ + for(int ind = 0; ind < get_num_joints(); ++ind) + { + q.data[ind] = q_guess[ind]; + } + transformEigenToKDL(tf, frame); + + auto ret = ik_solver_lma->CartToJnt(q, frame, q_out); + + return { -1.0, -1, -1, ret == 0, "" }; +} +} // namespace kdl_impl diff --git a/test/urdf/pr2.urdf b/test/urdf/pr2.urdf index a1efe15..dd95c4f 100644 --- a/test/urdf/pr2.urdf +++ b/test/urdf/pr2.urdf @@ -785,7 +785,7 @@ - diff --git a/test/urdf/ur5.urdf b/test/urdf/ur5.urdf index c4dfbfd..f436283 100644 --- a/test/urdf/ur5.urdf +++ b/test/urdf/ur5.urdf @@ -326,4 +326,4 @@ - \ No newline at end of file +