From a86cea0965a9106339e53361ad6c4eedddba5316 Mon Sep 17 00:00:00 2001 From: Cfather Date: Fri, 4 Oct 2024 17:07:13 +0000 Subject: [PATCH] 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