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/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/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/kdl/kdl_implementation.cpp b/test/kdl/kdl_implementation.cpp index b4b5c8b..565719c 100644 --- a/test/kdl/kdl_implementation.cpp +++ b/test/kdl/kdl_implementation.cpp @@ -48,4 +48,4 @@ namespace kdl_impl { return {-1.0, -1, -1, ret == 0, ""}; } -} \ No newline at end of file +} 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 +