diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index cf800c23..9e4c291c 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,7 +1,7 @@ // For format details, see https://aka.ms/devcontainer.json. For config options, see the // README at: https://github.com/devcontainers/templates/tree/main/src/docker-existing-dockerfile { - "name": "RAPTOR", + "name": "RAPTOR-zichang", "build": { // Sets the run context to one level up instead of the .devcontainer folder. "context": "..", diff --git a/.github/workflows/build-and-test.yml b/.github/workflows/build-and-test.yml new file mode 100644 index 00000000..d3e29482 --- /dev/null +++ b/.github/workflows/build-and-test.yml @@ -0,0 +1,362 @@ +name: Build and Test RAPTOR + +on: + push: + branches: + - unit_test_dev + paths-ignore: + # Compiled Object files and Libraries + - '*.d' + - '*.slo' + - '*.lo' + - '*.o' + - '*.obj' + - '*.gch' + - '*.pch' + - '*.so' + - '*.dylib' + - '*.dll' + - '*.mod' + - '*.smod' + - '*.lai' + - '*.la' + - '*.a' + - '*.lib' + + # Executables + - '*.exe' + - '*.out' + - '*.app' + + # Matlab related + - '*.asv' + - '*.mat' + + - '.vscode/**' + + - 'build/**' + - 'data/**' + - 'data_safecopy/**' + - 'Examples/Kinova/SystemIdentification/ExcitingTrajectories/data/**' + + + - 'test_oracle.py' + + - 'docker/HSL.zip' + - 'docker/pardiso/**' + - 'HSL/**' + + - 'Examples/Digit/singlestep_optimization_settings.yaml' + - 'Examples/Digit-modified/singlestep_optimization_settings.yaml' + + - 'test_nanobind_planner.py' + + pull_request: + branches: + - unit_test_dev + paths-ignore: + # Same patterns as for push events + - '*.d' + - '*.slo' + - '*.lo' + - '*.o' + - '*.obj' + - '*.gch' + - '*.pch' + - '*.so' + - '*.dylib' + - '*.dll' + - '*.mod' + - '*.smod' + - '*.lai' + - '*.la' + - '*.a' + - '*.lib' + + # Executables + - '*.exe' + - '*.out' + - '*.app' + + # Matlab related + - '*.asv' + - '*.mat' + + - '.vscode/**' + + - 'build/**' + - 'data/**' + - 'data_safecopy/**' + - 'Examples/Kinova/SystemIdentification/ExcitingTrajectories/data/**' + + + - 'test_oracle.py' + + - 'docker/HSL.zip' + - 'docker/pardiso/**' + - 'HSL/**' + + - 'Examples/Digit/singlestep_optimization_settings.yaml' + - 'Examples/Digit-modified/singlestep_optimization_settings.yaml' + + - 'test_nanobind_planner.py' + + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + build: + + runs-on: ubuntu-latest + container: ${{ matrix.container }} + strategy: + matrix: + container: ['ubuntu:20.04', 'ubuntu:22.04', 'ubuntu:24.04'] + cmake-version: [3.18.0, 3.22.0, 3.25.0] +# not sure + env: + CCACHE_BASEDIR: ${{ github.workspace }} + CCACHE_DIR: ${{ github.workspace }}/.ccache + CCACHE_COMPRESS: true + CCACHE_COMPRESSLEVEL: 5 + + steps: + - name: Setup Container + run: | + apt update && DEBIAN_FRONTEND="noninteractive" apt install -y sudo lsb-release gnupg2 cmake git python3 + + - name: Checkout Repository + uses: actions/checkout@v4 + with: + fetch-depth: 0 + submodules: recursive + + - name: Setup ccache + uses: actions/cache@v4 + with: + path: .ccache + save-state: true + key: ccache-linux-${{ matrix.container }}-${{ github.sha }} + restore-keys: | + ccache-linux-${{ matrix.container }}- + + - name: Get branch name (merge) + if: github.event_name != 'pull_request' + shell: bash + run: echo "BRANCH_NAME=$(echo ${GITHUB_REF#refs/heads/})" >> $GITHUB_ENV + + - name: Get branch name (push) + if: github.event_name != 'pull_request' + shell: bash + run: echo "BRANCH_NAME=$(echo ${GITHUB_REF#refs/heads/})" >> $GITHUB_ENV + + - name: Get branch name (pull request) + if: github.event_name == 'pull_request' + shell: bash + run: echo "BRANCH_NAME=$(echo ${GITHUB_HEAD_REF})" >> $GITHUB_ENV + + - name: Debug Branch Name + run: echo ${{ env.BRANCH_NAME }} + + - name: Register robotpkg + run: | + sh -c "echo \"deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg\" >> /etc/apt/sources.list" + apt-key adv --fetch-keys http://robotpkg.openrobots.org/packages/debian/robotpkg.key + + - name: Set and install dependencies + run: | + rm -rf /usr/local/share/boost/1.69.0 + export PYTHON3_VERSION=$(python3 -c "import sys; print(str(sys.version_info.major)+str(sys.version_info.minor))") + export APT_DEPENDENCIES="doxygen \ + ccache \ + curl \ + cppcheck \ + libomp-dev \ + libomp5 \ + libboost-system-dev \ + libboost-test-dev \ + libboost-filesystem-dev \ + libboost-program-options-dev \ + libeigen3-dev \ + liburdfdom-dev \ + texlive-font-utils \ + libboost-python-dev \ + python3-numpy \ + python3-matplotlib \ + robotpkg-py${PYTHON3_VERSION}-eigenpy \ + robotpkg-py${PYTHON3_VERSION}-hpp-fcl \ + robotpkg-py${PYTHON3_VERSION}-casadi" + echo "APT Dependencies: $APT_DEPENDENCIES" + + apt-get update -qq + DEBIAN_FRONTEND="noninteractive" apt-get install -qq ${APT_DEPENDENCIES} + + - name: Install Python Packages + run: | + pip3 install --upgrade pip + pip3 install scipy matplotlib pyyaml torch pybullet==3.2.5 mujoco==2.3.7 glfw==2.6.2 pyopengl==3.1.7 numpy==1.25.2 nanobind pygccxml pyplusplus + + - name: Install Pinocchio Dependencies + run: | + bash docker/scripts/install-pinocchio.sh + + - name: Install HSL Solver + run: | + bash docker/scripts/install-hsl.sh + + - name: Install Ipopt Solver + run: | + bash docker/scripts/install-ipopt.sh + + - name: Install OMPL + run: | + bash docker/scripts/install-ompl.sh + + - name: Free disk space + run: | + apt clean + df -h + + - name: Configure CMake + run: | + # Add cloned repo to safe.directory, since it was not cloned by the container + git config --global --add safe.directory "$GITHUB_WORKSPACE" + git submodule update --init + export PATH=$PATH:/opt/openrobots/bin + export PYTHON3_DOT_VERSION=$(python3 -c "import sys; print(str(sys.version_info.major)+'.'+str(sys.version_info.minor))") + export PYTHONPATH=${PYTHONPATH}:/opt/openrobots/lib/python$PYTHON3_DOT_VERSION/site-packages + export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/openrobots/lib:/usr/local/lib:/usr/lib:/usr/lib/x86_64-linux-gnu + + mkdir build + cd build + cmake .. \ + -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_WITH_COLLISION_SUPPORT=ON \ + -DBUILD_ADVANCED_TESTING=OFF \ + -DBUILD_WITH_CASADI_SUPPORT=ON \ + -DPYTHON_EXECUTABLE=$(which python3) \ + -DBUILD_WITH_OPENMP_SUPPORT=ON \ + -DINSTALL_DOCUMENTATION=ON \ + -DCMAKE_TOOLCHAIN_FILE=${GITHUB_WORKSPACE}/.github/workflows/cmake/linux-debug-toolchain.cmake + + - name: Build Project + run: | + cd build + make -j$(nproc) +# may overlap + # - name: Build Tests + # run: | + # cd build + # make -j$(nproc) build_tests + + - name: Run Tests + env: + CTEST_OUTPUT_ON_FAILURE: 1 + run: | + cd build + ctest --output-on-failure + + - name: Upload Build Artifacts + if: always() + uses: actions/upload-artifact@v3 + with: + name: build-artifacts + path: build/ + + - name: Run Tests + env: + CTEST_OUTPUT_ON_FAILURE: 1 + run: | + cd build + mkdir -p TestResults + ctest --output-on-failure --output-junit TestResults/test-results.xml + + - name: Upload Test Results + if: always() + uses: actions/upload-artifact@v3 + with: + name: test-results + path: build/TestResults/test-results.xml + + - name: Upload Test Results to Test Reporter + if: always() + uses: dorny/test-reporter@v1 + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + with: + name: CTest Results + path: build/TestResults/test-results.xml + reporter: junit + + + - name: Test Packaging + run: | + export PATH=$PATH:/opt/openrobots/bin + export PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/opt/openrobots/lib/pkgconfig + export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/openrobots/lib:/usr/local/lib:/usr/lib:/usr/lib/x86_64-linux-gnu + + # Test CMake module packaging + cd ./unittest/packaging/cmake + mkdir build && cd build + export CMAKE_PREFIX_PATH=/usr/local + cmake .. \ + -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ + -DPYTHON_EXECUTABLE=$(which python3) + make -j$(nproc) + ./run_rnea + ./load_urdf + + # Test pkg-config packaging + cd ../../pkgconfig + mkdir build && cd build + cmake .. \ + -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ + -DPYTHON_EXECUTABLE=$(which python3) + make -j$(nproc) + ./run_rnea + ./load_urdf + + # Test FetchContent packaging + cd ../../external + export PINOCCHIO_GIT_REPOSITORY="file://$GITHUB_WORKSPACE" + export PINOCCHIO_GIT_TAG="test-external-$(git rev-parse --short HEAD)" + git tag $PINOCCHIO_GIT_TAG + mkdir build && cd build + cmake .. \ + -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ + -DPYTHON_EXECUTABLE=$(which python3) + make -j$(nproc) + ./run_rnea + ./load_urdf + + # Test CMake module packaging and pinocchio_header target + cd ../../pinocchio_header + mkdir build && cd build + cmake .. \ + -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ + -DPYTHON_EXECUTABLE=$(which python3) + make -j$(nproc) + ./run_fk + + - name: Uninstall Project + run: | + cd build + sudo make uninstall + + check: + if: always() + name: check-linux-apt + + needs: + - build + + runs-on: ubuntu-latest + + steps: + - name: Decide whether the needed jobs succeeded or failed + uses: re-actors/alls-green@release/v1 + with: + jobs: ${{ toJSON(needs) }} diff --git a/CMakeLists.txt b/CMakeLists.txt index c5fd5127..f80f0c05 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,7 @@ # Minimum required CMake version cmake_minimum_required(VERSION 3.18) + # Project name project(RAPTOR DESCRIPTION "Rapid and Robust Trajectory Optimization for Robots") @@ -8,6 +9,17 @@ project(RAPTOR if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() +# + +include(CTest) +set(CTEST_OUTPUT_ON_FAILURE TRUE) +set(CTEST_CUSTOM_TESTS_IGNORE "") + +set(CTEST_JUNIT_OUTPUT_PATH "${CMAKE_BINARY_DIR}/TestResults") +set(CTEST_CUSTOM_TESTS_IGNORE "") + +enable_testing() +enable_testing() # set(CMAKE_CXX_FLAGS "-Wall -Wextra") set(CMAKE_CXX_FLAGS_DEBUG "-g -fopenmp") diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index 7756f068..411d66af 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -1,75 +1,131 @@ -set(COMMON_LIBRARIES - trajlib - IDlib - Conslib - Optlib - ${Boost_LIBRARIES} - pinocchio::pinocchio - ipopt - coinhsl -) -add_executable(ForwardKinematics_test - KinematicsDynamics/TestForwardKinematicsSolver.cpp) -target_link_libraries(ForwardKinematics_test PUBLIC - IDlib - ${BOOST_LIBRARIES} - pinocchio::pinocchio) +# set(COMMON_LIBRARIES +# trajlib +# IDlib +# Conslib +# Optlib +# ${Boost_LIBRARIES} +# pinocchio::pinocchio +# ipopt +# coinhsl +# ) -target_compile_options(ForwardKinematics_test PUBLIC - ${PINOCCHIO_FLAGS}) +# add_executable(ForwardKinematics_test +# KinematicsDynamics/TestForwardKinematicsSolver.cpp) -add_executable(ForwardKinematicsGradient_test - KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp) +# target_link_libraries(ForwardKinematics_test PUBLIC +# IDlib +# ${BOOST_LIBRARIES} +# pinocchio::pinocchio) -target_link_libraries(ForwardKinematicsGradient_test PUBLIC - IDlib - Optlib - Conslib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) +# target_compile_options(ForwardKinematics_test PUBLIC +# ${PINOCCHIO_FLAGS}) -target_compile_options(ForwardKinematicsGradient_test PUBLIC - ${PINOCCHIO_FLAGS}) +# add_test(NAME ForwardKinematics_test COMMAND ForwardKinematics_test) +# set_tests_properties(ForwardKinematics_test PROPERTIES WORKING_DIRECTORY /workspaces/RAPTOR/build) -add_executable(KinematicsConstraints_test - Constraints/TestKinematicsConstraints.cpp) +# add_executable(ForwardKinematicsGradient_test +# KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp) -target_link_libraries(KinematicsConstraints_test PUBLIC - trajlib - IDlib - Conslib - ${BOOST_LIBRARIES} - pinocchio::pinocchio) +# target_link_libraries(ForwardKinematicsGradient_test PUBLIC +# IDlib +# Optlib +# Conslib +# ipopt +# coinhsl +# ${BOOST_LIBRARIES} +# pinocchio::pinocchio) -target_compile_options(KinematicsConstraints_test PUBLIC - ${PINOCCHIO_FLAGS}) +# target_compile_options(ForwardKinematicsGradient_test PUBLIC +# ${PINOCCHIO_FLAGS}) -add_executable(CustomizedInverseDynamics_test - KinematicsDynamics/TestCustomizedInverseDynamics.cpp) +# add_test(NAME ForwardKinematicsGradient_test COMMAND ForwardKinematicsGradient_test) +# set_tests_properties(ForwardKinematicsGradient_test PROPERTIES WORKING_DIRECTORY /workspaces/RAPTOR/build) -target_link_libraries(CustomizedInverseDynamics_test PUBLIC - trajlib - IDlib - Conslib - ${BOOST_LIBRARIES} - pinocchio::pinocchio) -target_compile_options(CustomizedInverseDynamics_test PUBLIC - ${PINOCCHIO_FLAGS}) +# add_executable(KinematicsConstraints_test +# Constraints/TestKinematicsConstraints.cpp) -add_executable(RegressorInverseDynamics_test - KinematicsDynamics/TestRegressorInverseDynamics.cpp) +# target_link_libraries(KinematicsConstraints_test PUBLIC +# trajlib +# IDlib +# Conslib +# ${BOOST_LIBRARIES} +# pinocchio::pinocchio) -target_link_libraries(RegressorInverseDynamics_test PUBLIC +# target_compile_options(KinematicsConstraints_test PUBLIC +# ${PINOCCHIO_FLAGS}) + +# add_test(NAME KinematicsConstraints_test COMMAND KinematicsConstraints_test) +# set_tests_properties(KinematicsConstraints_test PROPERTIES WORKING_DIRECTORY /workspaces/RAPTOR/build) + +# add_executable(CustomizedInverseDynamics_test +# KinematicsDynamics/TestCustomizedInverseDynamics.cpp) + +# target_link_libraries(CustomizedInverseDynamics_test PUBLIC +# trajlib +# IDlib +# Conslib +# ${BOOST_LIBRARIES} +# pinocchio::pinocchio) + +# target_compile_options(CustomizedInverseDynamics_test PUBLIC +# ${PINOCCHIO_FLAGS}) + +# add_test(NAME CustomizedInverseDynamics_test COMMAND CustomizedInverseDynamics_test) +# set_tests_properties(CustomizedInverseDynamics_test PROPERTIES WORKING_DIRECTORY /workspaces/RAPTOR/build) + +# add_executable(RegressorInverseDynamics_test +# KinematicsDynamics/TestRegressorInverseDynamics.cpp) + +# target_link_libraries(RegressorInverseDynamics_test PUBLIC +# trajlib +# IDlib +# Conslib +# ${BOOST_LIBRARIES} +# pinocchio::pinocchio) + +# target_compile_options(RegressorInverseDynamics_test PUBLIC +# ${PINOCCHIO_FLAGS}) +# add_test(NAME RegressorInverseDynamics_test COMMAND RegressorInverseDynamics_test) +# set_tests_properties(RegressorInverseDynamics_test PROPERTIES WORKING_DIRECTORY /workspaces/RAPTOR/build) + + + +# Define common libraries +set(COMMON_LIBRARIES trajlib IDlib Conslib - ${BOOST_LIBRARIES} - pinocchio::pinocchio) + Optlib + ${Boost_LIBRARIES} + pinocchio::pinocchio + ipopt + coinhsl +) + +# Define a function to add tests +function(add_raptor_test test_name source_file) + add_executable(${test_name} ${source_file}) + target_link_libraries(${test_name} PRIVATE ${COMMON_LIBRARIES}) + target_compile_options(${test_name} PRIVATE ${PINOCCHIO_FLAGS}) + add_test(NAME ${test_name} COMMAND ${test_name}) + set_tests_properties(${test_name} PROPERTIES WORKING_DIRECTORY "${CMAKE_BINARY_DIR}") +endfunction() + +# Add individual tests +add_raptor_test(ForwardKinematics_test + KinematicsDynamics/TestForwardKinematicsSolver.cpp) + +add_raptor_test(ForwardKinematicsGradient_test + KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp) + +add_raptor_test(KinematicsConstraints_test + Constraints/TestKinematicsConstraints.cpp) -target_compile_options(RegressorInverseDynamics_test PUBLIC - ${PINOCCHIO_FLAGS}) +add_raptor_test(CustomizedInverseDynamics_test + KinematicsDynamics/TestCustomizedInverseDynamics.cpp) + +add_raptor_test(RegressorInverseDynamics_test + KinematicsDynamics/TestRegressorInverseDynamics.cpp) diff --git a/Tests/Constraints/TestKinematicsConstraints.cpp b/Tests/Constraints/TestKinematicsConstraints.cpp index b7200e42..0ebfb379 100644 --- a/Tests/Constraints/TestKinematicsConstraints.cpp +++ b/Tests/Constraints/TestKinematicsConstraints.cpp @@ -1,3 +1,6 @@ +#define BOOST_TEST_MODULE KinematicsConstraintsTest +#include + #include "KinematicsConstraints.h" // #include "Plain.h" #include "Polynomials.h" @@ -5,7 +8,10 @@ using namespace RAPTOR; -int main() { + +BOOST_AUTO_TEST_SUITE(KinematicsConstraintsTest) +BOOST_AUTO_TEST_CASE(owngradientTest) +{ // Define robot model // const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf"; const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; @@ -28,18 +34,19 @@ int main() { // simple test when difference is small Eigen::VectorXd z_test = z.array() + 1e-6; - kc.compute(z_test, false); - std::cout << kc.g << std::endl << std::endl; + // kc.compute(z_test, false); + // std::cout << kc.g << std::endl << std::endl; // simple test when difference is large - z_test = z.array() + 1.0; - auto start_clock = std::chrono::high_resolution_clock::now(); - kc.compute(z_test, true, true); - auto end_clock = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(end_clock - start_clock); - std::cout << "Time taken (including gradient and hessian): " << duration.count() << " microseconds" << std::endl; + // z_test = z.array() + 1.0; + + // auto start_clock = std::chrono::high_resolution_clock::now(); + // kc.compute(z_test, true, true); + // auto end_clock = std::chrono::high_resolution_clock::now(); + // auto duration = std::chrono::duration_cast(end_clock - start_clock); + // std::cout << "Time taken (including gradient and hessian): " << duration.count() << " microseconds" << std::endl; - std::cout << kc.g << std::endl << std::endl; + // std::cout << kc.g << std::endl << std::endl; // test gradient const Eigen::MatrixXd J_analytical = kc.pg_pz; @@ -59,9 +66,43 @@ int main() { // std::cout << "Analytical gradient: " << std::endl << J_analytical << std::endl << std::endl; // std::cout << "Numerical gradient: " << std::endl << J_numerical << std::endl << std::endl; std::cout << J_analytical - J_numerical << std::endl << std::endl; + BOOST_CHECK_SMALL((J_analytical - J_numerical).norm(), 1e-10); + } + // test hessian + + BOOST_AUTO_TEST_CASE(ownHessianTest){ + const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; + + pinocchio::Model model; + pinocchio::urdf::buildModel(urdf_filename, model); + + // std::shared_ptr trajPtr_ = std::make_shared(model.nv); + std::shared_ptr trajPtr_ = std::make_shared(2.0, 10, model.nv, TimeDiscretization::Chebyshev, 3); + ForwardKinematicsSolver fkSolver(&model); + + + // compute a valid transform using forward kinematics + std::srand(std::time(nullptr)); + Eigen::VectorXd z = 2 * M_PI * Eigen::VectorXd::Random(trajPtr_->varLength).array() - M_PI; + int start = 0; + int end = model.getJointId("joint_7"); + fkSolver.compute(start, end, z); + + // check error + bool hasError = false; + double max_diff = 0.0; + std::stringstream error_message; + + KinematicsConstraints kc(trajPtr_, &model, end, 6, fkSolver.getTransform()); + + // simple test when difference is small + Eigen::VectorXd z_test = z.array() + 1e-6; Eigen::Array H_analytical = kc.pg_pz_pz; + + + for (int i = 0; i < z_test.size(); i++) { Eigen::VectorXd q_plus = z_test; q_plus(i) += 1e-7; @@ -73,10 +114,43 @@ int main() { const Eigen::MatrixXd J_minus = kc.pg_pz; const Eigen::MatrixXd H_numerical_row = (J_plus - J_minus) / 2e-7; + // check error for (int j = 0; j < 3; j++) { - std::cout << H_analytical(j).row(i) - H_numerical_row.row(j) << std::endl; + // sstd::cout << H_analytical(j).row(i) - H_numerical_row.row(j) << std::endl; + + //check each loop + // BOOST_CHECK_SMALL((H_analytical(j).row(i) - H_numerical_row.row(j) ).norm(), 1e-10); + + //check one time + double diff = (H_analytical(j).row(i) - H_numerical_row.row(j) ).norm(); + if (diff >1e-10){ + hasError = true; + if (diff >max_diff) max_diff = diff; + error_message << "error found at i=" << i << ", j=" << j + << " with difference: " << diff << "\n"; + + } + } + + } + // bool hasError = false; + // double max_diff = 0.0; + // for (int i = 0; i < z_test.size(); i++) { + // for (int j = 0; j < 3; j++){ + // double diff = (H_analytical(j).row(i) - H_numerical_row.row(j) ).norm() + // if (diff >1e-10){ + // hasError = true; + // if (diff >max_diff) max_diff = diff; + // } + // error_message << "Discrepancy found at i=" << i << ", j=" << j + // << " with difference: " << diff << "\n"; + // } + // } + BOOST_CHECK_MESSAGE(!hasError, "Hessian discrepancies found:\n" + error_message.str()); + - return 0; + } +BOOST_AUTO_TEST_SUITE_END() diff --git a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp index a60e2678..b9fb34e1 100644 --- a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp @@ -1,3 +1,6 @@ +#define BOOST_TEST_MODULE CustomizedInverseDynamicsTest +#include + #include "pinocchio/algorithm/model.hpp" #include "CustomizedInverseDynamics.h" @@ -6,8 +9,17 @@ using namespace RAPTOR; -int main() { +BOOST_AUTO_TEST_SUITE(InverseDynamicsTestSuite) + +BOOST_AUTO_TEST_CASE(test_inverse_dynamics_without_fixed_joints) +{ // define robot model + char cwd[1024]; + if (getcwd(cwd, sizeof(cwd)) != nullptr) { + std::cout << "Current working directory: " << cwd << std::endl; + } else { + perror("getcwd() error"); + } const std::string urdf_filename = "../Robots/kinova-gen3/kinova_grasp.urdf"; pinocchio::Model model; @@ -30,31 +42,52 @@ int main() { // Test without fixed joints // compute inverse dynamics using pinocchio - auto start_clock = std::chrono::high_resolution_clock::now(); + // auto start_clock = std::chrono::high_resolution_clock::now(); for (int i = 0; i < trajPtr->N; i++) { pinocchio::rnea(model, data, trajPtr->q(0), trajPtr->q_d(0), trajPtr->q_dd(0)); } - auto stop_clock = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "Pinocchio ID: " << duration.count() << " nanoseconds" << std::endl; + // auto stop_clock = std::chrono::high_resolution_clock::now(); + // auto duration = std::chrono::duration_cast(stop_clock - start_clock); + // std::cout << "Pinocchio ID: " << duration.count() << " nanoseconds" << std::endl; // compute inverse dynamics using RAPTOR std::shared_ptr cidPtr = std::make_shared( model, trajPtr); - start_clock = std::chrono::high_resolution_clock::now(); + // start_clock = std::chrono::high_resolution_clock::now(); cidPtr->compute(z, false); - stop_clock = std::chrono::high_resolution_clock::now(); - duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "RAPTOR ID: " << duration.count() << " nanoseconds" << std::endl; + // stop_clock = std::chrono::high_resolution_clock::now(); + // duration = std::chrono::duration_cast(stop_clock - start_clock); + // std::cout << "RAPTOR ID: " << duration.count() << " nanoseconds" << std::endl; // compare the results std::cout << "Pinocchio: " << data.tau.transpose() << std::endl; std::cout << "RAPTOR: " << cidPtr->tau(0).transpose() << std::endl; + BOOST_CHECK_SMALL((data.tau - cidPtr->tau(0)).norm(), 1e-10); + +} // Test with fixed joints +BOOST_AUTO_TEST_CASE(test_inverse_dynamics_with_fixed_joints) +{ + // define robot model + const std::string urdf_filename = "../Robots/kinova-gen3/kinova_grasp.urdf"; + + pinocchio::Model model; + pinocchio::urdf::buildModel(urdf_filename, model); + + model.rotorInertia.setZero(); + model.damping.setZero(); + model.friction.setZero(); + + std::shared_ptr trajPtr = + std::make_shared( + Eigen::VectorXd::LinSpaced(5, 0, 1), model.nq, 5); + + + Eigen::VectorXi jtype = convertPinocchioJointType(model); jtype(jtype.size() - 1) = 0; // fix the last joint @@ -63,35 +96,40 @@ int main() { pinocchio::buildReducedModel(model, list_of_joints_to_lock_by_id, Eigen::VectorXd::Zero(model.nv), model_reduced); pinocchio::Data data_reduced(model_reduced); - z = Eigen::VectorXd::Random(trajPtr->varLength); + Eigen::VectorXd z = Eigen::VectorXd::Random(trajPtr->varLength); + trajPtr = std::make_shared( Eigen::VectorXd::LinSpaced(5, 0, 1), model_reduced.nq, 5); trajPtr->compute(z); // compute inverse dynamics using pinocchio - start_clock = std::chrono::high_resolution_clock::now(); + // start_clock = std::chrono::high_resolution_clock::now(); for (int i = 0; i < trajPtr->N; i++) { pinocchio::rnea(model_reduced, data_reduced, trajPtr->q(0).head(model_reduced.nq), trajPtr->q_d(0).head(model_reduced.nv), trajPtr->q_dd(0).head(model_reduced.nv)); } - stop_clock = std::chrono::high_resolution_clock::now(); - duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "Pinocchio ID (fixed joints): " << duration.count() << " nanoseconds" << std::endl; + // stop_clock = std::chrono::high_resolution_clock::now(); + // duration = std::chrono::duration_cast(stop_clock - start_clock); + // std::cout << "Pinocchio ID (fixed joints): " << duration.count() << " nanoseconds" << std::endl; // compute inverse dynamics using RAPTOR - cidPtr = std::make_shared( + std::shared_ptr cidPtr = std::make_shared( model, trajPtr, jtype); - auto start = std::chrono::high_resolution_clock::now(); + // auto start = std::chrono::high_resolution_clock::now(); cidPtr->compute(z, true); - auto end = std::chrono::high_resolution_clock::now(); - duration = std::chrono::duration_cast(end - start); - std::cout << "RAPTOR ID (fixed joints): " << duration.count() << " nanoseconds" << std::endl; + // auto end = std::chrono::high_resolution_clock::now(); + // duration = std::chrono::duration_cast(end - start); + // std::cout << "RAPTOR ID (fixed joints): " << duration.count() << " nanoseconds" << std::endl; // compare the results std::cout << "Pinocchio (fixed joints): " << data_reduced.tau.transpose() << std::endl; std::cout << "RAPTOR (fixed joints): " << cidPtr->tau(0).transpose() << std::endl; - return 0; -} \ No newline at end of file + + BOOST_CHECK_SMALL((data_reduced.tau - cidPtr->tau(0)).norm(), 1e-10); + + +} +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file diff --git a/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp b/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp index 6462f3d1..73cf3bcc 100644 --- a/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp +++ b/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp @@ -1,3 +1,6 @@ +#define BOOST_TEST_MODULE FKGradientCheckerTest +#include + #include "Optimizer.h" #include "ForwardKinematics.h" @@ -173,7 +176,25 @@ class FKGradientChecker : public Optimizer { const double yaw_weight = 1.0; }; -int main() { +bool check_gradient_output(const std::string& filename, const std::string& keyword) { + std::ifstream file(filename); + if (!file.is_open()) { + BOOST_TEST_MESSAGE("Can not open file " + filename); + return false; + } + std::string line; + while (std::getline(file, line)) { + if (line.find(keyword) != std::string::npos) { + return true; + } + } + return false; +} + +BOOST_AUTO_TEST_SUITE(FKGradientCheckerSuite) + +BOOST_AUTO_TEST_CASE(test_FKGradientChecker){ + // Define robot model const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; @@ -184,15 +205,16 @@ int main() { // Initialize gradient checker SmartPtr mynlp = new FKGradientChecker(); + try { - mynlp->set_parameters(z0, - model); - } - catch (std::exception& e) { + mynlp->set_parameters(z0, model); + } catch (const std::exception& e) { std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); + BOOST_FAIL("Error initializing Ipopt class! Check previous error message!"); } + + SmartPtr app = IpoptApplicationFactory(); app->Options()->SetNumericValue("max_wall_time", 1e-5); @@ -205,19 +227,28 @@ int main() { app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); app->Options()->SetNumericValue("derivative_test_tol", 1e-5); + // Initialize the IpoptApplication and process the options ApplicationReturnStatus status; status = app->Initialize(); if( status != Solve_Succeeded ) { - throw std::runtime_error("Error during initialization of optimization!"); + BOOST_FAIL("Error during initialization of optimization!"); } try { status = app->OptimizeTNLP(mynlp); } catch (std::exception& e) { - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); + BOOST_FAIL("Error solving optimization problem! Check previous error message!"); } - return 0; -} \ No newline at end of file + std::cout <<"status" < #include "ForwardKinematics.h" #include using namespace RAPTOR; -int main() { +BOOST_AUTO_TEST_SUITE(ForwardKinematicsSuite) +BOOST_AUTO_TEST_CASE(TestForwardKinematicsAccuracy) +{ // Define robot model const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf"; @@ -16,28 +20,35 @@ int main() { // set joint angles std::srand(std::time(nullptr)); Eigen::VectorXd q = 2 * M_PI * Eigen::VectorXd::Random(model.nq).array() - M_PI; + pinocchio::forwardKinematics(model, data, q); // compute forward kinematics using pinocchio - auto start_clock = std::chrono::high_resolution_clock::now(); - pinocchio::forwardKinematics(model, data, q); - auto stop_clock = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "Pinocchio FK: " << duration.count() << " nanoseconds" << std::endl; + // auto start_clock = std::chrono::high_resolution_clock::now(); + // pinocchio::forwardKinematics(model, data, q); + // auto stop_clock = std::chrono::high_resolution_clock::now(); + // auto duration = std::chrono::duration_cast(stop_clock - start_clock); + // std::cout << "Pinocchio FK: " << duration.count() << " nanoseconds" << std::endl; // set the start and end joint int start = 0; int end = model.getJointId("left_toe_B"); + fkSolver.compute(start, end, q); // compute forward kinematics using RAPTOR - start_clock = std::chrono::high_resolution_clock::now(); - fkSolver.compute(start, end, q); - stop_clock = std::chrono::high_resolution_clock::now(); - duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "RAPTOR FK: " << duration.count() << " nanoseconds" << std::endl; + // start_clock = std::chrono::high_resolution_clock::now(); + // fkSolver.compute(start, end, q); + // stop_clock = std::chrono::high_resolution_clock::now(); + // duration = std::chrono::duration_cast(stop_clock - start_clock); + // std::cout << "RAPTOR FK: " << duration.count() << " nanoseconds" << std::endl; // compare the results - std::cout << "Pinocchio: " << data.oMi[model.getJointId("left_toe_B")].translation().transpose() << std::endl; - std::cout << "RAPTOR: " << fkSolver.getTranslation().transpose() << std::endl; + Eigen::Vector3d pinocchio_translation = data.oMi[model.getJointId("left_toe_B")].translation(); + Eigen::Vector3d raptor_translation = fkSolver.getTranslation(); + + std::cout << "Pinocchio: " << pinocchio_translation.transpose() << std::endl; + std::cout << "RAPTOR: " << raptor_translation.transpose() << std::endl; + + BOOST_CHECK_SMALL((pinocchio_translation - raptor_translation).norm(), 1e-10); +} - return 0; -} \ No newline at end of file +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file diff --git a/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp b/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp index 313eb690..cc584854 100644 --- a/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp @@ -1,12 +1,16 @@ +#define BOOST_TEST_MODULE RegressorInverseDynamicsTest +#include #include "RegressorInverseDynamics.h" -#include +// #include #include "pinocchio/algorithm/rnea.hpp" #include "Polynomials.h" using VecX = Eigen::VectorXd; using namespace RAPTOR; -int main() { +BOOST_AUTO_TEST_SUITE(RegressorInverseDynamicsSuite) +BOOST_AUTO_TEST_CASE(RegressorInverseDynamicsAccuracy) +{ // Define robot model const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; @@ -35,19 +39,19 @@ int main() { VecX z = 2 * M_PI * VecX::Random(trajPtr->varLength).array() - M_PI; // Compute inverse dynamics using RegressorInverseDynamics - auto start_clock = std::chrono::high_resolution_clock::now(); + // auto start_clock = std::chrono::high_resolution_clock::now(); regressor_id.compute(z, false); - auto stop_clock = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "RegressorInverseDynamics: " << duration.count() << " nanoseconds" << std::endl; + // auto stop_clock = std::chrono::high_resolution_clock::now(); + // auto duration = std::chrono::duration_cast(stop_clock - start_clock); + // std::cout << "RegressorInverseDynamics: " << duration.count() << " nanoseconds" << std::endl; // Compute inverse dynamics using pinocchio::rnea trajPtr->compute(z, false); - start_clock = std::chrono::high_resolution_clock::now(); + // start_clock = std::chrono::high_resolution_clock::now(); VecX tau_pinocchio = pinocchio::rnea(model, data, trajPtr->q(0), trajPtr->q_d(0), trajPtr->q_dd(0)); - stop_clock = std::chrono::high_resolution_clock::now(); - duration = std::chrono::duration_cast(stop_clock - start_clock); - std::cout << "Pinocchio RNEA: " << duration.count() << " nanoseconds" << std::endl; + // stop_clock = std::chrono::high_resolution_clock::now(); + // duration = std::chrono::duration_cast(stop_clock - start_clock); + // std::cout << "Pinocchio RNEA: " << duration.count() << " nanoseconds" << std::endl; // Compare the results std::cout << "RegressorInverseDynamics result:" << std::endl; @@ -55,5 +59,10 @@ int main() { std::cout << "Pinocchio RNEA result:" << std::endl; std::cout << tau_pinocchio.transpose() << std::endl; - return 0; -} \ No newline at end of file + BOOST_CHECK_SMALL((regressor_id.tau(0) -tau_pinocchio).norm(), 1e-10); + + +} + + +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file