diff --git a/CMakeLists.txt b/CMakeLists.txt index c5fd5127..d3880581 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,15 @@ 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() + # set(CMAKE_CXX_FLAGS "-Wall -Wextra") set(CMAKE_CXX_FLAGS_DEBUG "-g -fopenmp") set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fopenmp") diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index 7756f068..ecac83c5 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -1,3 +1,4 @@ +# Define common libraries set(COMMON_LIBRARIES trajlib IDlib @@ -9,67 +10,27 @@ set(COMMON_LIBRARIES coinhsl ) -add_executable(ForwardKinematics_test +# 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) -target_link_libraries(ForwardKinematics_test PUBLIC - IDlib - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(ForwardKinematics_test PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(ForwardKinematicsGradient_test +add_raptor_test(ForwardKinematicsGradient_test KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp) -target_link_libraries(ForwardKinematicsGradient_test PUBLIC - IDlib - Optlib - Conslib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(ForwardKinematicsGradient_test PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(KinematicsConstraints_test +add_raptor_test(KinematicsConstraints_test Constraints/TestKinematicsConstraints.cpp) -target_link_libraries(KinematicsConstraints_test PUBLIC - trajlib - IDlib - Conslib - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(KinematicsConstraints_test PUBLIC - ${PINOCCHIO_FLAGS}) - -add_executable(CustomizedInverseDynamics_test +add_raptor_test(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_executable(RegressorInverseDynamics_test + +add_raptor_test(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}) diff --git a/Tests/Constraints/TestKinematicsConstraints.cpp b/Tests/Constraints/TestKinematicsConstraints.cpp index b7200e42..fa3bdffe 100644 --- a/Tests/Constraints/TestKinematicsConstraints.cpp +++ b/Tests/Constraints/TestKinematicsConstraints.cpp @@ -1,11 +1,17 @@ +#define BOOST_TEST_MODULE KinematicsConstraintsTest +#include + #include "KinematicsConstraints.h" -// #include "Plain.h" #include "Polynomials.h" #include using namespace RAPTOR; -int main() { +BOOST_AUTO_TEST_SUITE(KinematicsConstraintsTest) + +// test gradient +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"; @@ -13,7 +19,6 @@ int main() { 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); @@ -26,57 +31,75 @@ int main() { KinematicsConstraints kc(trajPtr_, &model, end, 6, fkSolver.getTransform()); - // 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; - - // 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; + kc.compute(z, true, false); - std::cout << kc.g << std::endl << std::endl; - - // test gradient const Eigen::MatrixXd J_analytical = kc.pg_pz; Eigen::MatrixXd J_numerical = Eigen::MatrixXd::Zero(J_analytical.rows(), J_analytical.cols()); - for (int i = 0; i < z_test.size(); i++) { - Eigen::VectorXd q_plus = z_test; + for (int i = 0; i < z.size(); i++) { + Eigen::VectorXd q_plus = z; q_plus(i) += 1e-7; kc.compute(q_plus, false); const Eigen::VectorXd g_plus = kc.g; - Eigen::VectorXd q_minus = z_test; + Eigen::VectorXd q_minus = z; q_minus(i) -= 1e-7; kc.compute(q_minus, false); const Eigen::VectorXd g_minus = kc.g; J_numerical.col(i) = (g_plus - g_minus) / 2e-7; } - // 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-6); +} + +// 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); + + KinematicsConstraints kc(trajPtr_, &model, end, 6, fkSolver.getTransform()); + + kc.compute(z, true, true); - // test hessian Eigen::Array H_analytical = kc.pg_pz_pz; - for (int i = 0; i < z_test.size(); i++) { - Eigen::VectorXd q_plus = z_test; + + // params for error checking + bool hasError = false; + double max_diff = 0.0; + std::stringstream error_message; + + for (int i = 0; i < z.size(); i++) { + Eigen::VectorXd q_plus = z; q_plus(i) += 1e-7; kc.compute(q_plus, true, false); const Eigen::MatrixXd J_plus = kc.pg_pz; - Eigen::VectorXd q_minus = z_test; + Eigen::VectorXd q_minus = z; q_minus(i) -= 1e-7; kc.compute(q_minus, true, false); const Eigen::MatrixXd J_minus = kc.pg_pz; const Eigen::MatrixXd H_numerical_row = (J_plus - J_minus) / 2e-7; for (int j = 0; j < 3; j++) { - std::cout << H_analytical(j).row(i) - H_numerical_row.row(j) << std::endl; + double diff = (H_analytical(j).row(i) - H_numerical_row.row(j) ).norm(); + if (diff > 1e-6){ + hasError = true; + if (diff >max_diff) max_diff = diff; + error_message << "error found at i=" << i << ", j=" << j + << " with difference: " << diff << "\n"; + } } } - - return 0; + BOOST_CHECK_MESSAGE(!hasError, "Hessian discrepancies found:\n" + error_message.str()); } +BOOST_AUTO_TEST_SUITE_END() diff --git a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp index a60e2678..79f28bcb 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,7 +9,10 @@ using namespace RAPTOR; -int main() { +BOOST_AUTO_TEST_SUITE(InverseDynamicsTestSuite) + // Test without fixed joints +BOOST_AUTO_TEST_CASE(test_inverse_dynamics_without_fixed_joints) +{ // define robot model const std::string urdf_filename = "../Robots/kinova-gen3/kinova_grasp.urdf"; @@ -28,33 +34,38 @@ int main() { Eigen::VectorXd z = Eigen::VectorXd::Random(trajPtr->varLength); trajPtr->compute(z); -// Test without fixed joints - // compute inverse dynamics using pinocchio - 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; // compute inverse dynamics using RAPTOR std::shared_ptr cidPtr = std::make_shared( model, trajPtr); - - 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; - // compare the results - std::cout << "Pinocchio: " << data.tau.transpose() << std::endl; - std::cout << "RAPTOR: " << cidPtr->tau(0).transpose() << std::endl; + //check the error + 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 +74,25 @@ 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(); 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; - // 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(); 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; - - // 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 + //check the error + 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..df447c84 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,24 @@ class FKGradientChecker : public Optimizer { const double yaw_weight = 1.0; }; -int main() { +// extract words from out file +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,13 +204,12 @@ 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(); @@ -209,15 +228,21 @@ int main() { 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!"); } + // check the grad + bool gradient_check_passed = check_gradient_output("ipopt.out", "No errors detected by derivative checker"); + BOOST_CHECK_MESSAGE(gradient_check_passed, "Derivative_test not pass"); + + // check the nlp + BOOST_CHECK(status == 0 || status == 1); //success or feasible +} - return 0; -} \ No newline at end of file +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file diff --git a/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp b/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp index d22121b7..e6cb93dd 100644 --- a/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp +++ b/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp @@ -1,9 +1,13 @@ +#define BOOST_TEST_MODULE ForwardKinematicsTest +#include #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"; @@ -18,26 +22,18 @@ int main() { Eigen::VectorXd q = 2 * M_PI * Eigen::VectorXd::Random(model.nq).array() - M_PI; // 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; // set the start and end joint int start = 0; int end = model.getJointId("left_toe_B"); - - // 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; + fkSolver.compute(start, end, q);; // 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(); - return 0; -} \ No newline at end of file + // check the error + BOOST_CHECK_SMALL((pinocchio_translation - raptor_translation).norm(), 1e-10); +} +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..c5c1edd1 100644 --- a/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp @@ -1,14 +1,17 @@ +#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"; pinocchio::Model model; @@ -35,25 +38,13 @@ 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(); 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; // Compute inverse dynamics using pinocchio::rnea trajPtr->compute(z, false); - 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; - - // Compare the results - std::cout << "RegressorInverseDynamics result:" << std::endl; - std::cout << regressor_id.tau(0).transpose() << std::endl; - std::cout << "Pinocchio RNEA result:" << std::endl; - std::cout << tau_pinocchio.transpose() << std::endl; - - return 0; -} \ No newline at end of file + + // compare the results + BOOST_CHECK_SMALL((regressor_id.tau(0) -tau_pinocchio).norm(), 1e-10); +} +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file