From 7171003fe885ec87103cab34df56025c5dac70ab Mon Sep 17 00:00:00 2001 From: Zichang Zhou Date: Thu, 12 Sep 2024 22:50:21 +0000 Subject: [PATCH 1/8] optimal condition number on the effector --- Examples/Kinova/CMakeLists.txt | 24 ++ .../KinovaRegressorExampleEndeffector.cpp | 168 ++++++++++++ .../EndeffectorConditionNumberOptimizer.h | 105 ++++++++ .../EndEffectorConditionalNumberOptimizer.cpp | 239 ++++++++++++++++++ 4 files changed, 536 insertions(+) create mode 100644 Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp create mode 100644 Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h create mode 100644 Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp diff --git a/Examples/Kinova/CMakeLists.txt b/Examples/Kinova/CMakeLists.txt index 99812885..568be783 100644 --- a/Examples/Kinova/CMakeLists.txt +++ b/Examples/Kinova/CMakeLists.txt @@ -6,6 +6,7 @@ add_library(Kinovalib SHARED WaitrDiscrete/src/KinovaWaitrOptimizer.cpp SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp SystemIdentification/ExcitingTrajectories/src/ConditionNumberOptimizer.cpp + SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp ) target_include_directories(Kinovalib PUBLIC @@ -159,6 +160,29 @@ target_compile_options(KinovaExciting_example PUBLIC ${PINOCCHIO_FLAGS}) +add_executable(KinovaExciting_example_Endeffector + SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp) + +target_include_directories(KinovaExciting_example_Endeffector PUBLIC + ystemIdentification/DataFilter/include + SystemIdentification/ExcitingTrajectories/include + SystemIdentification/IterativeSystemIdentification/include) + +target_link_libraries(KinovaExciting_example_Endeffector PUBLIC + trajlib + IDlib + Conslib + Optlib + Kinovalib + ipopt + coinhsl + ${BOOST_LIBRARIES} + pinocchio::pinocchio) + +target_compile_options(KinovaExciting_example_Endeffector PUBLIC + ${PINOCCHIO_FLAGS}) + + ### Python bindings nanobind_add_module(KinovaHLP_nanobind NB_SHARED LTO diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp new file mode 100644 index 00000000..2594874e --- /dev/null +++ b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp @@ -0,0 +1,168 @@ +#include "EndeffectorConditionNumberOptimizer.h" + +using namespace RAPTOR; +using namespace Kinova; +using namespace Ipopt; + +int main(int argc, char* argv[]) { + const std::string regroupMatrixFileName = "../Examples/Kinova/SystemIdentification/RegroupMatrix.csv"; + // const std::string regroupMatrixFileName = "../Examples/Kinova/SystemIdentification/RegroupMatrix_withoutmotordynamics.csv"; + + // Define robot model + const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; + + pinocchio::Model model; + pinocchio::urdf::buildModel(urdf_filename, model); + + model.gravity.linear()(2) = GRAVITY; + model.friction.setZero(); + // model.damping.setZero(); + // model.rotorInertia.setZero(); + + // Define trajectory parameters + const double T = 10.0; + const int N = 50; + const int degree = 5; + const double base_frequency = 2.0 * M_PI / T; + + // Define initial guess + std::srand(static_cast(time(0))); + Eigen::VectorXd z = 2 * 0.2 * Eigen::VectorXd::Random((2 * degree + 3) * model.nv).array() - 0.1; + z.segment((2 * degree + 1) * model.nv, model.nv) = + 2 * 1.0 * Eigen::VectorXd::Random(model.nv).array() - 1.0; + z.segment((2 * degree + 1) * model.nv + model.nv, model.nv) = + 2 * 0.5 * Eigen::VectorXd::Random(model.nv).array() - 0.5; + + // Define limits buffer + Eigen::VectorXd joint_limits_buffer(model.nq); + joint_limits_buffer.setConstant(0.02); + Eigen::VectorXd velocity_limits_buffer(model.nq); + velocity_limits_buffer.setConstant(0.05); + Eigen::VectorXd torque_limits_buffer(model.nq); + torque_limits_buffer.setConstant(0.5); + + // Initialize Kinova optimizer + SmartPtr mynlp = new EndeffectorConditionNumberOptimizer(); + try { + mynlp->set_parameters(z, + T, + N, + degree, + base_frequency, + model, + // regroupMatrixFileName, + joint_limits_buffer, + velocity_limits_buffer, + torque_limits_buffer); + mynlp->constr_viol_tol = 1e-5; + } + catch (std::exception& e) { + std::cerr << e.what() << std::endl; + throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); + } + + SmartPtr app = IpoptApplicationFactory(); + + app->Options()->SetNumericValue("tol", 1e-6); + app->Options()->SetNumericValue("constr_viol_tol", mynlp->constr_viol_tol); + app->Options()->SetNumericValue("max_wall_time", 60.0); + app->Options()->SetIntegerValue("print_level", 5); + app->Options()->SetStringValue("mu_strategy", "adaptive"); + app->Options()->SetStringValue("linear_solver", "ma57"); + app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); + if (mynlp->enable_hessian) { + app->Options()->SetStringValue("hessian_approximation", "exact"); + } + else { + app->Options()->SetStringValue("hessian_approximation", "limited-memory"); + } + + // For gradient checking + // app->Options()->SetStringValue("output_file", "ipopt.out"); + // app->Options()->SetStringValue("derivative_test", "first-order"); + // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); + // app->Options()->SetNumericValue("derivative_test_tol", 1e-3); + + // 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!"); + } + + // Run ipopt to solve the optimization problem + double solve_time = 0; + try { + auto start = std::chrono::high_resolution_clock::now(); + + // Ask Ipopt to solve the problem + status = app->OptimizeTNLP(mynlp); + + auto end = std::chrono::high_resolution_clock::now(); + solve_time = std::chrono::duration_cast(end - start).count(); + std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; + + const Eigen::VectorXd initial_position_part = mynlp->solution.segment((2 * degree + 1) * model.nv, model.nv); + mynlp->solution.segment((2 * degree + 1) * model.nv, model.nv) = + Utils::wrapToPi(initial_position_part); + } + catch (std::exception& e) { + std::cerr << e.what() << std::endl; + throw std::runtime_error("Error solving optimization problem! Check previous error message!"); + } + + // Re-evaluate the solution at a higher resolution and print the results. + if (mynlp->ifFeasible) { + std::shared_ptr traj = std::make_shared(T, + 2000, + model.nv, + TimeDiscretization::Uniform, + degree, + base_frequency); + traj->compute(mynlp->solution, false); + + if (argc > 1) { + const std::string outputfolder = "../Examples/Kinova/SystemIdentification/ExcitingTrajectories/data/T10_d5_slower/"; + std::ofstream solution(outputfolder + "exciting-solution-" + std::string(argv[1]) + ".csv"); + std::ofstream position(outputfolder + "exciting-position-" + std::string(argv[1]) + ".csv"); + std::ofstream velocity(outputfolder + "exciting-velocity-" + std::string(argv[1]) + ".csv"); + std::ofstream acceleration(outputfolder + "exciting-acceleration-" + std::string(argv[1]) + ".csv"); + + solution << std::setprecision(16); + position << std::setprecision(16); + velocity << std::setprecision(16); + acceleration << std::setprecision(16); + + for (int i = 0; i < traj->N; i++) { + position << traj->q(i).transpose() << std::endl; + velocity << traj->q_d(i).transpose() << std::endl; + acceleration << traj->q_dd(i).transpose() << std::endl; + } + + for (int i = 0; i < mynlp->solution.size(); i++) { + solution << mynlp->solution(i) << std::endl; + } + solution << base_frequency << std::endl; + } + else { + std::ofstream solution("exciting-solution.csv"); + std::ofstream position("exciting-position.csv"); + std::ofstream velocity("exciting-velocity.csv"); + std::ofstream acceleration("exciting-acceleration.csv"); + + for (int i = 0; i < traj->N; i++) { + position << traj->q(i).transpose() << std::endl; + velocity << traj->q_d(i).transpose() << std::endl; + acceleration << traj->q_dd(i).transpose() << std::endl; + } + + for (int i = 0; i < mynlp->solution.size(); i++) { + solution << mynlp->solution(i) << std::endl; + } + } + } + + + + return 0; +} \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h new file mode 100644 index 00000000..5204a9aa --- /dev/null +++ b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h @@ -0,0 +1,105 @@ +#ifndef CONDITION_NUMBER_OPTIMIZER_H +#define CONDITION_NUMBER_OPTIMIZER_H + +#include "KinovaConstants.h" + +#include "Optimizer.h" + +#include "RegressorInverseDynamics.h" +#include "FixedFrequencyFourierCurves.h" + +#include "JointLimits.h" +#include "VelocityLimits.h" +#include "TorqueLimits.h" +#include "KinovaCustomizedConstraints.h" + +namespace RAPTOR { +namespace Kinova { + +class EndeffectorConditionNumberOptimizer : public Optimizer { +public: + using Model = pinocchio::Model; + using Vec3 = Eigen::Vector3d; + using VecX = Eigen::VectorXd; + using MatX = Eigen::MatrixXd; + + /** Default constructor */ + EndeffectorConditionNumberOptimizer() = default; + + /** Default destructor */ + ~EndeffectorConditionNumberOptimizer() = default; + + // [set_parameters] + bool set_parameters( + const VecX& x0_input, + const double T_input, + const int N_input, + const int degree_input, + const double base_frequency_input, + const Model& model_input, + // const std::string& regroupMatrixFileName, + const VecX& joint_limits_buffer_input, + const VecX& velocity_limits_buffer_input, + const VecX& torque_limits_buffer_input, + Eigen::VectorXi jtype_input = Eigen::VectorXi(0) + ); + + /**@name Overloaded from TNLP */ + //@{ + /** Method to return some info about the NLP */ + bool get_nlp_info( + Index& n, + Index& m, + Index& nnz_jac_g, + Index& nnz_h_lag, + IndexStyleEnum& index_style + ) final override; + + /** Method to return the objective value */ + bool eval_f( + Index n, + const Number* x, + bool new_x, + Number& obj_value + ) final override; + + /** Method to return the gradient of the objective */ + bool eval_grad_f( + Index n, + const Number* x, + bool new_x, + Number* grad_f + ) final override; + + /**@name Methods to block default compiler methods. + * + * The compiler automatically generates the following three methods. + * Since the default compiler implementation is generally not what + * you want (for all but the most simple classes), we usually + * put the declarations of these methods in the private section + * and never implement them. This prevents the compiler from + * implementing an incorrect "default" behavior without us + * knowing. (See Scott Meyers book, "Effective C++") + */ + //@{ + EndeffectorConditionNumberOptimizer( + const EndeffectorConditionNumberOptimizer& + ); + + EndeffectorConditionNumberOptimizer& operator=( + const EndeffectorConditionNumberOptimizer& + ); + + // MatX regroupMatrix; + + std::shared_ptr trajPtr_; + + std::shared_ptr ridPtr_; + + int joint_num; +}; + +}; // namespace Kinova +}; // namespace RAPTOR + +#endif // CONDITION_NUMBER_OPTIMIZER_H \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp new file mode 100644 index 00000000..8ed07196 --- /dev/null +++ b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp @@ -0,0 +1,239 @@ +#include "EndeffectorConditionNumberOptimizer.h" + + + +namespace RAPTOR { +namespace Kinova { + +// // constructor +// EndeffectorConditionNumberOptimizer::EndeffectorConditionNumberOptimizer() +// { +// } + + +// // destructor +// EndeffectorConditionNumberOptimizer::~EndeffectorConditionNumberOptimizer() +// { +// } + +bool EndeffectorConditionNumberOptimizer::set_parameters( + const VecX& x0_input, + const Number T_input, + const int N_input, + const int degree_input, + const double base_frequency_input, + const Model& model_input, + // const std::string& regroupMatrixFileName, + const VecX& joint_limits_buffer_input, + const VecX& velocity_limits_buffer_input, + const VecX& torque_limits_buffer_input, + Eigen::VectorXi jtype_input +) { + enable_hessian = false; + x0 = x0_input; + joint_num =model_input.nv; + + // fixed frequency fourier curves with 0 initial velocity + trajPtr_ = std::make_shared(T_input, + N_input, + model_input.nv, + TimeDiscretization::Uniform, + degree_input, + base_frequency_input); + + ridPtr_ = std::make_shared(model_input, + trajPtr_, + jtype_input); + + // read joint limits from KinovaConstants.h + VecX JOINT_LIMITS_LOWER_VEC = + Utils::deg2rad( + Utils::initializeEigenVectorFromArray(JOINT_LIMITS_LOWER, NUM_JOINTS)) + + joint_limits_buffer_input; + + VecX JOINT_LIMITS_UPPER_VEC = + Utils::deg2rad( + Utils::initializeEigenVectorFromArray(JOINT_LIMITS_UPPER, NUM_JOINTS)) - + joint_limits_buffer_input; + + // read velocity limits from KinovaConstants.h + VecX VELOCITY_LIMITS_LOWER_VEC = + Utils::deg2rad( + Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_LOWER, NUM_JOINTS)) + + velocity_limits_buffer_input; + + VecX VELOCITY_LIMITS_UPPER_VEC = + Utils::deg2rad( + Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_UPPER, NUM_JOINTS)) - + velocity_limits_buffer_input; + + // read torque limits from KinovaConstants.h + VecX TORQUE_LIMITS_LOWER_VEC = + Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_LOWER, NUM_JOINTS) + + torque_limits_buffer_input; + + VecX TORQUE_LIMITS_UPPER_VEC = + Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_UPPER, NUM_JOINTS) - + torque_limits_buffer_input; + + // Joint limits + constraintsPtrVec_.push_back(std::make_unique(trajPtr_, + JOINT_LIMITS_LOWER_VEC, + JOINT_LIMITS_UPPER_VEC)); + constraintsNameVec_.push_back("joint limits"); + + // Velocity limits + constraintsPtrVec_.push_back(std::make_unique(trajPtr_, + VELOCITY_LIMITS_LOWER_VEC, + VELOCITY_LIMITS_UPPER_VEC)); + constraintsNameVec_.push_back("velocity limits"); + + // Torque limits + constraintsPtrVec_.push_back(std::make_unique(trajPtr_, + ridPtr_, + TORQUE_LIMITS_LOWER_VEC, + TORQUE_LIMITS_UPPER_VEC)); + constraintsNameVec_.push_back("torque limits"); + + // // Customized constraints (collision avoidance with ground) + std::vector groundCenter = {Vec3(0.0, 0.0, 0.04)}; + std::vector groundOrientation = {Vec3(0.0, 0.0, 0.0)}; + std::vector groundSize = {Vec3(5.0, 5.0, 0.01)}; + constraintsPtrVec_.push_back(std::make_unique(trajPtr_, + model_input, + groundCenter, + groundOrientation, + groundSize, + 0.0, + jtype_input)); + constraintsNameVec_.push_back("obstacle avoidance constraints"); + + // // check dimensions of regroupMatrix + // if (ridPtr_->Y.cols() != regroupMatrix.rows()) { + // throw std::invalid_argument("EndeffectorConditionNumberOptimizer: regroupMatrix has wrong dimensions!"); + // } + + return true; +} + +bool EndeffectorConditionNumberOptimizer::get_nlp_info( + Index& n, + Index& m, + Index& nnz_jac_g, + Index& nnz_h_lag, + IndexStyleEnum& index_style +) { + // number of decision variables + numVars = trajPtr_->varLength; + n = numVars; + + // number of inequality constraint + numCons = 0; + for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { + numCons += constraintsPtrVec_[i]->m; + } + m = numCons; + + nnz_jac_g = n * m; + nnz_h_lag = n * (n + 1) / 2; + + // use the C style indexing (0-based) + index_style = TNLP::C_STYLE; + + return true; +} + +bool EndeffectorConditionNumberOptimizer::eval_f( + Index n, + const Number* x, + bool new_x, + Number& obj_value +) { + if(n != numVars){ + THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); + } + + VecX z = Utils::initializeEigenVectorFromArray(x, n); + + ridPtr_->compute(z, false); + + MatX ObservationMatrix = ridPtr_->Y; + int numParams = ridPtr_->phi.size(); + + // last numparams cols and row every numParams/joint_num rows + MatX lastCols = ObservationMatrix.rightCols(numParams/joint_num); + MatX EndeffectorY = lastCols(Eigen::seq(0, Eigen::last, joint_num-1), Eigen::all); + + Eigen::JacobiSVD svd(EndeffectorY, + Eigen::ComputeThinU | Eigen::ComputeThinV); + const VecX& singularValues = svd.singularValues(); + const MatX& U = svd.matrixU(); + const MatX& V = svd.matrixV(); + + Number sigmaMax = singularValues(0); + Number sigmaMin = singularValues(singularValues.size() - 1); + + // log of 2-norm condition number (sigmaMax / sigmaMin) + obj_value = std::log(sigmaMax) - std::log(sigmaMin); + + update_minimal_cost_solution(n, z, new_x, obj_value); + + return true; +} + +bool EndeffectorConditionNumberOptimizer::eval_grad_f( + Index n, + const Number* x, + bool new_x, + Number* grad_f +) { + if(n != numVars){ + THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); + } + + VecX z = Utils::initializeEigenVectorFromArray(x, n); + + ridPtr_->compute(z, true); + + + MatX ObservationMatrix = ridPtr_->Y; + int numParams = ridPtr_->phi.size(); + std::cout << numParams << "parems" < svd(EndeffectorY, + Eigen::ComputeThinU | Eigen::ComputeThinV); + + const VecX& singularValues = svd.singularValues(); + const MatX& U = svd.matrixU(); + const MatX& V = svd.matrixV(); + + Index lastRow = singularValues.size() - 1; + Number sigmaMax = singularValues(0); + Number sigmaMin = singularValues(lastRow); + + // refer to https://j-towns.github.io/papers/svd-derivative.pdf + // for analytical gradient of singular values + for (Index i = 0; i < n; i++) { + + MatX gradObservationMatrix = ridPtr_->pY_pz(i); + + MatX lastCols = gradObservationMatrix.rightCols(numParams/joint_num); + MatX gradEndeffectorY = lastCols(Eigen::seq(0, Eigen::last, joint_num -1), Eigen::all); + + + Number gradSigmaMax = U.col(0).transpose() * gradEndeffectorY * V.col(0); + Number gradSigmaMin = U.col(lastRow).transpose() * gradEndeffectorY * V.col(lastRow); + + grad_f[i] = gradSigmaMax / sigmaMax - gradSigmaMin / sigmaMin; + } + + return true; +} + +}; // namespace Kinova +}; // namespace RAPTOR \ No newline at end of file From 2686a0ba339634fd45c6676cc6886c29c95e7ca7 Mon Sep 17 00:00:00 2001 From: Zichang Zhou Date: Sat, 14 Sep 2024 20:21:50 +0000 Subject: [PATCH 2/8] using boost on the /test folder, change the test cmakelist. and add ctest. above passed test but not clear the code. the git action workflow not test --- .devcontainer/devcontainer.json | 2 +- .github/workflows/build-and-test.yml | 362 ++++++++++++++++++ CMakeLists.txt | 12 + Tests/CMakeLists.txt | 170 +++++--- .../Constraints/TestKinematicsConstraints.cpp | 98 ++++- .../TestCustomizedInverseDynamics.cpp | 80 +++- .../TestForwardKinematicsRPYDerivatives.cpp | 51 ++- .../TestForwardKinematicsSolver.cpp | 41 +- .../TestRegressorInverseDynamics.cpp | 33 +- 9 files changed, 721 insertions(+), 128 deletions(-) create mode 100644 .github/workflows/build-and-test.yml 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 From 94fb7d1b99657e4ce641f3435ebc0bf29135ac32 Mon Sep 17 00:00:00 2001 From: Zichang Zhou Date: Sat, 14 Sep 2024 20:37:53 +0000 Subject: [PATCH 3/8] test workflow --- .github/workflows/build-and-test.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build-and-test.yml b/.github/workflows/build-and-test.yml index d3e29482..ee7be1e9 100644 --- a/.github/workflows/build-and-test.yml +++ b/.github/workflows/build-and-test.yml @@ -110,11 +110,11 @@ 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] + # 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 }} From 4d0bf06533607c8d81d0687fa43cd27c77e83f44 Mon Sep 17 00:00:00 2001 From: Zichang Zhou Date: Sun, 15 Sep 2024 01:33:39 +0000 Subject: [PATCH 4/8] final one, waiting check --- .github/workflows/build-and-test.yml | 362 ------------------ Tests/CMakeLists.txt | 95 ----- .../Constraints/TestKinematicsConstraints.cpp | 16 +- .../TestCustomizedInverseDynamics.cpp | 24 +- .../TestForwardKinematicsSolver.cpp | 4 +- .../TestRegressorInverseDynamics.cpp | 13 +- 6 files changed, 27 insertions(+), 487 deletions(-) delete mode 100644 .github/workflows/build-and-test.yml diff --git a/.github/workflows/build-and-test.yml b/.github/workflows/build-and-test.yml deleted file mode 100644 index ee7be1e9..00000000 --- a/.github/workflows/build-and-test.yml +++ /dev/null @@ -1,362 +0,0 @@ -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/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index 411d66af..e9d40055 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -1,98 +1,3 @@ - - -# 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) - -# target_compile_options(ForwardKinematics_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(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_test(NAME ForwardKinematicsGradient_test COMMAND ForwardKinematicsGradient_test) -# set_tests_properties(ForwardKinematicsGradient_test PROPERTIES WORKING_DIRECTORY /workspaces/RAPTOR/build) - - -# add_executable(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_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 diff --git a/Tests/Constraints/TestKinematicsConstraints.cpp b/Tests/Constraints/TestKinematicsConstraints.cpp index 0ebfb379..806ce127 100644 --- a/Tests/Constraints/TestKinematicsConstraints.cpp +++ b/Tests/Constraints/TestKinematicsConstraints.cpp @@ -65,7 +65,7 @@ BOOST_AUTO_TEST_CASE(owngradientTest) // 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; + // std::cout << J_analytical - J_numerical << std::endl << std::endl; BOOST_CHECK_SMALL((J_analytical - J_numerical).norm(), 1e-10); } @@ -135,19 +135,7 @@ BOOST_AUTO_TEST_CASE(owngradientTest) } - // 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()); diff --git a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp index b9fb34e1..706bdd92 100644 --- a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp @@ -63,8 +63,8 @@ BOOST_AUTO_TEST_CASE(test_inverse_dynamics_without_fixed_joints) // 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; + // 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); } @@ -110,22 +110,28 @@ BOOST_AUTO_TEST_CASE(test_inverse_dynamics_with_fixed_joints) trajPtr->q_d(0).head(model_reduced.nv), trajPtr->q_dd(0).head(model_reduced.nv)); } + // compute inverse dynamics using RAPTOR + std::shared_ptr cidPtr = std::make_shared( + model, trajPtr, jtype); + cidPtr->compute(z, true); + + // 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 - std::shared_ptr cidPtr = std::make_shared( - model, trajPtr, jtype); - // auto start = std::chrono::high_resolution_clock::now(); - cidPtr->compute(z, true); + // 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; + // // 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; BOOST_CHECK_SMALL((data_reduced.tau - cidPtr->tau(0)).norm(), 1e-10); diff --git a/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp b/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp index 1eff4646..13ef9f30 100644 --- a/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp +++ b/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp @@ -45,8 +45,8 @@ BOOST_AUTO_TEST_CASE(TestForwardKinematicsAccuracy) 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; + // 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); } diff --git a/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp b/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp index cc584854..66a4a146 100644 --- a/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp @@ -47,17 +47,20 @@ BOOST_AUTO_TEST_CASE(RegressorInverseDynamicsAccuracy) // 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)); + + // 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; + // 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; BOOST_CHECK_SMALL((regressor_id.tau(0) -tau_pinocchio).norm(), 1e-10); From 60d9f7c3232ec5a6dd346fc4abca4e41d3cb1d73 Mon Sep 17 00:00:00 2001 From: Zichang Zhou Date: Sun, 15 Sep 2024 01:59:08 +0000 Subject: [PATCH 5/8] waiting for check --- .../Constraints/TestKinematicsConstraints.cpp | 40 ++++++------------- .../TestCustomizedInverseDynamics.cpp | 38 ++---------------- .../TestForwardKinematicsRPYDerivatives.cpp | 11 +++-- .../TestForwardKinematicsSolver.cpp | 20 ++-------- .../TestRegressorInverseDynamics.cpp | 18 +-------- 5 files changed, 28 insertions(+), 99 deletions(-) diff --git a/Tests/Constraints/TestKinematicsConstraints.cpp b/Tests/Constraints/TestKinematicsConstraints.cpp index 806ce127..e1718954 100644 --- a/Tests/Constraints/TestKinematicsConstraints.cpp +++ b/Tests/Constraints/TestKinematicsConstraints.cpp @@ -10,6 +10,8 @@ using namespace RAPTOR; BOOST_AUTO_TEST_SUITE(KinematicsConstraintsTest) + +// test gradient BOOST_AUTO_TEST_CASE(owngradientTest) { // Define robot model @@ -19,7 +21,6 @@ BOOST_AUTO_TEST_CASE(owngradientTest) 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); @@ -34,21 +35,10 @@ BOOST_AUTO_TEST_CASE(owngradientTest) // 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; - - // 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++) { @@ -63,16 +53,12 @@ BOOST_AUTO_TEST_CASE(owngradientTest) 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-10); - } +} - // test hessian - - BOOST_AUTO_TEST_CASE(ownHessianTest){ +// test hessian +BOOST_AUTO_TEST_CASE(ownHessianTest){ const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; pinocchio::Model model; @@ -90,10 +76,6 @@ BOOST_AUTO_TEST_CASE(owngradientTest) 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()); @@ -101,7 +83,13 @@ BOOST_AUTO_TEST_CASE(owngradientTest) Eigen::VectorXd z_test = z.array() + 1e-6; Eigen::Array H_analytical = kc.pg_pz_pz; + // simple test when difference is large + // z_test = z.array() + 1.0; + // params for error checking + bool hasError = false; + double max_diff = 0.0; + std::stringstream error_message; for (int i = 0; i < z_test.size(); i++) { Eigen::VectorXd q_plus = z_test; @@ -116,12 +104,10 @@ BOOST_AUTO_TEST_CASE(owngradientTest) // check error for (int j = 0; j < 3; j++) { - // 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 + //record the data, check when loop end double diff = (H_analytical(j).row(i) - H_numerical_row.row(j) ).norm(); if (diff >1e-10){ hasError = true; diff --git a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp index 706bdd92..d526879e 100644 --- a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp @@ -11,6 +11,8 @@ using namespace RAPTOR; BOOST_AUTO_TEST_SUITE(InverseDynamicsTestSuite) + + // Test without fixed joints BOOST_AUTO_TEST_CASE(test_inverse_dynamics_without_fixed_joints) { // define robot model @@ -40,31 +42,18 @@ BOOST_AUTO_TEST_CASE(test_inverse_dynamics_without_fixed_joints) 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); } @@ -103,7 +92,6 @@ BOOST_AUTO_TEST_CASE(test_inverse_dynamics_with_fixed_joints) 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), @@ -115,25 +103,7 @@ BOOST_AUTO_TEST_CASE(test_inverse_dynamics_with_fixed_joints) model, trajPtr, jtype); cidPtr->compute(z, true); - - // 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 - // 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; - - + //check the error BOOST_CHECK_SMALL((data_reduced.tau - cidPtr->tau(0)).norm(), 1e-10); diff --git a/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp b/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp index 73cf3bcc..ea1537de 100644 --- a/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp +++ b/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp @@ -176,6 +176,7 @@ class FKGradientChecker : public Optimizer { const double yaw_weight = 1.0; }; +// 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()) { @@ -241,13 +242,15 @@ BOOST_AUTO_TEST_CASE(test_FKGradientChecker){ catch (std::exception& e) { BOOST_FAIL("Error solving optimization problem! Check previous error message!"); } - - std::cout <<"status" <(stop_clock - start_clock); - // std::cout << "Pinocchio FK: " << duration.count() << " nanoseconds" << std::endl; + pinocchio::forwardKinematics(model, data, q); // 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; + fkSolver.compute(start, end, q);; // compare the results 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; - + // check the error BOOST_CHECK_SMALL((pinocchio_translation - raptor_translation).norm(), 1e-10); } diff --git a/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp b/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp index 66a4a146..b9f05802 100644 --- a/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp @@ -39,29 +39,13 @@ BOOST_AUTO_TEST_CASE(RegressorInverseDynamicsAccuracy) 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); VecX tau_pinocchio = pinocchio::rnea(model, data, trajPtr->q(0), trajPtr->q_d(0), trajPtr->q_dd(0)); - // 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; - + // compare the results BOOST_CHECK_SMALL((regressor_id.tau(0) -tau_pinocchio).norm(), 1e-10); From 0ba066c07502ad04f917656f4c6ba7029206a0fa Mon Sep 17 00:00:00 2001 From: Zichang Zhou Date: Mon, 16 Sep 2024 21:36:10 +0000 Subject: [PATCH 6/8] resolve commented issue --- .devcontainer/devcontainer.json | 2 +- Examples/Kinova/CMakeLists.txt | 23 -- .../KinovaRegressorExampleEndeffector.cpp | 168 ------------ .../EndeffectorConditionNumberOptimizer.h | 105 -------- .../EndEffectorConditionalNumberOptimizer.cpp | 239 ------------------ .../TestCustomizedInverseDynamics.cpp | 5 - .../TestForwardKinematicsRPYDerivatives.cpp | 11 +- .../TestForwardKinematicsSolver.cpp | 1 - .../TestRegressorInverseDynamics.cpp | 5 - 9 files changed, 2 insertions(+), 557 deletions(-) delete mode 100644 Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp delete mode 100644 Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h delete mode 100644 Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 9e4c291c..cf800c23 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-zichang", + "name": "RAPTOR", "build": { // Sets the run context to one level up instead of the .devcontainer folder. "context": "..", diff --git a/Examples/Kinova/CMakeLists.txt b/Examples/Kinova/CMakeLists.txt index 568be783..a1567281 100644 --- a/Examples/Kinova/CMakeLists.txt +++ b/Examples/Kinova/CMakeLists.txt @@ -160,29 +160,6 @@ target_compile_options(KinovaExciting_example PUBLIC ${PINOCCHIO_FLAGS}) -add_executable(KinovaExciting_example_Endeffector - SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp) - -target_include_directories(KinovaExciting_example_Endeffector PUBLIC - ystemIdentification/DataFilter/include - SystemIdentification/ExcitingTrajectories/include - SystemIdentification/IterativeSystemIdentification/include) - -target_link_libraries(KinovaExciting_example_Endeffector PUBLIC - trajlib - IDlib - Conslib - Optlib - Kinovalib - ipopt - coinhsl - ${BOOST_LIBRARIES} - pinocchio::pinocchio) - -target_compile_options(KinovaExciting_example_Endeffector PUBLIC - ${PINOCCHIO_FLAGS}) - - ### Python bindings nanobind_add_module(KinovaHLP_nanobind NB_SHARED LTO diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp deleted file mode 100644 index 2594874e..00000000 --- a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp +++ /dev/null @@ -1,168 +0,0 @@ -#include "EndeffectorConditionNumberOptimizer.h" - -using namespace RAPTOR; -using namespace Kinova; -using namespace Ipopt; - -int main(int argc, char* argv[]) { - const std::string regroupMatrixFileName = "../Examples/Kinova/SystemIdentification/RegroupMatrix.csv"; - // const std::string regroupMatrixFileName = "../Examples/Kinova/SystemIdentification/RegroupMatrix_withoutmotordynamics.csv"; - - // Define robot model - const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; - - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - - model.gravity.linear()(2) = GRAVITY; - model.friction.setZero(); - // model.damping.setZero(); - // model.rotorInertia.setZero(); - - // Define trajectory parameters - const double T = 10.0; - const int N = 50; - const int degree = 5; - const double base_frequency = 2.0 * M_PI / T; - - // Define initial guess - std::srand(static_cast(time(0))); - Eigen::VectorXd z = 2 * 0.2 * Eigen::VectorXd::Random((2 * degree + 3) * model.nv).array() - 0.1; - z.segment((2 * degree + 1) * model.nv, model.nv) = - 2 * 1.0 * Eigen::VectorXd::Random(model.nv).array() - 1.0; - z.segment((2 * degree + 1) * model.nv + model.nv, model.nv) = - 2 * 0.5 * Eigen::VectorXd::Random(model.nv).array() - 0.5; - - // Define limits buffer - Eigen::VectorXd joint_limits_buffer(model.nq); - joint_limits_buffer.setConstant(0.02); - Eigen::VectorXd velocity_limits_buffer(model.nq); - velocity_limits_buffer.setConstant(0.05); - Eigen::VectorXd torque_limits_buffer(model.nq); - torque_limits_buffer.setConstant(0.5); - - // Initialize Kinova optimizer - SmartPtr mynlp = new EndeffectorConditionNumberOptimizer(); - try { - mynlp->set_parameters(z, - T, - N, - degree, - base_frequency, - model, - // regroupMatrixFileName, - joint_limits_buffer, - velocity_limits_buffer, - torque_limits_buffer); - mynlp->constr_viol_tol = 1e-5; - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); - } - - SmartPtr app = IpoptApplicationFactory(); - - app->Options()->SetNumericValue("tol", 1e-6); - app->Options()->SetNumericValue("constr_viol_tol", mynlp->constr_viol_tol); - app->Options()->SetNumericValue("max_wall_time", 60.0); - app->Options()->SetIntegerValue("print_level", 5); - app->Options()->SetStringValue("mu_strategy", "adaptive"); - app->Options()->SetStringValue("linear_solver", "ma57"); - app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); - if (mynlp->enable_hessian) { - app->Options()->SetStringValue("hessian_approximation", "exact"); - } - else { - app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - } - - // For gradient checking - // app->Options()->SetStringValue("output_file", "ipopt.out"); - // app->Options()->SetStringValue("derivative_test", "first-order"); - // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); - // app->Options()->SetNumericValue("derivative_test_tol", 1e-3); - - // 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!"); - } - - // Run ipopt to solve the optimization problem - double solve_time = 0; - try { - auto start = std::chrono::high_resolution_clock::now(); - - // Ask Ipopt to solve the problem - status = app->OptimizeTNLP(mynlp); - - auto end = std::chrono::high_resolution_clock::now(); - solve_time = std::chrono::duration_cast(end - start).count(); - std::cout << "Total solve time: " << solve_time << " milliseconds.\n"; - - const Eigen::VectorXd initial_position_part = mynlp->solution.segment((2 * degree + 1) * model.nv, model.nv); - mynlp->solution.segment((2 * degree + 1) * model.nv, model.nv) = - Utils::wrapToPi(initial_position_part); - } - catch (std::exception& e) { - std::cerr << e.what() << std::endl; - throw std::runtime_error("Error solving optimization problem! Check previous error message!"); - } - - // Re-evaluate the solution at a higher resolution and print the results. - if (mynlp->ifFeasible) { - std::shared_ptr traj = std::make_shared(T, - 2000, - model.nv, - TimeDiscretization::Uniform, - degree, - base_frequency); - traj->compute(mynlp->solution, false); - - if (argc > 1) { - const std::string outputfolder = "../Examples/Kinova/SystemIdentification/ExcitingTrajectories/data/T10_d5_slower/"; - std::ofstream solution(outputfolder + "exciting-solution-" + std::string(argv[1]) + ".csv"); - std::ofstream position(outputfolder + "exciting-position-" + std::string(argv[1]) + ".csv"); - std::ofstream velocity(outputfolder + "exciting-velocity-" + std::string(argv[1]) + ".csv"); - std::ofstream acceleration(outputfolder + "exciting-acceleration-" + std::string(argv[1]) + ".csv"); - - solution << std::setprecision(16); - position << std::setprecision(16); - velocity << std::setprecision(16); - acceleration << std::setprecision(16); - - for (int i = 0; i < traj->N; i++) { - position << traj->q(i).transpose() << std::endl; - velocity << traj->q_d(i).transpose() << std::endl; - acceleration << traj->q_dd(i).transpose() << std::endl; - } - - for (int i = 0; i < mynlp->solution.size(); i++) { - solution << mynlp->solution(i) << std::endl; - } - solution << base_frequency << std::endl; - } - else { - std::ofstream solution("exciting-solution.csv"); - std::ofstream position("exciting-position.csv"); - std::ofstream velocity("exciting-velocity.csv"); - std::ofstream acceleration("exciting-acceleration.csv"); - - for (int i = 0; i < traj->N; i++) { - position << traj->q(i).transpose() << std::endl; - velocity << traj->q_d(i).transpose() << std::endl; - acceleration << traj->q_dd(i).transpose() << std::endl; - } - - for (int i = 0; i < mynlp->solution.size(); i++) { - solution << mynlp->solution(i) << std::endl; - } - } - } - - - - return 0; -} \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h deleted file mode 100644 index 5204a9aa..00000000 --- a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef CONDITION_NUMBER_OPTIMIZER_H -#define CONDITION_NUMBER_OPTIMIZER_H - -#include "KinovaConstants.h" - -#include "Optimizer.h" - -#include "RegressorInverseDynamics.h" -#include "FixedFrequencyFourierCurves.h" - -#include "JointLimits.h" -#include "VelocityLimits.h" -#include "TorqueLimits.h" -#include "KinovaCustomizedConstraints.h" - -namespace RAPTOR { -namespace Kinova { - -class EndeffectorConditionNumberOptimizer : public Optimizer { -public: - using Model = pinocchio::Model; - using Vec3 = Eigen::Vector3d; - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - /** Default constructor */ - EndeffectorConditionNumberOptimizer() = default; - - /** Default destructor */ - ~EndeffectorConditionNumberOptimizer() = default; - - // [set_parameters] - bool set_parameters( - const VecX& x0_input, - const double T_input, - const int N_input, - const int degree_input, - const double base_frequency_input, - const Model& model_input, - // const std::string& regroupMatrixFileName, - const VecX& joint_limits_buffer_input, - const VecX& velocity_limits_buffer_input, - const VecX& torque_limits_buffer_input, - Eigen::VectorXi jtype_input = Eigen::VectorXi(0) - ); - - /**@name Overloaded from TNLP */ - //@{ - /** Method to return some info about the NLP */ - bool get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style - ) final override; - - /** Method to return the objective value */ - bool eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value - ) final override; - - /** Method to return the gradient of the objective */ - bool eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f - ) final override; - - /**@name Methods to block default compiler methods. - * - * The compiler automatically generates the following three methods. - * Since the default compiler implementation is generally not what - * you want (for all but the most simple classes), we usually - * put the declarations of these methods in the private section - * and never implement them. This prevents the compiler from - * implementing an incorrect "default" behavior without us - * knowing. (See Scott Meyers book, "Effective C++") - */ - //@{ - EndeffectorConditionNumberOptimizer( - const EndeffectorConditionNumberOptimizer& - ); - - EndeffectorConditionNumberOptimizer& operator=( - const EndeffectorConditionNumberOptimizer& - ); - - // MatX regroupMatrix; - - std::shared_ptr trajPtr_; - - std::shared_ptr ridPtr_; - - int joint_num; -}; - -}; // namespace Kinova -}; // namespace RAPTOR - -#endif // CONDITION_NUMBER_OPTIMIZER_H \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp b/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp deleted file mode 100644 index 8ed07196..00000000 --- a/Examples/Kinova/SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp +++ /dev/null @@ -1,239 +0,0 @@ -#include "EndeffectorConditionNumberOptimizer.h" - - - -namespace RAPTOR { -namespace Kinova { - -// // constructor -// EndeffectorConditionNumberOptimizer::EndeffectorConditionNumberOptimizer() -// { -// } - - -// // destructor -// EndeffectorConditionNumberOptimizer::~EndeffectorConditionNumberOptimizer() -// { -// } - -bool EndeffectorConditionNumberOptimizer::set_parameters( - const VecX& x0_input, - const Number T_input, - const int N_input, - const int degree_input, - const double base_frequency_input, - const Model& model_input, - // const std::string& regroupMatrixFileName, - const VecX& joint_limits_buffer_input, - const VecX& velocity_limits_buffer_input, - const VecX& torque_limits_buffer_input, - Eigen::VectorXi jtype_input -) { - enable_hessian = false; - x0 = x0_input; - joint_num =model_input.nv; - - // fixed frequency fourier curves with 0 initial velocity - trajPtr_ = std::make_shared(T_input, - N_input, - model_input.nv, - TimeDiscretization::Uniform, - degree_input, - base_frequency_input); - - ridPtr_ = std::make_shared(model_input, - trajPtr_, - jtype_input); - - // read joint limits from KinovaConstants.h - VecX JOINT_LIMITS_LOWER_VEC = - Utils::deg2rad( - Utils::initializeEigenVectorFromArray(JOINT_LIMITS_LOWER, NUM_JOINTS)) + - joint_limits_buffer_input; - - VecX JOINT_LIMITS_UPPER_VEC = - Utils::deg2rad( - Utils::initializeEigenVectorFromArray(JOINT_LIMITS_UPPER, NUM_JOINTS)) - - joint_limits_buffer_input; - - // read velocity limits from KinovaConstants.h - VecX VELOCITY_LIMITS_LOWER_VEC = - Utils::deg2rad( - Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_LOWER, NUM_JOINTS)) + - velocity_limits_buffer_input; - - VecX VELOCITY_LIMITS_UPPER_VEC = - Utils::deg2rad( - Utils::initializeEigenVectorFromArray(VELOCITY_LIMITS_UPPER, NUM_JOINTS)) - - velocity_limits_buffer_input; - - // read torque limits from KinovaConstants.h - VecX TORQUE_LIMITS_LOWER_VEC = - Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_LOWER, NUM_JOINTS) + - torque_limits_buffer_input; - - VecX TORQUE_LIMITS_UPPER_VEC = - Utils::initializeEigenVectorFromArray(TORQUE_LIMITS_UPPER, NUM_JOINTS) - - torque_limits_buffer_input; - - // Joint limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - JOINT_LIMITS_LOWER_VEC, - JOINT_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("joint limits"); - - // Velocity limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - VELOCITY_LIMITS_LOWER_VEC, - VELOCITY_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("velocity limits"); - - // Torque limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - ridPtr_, - TORQUE_LIMITS_LOWER_VEC, - TORQUE_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("torque limits"); - - // // Customized constraints (collision avoidance with ground) - std::vector groundCenter = {Vec3(0.0, 0.0, 0.04)}; - std::vector groundOrientation = {Vec3(0.0, 0.0, 0.0)}; - std::vector groundSize = {Vec3(5.0, 5.0, 0.01)}; - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - model_input, - groundCenter, - groundOrientation, - groundSize, - 0.0, - jtype_input)); - constraintsNameVec_.push_back("obstacle avoidance constraints"); - - // // check dimensions of regroupMatrix - // if (ridPtr_->Y.cols() != regroupMatrix.rows()) { - // throw std::invalid_argument("EndeffectorConditionNumberOptimizer: regroupMatrix has wrong dimensions!"); - // } - - return true; -} - -bool EndeffectorConditionNumberOptimizer::get_nlp_info( - Index& n, - Index& m, - Index& nnz_jac_g, - Index& nnz_h_lag, - IndexStyleEnum& index_style -) { - // number of decision variables - numVars = trajPtr_->varLength; - n = numVars; - - // number of inequality constraint - numCons = 0; - for ( Index i = 0; i < constraintsPtrVec_.size(); i++ ) { - numCons += constraintsPtrVec_[i]->m; - } - m = numCons; - - nnz_jac_g = n * m; - nnz_h_lag = n * (n + 1) / 2; - - // use the C style indexing (0-based) - index_style = TNLP::C_STYLE; - - return true; -} - -bool EndeffectorConditionNumberOptimizer::eval_f( - Index n, - const Number* x, - bool new_x, - Number& obj_value -) { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - ridPtr_->compute(z, false); - - MatX ObservationMatrix = ridPtr_->Y; - int numParams = ridPtr_->phi.size(); - - // last numparams cols and row every numParams/joint_num rows - MatX lastCols = ObservationMatrix.rightCols(numParams/joint_num); - MatX EndeffectorY = lastCols(Eigen::seq(0, Eigen::last, joint_num-1), Eigen::all); - - Eigen::JacobiSVD svd(EndeffectorY, - Eigen::ComputeThinU | Eigen::ComputeThinV); - const VecX& singularValues = svd.singularValues(); - const MatX& U = svd.matrixU(); - const MatX& V = svd.matrixV(); - - Number sigmaMax = singularValues(0); - Number sigmaMin = singularValues(singularValues.size() - 1); - - // log of 2-norm condition number (sigmaMax / sigmaMin) - obj_value = std::log(sigmaMax) - std::log(sigmaMin); - - update_minimal_cost_solution(n, z, new_x, obj_value); - - return true; -} - -bool EndeffectorConditionNumberOptimizer::eval_grad_f( - Index n, - const Number* x, - bool new_x, - Number* grad_f -) { - if(n != numVars){ - THROW_EXCEPTION(IpoptException, "*** Error wrong value of n in eval_grad_f!"); - } - - VecX z = Utils::initializeEigenVectorFromArray(x, n); - - ridPtr_->compute(z, true); - - - MatX ObservationMatrix = ridPtr_->Y; - int numParams = ridPtr_->phi.size(); - std::cout << numParams << "parems" < svd(EndeffectorY, - Eigen::ComputeThinU | Eigen::ComputeThinV); - - const VecX& singularValues = svd.singularValues(); - const MatX& U = svd.matrixU(); - const MatX& V = svd.matrixV(); - - Index lastRow = singularValues.size() - 1; - Number sigmaMax = singularValues(0); - Number sigmaMin = singularValues(lastRow); - - // refer to https://j-towns.github.io/papers/svd-derivative.pdf - // for analytical gradient of singular values - for (Index i = 0; i < n; i++) { - - MatX gradObservationMatrix = ridPtr_->pY_pz(i); - - MatX lastCols = gradObservationMatrix.rightCols(numParams/joint_num); - MatX gradEndeffectorY = lastCols(Eigen::seq(0, Eigen::last, joint_num -1), Eigen::all); - - - Number gradSigmaMax = U.col(0).transpose() * gradEndeffectorY * V.col(0); - Number gradSigmaMin = U.col(lastRow).transpose() * gradEndeffectorY * V.col(lastRow); - - grad_f[i] = gradSigmaMax / sigmaMax - gradSigmaMin / sigmaMin; - } - - return true; -} - -}; // namespace Kinova -}; // namespace RAPTOR \ No newline at end of file diff --git a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp index d526879e..3da99147 100644 --- a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp @@ -10,8 +10,6 @@ using namespace RAPTOR; BOOST_AUTO_TEST_SUITE(InverseDynamicsTestSuite) - - // Test without fixed joints BOOST_AUTO_TEST_CASE(test_inverse_dynamics_without_fixed_joints) { @@ -55,7 +53,6 @@ BOOST_AUTO_TEST_CASE(test_inverse_dynamics_without_fixed_joints) //check the error BOOST_CHECK_SMALL((data.tau - cidPtr->tau(0)).norm(), 1e-10); - } // Test with fixed joints @@ -105,7 +102,5 @@ BOOST_AUTO_TEST_CASE(test_inverse_dynamics_with_fixed_joints) //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 ea1537de..df447c84 100644 --- a/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp +++ b/Tests/KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp @@ -193,9 +193,7 @@ bool check_gradient_output(const std::string& filename, const std::string& keywo } BOOST_AUTO_TEST_SUITE(FKGradientCheckerSuite) - -BOOST_AUTO_TEST_CASE(test_FKGradientChecker){ - +BOOST_AUTO_TEST_CASE(test_FKGradientChecker){ // Define robot model const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; @@ -214,8 +212,6 @@ BOOST_AUTO_TEST_CASE(test_FKGradientChecker){ BOOST_FAIL("Error initializing Ipopt class! Check previous error message!"); } - - SmartPtr app = IpoptApplicationFactory(); app->Options()->SetNumericValue("max_wall_time", 1e-5); @@ -228,7 +224,6 @@ BOOST_AUTO_TEST_CASE(test_FKGradientChecker){ 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(); @@ -248,10 +243,6 @@ BOOST_AUTO_TEST_CASE(test_FKGradientChecker){ // check the nlp BOOST_CHECK(status == 0 || status == 1); //success or feasible - - - - } BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file diff --git a/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp b/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp index 7409606d..e6cb93dd 100644 --- a/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp +++ b/Tests/KinematicsDynamics/TestForwardKinematicsSolver.cpp @@ -36,5 +36,4 @@ BOOST_AUTO_TEST_CASE(TestForwardKinematicsAccuracy) // 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 b9f05802..c5c1edd1 100644 --- a/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestRegressorInverseDynamics.cpp @@ -12,7 +12,6 @@ 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; @@ -47,9 +46,5 @@ BOOST_AUTO_TEST_CASE(RegressorInverseDynamicsAccuracy) // 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 From 4857f2c5fec231591b9774248b716d9fee76196f Mon Sep 17 00:00:00 2001 From: Zichang Zhou Date: Mon, 16 Sep 2024 21:39:15 +0000 Subject: [PATCH 7/8] resolve commented issue --- Examples/Kinova/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/Examples/Kinova/CMakeLists.txt b/Examples/Kinova/CMakeLists.txt index a1567281..99812885 100644 --- a/Examples/Kinova/CMakeLists.txt +++ b/Examples/Kinova/CMakeLists.txt @@ -6,7 +6,6 @@ add_library(Kinovalib SHARED WaitrDiscrete/src/KinovaWaitrOptimizer.cpp SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp SystemIdentification/ExcitingTrajectories/src/ConditionNumberOptimizer.cpp - SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp ) target_include_directories(Kinovalib PUBLIC From a86cea0965a9106339e53361ad6c4eedddba5316 Mon Sep 17 00:00:00 2001 From: Cfather Date: Fri, 4 Oct 2024 17:07:13 +0000 Subject: [PATCH 8/8] final clean up --- CMakeLists.txt | 3 - .../SysIDAlgFull.cpp | 139 ------------------ .../SysIDAlgFull.h | 86 ----------- .../QRDecompositionSolver.cpp | 132 ----------------- .../QRDecompositionSolver.h | 51 ------- Tests/CMakeLists.txt | 10 +- .../Constraints/TestKinematicsConstraints.cpp | 47 ++---- .../TestCustomizedInverseDynamics.cpp | 8 - 8 files changed, 16 insertions(+), 460 deletions(-) delete mode 100644 Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.cpp delete mode 100644 Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.h delete mode 100644 Examples/Kinova/SystemIdentification/QRDecompositionSolver.cpp delete mode 100644 Examples/Kinova/SystemIdentification/QRDecompositionSolver.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f80f0c05..d3880581 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,6 @@ # Minimum required CMake version cmake_minimum_required(VERSION 3.18) - # Project name project(RAPTOR DESCRIPTION "Rapid and Robust Trajectory Optimization for Robots") @@ -9,7 +8,6 @@ project(RAPTOR if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -# include(CTest) set(CTEST_OUTPUT_ON_FAILURE TRUE) @@ -18,7 +16,6 @@ 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") diff --git a/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.cpp b/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.cpp deleted file mode 100644 index 3bef67d7..00000000 --- a/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.cpp +++ /dev/null @@ -1,139 +0,0 @@ -#include "SysIDAlgFull.h" -#include "FirstOptimization.h" -#include "SecondOptimization.h" -#include - -SysIDAlgFull::SysIDAlgFull( - const std::vector& variables, - const std::vector& AlgOptions, - const std::vector& na_idx, - const std::vector& nu_idx, - const Eigen::VectorXd& lb, - const Eigen::VectorXd& ub, - const Eigen::VectorXd& lba, - const Eigen::VectorXd& uba, - const Eigen::VectorXd& X0_1, - const Eigen::VectorXd& T, - const Eigen::MatrixXd& data, - const Eigen::MatrixXd& dataFull, - const Eigen::MatrixXd& W_ip, - const Eigen::MatrixXd& Ginv, - const Eigen::MatrixXd& Aid, - const Eigen::MatrixXd& Ad, - const Eigen::MatrixXd& Kd -) - : na_idx_(na_idx), nu_idx_(nu_idx), - lb_(lb), ub_(ub), lba_(lba), uba_(uba), - X0_1_(X0_1), T_(T), data_(data), dataFull_(dataFull), - W_ip_(W_ip), Ginv_(Ginv), Aid_(Aid), Ad_(Ad), Kd_(Kd) -{ - // 初始化变量 - n_ = static_cast(variables[0]); - m_ = static_cast(variables[1]); - b_ = static_cast(variables[2]); - d_ = static_cast(variables[3]); - - // 算法选项 - k_ = AlgOptions[0]; - tol_ = AlgOptions[1]; - MS1_ = static_cast(AlgOptions[2]); - MS2_ = static_cast(AlgOptions[3]); - regroup_ = static_cast(AlgOptions[4]); - SearchAlpha_ = static_cast(AlgOptions[7]); - includeOffset_ = static_cast(AlgOptions[8]); - includeConstraints_ = static_cast(AlgOptions[10]); - constraintVariant_ = static_cast(AlgOptions[11]); - - initialize(); -} - -void SysIDAlgFull::initialize() { - // 初始化参数维度 - p_ip_ = 10 * n_; - - if (includeOffset_) { - p_full_ = p_ip_ + 4 * n_; - b_full_ = b_ + 4 * n_; - } else { - p_full_ = p_ip_ + 3 * n_; - b_full_ = b_ + 3 * n_; - } - - - - - // 其他初始化操作 -} - -void SysIDAlgFull::runAlgorithm() { - // 处理约束 - processConstraints(); - - // 参数重组 - if (regroup_) { - regroupParameters(); - } - - // 运行第一个优化问题 - runFirstOptimization(); - - // 根据需要运行第二个优化问题 - if (SearchAlpha_) { - runSecondOptimization(); - } - - // 最终结果保存在 X_, Wfull_, alphanew_ -} - -void SysIDAlgFull::processConstraints() { - // 根据 includeConstraints_ 和 constraintVariant_ 处理约束 - // 具体实现省略 -} - -void SysIDAlgFull::regroupParameters() { - // 参数重组的实现 - // 具体实现省略 -} - -void SysIDAlgFull::runFirstOptimization() { - // 构建第一个优化问题的对象 - FirstOptimization firstOpt(/* 传入必要的参数 */); - - // 设置初始点 - firstOpt.setStartingPoint(X0_1_); - - // 运行优化 - firstOpt.solve(); - - // 获取结果 - X_ = firstOpt.getSolution(); -} - -void SysIDAlgFull::runSecondOptimization() { - // 构建第二个优化问题的对象 - SecondOptimization secondOpt(/* 传入必要的参数 */); - - // 设置初始点 - Eigen::VectorXd X0_2; // 根据需要初始化 - secondOpt.setStartingPoint(X0_2); - - // 运行优化 - secondOpt.solve(); - - // 获取结果 - alphanew_ = secondOpt.getAlphaNew(); - // 更新 X_ - // 具体实现省略 -} - -Eigen::VectorXd SysIDAlgFull::getX() const { - return X_; -} - -Eigen::MatrixXd SysIDAlgFull::getWfull() const { - return Wfull_; -} - -Eigen::VectorXd SysIDAlgFull::getAlphaNew() const { - return alphanew_; -} diff --git a/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.h b/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.h deleted file mode 100644 index a331a349..00000000 --- a/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.h +++ /dev/null @@ -1,86 +0,0 @@ -#ifndef SYSIDALGFULL_H -#define SYSIDALGFULL_H - -#include -#include -#include "Utils.h" - -class SysIDAlgFull { -public: - SysIDAlgFull( - const std::vector& variables, - const std::vector& AlgOptions, - const std::vector& na_idx, - const std::vector& nu_idx, - const Eigen::VectorXd& lb, - const Eigen::VectorXd& ub, - const Eigen::VectorXd& lba, - const Eigen::VectorXd& uba, - const Eigen::VectorXd& X0_1, - const Eigen::VectorXd& T, - const Eigen::MatrixXd& data, - const Eigen::MatrixXd& dataFull, - const Eigen::MatrixXd& W_ip, - const Eigen::MatrixXd& Ginv, - const Eigen::MatrixXd& Aid, - const Eigen::MatrixXd& Ad, - const Eigen::MatrixXd& Kd - ); - - void runAlgorithm(); - - Eigen::VectorXd getX() const; - Eigen::MatrixXd getWfull() const; - Eigen::VectorXd getAlphaNew() const; - -private: - // 输入参数 - int n_; - int m_; - int b_; - int d_; - std::vector na_idx_; - std::vector nu_idx_; - Eigen::VectorXd lb_; - Eigen::VectorXd ub_; - Eigen::VectorXd lba_; - Eigen::VectorXd uba_; - Eigen::VectorXd X0_1_; - Eigen::VectorXd T_; - Eigen::MatrixXd data_; - Eigen::MatrixXd dataFull_; - Eigen::MatrixXd W_ip_; - Eigen::MatrixXd Ginv_; - Eigen::MatrixXd Aid_; - Eigen::MatrixXd Ad_; - Eigen::MatrixXd Kd_; - - // 算法选项 - double k_; - double tol_; - int MS1_; - int MS2_; - bool regroup_; - bool SearchAlpha_; - bool includeOffset_; - bool includeConstraints_; - int constraintVariant_; - - // 中间变量 - int p_ip_; - int p_full_; - int b_full_; - Eigen::VectorXd X_; - Eigen::MatrixXd Wfull_; - Eigen::VectorXd alphanew_; - - // 私有方法 - void initialize(); - void processConstraints(); - void regroupParameters(); - void runFirstOptimization(); - void runSecondOptimization(); - // 其他辅助方法 -}; - -#endif // SYSIDALGFULL_H \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/QRDecompositionSolver.cpp b/Examples/Kinova/SystemIdentification/QRDecompositionSolver.cpp deleted file mode 100644 index c62a5d5e..00000000 --- a/Examples/Kinova/SystemIdentification/QRDecompositionSolver.cpp +++ /dev/null @@ -1,132 +0,0 @@ -#include "QRDecompositionSolver.h" -#include -#include -#include -#include -#include - -namespace RAPTOR { -namespace Kinova { - -QRDecompositionSolver::QRDecompositionSolver(){ - getdata(); -} - -QRDecompositionSolver::QRDecompositionSolver(const MatX& ObservationMatrix, const VecX& InParam) { - compute(ObservationMatrix, InParam); -} - -QRDecompositionSolver::~QRDecompositionSolver() {} - - -void QRDecompositionSolver::getdata(){ - // Load robot model - const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; - pinocchio::Model model; - pinocchio::urdf::buildModel(urdf_filename, model); - pinocchio::Data data(model); - - // Create a trajectory - int N = 1000; // number of time steps - double T = 1000.0; // total time - int degree = 5; // degree of the polynomial - trajPtr_ = std::make_shared(T, N, model.nv, Uniform, degree); - ridPtr_ = std::shared_ptr(model, trajPtr_); - - // Generate random joint p, v, and a (not accurate) - std::srand(std::time(nullptr)); - VecX z = 2 * M_PI * VecX::Random(trajPtr->varLength).array() - M_PI; - - ridPtr_.compute(z, false); - MatX ObservationMatrix = ridPtr_.Y; - VecX InParam = VecX::Zero(91); - - // compute - compute(ObservationMatrix, InParam); -} - -void RDecompositionSolver::compute(const MatX& W, const VecX& InParam){ - - - // QR decomposition with column pivoting, improve stability - Eigen::ColPivHouseholderQR qr(W); - - // Permutation matrix - Eigen::PermutationMatrix perm = qr.colsPermutation(); - - // Redefine the permutation matrix meaning, same as matlab - // perm.indices()[i] = j old matrix ith col move to new matrix jth col - // M[i] = j old j jth col is M ith col - std::vector M(perm.indices().size()); - for (int i =0; i< perm.indices().size(); ++i){ - M[perm.indices()[i]]= i; - } - - - int rankW = qr.rank(); - std::vector idx(M.begin(), M.begin() + rankW); - std::sort(idx.begin(), idx.end()); - std::vector idx_(M.begin() + rankW, M.end()); - std::sort(idx_.begin(), idx_.end()); - - int p = W.cols(); - Aid = MatX::Zero(p, rankW); - for(int i = 0; i < rankW; ++i) { - Aid(idx[i], i) = 1.0; - } - - Ad = MatX::Zero(p, p - rankW); - for(int i = 0; i < static_cast(idx_.size()); ++i) { - Ad(idx_[i], i) = 1.0; - } - - // Combine Aid and Ad - MatX A(p,p); - A << Aid, Ad; - - // QR decomposition on W * A - Eigen::HouseholderQR qr_A(W * A); - MatX R_full = qr_A.matrixQR().triangularView(); - - // Extract indep R1 (upper-left block) and dep R2 (upper-right block) - MatX R1 = R_full.topLeftCorner(rankW, rankW); - MatX R2 = R_full.topRightCorner(rankW, p - rankW); - - // kd = (R1^-1)*R2 b*(p-b) - Kd = R1.solve(R2) - - // get rid of extremely small numbers for more numerical stability - double threshold = std::sqrt(eps); - for(int i = 0; i < Kd.rows(); ++i) { - for(int j = 0; j < Kd.cols(); ++j) { - if(std::abs(Kd(i, j)) < threshold) { - Kd(i, j) = 0.0; - } - } - } - - // Get independant and dependant parameters - VecX pi_id = Aid.transpose() * InParam; - VecX pi_d = Ad.transpose() * InParam; - - // Compute base inertial parameters - beta = pi_id + Kd * pi_d; - - // compute Ginv - MatX KG_(p,p); - KG_.setZero(); - KG_.topLeftCorner(rankW, rankW) = MatX::Identity(rankW, rankW); - KG_.topRightCorner(rankW, p - rankW) = -Kd; - KG_.bottomRightCorner(p - rankW, p - rankW) = MatX::Identity(p - rankW, p - rankW); - Ginv = A * KG_; - - // Set the dimensions of independent and dependent parameters - dim_id = pi_id.size(); - dim_d = pi_d.size(); - - // Compute RegroupMatrix - RegroupMatrix = Aid + Ad * Kd.transpose() -} - -}; // namespace Kinova -}; // namespace RAPTOR diff --git a/Examples/Kinova/SystemIdentification/QRDecompositionSolver.h b/Examples/Kinova/SystemIdentification/QRDecompositionSolver.h deleted file mode 100644 index 32f4fefe..00000000 --- a/Examples/Kinova/SystemIdentification/QRDecompositionSolver.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef QRDECOMPOSITIONSOLVER_H -#define QRDECOMPOSITIONSOLVER_H - -#include -#include -#include -#include -#include - -#include "RegressorInverseDynamics.h" -#include "Trajectories.h" - -namespace RAPTOR { -namespace Kinova { - -class QRDecompositionSolver{ -public: - using VecX = Eigen::VectorXd; - using MatX = Eigen::MatrixXd; - - MatX Aid; - MatX Ad; - MatX Kd; - MatX Ginv; - VecX Beta; - MatX RegroupMatrix; - - std::shared_ptr trajPtr_; - std::shared_ptr ridPtr_; - // Eigen::VectorXi jtype = Eigen::VectorXi(0); - - - int dim_id; - int dim_d; - double eps = 1e-8; - - QRDecompositionSolver(); - QRDecompositionSolver(const MatX& ObservationMatrix, const VecX& InParam); - ~QRDecompositionSolver(); - -private: - void getdata(); - void compute(const MatX& W, const Eigen::VectorXd& InParam); -}; - -}; // namespace Kinova -}; // namespace RAPTOR - - - -#endif \ No newline at end of file diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index e9d40055..ecac83c5 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -21,16 +21,16 @@ endfunction() # Add individual tests add_raptor_test(ForwardKinematics_test - KinematicsDynamics/TestForwardKinematicsSolver.cpp) + KinematicsDynamics/TestForwardKinematicsSolver.cpp) add_raptor_test(ForwardKinematicsGradient_test - KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp) + KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp) add_raptor_test(KinematicsConstraints_test - Constraints/TestKinematicsConstraints.cpp) + Constraints/TestKinematicsConstraints.cpp) add_raptor_test(CustomizedInverseDynamics_test - KinematicsDynamics/TestCustomizedInverseDynamics.cpp) + KinematicsDynamics/TestCustomizedInverseDynamics.cpp) add_raptor_test(RegressorInverseDynamics_test - KinematicsDynamics/TestRegressorInverseDynamics.cpp) + KinematicsDynamics/TestRegressorInverseDynamics.cpp) diff --git a/Tests/Constraints/TestKinematicsConstraints.cpp b/Tests/Constraints/TestKinematicsConstraints.cpp index e1718954..fa3bdffe 100644 --- a/Tests/Constraints/TestKinematicsConstraints.cpp +++ b/Tests/Constraints/TestKinematicsConstraints.cpp @@ -2,13 +2,11 @@ #include #include "KinematicsConstraints.h" -// #include "Plain.h" #include "Polynomials.h" #include using namespace RAPTOR; - BOOST_AUTO_TEST_SUITE(KinematicsConstraintsTest) // test gradient @@ -33,30 +31,25 @@ BOOST_AUTO_TEST_CASE(owngradientTest) KinematicsConstraints kc(trajPtr_, &model, end, 6, fkSolver.getTransform()); - // simple test when difference is small - Eigen::VectorXd z_test = z.array() + 1e-6; - - // simple test when difference is large - // z_test = z.array() + 1.0; + kc.compute(z, true, false); 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; } - BOOST_CHECK_SMALL((J_analytical - J_numerical).norm(), 1e-10); + 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"; @@ -68,7 +61,6 @@ BOOST_AUTO_TEST_CASE(ownHessianTest){ 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; @@ -76,55 +68,38 @@ BOOST_AUTO_TEST_CASE(ownHessianTest){ int end = model.getJointId("joint_7"); fkSolver.compute(start, end, z); - 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; + kc.compute(z, true, true); - // simple test when difference is large - // z_test = z.array() + 1.0; + Eigen::Array H_analytical = kc.pg_pz_pz; // params for error checking bool hasError = false; double max_diff = 0.0; std::stringstream error_message; - 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, 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; - // check error for (int j = 0; j < 3; j++) { - //check each loop - // BOOST_CHECK_SMALL((H_analytical(j).row(i) - H_numerical_row.row(j) ).norm(), 1e-10); - - //record the data, check when loop end double diff = (H_analytical(j).row(i) - H_numerical_row.row(j) ).norm(); - if (diff >1e-10){ + 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"; - } - } - - } - 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 3da99147..79f28bcb 100644 --- a/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp +++ b/Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp @@ -14,12 +14,6 @@ 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; @@ -72,8 +66,6 @@ BOOST_AUTO_TEST_CASE(test_inverse_dynamics_with_fixed_joints) 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