From 583330f3abf246babfff87c7f6878feb6b2a2a61 Mon Sep 17 00:00:00 2001 From: Nimesh Khandelwal Date: Mon, 18 Mar 2024 15:09:59 -0400 Subject: [PATCH] updated planner, filter cutoffs, and controller --- locomotion/src/execution/CMakeLists.txt | 23 +- .../include/Controller/Controller.hpp | 62 ++- locomotion/src/execution/include/Executor.hpp | 5 +- .../src/execution/include/Planner/Gait.hpp | 2 + .../src/execution/include/Planner/Planner.hpp | 5 +- .../execution/src/Controller/Controller.cpp | 368 ++++++++++++++++-- .../src/Controller/DistributedIDC.cpp | 69 ++-- .../src/execution/src/Estimator/Estimator.cpp | 4 +- .../execution/src/Estimator/QuadEstimator.cpp | 2 +- locomotion/src/execution/src/Executor.cpp | 53 ++- locomotion/src/execution/src/Planner/Gait.cpp | 17 +- .../src/execution/src/Planner/Planner.cpp | 23 +- .../execution/src/Planner/ReactivePlanner.cpp | 45 ++- locomotion/src/execution/src/execute.cpp | 7 +- .../src/execution/src/policy_execute.py | 9 +- 15 files changed, 578 insertions(+), 116 deletions(-) diff --git a/locomotion/src/execution/CMakeLists.txt b/locomotion/src/execution/CMakeLists.txt index 4d979a4..495d33d 100644 --- a/locomotion/src/execution/CMakeLists.txt +++ b/locomotion/src/execution/CMakeLists.txt @@ -1,9 +1,11 @@ find_package(iir REQUIRED) find_package(Threads REQUIRED) +find_package(qpOASES REQUIRED) include_directories( include ${COMMON_INCLUDE_DIR} + ${qpOASES_INCLUDE_DIR} ) if(INSTALL_HEADERS_ONLY) @@ -18,20 +20,34 @@ install(DIRECTORY include/ ) else() -add_library(pd_controller SHARED +add_library(pd_controller STATIC src/Controller/Controller.cpp ) +target_compile_options(pd_controller PRIVATE "-fPIC") target_link_libraries(pd_controller quad_dynamics + iir::iir + ${qpOASES_LIBRARY} ) -add_library(didc SHARED +add_library(didc STATIC src/Controller/DistributedIDC.cpp ) +target_compile_options(didc PRIVATE "-fPIC") target_link_libraries(didc quad_dynamics pd_controller ) +add_library(robust_didc STATIC + src/Controller/RobustDIDC.cpp +) +target_compile_options(robust_didc PRIVATE "-fPIC") +target_link_libraries(robust_didc + quad_dynamics + pd_controller + gp_regressor +) + add_library(quad_estimator SHARED src/Estimator/Estimator.cpp src/Estimator/QuadEstimator.cpp @@ -58,6 +74,7 @@ target_link_libraries(reactive_planner install(TARGETS pd_controller didc + robust_didc quad_estimator base_planner reactive_planner @@ -77,6 +94,7 @@ add_dependencies(execute quad_communication # pd_controller didc + # robust_didc quad_dynamics quad_estimator # base_planner @@ -85,6 +103,7 @@ add_dependencies(execute target_link_libraries(execute # pd_controller didc + # robust_didc quad_dynamics quad_estimator base_planner diff --git a/locomotion/src/execution/include/Controller/Controller.hpp b/locomotion/src/execution/include/Controller/Controller.hpp index fb68d29..4303c44 100644 --- a/locomotion/src/execution/include/Controller/Controller.hpp +++ b/locomotion/src/execution/include/Controller/Controller.hpp @@ -7,6 +7,16 @@ #include "memory_types.hpp" #include "utils.hpp" #include "timer.hpp" +// #include "Iir.h" +#include + +using namespace qpOASES; + +#define MU 0.5 +#define GRAVITY 9.8 + +static const double NEGATIVE_NUMBER = -1000000.0; +static const double POSITIVE_NUMBER = 1000000.0; class Controller { public: @@ -16,7 +26,8 @@ public: ~Controller(); - vec12 GetDesiredContactForcePGD(const vec6 &b); + vec12 GetDesiredContactForcePGD(const Eigen::MatrixXd& JabT, const vec6 &b); + vec12 getDesiredContactForceqpOASES(const vec6 &b); virtual vec12 CalculateFeedForwardTorque() { return vec12::Zero(); @@ -76,6 +87,24 @@ protected: // To read QuadrupedEstimationData* m_estimation_data_ptr; QuadrupedPlannerData* m_planner_data_ptr; + + void resize_qpOASES_vars(); + + void resize_eigen_vars(); + + void update_problem_size(); + + void print_real_t(real_t *matrix, int nRows, int nCols); + + void copy_Eigen_to_real_t(real_t *target, Eigen::MatrixXd &source, int nRows, int nCols); + + void copy_real_t_to_Eigen(Eigen::VectorXd &target, real_t *source, int len); + + void print_QPData(); + + Eigen::VectorXd clipVector(const Eigen::VectorXd &b, float F); + + void cleanFc(vec12& Fc); private: std::string m_name; @@ -89,4 +118,35 @@ private: bool m_starting_from_sleep = false; vec12 F_prev; + + // float m_loop_rate = 1000; + // Iir::Butterworth::LowPass<2> m_Fc_filter[12]; + // qpOASES related variables + real_t *H_qpOASES; + + real_t *A_qpOASES; + + real_t *g_qpOASES; + + real_t *lbA_qpOASES; + + real_t *ubA_qpOASES; + + real_t *xOpt_qpOASES; + + real_t *xOpt_initialGuess; + + Eigen::MatrixXd H_eigen, A_eigen, g_eigen, lbA_eigen, ubA_eigen; + + Eigen::VectorXd xOpt_eigen; + + uint8_t real_allocated, num_vars_qp, num_constr_qp, c_st; + + double fz_min, fz_max; + + int_t qp_exit_flag; + + int_t nWSR_qpOASES; + + real_t cpu_time; }; \ No newline at end of file diff --git a/locomotion/src/execution/include/Executor.hpp b/locomotion/src/execution/include/Executor.hpp index aace5a0..0add653 100644 --- a/locomotion/src/execution/include/Executor.hpp +++ b/locomotion/src/execution/include/Executor.hpp @@ -31,7 +31,7 @@ private: std::string m_name; float m_rate = 1000; float m_dt = 0.001; - float t_curr = 0; + double t_curr = 0; float t_last = 0; int delay_ms = 1; int exec_time_ms = 0; @@ -64,6 +64,7 @@ private: Gait stance; Gait trot; + Gait walk; QuadrupedSensorData m_sensor_data; QuadrupedCommandData m_cmd_data; @@ -76,4 +77,6 @@ private: std::chrono::time_point m_currentTimePoint; std::chrono::time_point m_lastTimePoint; std::thread m_comm_thread; + + bool use_plant_time = true; }; \ No newline at end of file diff --git a/locomotion/src/execution/include/Planner/Gait.hpp b/locomotion/src/execution/include/Planner/Gait.hpp index 8e7cce2..f0b79c4 100644 --- a/locomotion/src/execution/include/Planner/Gait.hpp +++ b/locomotion/src/execution/include/Planner/Gait.hpp @@ -28,6 +28,8 @@ public: void ShowParams(); + void updateInitTime(const float& t0); + Eigen::Array4i GetScheduledContact(const float& t); float GetStridePhase(const float& t, const int& leg_id); diff --git a/locomotion/src/execution/include/Planner/Planner.hpp b/locomotion/src/execution/include/Planner/Planner.hpp index 9e19762..af3c9ea 100644 --- a/locomotion/src/execution/include/Planner/Planner.hpp +++ b/locomotion/src/execution/include/Planner/Planner.hpp @@ -29,6 +29,7 @@ public: void setGait(Gait& gait) { m_gait = gait; + // m_gait.updateInitTime(m_t_curr); } void setPlannerDataPtr(QuadrupedPlannerData* pd) { @@ -71,11 +72,11 @@ protected: double m_a_max_x = 0.2; double m_a_max_y = 0.2; double m_a_max_z = 0.2; - double m_a_max_yaw = 1.0; + double m_a_max_yaw = 0.2; double max_vel_x = 1.0; double max_vel_y = 1.0; double max_vel_z = 1.0; - double max_vel_yaw = 3.0; + double max_vel_yaw = 1.0; double m_loop_rate = 1000; double m_dt = 0.001; diff --git a/locomotion/src/execution/src/Controller/Controller.cpp b/locomotion/src/execution/src/Controller/Controller.cpp index 076781f..193266b 100644 --- a/locomotion/src/execution/src/Controller/Controller.cpp +++ b/locomotion/src/execution/src/Controller/Controller.cpp @@ -63,15 +63,30 @@ void Controller::InitClass() { // used in GetDesiredContactForcePGD() F_prev = vec12::Zero(); + + + // float cutoff_freq = 2; + // for (int i = 0; i < 12; ++i) { + // m_Fc_filter[i].setup(m_loop_rate, cutoff_freq); + // } + + real_allocated = 0; + num_vars_qp = 0; + num_constr_qp = 0; + c_st = 0; + fz_min = 10; + fz_max = 100; } void Controller::update_contact_flag_for_controller() { m_contact_flag_for_controller = int4::Zero(); - for (int leg_id = 0; leg_id < 4; leg_id++) { - // contact flag for controller is set as 1 iff the leg is (m_cs) and should be (m_expected_stance_flag) in contact - if (m_expected_stance_flag(leg_id) && m_cs(leg_id)) - m_contact_flag_for_controller(leg_id) = 1; - } + // for (int leg_id = 0; leg_id < 4; leg_id++) { + // // contact flag for controller is set as 1 iff the leg is (m_cs) and should be (m_expected_stance_flag) in contact + // if (m_expected_stance_flag(leg_id) && m_cs(leg_id)) + // m_contact_flag_for_controller(leg_id) = 1; + // } + + m_contact_flag_for_controller = m_cs; m_num_contact = m_contact_flag_for_controller.sum(); } @@ -146,11 +161,11 @@ void Controller::updateJointCommand() { m_joint_command_ptr->q = m_joint_state_ref.block<12,1>(7, 0); m_joint_command_ptr->qd = m_joint_vel_ref.block<12,1>(6, 0); m_joint_command_ptr->tau = m_ff_torque; - m_joint_command_ptr->kp = 20 * vec12::Ones(); + m_joint_command_ptr->kp = 60 * vec12::Ones(); m_joint_command_ptr->kd = 5 * vec12::Ones(); } -vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { +vec12 Controller::GetDesiredContactForcePGD(const Eigen::MatrixXd& JabT, const vec6 &b) { // Timer timer("GPGD Step"); // std::cout << "NUM CONTACT: " << m_num_contact << "\n"; // std::cout << "m_cs: " << m_cs.transpose() << std::endl; @@ -173,8 +188,9 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { { if (m_contact_flag_for_controller(i)) { // if (m_cs(i)) { - A.block<3, 3>(0, 3 * nc) = Eigen::MatrixXd::Identity(3, 3); - A.block<3, 3>(3, 3 * nc) = pinocchio::skew_symm(pf.block<3, 1>(3 * i, 0) - rc); + // A.block<3, 3>(0, 3 * nc) = Eigen::MatrixXd::Identity(3, 3); + // A.block<3, 3>(3, 3 * nc) = pinocchio::skew_symm(pf.block<3, 1>(3 * i, 0) - rc); + A.block<6, 3>(0, 3 * nc) = JabT.block<6,3>(0, 3 * i); f_prev.block<3, 1>(3 * nc, 0) = F_prev.block<3, 1>(3 * i, 0); nc++; } @@ -182,15 +198,25 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { Eigen::VectorXd s(6); // s << 1, 1, 2, 10, 10, 5; - s << 1, 1, 5, 100, 100, 50; + s << 10, 10, 20, 100, 100, 20; + // s << 1, 1, 1, 1, 1, 1; Eigen::MatrixXd S = s.asDiagonal(); - Eigen::MatrixXd W = 1e-5 * Eigen::MatrixXd::Identity(3 * m_num_contact, 3 * m_num_contact); + + Eigen::VectorXd w = Eigen::VectorXd::Zero(3 * m_num_contact); + + for (int i = 0; i < m_num_contact; ++i) { + w << 1, 1, 1; + } + + // Eigen::MatrixXd W = 10 * Eigen::MatrixXd::Identity(3 * m_num_contact, 3 * m_num_contact); + Eigen::MatrixXd W = 1e-1 * w.asDiagonal(); Eigen::MatrixXd V = 1e-3 * Eigen::MatrixXd::Identity(3 * m_num_contact, 3 * m_num_contact); Eigen::MatrixXd Q = (A.transpose() * S * A + W + V); - Eigen::MatrixXd P = -2 * (A.transpose() * S * b + V * f_prev); + Eigen::MatrixXd P_T = -2 * (A.transpose() * S * b + V * f_prev); + // Eigen::MatrixXd P = -b.transpose() * (S * A + A * S) - 2 * f_prev.transpose() * V; - Eigen::VectorXd d_L = 2 * Q * f_prev + P; + Eigen::VectorXd d_L = 2 * Q * f_prev + P_T; Eigen::MatrixXd dd_L = 0.5 * Q.inverse(); Eigen::VectorXd r; @@ -203,12 +229,12 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { bool converged = false; float f_t = 1; float f_z = 10; - float mu = 0.2; - float fn_max = 200; - float fn_min = 5; + float mu = 0.8; + float fn_max = 60; + float fn_min = 0; float pf_n = 10; - int max_iters = 1e2; + int max_iters = 1e3; int iters = 0; float alpha = 1e-2; @@ -217,19 +243,18 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { while (iters <= max_iters) { - d_L = 2 * Q * f + P; + d_L = 2 * Q * f + P_T; f = f - dd_L * d_L; // f = f - alpha * d_L; // projection on friction cone for (int i = 0; i < m_num_contact; i++) { - f(3 * i + 2) = fminf(fmaxf(f(3 * i + 2), fn_min), fn_max); f_t = f.block<2, 1>(3 * i, 0).norm(); f_z = f(3 * i + 2); - - f.block<2, 1>(3 * i, 0) = mu * f(3 * i + 2) * (f.block<2, 1>(3 * i, 0) / f_t); if (f_t <= mu * f_z) { - f_proj.block<3, 1>(3 * i, 0) = f.block<3, 1>(3 * i, 0); + pf_n = fminf(fmaxf(f_z, fn_min), fn_max); + f_proj.block<2, 1>(3 * i, 0) = mu * pf_n * f.block<2, 1>(3 * i, 0) / f_t; + f_proj(3 * i + 2) = pf_n; } else if (f_t <= -f_z / mu) { f_proj.block<3, 1>(3 * i, 0) = Eigen::Vector3d(0, 0, 0); @@ -245,8 +270,8 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { f = f_proj; r = A * f - b; - if ((f - f_).norm() < 1e-3) { - // if ((f - f_).norm() < 1e-3 && r.norm() < 50) { + // if ((f - f_).norm() < 1e-3) { + if (r.norm() < 5) { converged = true; break; } @@ -255,11 +280,12 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { } // std::cout << "Residual: " << r.transpose() << std::endl; + // std::cout << "b_hat: " << (A * f).transpose() << "\n"; vec12 F = vec12::Zero(); // // First order filtering - float a = 0.2; + float a = 0; nc = 0; for (int i = 0; i < 4; i++) { @@ -268,9 +294,18 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { F.block<3, 1>(3 * i, 0) = Eigen::Vector3d(0, 0, 0); continue; } - F.block<3, 1>(3 * i, 0) = a * F_prev.block<3,1>(3 * i, 0) + (1 - alpha) * f.block<3, 1>(3 * nc++, 0); + // a = 1 - 0.1 * () + // F.block<3, 1>(3 * i, 0) = a * F_prev.block<3,1>(3 * i, 0) + (1 - a) * f.block<3, 1>(3 * nc++, 0); + F.block<3, 1>(3 * i, 0) = f.block<3, 1>(3 * nc++, 0); + // F(3 * i) = m_Fc_filter[3 * i].filter(f(3 * i)); + // F(3 * i + 1) = m_Fc_filter[3 * i + 1].filter(f(3 * i + 1)); + // F(3 * i + 2) = m_Fc_filter[3 * i + 2].filter(f(3 * i + 2)); } + // for (int i = 0; i < 12; ++i) { + // F(i) = m_Fc_filter[i].filter(F(i)); + // } + // F = a * F_prev + (1 - a) * F; @@ -279,6 +314,287 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) { return F; } +// QP resizing +void Controller::resize_qpOASES_vars() +{ + if (real_allocated) + { + free(H_qpOASES); + free(A_qpOASES); + free(g_qpOASES); + free(lbA_qpOASES); + free(ubA_qpOASES); + free(xOpt_qpOASES); + free(xOpt_initialGuess); + } + + H_qpOASES = (real_t *)malloc(num_vars_qp * num_vars_qp * sizeof(real_t)); + A_qpOASES = (real_t *)malloc(num_constr_qp * num_vars_qp * sizeof(real_t)); + g_qpOASES = (real_t *)malloc(num_vars_qp * 1 * sizeof(real_t)); + lbA_qpOASES = (real_t *)malloc(num_constr_qp * 1 * sizeof(real_t)); + ubA_qpOASES = (real_t *)malloc(num_constr_qp * 1 * sizeof(real_t)); + xOpt_qpOASES = (real_t *)malloc(num_vars_qp * 1 * sizeof(real_t)); + xOpt_initialGuess = (real_t *)malloc(num_vars_qp * 1 * sizeof(real_t)); + + real_allocated = 1; + // std::cout << "Resized QP vars." << std::endl; +} + +// Eigen QP matrices resizing +void Controller::resize_eigen_vars() +{ + H_eigen.resize(num_vars_qp, num_vars_qp); + A_eigen.resize(num_constr_qp, num_vars_qp); + g_eigen.resize(num_vars_qp, 1); + lbA_eigen.resize(num_constr_qp, 1); + ubA_eigen.resize(num_constr_qp, 1); + xOpt_eigen.resize(num_vars_qp, 1); + + H_eigen.setZero(); + A_eigen.setZero(); + g_eigen.setZero(); + lbA_eigen.setZero(); + ubA_eigen.setZero(); + xOpt_eigen.setZero(); + + // std::cout << "Resized Eigen vars." << std::endl; +} + +void Controller::update_problem_size() +{ + c_st = 0; + for (int i = 0; i < 4; i++) + { + if (m_contact_flag_for_controller(i)) + // if (m_cs(i)) + { + c_st++; + } + } + + num_vars_qp = 3 * c_st; + num_constr_qp = 5 * c_st; + // std::cout << "Updated problem size." << std::endl; +} + +void Controller::copy_Eigen_to_real_t(real_t *target, Eigen::MatrixXd &source, int nRows, int nCols) +{ + int count = 0; + + // Strange Behavior: Eigen matrix matrix(count) is stored by columns (not rows) + for (int i = 0; i < nRows; i++) + { + for (int j = 0; j < nCols; j++) + { + target[count] = source(i, j); + count++; + } + } +} + +void Controller::copy_real_t_to_Eigen(Eigen::VectorXd &target, real_t *source, int len) +{ + for (int i = 0; i < len; i++) + { + target(i) = source[i]; + } +} + +void Controller::print_real_t(real_t *matrix, int nRows, int nCols) +{ + int count = 0; + for (int i = 0; i < nRows; i++) + { + for (int j = 0; j < nCols; j++) + { + std::cout << matrix[count] << "\t"; + count++; + } + std::cout << "\n"; + } +} + +void Controller::print_QPData() +{ + std::cout << "\n\n"; + std::cout << "\n\nH = "; + + print_real_t(H_qpOASES, num_vars_qp, num_vars_qp); + std::cout << "\n\nA = "; + print_real_t(A_qpOASES, num_constr_qp, num_vars_qp); + std::cout << "\n\ng = "; + print_real_t(g_qpOASES, num_vars_qp, 1); + std::cout << "\n\nlbA = "; + print_real_t(lbA_qpOASES, num_constr_qp, 1); + std::cout << "\n\nubA = "; + print_real_t(ubA_qpOASES, num_constr_qp, 1); +} + +Eigen::VectorXd Controller::clipVector(const Eigen::VectorXd &b, float F) +{ + Eigen::VectorXd result = b; + for (int i = 0; i < result.size() / 2; ++i) + { + if (result[i] > F) + { + result[i] = F; + } + else if (result[i] < -F) + { + result[i] = -F; + } + } + return result; +} + +void Controller::cleanFc(vec12 &Fc) +{ + for (int i = 0; i < 4; i++) + { + if (Fc(3 * i + 2) < 0) + Fc(3 * i + 2) = fz_min; + + if (Fc(3 * i + 2) > fz_max) + Fc(3 * i + 2) = fz_max; + } +} + +// balance controller for the stance legs +vec12 Controller::getDesiredContactForceqpOASES(const vec6& b) { + Eigen::VectorXd rc = m_joint_state_act.block<3, 1>(0, 0); // CoM actual position + Eigen::VectorXd pf = m_robot.forwardKinematics(m_joint_state_act); // computing the actual sim feet position in the inertial frame + + // std::cout << "b:" << b.lpNorm() << std::endl; + + // update the QP matrices as per the current contact state + update_problem_size(); + resize_qpOASES_vars(); + resize_eigen_vars(); + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(6, num_vars_qp); + Eigen::MatrixXd f_prev(num_vars_qp, 1); + + // compute the actual yaw rotation matrix + // float yaw = m_joint_state_act(5); + Eigen::Matrix3d R_yaw_act = Eigen::Matrix3d::Identity(); + + int nc = 0; + for (int i = 0; i < 4; i++) + { + if (m_contact_flag_for_controller(i)) + // if (m_cs(i)) + { + A.block<3, 3>(0, 3 * nc) = R_yaw_act.transpose() * Eigen::MatrixXd::Identity(3, 3); + A.block<3, 3>(3, 3 * nc) = R_yaw_act.transpose() * pinocchio::skew_symm(pf.block<3, 1>(3 * i, 0) - rc); + f_prev.block<3, 1>(3 * nc, 0) = F_prev.block<3, 1>(3 * i, 0); + nc++; + } + } + + Eigen::VectorXd s(6); + // s << 1, 1, 5, 20, 20, 5; + s << 1, 1, 5, 100, 100, 50; + Eigen::MatrixXd S = s.asDiagonal(); + Eigen::MatrixXd W = 1e-0 * Eigen::MatrixXd::Identity(num_vars_qp, num_vars_qp); + Eigen::MatrixXd V = 1e-3 * Eigen::MatrixXd::Identity(num_vars_qp, num_vars_qp); + + // CALCULATE H + H_eigen = 2 * (A.transpose() * S * A + W + V); + + copy_Eigen_to_real_t(H_qpOASES, H_eigen, num_vars_qp, num_vars_qp); + + // CALCULATE g + g_eigen = -2 * A.transpose() * S * b; + g_eigen += -2 * V * f_prev; + + copy_Eigen_to_real_t(g_qpOASES, g_eigen, num_vars_qp, 1); + + // CALCULATE A + // for now assuming mu is fixed, and n_i = [0 0 1], t1 = [1 0 0] and t2 = [0 1 0] + Eigen::MatrixXd C_i(5, 3); + C_i << 1, 0, -MU, + 0, 1, -MU, + 0, 1, MU, + 1, 0, MU, + 0, 0, 1; + + for (int i = 0; i < c_st; i++) + { + A_eigen.block<5, 3>(5 * i, 3 * i) = C_i; + } + + copy_Eigen_to_real_t(A_qpOASES, A_eigen, num_constr_qp, num_vars_qp); + + // CALCULATE lbA and ubA + Eigen::VectorXd di_lb(5); + Eigen::VectorXd di_ub(5); + + di_lb << NEGATIVE_NUMBER, + NEGATIVE_NUMBER, + 0, + 0, + fz_min; + + di_ub << 0, + 0, + POSITIVE_NUMBER, + POSITIVE_NUMBER, + fz_max; + + for (int i = 0; i < c_st; i++) + { + lbA_eigen.block<5, 1>(5 * i, 0) = di_lb; + ubA_eigen.block<5, 1>(5 * i, 0) = di_ub; + } + + copy_Eigen_to_real_t(lbA_qpOASES, lbA_eigen, num_constr_qp, 1); + copy_Eigen_to_real_t(ubA_qpOASES, ubA_eigen, num_constr_qp, 1); + + // update the previous time-step's data in the initial guess + copy_Eigen_to_real_t(xOpt_initialGuess, f_prev, num_vars_qp, 1); + + // solve the QP only if there is at least a leg in stance + if (num_vars_qp) + { + QProblem qp_obj(num_vars_qp, num_constr_qp); + Options options; + options.setToMPC(); + options.printLevel = PL_NONE; + qp_obj.setOptions(options); + + // print_QPData(); + + nWSR_qpOASES = 1000; + qp_exit_flag = qp_obj.init( + H_qpOASES, g_qpOASES, A_qpOASES, nullptr, nullptr, lbA_qpOASES, + ubA_qpOASES, nWSR_qpOASES, &cpu_time); + + std::cout << "Exit flag: " << qp_exit_flag << std::endl; + // std::cout << "num_vars_qp: " << unsigned(num_vars_qp) << std::endl; + + qp_obj.getPrimalSolution(xOpt_qpOASES); + copy_real_t_to_Eigen(xOpt_eigen, xOpt_qpOASES, num_vars_qp); + } + + Eigen::VectorXd F = Eigen::VectorXd(12); + + nc = 0; + for (int i = 0; i < 4; i++) + { + if (m_contact_flag_for_controller(i) == 0) + // if (m_cs(i) == 0) + { + F.block<3, 1>(3 * i, 0) = Eigen::Vector3d(0, 0, 0); + continue; + } + F.block<3, 1>(3 * i, 0) = xOpt_eigen.block<3, 1>(3 * nc++, 0); + } + + F_prev = F; + + return F; // this is in inertial frame +} + Controller::~Controller() { // set zero torques, vel and init ee_state m_joint_command_ptr -> q = m_joint_state_init.block<12,1>(7, 0); diff --git a/locomotion/src/execution/src/Controller/DistributedIDC.cpp b/locomotion/src/execution/src/Controller/DistributedIDC.cpp index a8b687d..3713d6d 100644 --- a/locomotion/src/execution/src/Controller/DistributedIDC.cpp +++ b/locomotion/src/execution/src/Controller/DistributedIDC.cpp @@ -36,7 +36,7 @@ void DistributedIDC::InitClass() { // load gains from a param file vec6 kpb; - kpb << 0.0 * 316.22, 0.0 * 316.22, 26.22, + kpb << 1.0 * 316.22, 1.0 * 316.22, 316.22, 316.22, 316.22, 316.22; // kpb << 0, 0, 26.22, // 316.22, 316.22, 26.26; @@ -45,7 +45,7 @@ void DistributedIDC::InitClass() { Kpb = kpb.asDiagonal(); vec6 kdb; - kdb << 150.40, 150.40, 50.40, + kdb << 50.40, 50.40, 50.40, 50.4, 50.4, 50.40; // kdb << 0 * 13.40, 0 * 13.40, 13.40, // 50.4, 50.4, 0 * 13.40; @@ -105,8 +105,8 @@ vec12 DistributedIDC::CalculateFeedForwardTorque() { r_B_p_act -= (feet_pos_act.block<3,1>(3 * i, 0)); } } - r_B_p_ref /= 1./m_num_contact; - r_B_p_act /= 1./m_num_contact; + r_B_p_ref /= m_num_contact; + r_B_p_act /= m_num_contact; base_err.block<3,1>(0, 0) = r_B_p_ref - r_B_p_act; mat3x3 R_ref = pinocchio::quat2rot(q_r.block<4,1>(3, 0)); @@ -115,11 +115,14 @@ vec12 DistributedIDC::CalculateFeedForwardTorque() { base_err.block<3,1>(3, 0) = pinocchio::matrixLogRot(R_ref * R_act.transpose()); // std::cout << "Orientation error: " << pinocchio::matrixLogRot(R_ref * R_act.transpose()).transpose() << "\n"; // base_err.block<3,1>(3, 0) = pinocchio::matrixLogRot(R_ref.transpose() * R_act); - // std::cout << "base err: " << base_err.transpose() << "\n"; + // std::cout << "base position err: " << base_err.block<3,1>(0, 0).transpose() << "\n"; + // std::cout << "base orientation err: " << base_err.block<3,1>(3, 0).transpose() << "\n"; PD.block<6, 1>(0, 0) = 1 * Kpb * (base_err) + 1 * Kdb * (qd_r.block<6, 1>(0, 0) - qd.block<6, 1>(0, 0)); PD.block<12, 1>(6, 0) = 1 * Kpa * (q_r.block<12, 1>(7, 0) - q.block<12, 1>(7, 0)) + 1 * Kda * (qd_r.block<12, 1>(6, 0) - qd.block<12, 1>(6, 0)); + + // std::cout << "Position PD: " << PD.block<3,1>(0, 0).transpose() << "\n"; // err.block<18,1>(0, 0) = q_r - q; // err.block<18,1>(18, 0) = qd_r - qd; @@ -145,8 +148,38 @@ vec12 DistributedIDC::CalculateFeedForwardTorque() { a_cmd = qdd_r + PD; tau_full = M * (a_cmd) + H + G; + // std::cout << "a_cmd: " << a_cmd.block<3,1>(0, 0).transpose() << "\n"; + // std::cout << "M * a_cmd: " << ( M * a_cmd ).block<3,1>(0, 0).transpose() << "\n"; + // std::cout << "tau_full: " << tau_full.transpose() << "\n"; + std::cout << "Contact state: " << m_contact_flag_for_controller.transpose() << "\n"; + + // b.block<3,1>(0, 0) = tau_full.block<3,1>(0, 0); + b = tau_full.block<6,1>(0, 0); + // Fc = getFeetForces(clipVector(b, 50)); + // std::cout << "b: " << b.transpose() << "\n"; + b(0) = std::min(std::max(b(0), -20.), 20.); + b(1) = std::min(std::max(b(1), -50.), 50.); + b(2) = std::min(std::max(b(2), -200.), 200.); + b(3) = std::min(std::max(b(3), -100.), 100.); + b(4) = std::min(std::max(b(4), -100.), 100.); + b(5) = std::min(std::max(b(5), -100.), 100.); + std::cout << "b_clipped: " << b.transpose() << "\n"; + // Fc = GetDesiredContactForcePGD(J.block<12, 6>(6, 0).transpose(), b); + Fc = getDesiredContactForceqpOASES(b); + cleanFc(Fc); + // std::cout << "cs: " << m_contact_flag_for_controller.transpose() << "\n"; + // std::cout << "Fc: " << Fc.transpose() << std::endl; + + // Transform Fc in robot reference yaw frame + vec3 eul = pinocchio::Rot2EulXYZ(pinocchio::quat2rot(q_r.block<4,1>(3, 0))); + mat3x3 R_yaw_ref_T = pinocchio::Rz(eul(2)).transpose(); + + // for (int i = 0; i < 4; ++i) { + // Fc.block<3,1>(3 * i, 0) = R_yaw_ref_T * Fc.block<3,1>(3 * i, 0); + // } + // distributing the 18x1 generalized force into 12x1 joint space torques Eigen::MatrixXd JabT = J.block<12, 6>(6, 0).transpose(); Eigen::MatrixXd Jaa = J.block<12, 12>(6, 6); @@ -158,31 +191,13 @@ vec12 DistributedIDC::CalculateFeedForwardTorque() { Eigen::MatrixXd J_b2t = -JaaT * JabT_inv; Eigen::MatrixXd N_b2t = (Eigen::MatrixXd::Identity(12, 12) - J_b2t * (J_b2t.transpose() * J_b2t).inverse() * J_b2t.transpose()); - b.block<3,1>(0, 0) = tau_full.block<3,1>(0, 0); - b.block<3,1>(3, 0) = tau_full.block<3,1>(3, 0); - // Fc = getFeetForces(clipVector(b, 50)); - // std::cout << "b: " << b.transpose() << "\n"; - b(0) = std::min(std::max(b(0), -20.), 20.); - b(1) = std::min(std::max(b(1), -20.), 20.); - b(3) = std::min(std::max(b(3), -100.), 100.); - b(4) = std::min(std::max(b(4), -100.), 100.); - b(5) = std::min(std::max(b(5), -100.), 100.); - // std::cout << "b_clipped: " << b.transpose() << "\n"; - Fc = GetDesiredContactForcePGD(b); - // std::cout << "cs: " << m_contact_flag_for_controller.transpose() << "\n"; - // std::cout << "Fc: " << Fc.transpose() << std::endl; - - // Transform Fc in robot reference yaw frame - vec3 eul = pinocchio::Rot2EulXYZ(pinocchio::quat2rot(q.block<4,1>(3, 0))); - mat3x3 R_yaw_ref_T = pinocchio::Rz(eul(2)).transpose(); - - for (int i = 0; i < 4; ++i) { - Fc.block<3,1>(3 * i, 0) = R_yaw_ref_T * Fc.block<3,1>(3 * i, 0); - } - tau_ff = -JaaT * Fc + N_b2t * tau_full.block<12,1>(6, 0); + // tau_ff = -JaaT * Fc + tau_full.block<12,1>(6, 0); // tau_ff = tau_full.block<12,1>(6, 0); + std::cout << "Realized b: " << (JabT * Fc).transpose() << "\n"; + std::cout << "Fc: " << Fc.transpose() << std::endl; + #ifdef PRINT_DEBUG // std::cout << "Fc: " << Fc.transpose() << std::endl; // std::cout << "tau_ff: " << tau_ff.transpose() << std::endl; diff --git a/locomotion/src/execution/src/Estimator/Estimator.cpp b/locomotion/src/execution/src/Estimator/Estimator.cpp index 52fb9f5..aa65565 100644 --- a/locomotion/src/execution/src/Estimator/Estimator.cpp +++ b/locomotion/src/execution/src/Estimator/Estimator.cpp @@ -20,8 +20,8 @@ void Estimator::InitClass() { float sampling_freq = m_loop_rate; // std::cout << "Sampling freq: " << sampling_freq << "\n"; - float cutoff_freq_imu = 2; - float cutoff_freq_encoder = 5; + float cutoff_freq_imu = 5; + float cutoff_freq_encoder = 20; for (int i = 0; i < 12; ++i) { m_q_filter[i].setup(sampling_freq, cutoff_freq_encoder); m_qd_filter[i].setup(sampling_freq, cutoff_freq_encoder); diff --git a/locomotion/src/execution/src/Estimator/QuadEstimator.cpp b/locomotion/src/execution/src/Estimator/QuadEstimator.cpp index a4bb629..88d10f9 100644 --- a/locomotion/src/execution/src/Estimator/QuadEstimator.cpp +++ b/locomotion/src/execution/src/Estimator/QuadEstimator.cpp @@ -262,7 +262,7 @@ void QuadEstimator::UpdatePoseEstimate() { est_data.jv.block<3,1>(3, 0) = sensor_data.w_W; est_data.js.block<12,1>(7, 0) = sensor_data.q; - est_data.jv.block<12,1>(7, 0) = sensor_data.qd; + est_data.jv.block<12,1>(6, 0) = sensor_data.qd; est_data.ja = (est_data.jv - m_jv_prev) * m_loop_rate; diff --git a/locomotion/src/execution/src/Executor.cpp b/locomotion/src/execution/src/Executor.cpp index 392ab2f..d70ddc0 100644 --- a/locomotion/src/execution/src/Executor.cpp +++ b/locomotion/src/execution/src/Executor.cpp @@ -26,7 +26,7 @@ Executor::~Executor() { float shutdown_start = t_curr; bool shutdown_complete = false; - while (shutdown_timer < 5) { + while (shutdown_timer < 1) { t_last = t_curr; t_curr = updateTimer(); m_dt = t_curr - t_last; @@ -71,6 +71,7 @@ void Executor::initClass() { m_plant_data_ptr->setSensorDataPtr(&m_sensor_data); m_plant_data_ptr->setCommandDataPtr(&m_cmd_data); m_plant_data_ptr->setEstimationDataPtr(&m_est_data); + // m_plant_data_ptr->setPlantTimePtr(&t_curr); #elif defined(USE_DDS_COMM) m_plant_data_ptr = std::make_shared(m_name, DATA_ACCESS_MODE::EXECUTOR); m_plant_data_ptr->setUpdateRate(1000); @@ -111,13 +112,13 @@ void Executor::initClass() { stance = Gait(Gait::STANCE); - Gait walk(Gait::TROT); + walk = Gait(Gait::TROT); walk.SetParams(5.0, 0, 0.09, Eigen::Vector4d(0.9, 0.9, 0.9, 0.9), Eigen::Vector4d(0.75, 0.25, 0.5, 0.0)); // set velocity to ~0.05 to work with gait period of 5 m_planner.reset(); m_planner.setGait(stance); - m_planner.setDesiredVelocity(0.0, 0.0, 0.0); + m_planner.setDesiredVelocity(0.00, 0.0, 0.0); // m_planner_data.x = m_robot.getStanceStates(); m_planner_data.x = m_robot.getSleepStates(); @@ -137,7 +138,7 @@ double Executor::updateTimer() { } void Executor::step() { - // Timer timer("Executor Step"); + Timer timer("Executor Step"); { // Timer timer1("Reading sensor data"); m_plant_data_ptr->getSensorData(m_sensor_data); @@ -151,7 +152,7 @@ void Executor::step() { m_estimator.Step(m_dt); } { - // Timer timer1("Controller step"); + Timer timer1("Controller step"); m_controller.Step(); } { @@ -170,9 +171,15 @@ void Executor::run() { while (true) { t_last = t_curr; - t_curr = updateTimer(); + if (use_plant_time) { + m_plant_data_ptr -> getPlantTime(t_curr); + } else { + t_curr = updateTimer(); + } m_dt = t_curr - t_last; + // std::cout << "t_curr: " << t_curr << "\n"; + // if (t_curr < 5) { // m_planner.setGait(trot); // m_planner.setDesiredVelocity(0.5, 0.0, 0.0); @@ -185,21 +192,19 @@ void Executor::run() { // if (t_curr < 50) { // if (t_curr < 15) { - if(t_curr < 5) { - // if(!m_planner.setTargetBasePosition(vec3(0, 0, 0.34), 0, vec3(0, 0, 0.2))) { - // std::cout << "Reached\n"; + if(t_curr < 5) { m_planner.sleepToStance(); - } else if (t_curr < 8) { - // m_planner.setGait(trot); - m_planner.setGait(trot); - m_planner.setDesiredVelocity(0.2, 0, 0.0); - // m_planner.setTargetBasePosition(vec3(5.0, 0, 0.34), 0.0); - // std::cout << "Reaching\n"; - } else if (t_curr < 9) { - m_planner.setGait(stance); } else { - break; + m_planner.setGait(trot); + m_planner.setDesiredVelocity(0.0, 0.5, 0.0); + // m_planner.setDesiredVelocity(0.5, 0.0, 0.5); + // m_planner.setTargetBasePosition(vec3(5, 5, 0.34), 0); } + // if (t_curr < 1) { + // m_planner.setGait(stance); + // } else { + // m_planner.setGait(trot); + // m_planner.setDesiredVelocity(0.2, 0, 0.0); // } // if(m_joystick_data.mode == 0) { @@ -230,12 +235,20 @@ void Executor::run() { // m_planner.setDesiredVelocity(0, 0, 0); // } - step(); + if (std::fabs(t_curr - t_last) > 1e-6) { + step(); + } + // Sleep for specified time to maintain update rate // std::cout << "time: " << t_curr << "\n"; // std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); - float t_end = updateTimer(); + double t_end = 0; + if (use_plant_time) { + m_plant_data_ptr -> getPlantTime(t_end); + } else { + t_end = updateTimer(); + } int delay_time = delay_ms - (t_end - t_curr) * 1e3; if (delay_time > 0) { std::this_thread::sleep_for(std::chrono::milliseconds(delay_time)); diff --git a/locomotion/src/execution/src/Planner/Gait.cpp b/locomotion/src/execution/src/Planner/Gait.cpp index 1db8ac5..485ecdc 100644 --- a/locomotion/src/execution/src/Planner/Gait.cpp +++ b/locomotion/src/execution/src/Planner/Gait.cpp @@ -40,6 +40,10 @@ void Gait::SetParams(float gait_period, float t0, float step_height, Eigen::Vect m_step_height = step_height; } +void Gait::updateInitTime(const float& t0) { + m_t0 = t0; +} + void Gait::ShowParams() { std::cout << "Gait Type: " << m_gait_type << "\n"; @@ -143,14 +147,21 @@ void Gait::InitClass() m_phi_thresh = Eigen::Vector4d::Ones(); m_phi_offset = Eigen::Vector4d::Zero(); m_cs_ref_data = Eigen::Array4i::Ones(); - m_gait_period = 1; + m_gait_period = 10; + m_step_height = 0.0; + } else if (m_gait_type == STANCE) { + m_t0 = 0; + m_phi_thresh = Eigen::Vector4d::Ones(); + m_phi_offset = Eigen::Vector4d::Zero(); + m_cs_ref_data = Eigen::Array4i::Ones(); + m_gait_period = 10; m_step_height = 0.0; } else { m_t0 = 0; m_phi_thresh = Eigen::Vector4d::Ones(); m_phi_offset = Eigen::Vector4d::Zero(); m_cs_ref_data = Eigen::Array4i::Ones(); - m_gait_period = 1; - m_step_height = 0.08; + m_gait_period = 10; + m_step_height = 0.0; } } \ No newline at end of file diff --git a/locomotion/src/execution/src/Planner/Planner.cpp b/locomotion/src/execution/src/Planner/Planner.cpp index fbf796b..f73066d 100644 --- a/locomotion/src/execution/src/Planner/Planner.cpp +++ b/locomotion/src/execution/src/Planner/Planner.cpp @@ -1,10 +1,10 @@ #include "Planner/Planner.hpp" -Planner::Planner() : m_name("svan_planner"), m_gait(Gait::TROT) { +Planner::Planner() : m_name("svan_planner") { initClass(); } -Planner::Planner(std::string name) : m_name(name), m_gait(Gait::TROT) { +Planner::Planner(std::string name) : m_name(name) { initClass(); } @@ -41,7 +41,7 @@ void Planner::startFromSleep(const bool& sleep_start) { // Use this carefully! Might be a good idea to just remove it. void Planner::setStance() { - m_gait = Gait::GAIT_TYPE::STANCE; + m_gait = Gait(Gait::GAIT_TYPE::STANCE); m_sleep_start = false; initClass(); // m_ee_state_ref = m_robot.getStanceStates(); @@ -63,16 +63,16 @@ void Planner::setDesiredVelocity(const float& vx, const float& vy, const float& return; } - Eigen::Vector2d vel_cmd = Eigen::Vector2d(m_v_cmd_x, m_v_cmd_y); + Eigen::Vector2d vel_cmd = Eigen::Vector2d(vx, vy); mat3x3 Rot_yaw = pinocchio::Rz(m_yaw); vec4 v_cmd = vec4::Zero(); v_cmd.block<2,1>(0, 0) = Rot_yaw.block<2,2>(0, 0) * vel_cmd; v_cmd(2) = m_v_cmd_z; - v_cmd(3) = m_v_cmd_yaw; + v_cmd(3) = vyaw; - float kp = 2.0; - float kd = 2.83; + // float kp = 2.0; + float kd = 1.5; m_ee_acc_ref.block<3,1>(0, 0) = kd * (v_cmd.block<3,1>(0, 0) - m_ee_vel_ref.block<3,1>(0, 0)); m_ee_acc_ref(5) = kd * (v_cmd(3) - m_ee_vel_ref(5)); @@ -131,7 +131,7 @@ bool Planner::sleepToStance() { static float motion_start_time = m_t_curr; bool finished = false; - m_gait = Gait::GAIT_TYPE::STANCE; + m_gait = Gait(Gait::GAIT_TYPE::STANCE); vec19 ee_stance = m_robot.getStanceStates(); float motion_timer = m_t_curr; // - motion_start_time; @@ -155,7 +155,7 @@ bool Planner::stanceToSleep() { static float motion_start_time = m_t_curr; bool finished = false; - m_gait = Gait::GAIT_TYPE::STANCE; + m_gait = Gait(Gait::GAIT_TYPE::STANCE); setDesiredVelocity(0, 0, 0); vec19 ee_sleep = m_robot.getSleepStates(); @@ -168,7 +168,7 @@ bool Planner::stanceToSleep() { // if (motion_timer < 4) { // finished = setTargetBasePosition(ee_sleep.block<3,1>(0, 0), m_yaw, vec3(0, 0, 0.2)); // } else { - // m_gait = Gait::GAIT_TYPE::STANCE; + // m_gait = Gait(Gait::GAIT_TYPE::STANCE); // } if (finished == true) { @@ -416,10 +416,11 @@ double Planner::getTimeSinceStart() { return duration * double(1e-6); } +// Recheck this function. Behavior has changed after addition of setStance(). void Planner::reset() { m_startTimePoint = std::chrono::high_resolution_clock::now(); - m_gait = Gait::GAIT_TYPE::STANCE; + m_gait = Gait(Gait::GAIT_TYPE::STANCE); setDesiredVelocity(0, 0, 0); if (m_planner_data_ptr == NULL) { diff --git a/locomotion/src/execution/src/Planner/ReactivePlanner.cpp b/locomotion/src/execution/src/Planner/ReactivePlanner.cpp index 75a072c..ba8ebc4 100644 --- a/locomotion/src/execution/src/Planner/ReactivePlanner.cpp +++ b/locomotion/src/execution/src/Planner/ReactivePlanner.cpp @@ -42,14 +42,16 @@ vec3 ReactivePlanner::getDesiredFootPosition(const uint8_t &leg_id, const float float gravity = 9.81; Eigen::Vector2d cp_cmd = m_use_capture_point_heuristic * std::sqrt(p_hip_i(2) / gravity) * (v_hip_act - v_hip_ref); - for (int i = 0; i < 2; ++i) { - float delta_v = std::fabs(v_hip_act(i)) - std::fabs(v_hip_ref(i)); - if (delta_v < 0) { - cp_cmd(i) = 0; - } - } + // for (int i = 0; i < 2; ++i) { + // float delta_v = std::fabs(v_hip_act(i)) - std::fabs(v_hip_ref(i)); + // if (delta_v < 0) { + // cp_cmd(i) = 0; + // } + // } - // std::cout << "Capture point command: " << cp_cmd.transpose() << "\n"; + // std::cout << "Raibert heiristic: " << leg_id << ": " << (getRaibertHeuristic(leg_id, t_stance) - p_hip_i).block<2,1>(0, 0).transpose() << "\n"; + // std::cout << "Capture point command: " << leg_id << ": " << cp_cmd.transpose() << "\n"; + // std::cout << "Base vel ref: " << base_vel_ref.transpose() << "\n"; p_step_i.block<2, 1>(0, 0) = getRaibertHeuristic(leg_id, t_stance).block<2,1>(0, 0) + 0.0 * cp_cmd; @@ -59,12 +61,17 @@ vec3 ReactivePlanner::getDesiredFootPosition(const uint8_t &leg_id, const float void ReactivePlanner::setTarget() { setFeetTarget(); setBaseTarget(); + if (use_vpsp) { Eigen::MatrixXd p_f_VPSP = getVPSPVertices(m_t_curr); + // std::cout << "polygon vertices: \n" << p_f_VPSP << "\n"; Eigen::Vector2d base_pos_from_VPSP = p_f_VPSP.rowwise().mean(); m_ee_state_ref.block<2,1>(0, 0) = base_pos_from_VPSP; } + // std::cout << "Base velocity: " << m_ee_vel_ref.block<2,1>(0, 0).transpose() << "\n"; + // std::cout << "Base acceleration: " << m_ee_acc_ref.block<2,1>(0, 0).transpose() << "\n"; + // std::cout << "Stability margin: " << getVPSPStabilityMargin() << "\n"; } @@ -126,14 +133,15 @@ double ReactivePlanner::getVPSPStabilityMargin() { vec19 dummy_js = vec19::Zero(); dummy_js.block<12,1>(7, 0) = m_theta; - dummy_js.block<3,1>(3, 0) = m_joint_state_act.block<3,1>(3, 0); - vec12 feet_pos = m_robot.forwardKinematics(m_joint_state_act); + dummy_js.block<4,1>(3, 0) = m_joint_state_act.block<4,1>(3, 0); + vec12 feet_pos = m_robot.forwardKinematics(dummy_js); // XY coordinates of the virtual predicitve support polygon for each of the four legs Eigen::MatrixXd p_f_VPSP = Eigen::MatrixXd::Zero(2, 4); // an array for storing the adjacent legs Eigen::ArrayXi adj_leg = Eigen::ArrayXi::Zero(6); - adj_leg << 3, 0, 1, 2, 3, 0; + // adj_leg << 3, 0, 1, 2, 3, 0; + adj_leg << 3, 1, 0, 2, 3, 1; for (int j = 1; j < 5; j++) { int i = j - 1; @@ -172,14 +180,23 @@ Eigen::MatrixXd ReactivePlanner::getVPSPVertices(const float &t) { Eigen::MatrixXd p_f_VPSP = Eigen::MatrixXd::Zero(2, 4); // an array for storing the adjacent legs - Eigen::ArrayXi adj_leg = Eigen::ArrayXi::Zero(6); - adj_leg << 3, 0, 1, 2, 3, 0; + // Eigen::ArrayXi adj_leg = Eigen::ArrayXi::Zero(6); + Eigen::MatrixXi adj_leg = Eigen::MatrixXi::Zero(4, 2); + // adj_leg << 3, 2, 0, 1, 3, 2; + // adj_leg << 3, 0, 1, 2, 3, 0; + // adj_leg << 3, 0, 1, 2, 3, 0; + adj_leg << 1, 2, + 3, 0, + 0, 3, + 2, 1; for (int j = 1; j < 5; j++) { int i = j - 1; - int i_prev = adj_leg(j - 1); - int i_next = adj_leg(j + 1); + // int i_prev = adj_leg(j - 1); + // int i_next = adj_leg(j + 1); + int i_prev = adj_leg(i, 0); + int i_next = adj_leg(i, 1); Eigen::Vector2d p_i, p_i_prev, p_i_next; p_i << m_ee_state_ref(7 + 3 * i), m_ee_state_ref(7 + 3 * i + 1); diff --git a/locomotion/src/execution/src/execute.cpp b/locomotion/src/execution/src/execute.cpp index 8303f7e..3a49c26 100644 --- a/locomotion/src/execution/src/execute.cpp +++ b/locomotion/src/execution/src/execute.cpp @@ -9,9 +9,10 @@ void signalHandler(int signum) { // quad_exec_ptr->~Executor(); // Add cleanup or exit logic as needed - if (quad_exec_ptr) { - delete quad_exec_ptr; - } + // if (quad_exec_ptr) { + // } + + delete quad_exec_ptr; exit(signum); } diff --git a/locomotion/src/execution/src/policy_execute.py b/locomotion/src/execution/src/policy_execute.py index ce61ba3..1666f4b 100644 --- a/locomotion/src/execution/src/policy_execute.py +++ b/locomotion/src/execution/src/policy_execute.py @@ -43,7 +43,8 @@ class Executor: def __init__(self, name): self.m_name = name self.device = "cpu" - path_label = "/home/meshin/dev/walk-these-ways/runs/gait-conditioned-agility/2024-02-18/train/211214.073611" + # path_label = "/home/meshin/dev/walk-these-ways/runs/gait-conditioned-agility/2024-02-18/train/211214.073611" + path_label = "/home/meshin/dev/walk-these-ways/runs/gait-conditioned-agility/2023-10-18/train/003846.138687" self.policy = Policy(path_label) self.loop_rate = 50. self.dt = 1./self.loop_rate @@ -67,7 +68,7 @@ class Executor: "trotting": [0.5, 0, 0], "bounding": [0, 0.5, 0], "pacing": [0, 0, 0.5]} - self.x_vel_cmd, self.y_vel_cmd, self.yaw_vel_cmd = 1.0, 0.0, 0.0 + self.x_vel_cmd, self.y_vel_cmd, self.yaw_vel_cmd = 1.0, 0.0, 0.6 self.body_height_cmd = 0.34 self.step_frequency_cmd = 3.0 self.gait = self.gaits["trotting"] @@ -75,7 +76,8 @@ class Executor: self.pitch_cmd = 0.0 self.roll_cmd = 0.0 self.stance_width_cmd = 0.25 - self.stance_length_cmd = 0.40 + self.stance_length_cmd = 0.05 + self.aux_reward_cmd = 0.0 self.commands_np = np.array([0.] * self.num_commands) self.commands_np[0] = self.x_vel_cmd self.commands_np[1] = self.y_vel_cmd @@ -89,6 +91,7 @@ class Executor: self.commands_np[11] = self.roll_cmd self.commands_np[12] = self.stance_width_cmd self.commands_np[13] = self.stance_length_cmd + self.commands_np[14] = self.aux_reward_cmd self.default_joint_pos = np.array([0.1, 0.8, -1.5, -0.1, 0.8, -1.5, 0.1, 1., -1.5, -0.1, 1., -1.5], dtype=np.float64) self.joint_pos_scale = 1.0 self.joint_vel_scale = 0.05