EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

updated planner, filter cutoffs, and controller

This commit is contained in:
Nimesh Khandelwal 2024-03-18 15:09:59 -04:00
parent a95a63d578
commit 583330f3ab
15 changed files with 578 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
locomotion/src/execution/CMakeLists.txt
View File

@ -1,9 +1,11 @@
find_package(iir REQUIRED) find_package(iir REQUIRED)
find_package(Threads REQUIRED) find_package(Threads REQUIRED)
find_package(qpOASES REQUIRED)
include_directories( include_directories(
include include
${COMMON_INCLUDE_DIR} ${COMMON_INCLUDE_DIR}
${qpOASES_INCLUDE_DIR}
) )
if(INSTALL_HEADERS_ONLY) if(INSTALL_HEADERS_ONLY)
@ -18,20 +20,34 @@ install(DIRECTORY include/
) )
else() else()
add_library(pd_controller SHARED add_library(pd_controller STATIC
src/Controller/Controller.cpp src/Controller/Controller.cpp
) )
target_compile_options(pd_controller PRIVATE "-fPIC")
target_link_libraries(pd_controller target_link_libraries(pd_controller
quad_dynamics quad_dynamics
iir::iir
${qpOASES_LIBRARY}
) )
add_library(didc SHARED add_library(didc STATIC
src/Controller/DistributedIDC.cpp src/Controller/DistributedIDC.cpp
) )
target_compile_options(didc PRIVATE "-fPIC")
target_link_libraries(didc target_link_libraries(didc
quad_dynamics quad_dynamics
pd_controller 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 add_library(quad_estimator SHARED
src/Estimator/Estimator.cpp src/Estimator/Estimator.cpp
src/Estimator/QuadEstimator.cpp src/Estimator/QuadEstimator.cpp
@ -58,6 +74,7 @@ target_link_libraries(reactive_planner
install(TARGETS install(TARGETS
pd_controller pd_controller
didc didc
robust_didc
quad_estimator quad_estimator
base_planner base_planner
reactive_planner reactive_planner
@ -77,6 +94,7 @@ add_dependencies(execute
quad_communication quad_communication
# pd_controller # pd_controller
didc didc
# robust_didc
quad_dynamics quad_dynamics
quad_estimator quad_estimator
# base_planner # base_planner
@ -85,6 +103,7 @@ add_dependencies(execute
target_link_libraries(execute target_link_libraries(execute
# pd_controller # pd_controller
didc didc
# robust_didc
quad_dynamics quad_dynamics
quad_estimator quad_estimator
base_planner base_planner

62
locomotion/src/execution/include/Controller/Controller.hpp
View File

@ -7,6 +7,16 @@
#include "memory_types.hpp" #include "memory_types.hpp"
#include "utils.hpp" #include "utils.hpp"
#include "timer.hpp" #include "timer.hpp"
// #include "Iir.h"
#include <qpOASES.hpp>
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 { class Controller {
public: public:
@ -16,7 +26,8 @@ public:
~Controller(); ~Controller();
vec12 GetDesiredContactForcePGD(const vec6 &b); vec12 GetDesiredContactForcePGD(const Eigen::MatrixXd& JabT, const vec6 &b);
vec12 getDesiredContactForceqpOASES(const vec6 &b);
virtual vec12 CalculateFeedForwardTorque() { virtual vec12 CalculateFeedForwardTorque() {
return vec12::Zero(); return vec12::Zero();
@ -76,6 +87,24 @@ protected:
// To read // To read
QuadrupedEstimationData* m_estimation_data_ptr; QuadrupedEstimationData* m_estimation_data_ptr;
QuadrupedPlannerData* m_planner_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: private:
std::string m_name; std::string m_name;
@ -89,4 +118,35 @@ private:
bool m_starting_from_sleep = false; bool m_starting_from_sleep = false;
vec12 F_prev; 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;
}; };

5
locomotion/src/execution/include/Executor.hpp
View File

@ -31,7 +31,7 @@ private:
std::string m_name; std::string m_name;
float m_rate = 1000; float m_rate = 1000;
float m_dt = 0.001; float m_dt = 0.001;
float t_curr = 0; double t_curr = 0;
float t_last = 0; float t_last = 0;
int delay_ms = 1; int delay_ms = 1;
int exec_time_ms = 0; int exec_time_ms = 0;
@ -64,6 +64,7 @@ private:
Gait stance; Gait stance;
Gait trot; Gait trot;
Gait walk;
QuadrupedSensorData m_sensor_data; QuadrupedSensorData m_sensor_data;
QuadrupedCommandData m_cmd_data; QuadrupedCommandData m_cmd_data;
@ -76,4 +77,6 @@ private:
std::chrono::time_point<std::chrono::high_resolution_clock> m_currentTimePoint; std::chrono::time_point<std::chrono::high_resolution_clock> m_currentTimePoint;
std::chrono::time_point<std::chrono::high_resolution_clock> m_lastTimePoint; std::chrono::time_point<std::chrono::high_resolution_clock> m_lastTimePoint;
std::thread m_comm_thread; std::thread m_comm_thread;
bool use_plant_time = true;
}; };

2
locomotion/src/execution/include/Planner/Gait.hpp
View File

@ -28,6 +28,8 @@ public:
void ShowParams(); void ShowParams();
void updateInitTime(const float& t0);
Eigen::Array4i GetScheduledContact(const float& t); Eigen::Array4i GetScheduledContact(const float& t);
float GetStridePhase(const float& t, const int& leg_id); float GetStridePhase(const float& t, const int& leg_id);

5
locomotion/src/execution/include/Planner/Planner.hpp
View File

@ -29,6 +29,7 @@ public:
void setGait(Gait& gait) { void setGait(Gait& gait) {
m_gait = gait; m_gait = gait;
// m_gait.updateInitTime(m_t_curr);
} }
void setPlannerDataPtr(QuadrupedPlannerData* pd) { void setPlannerDataPtr(QuadrupedPlannerData* pd) {
@ -71,11 +72,11 @@ protected:
double m_a_max_x = 0.2; double m_a_max_x = 0.2;
double m_a_max_y = 0.2; double m_a_max_y = 0.2;
double m_a_max_z = 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_x = 1.0;
double max_vel_y = 1.0; double max_vel_y = 1.0;
double max_vel_z = 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_loop_rate = 1000;
double m_dt = 0.001; double m_dt = 0.001;

368
locomotion/src/execution/src/Controller/Controller.cpp
View File

@ -63,15 +63,30 @@ void Controller::InitClass() {
// used in GetDesiredContactForcePGD() // used in GetDesiredContactForcePGD()
F_prev = vec12::Zero(); 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() { void Controller::update_contact_flag_for_controller() {
m_contact_flag_for_controller = int4::Zero(); m_contact_flag_for_controller = int4::Zero();
for (int leg_id = 0; leg_id < 4; leg_id++) { // 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 // // 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)) // if (m_expected_stance_flag(leg_id) && m_cs(leg_id))
m_contact_flag_for_controller(leg_id) = 1; // m_contact_flag_for_controller(leg_id) = 1;
} // }
m_contact_flag_for_controller = m_cs;
m_num_contact = m_contact_flag_for_controller.sum(); 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->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->qd = m_joint_vel_ref.block<12,1>(6, 0);
m_joint_command_ptr->tau = m_ff_torque; 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(); 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"); // Timer timer("GPGD Step");
// std::cout << "NUM CONTACT: " << m_num_contact << "\n"; // std::cout << "NUM CONTACT: " << m_num_contact << "\n";
// std::cout << "m_cs: " << m_cs.transpose() << std::endl; // 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_contact_flag_for_controller(i)) {
// if (m_cs(i)) { // if (m_cs(i)) {
A.block<3, 3>(0, 3 * nc) = Eigen::MatrixXd::Identity(3, 3); // 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>(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); f_prev.block<3, 1>(3 * nc, 0) = F_prev.block<3, 1>(3 * i, 0);
nc++; nc++;
} }
@ -182,15 +198,25 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) {
Eigen::VectorXd s(6); Eigen::VectorXd s(6);
// s << 1, 1, 2, 10, 10, 5; // 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 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 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 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::MatrixXd dd_L = 0.5 * Q.inverse();
Eigen::VectorXd r; Eigen::VectorXd r;
@ -203,12 +229,12 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) {
bool converged = false; bool converged = false;
float f_t = 1; float f_t = 1;
float f_z = 10; float f_z = 10;
float mu = 0.2; float mu = 0.8;
float fn_max = 200; float fn_max = 60;
float fn_min = 5; float fn_min = 0;
float pf_n = 10; float pf_n = 10;
int max_iters = 1e2; int max_iters = 1e3;
int iters = 0; int iters = 0;
float alpha = 1e-2; float alpha = 1e-2;
@ -217,19 +243,18 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) {
while (iters <= max_iters) 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 - dd_L * d_L;
// f = f - alpha * d_L; // f = f - alpha * d_L;
// projection on friction cone // projection on friction cone
for (int i = 0; i < m_num_contact; i++) { 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_t = f.block<2, 1>(3 * i, 0).norm();
f_z = f(3 * i + 2); 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) { 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) { else if (f_t <= -f_z / mu) {
f_proj.block<3, 1>(3 * i, 0) = Eigen::Vector3d(0, 0, 0); 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; f = f_proj;
r = A * f - b; r = A * f - b;
if ((f - f_).norm() < 1e-3) { // if ((f - f_).norm() < 1e-3) {
// if ((f - f_).norm() < 1e-3 && r.norm() < 50) { if (r.norm() < 5) {
converged = true; converged = true;
break; break;
} }
@ -255,11 +280,12 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) {
} }
// std::cout << "Residual: " << r.transpose() << std::endl; // std::cout << "Residual: " << r.transpose() << std::endl;
// std::cout << "b_hat: " << (A * f).transpose() << "\n";
vec12 F = vec12::Zero(); vec12 F = vec12::Zero();
// // First order filtering // // First order filtering
float a = 0.2; float a = 0;
nc = 0; nc = 0;
for (int i = 0; i < 4; i++) { 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); F.block<3, 1>(3 * i, 0) = Eigen::Vector3d(0, 0, 0);
continue; 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; // F = a * F_prev + (1 - a) * F;
@ -279,6 +314,287 @@ vec12 Controller::GetDesiredContactForcePGD(const vec6 &b) {
return F; 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<Eigen::Infinity>() << 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() { Controller::~Controller() {
// set zero torques, vel and init ee_state // set zero torques, vel and init ee_state
m_joint_command_ptr -> q = m_joint_state_init.block<12,1>(7, 0); m_joint_command_ptr -> q = m_joint_state_init.block<12,1>(7, 0);

69
locomotion/src/execution/src/Controller/DistributedIDC.cpp
View File

@ -36,7 +36,7 @@ void DistributedIDC::InitClass() {
// load gains from a param file // load gains from a param file
vec6 kpb; 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; 316.22, 316.22, 316.22;
// kpb << 0, 0, 26.22, // kpb << 0, 0, 26.22,
// 316.22, 316.22, 26.26; // 316.22, 316.22, 26.26;
@ -45,7 +45,7 @@ void DistributedIDC::InitClass() {
Kpb = kpb.asDiagonal(); Kpb = kpb.asDiagonal();
vec6 kdb; vec6 kdb;
kdb << 150.40, 150.40, 50.40, kdb << 50.40, 50.40, 50.40,
50.4, 50.4, 50.40; 50.4, 50.4, 50.40;
// kdb << 0 * 13.40, 0 * 13.40, 13.40, // kdb << 0 * 13.40, 0 * 13.40, 13.40,
// 50.4, 50.4, 0 * 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_act -= (feet_pos_act.block<3,1>(3 * i, 0));
} }
} }
r_B_p_ref /= 1./m_num_contact; r_B_p_ref /= m_num_contact;
r_B_p_act /= 1./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; 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)); 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()); 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"; // 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); // 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) 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)); + 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)) 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)); + 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>(0, 0) = q_r - q;
// err.block<18,1>(18, 0) = qd_r - qd; // err.block<18,1>(18, 0) = qd_r - qd;
@ -145,8 +148,38 @@ vec12 DistributedIDC::CalculateFeedForwardTorque() {
a_cmd = qdd_r + PD; a_cmd = qdd_r + PD;
tau_full = M * (a_cmd) + H + G; 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 << "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 // distributing the 18x1 generalized force into 12x1 joint space torques
Eigen::MatrixXd JabT = J.block<12, 6>(6, 0).transpose(); Eigen::MatrixXd JabT = J.block<12, 6>(6, 0).transpose();
Eigen::MatrixXd Jaa = J.block<12, 12>(6, 6); 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 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()); 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 + 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); // 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 #ifdef PRINT_DEBUG
// std::cout << "Fc: " << Fc.transpose() << std::endl; // std::cout << "Fc: " << Fc.transpose() << std::endl;
// std::cout << "tau_ff: " << tau_ff.transpose() << std::endl; // std::cout << "tau_ff: " << tau_ff.transpose() << std::endl;

4
locomotion/src/execution/src/Estimator/Estimator.cpp
View File

@ -20,8 +20,8 @@ void Estimator::InitClass() {
float sampling_freq = m_loop_rate; float sampling_freq = m_loop_rate;
// std::cout << "Sampling freq: " << sampling_freq << "\n"; // std::cout << "Sampling freq: " << sampling_freq << "\n";
float cutoff_freq_imu = 2; float cutoff_freq_imu = 5;
float cutoff_freq_encoder = 5; float cutoff_freq_encoder = 20;
for (int i = 0; i < 12; ++i) { for (int i = 0; i < 12; ++i) {
m_q_filter[i].setup(sampling_freq, cutoff_freq_encoder); m_q_filter[i].setup(sampling_freq, cutoff_freq_encoder);
m_qd_filter[i].setup(sampling_freq, cutoff_freq_encoder); m_qd_filter[i].setup(sampling_freq, cutoff_freq_encoder);

2
locomotion/src/execution/src/Estimator/QuadEstimator.cpp
View File

@ -262,7 +262,7 @@ void QuadEstimator::UpdatePoseEstimate() {
est_data.jv.block<3,1>(3, 0) = sensor_data.w_W; 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.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; est_data.ja = (est_data.jv - m_jv_prev) * m_loop_rate;

53
locomotion/src/execution/src/Executor.cpp
View File

@ -26,7 +26,7 @@ Executor::~Executor() {
float shutdown_start = t_curr; float shutdown_start = t_curr;
bool shutdown_complete = false; bool shutdown_complete = false;
while (shutdown_timer < 5) { while (shutdown_timer < 1) {
t_last = t_curr; t_last = t_curr;
t_curr = updateTimer(); t_curr = updateTimer();
m_dt = t_curr - t_last; 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->setSensorDataPtr(&m_sensor_data);
m_plant_data_ptr->setCommandDataPtr(&m_cmd_data); m_plant_data_ptr->setCommandDataPtr(&m_cmd_data);
m_plant_data_ptr->setEstimationDataPtr(&m_est_data); m_plant_data_ptr->setEstimationDataPtr(&m_est_data);
// m_plant_data_ptr->setPlantTimePtr(&t_curr);
#elif defined(USE_DDS_COMM) #elif defined(USE_DDS_COMM)
m_plant_data_ptr = std::make_shared<QuadDDSComm>(m_name, DATA_ACCESS_MODE::EXECUTOR); m_plant_data_ptr = std::make_shared<QuadDDSComm>(m_name, DATA_ACCESS_MODE::EXECUTOR);
m_plant_data_ptr->setUpdateRate(1000); m_plant_data_ptr->setUpdateRate(1000);
@ -111,13 +112,13 @@ void Executor::initClass() {
stance = Gait(Gait::STANCE); 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)); 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 // set velocity to ~0.05 to work with gait period of 5
m_planner.reset(); m_planner.reset();
m_planner.setGait(stance); 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.getStanceStates();
m_planner_data.x = m_robot.getSleepStates(); m_planner_data.x = m_robot.getSleepStates();
@ -137,7 +138,7 @@ double Executor::updateTimer() {
} }
void Executor::step() { void Executor::step() {
// Timer timer("Executor Step"); Timer timer("Executor Step");
{ {
// Timer timer1("Reading sensor data"); // Timer timer1("Reading sensor data");
m_plant_data_ptr->getSensorData(m_sensor_data); m_plant_data_ptr->getSensorData(m_sensor_data);
@ -151,7 +152,7 @@ void Executor::step() {
m_estimator.Step(m_dt); m_estimator.Step(m_dt);
} }
{ {
// Timer timer1("Controller step"); Timer timer1("Controller step");
m_controller.Step(); m_controller.Step();
} }
{ {
@ -170,9 +171,15 @@ void Executor::run() {
while (true) { while (true) {
t_last = t_curr; 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; m_dt = t_curr - t_last;
// std::cout << "t_curr: " << t_curr << "\n";
// if (t_curr < 5) { // if (t_curr < 5) {
// m_planner.setGait(trot); // m_planner.setGait(trot);
// m_planner.setDesiredVelocity(0.5, 0.0, 0.0); // m_planner.setDesiredVelocity(0.5, 0.0, 0.0);
@ -185,21 +192,19 @@ void Executor::run() {
// if (t_curr < 50) { // if (t_curr < 50) {
// if (t_curr < 15) { // if (t_curr < 15) {
if(t_curr < 5) { if(t_curr < 5) {
// if(!m_planner.setTargetBasePosition(vec3(0, 0, 0.34), 0, vec3(0, 0, 0.2))) {
// std::cout << "Reached\n";
m_planner.sleepToStance(); 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 { } 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) { // if(m_joystick_data.mode == 0) {
@ -230,12 +235,20 @@ void Executor::run() {
// m_planner.setDesiredVelocity(0, 0, 0); // 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 // Sleep for specified time to maintain update rate
// std::cout << "time: " << t_curr << "\n"; // std::cout << "time: " << t_curr << "\n";
// std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); // 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; int delay_time = delay_ms - (t_end - t_curr) * 1e3;
if (delay_time > 0) { if (delay_time > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(delay_time)); std::this_thread::sleep_for(std::chrono::milliseconds(delay_time));

17
locomotion/src/execution/src/Planner/Gait.cpp
View File

@ -40,6 +40,10 @@ void Gait::SetParams(float gait_period, float t0, float step_height, Eigen::Vect
m_step_height = step_height; m_step_height = step_height;
} }
void Gait::updateInitTime(const float& t0) {
m_t0 = t0;
}
void Gait::ShowParams() void Gait::ShowParams()
{ {
std::cout << "Gait Type: " << m_gait_type << "\n"; std::cout << "Gait Type: " << m_gait_type << "\n";
@ -143,14 +147,21 @@ void Gait::InitClass()
m_phi_thresh = Eigen::Vector4d::Ones(); m_phi_thresh = Eigen::Vector4d::Ones();
m_phi_offset = Eigen::Vector4d::Zero(); m_phi_offset = Eigen::Vector4d::Zero();
m_cs_ref_data = Eigen::Array4i::Ones(); 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; m_step_height = 0.0;
} else { } else {
m_t0 = 0; m_t0 = 0;
m_phi_thresh = Eigen::Vector4d::Ones(); m_phi_thresh = Eigen::Vector4d::Ones();
m_phi_offset = Eigen::Vector4d::Zero(); m_phi_offset = Eigen::Vector4d::Zero();
m_cs_ref_data = Eigen::Array4i::Ones(); m_cs_ref_data = Eigen::Array4i::Ones();
m_gait_period = 1; m_gait_period = 10;
m_step_height = 0.08; m_step_height = 0.0;
} }
} }

23
locomotion/src/execution/src/Planner/Planner.cpp
View File

@ -1,10 +1,10 @@
#include "Planner/Planner.hpp" #include "Planner/Planner.hpp"
Planner::Planner() : m_name("svan_planner"), m_gait(Gait::TROT) { Planner::Planner() : m_name("svan_planner") {
initClass(); initClass();
} }
Planner::Planner(std::string name) : m_name(name), m_gait(Gait::TROT) { Planner::Planner(std::string name) : m_name(name) {
initClass(); initClass();
} }
@ -41,7 +41,7 @@ void Planner::startFromSleep(const bool& sleep_start) {
// Use this carefully! Might be a good idea to just remove it. // Use this carefully! Might be a good idea to just remove it.
void Planner::setStance() { void Planner::setStance() {
m_gait = Gait::GAIT_TYPE::STANCE; m_gait = Gait(Gait::GAIT_TYPE::STANCE);
m_sleep_start = false; m_sleep_start = false;
initClass(); initClass();
// m_ee_state_ref = m_robot.getStanceStates(); // m_ee_state_ref = m_robot.getStanceStates();
@ -63,16 +63,16 @@ void Planner::setDesiredVelocity(const float& vx, const float& vy, const float&
return; 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); mat3x3 Rot_yaw = pinocchio::Rz(m_yaw);
vec4 v_cmd = vec4::Zero(); vec4 v_cmd = vec4::Zero();
v_cmd.block<2,1>(0, 0) = Rot_yaw.block<2,2>(0, 0) * vel_cmd; 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(2) = m_v_cmd_z;
v_cmd(3) = m_v_cmd_yaw; v_cmd(3) = vyaw;
float kp = 2.0; // float kp = 2.0;
float kd = 2.83; 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.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)); 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; static float motion_start_time = m_t_curr;
bool finished = false; bool finished = false;
m_gait = Gait::GAIT_TYPE::STANCE; m_gait = Gait(Gait::GAIT_TYPE::STANCE);
vec19 ee_stance = m_robot.getStanceStates(); vec19 ee_stance = m_robot.getStanceStates();
float motion_timer = m_t_curr; // - motion_start_time; float motion_timer = m_t_curr; // - motion_start_time;
@ -155,7 +155,7 @@ bool Planner::stanceToSleep() {
static float motion_start_time = m_t_curr; static float motion_start_time = m_t_curr;
bool finished = false; bool finished = false;
m_gait = Gait::GAIT_TYPE::STANCE; m_gait = Gait(Gait::GAIT_TYPE::STANCE);
setDesiredVelocity(0, 0, 0); setDesiredVelocity(0, 0, 0);
vec19 ee_sleep = m_robot.getSleepStates(); vec19 ee_sleep = m_robot.getSleepStates();
@ -168,7 +168,7 @@ bool Planner::stanceToSleep() {
// if (motion_timer < 4) { // if (motion_timer < 4) {
// finished = setTargetBasePosition(ee_sleep.block<3,1>(0, 0), m_yaw, vec3(0, 0, 0.2)); // finished = setTargetBasePosition(ee_sleep.block<3,1>(0, 0), m_yaw, vec3(0, 0, 0.2));
// } else { // } else {
// m_gait = Gait::GAIT_TYPE::STANCE; // m_gait = Gait(Gait::GAIT_TYPE::STANCE);
// } // }
if (finished == true) { if (finished == true) {
@ -416,10 +416,11 @@ double Planner::getTimeSinceStart() {
return duration * double(1e-6); return duration * double(1e-6);
} }
// Recheck this function. Behavior has changed after addition of setStance().
void Planner::reset() { void Planner::reset() {
m_startTimePoint = std::chrono::high_resolution_clock::now(); 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); setDesiredVelocity(0, 0, 0);
if (m_planner_data_ptr == NULL) { if (m_planner_data_ptr == NULL) {

45
locomotion/src/execution/src/Planner/ReactivePlanner.cpp
View File

@ -42,14 +42,16 @@ vec3 ReactivePlanner::getDesiredFootPosition(const uint8_t &leg_id, const float
float gravity = 9.81; 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); 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) { // for (int i = 0; i < 2; ++i) {
float delta_v = std::fabs(v_hip_act(i)) - std::fabs(v_hip_ref(i)); // float delta_v = std::fabs(v_hip_act(i)) - std::fabs(v_hip_ref(i));
if (delta_v < 0) { // if (delta_v < 0) {
cp_cmd(i) = 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; 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() { void ReactivePlanner::setTarget() {
setFeetTarget(); setFeetTarget();
setBaseTarget(); setBaseTarget();
if (use_vpsp) { if (use_vpsp) {
Eigen::MatrixXd p_f_VPSP = getVPSPVertices(m_t_curr); 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(); Eigen::Vector2d base_pos_from_VPSP = p_f_VPSP.rowwise().mean();
m_ee_state_ref.block<2,1>(0, 0) = base_pos_from_VPSP; 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"; // std::cout << "Stability margin: " << getVPSPStabilityMargin() << "\n";
} }
@ -126,14 +133,15 @@ double ReactivePlanner::getVPSPStabilityMargin() {
vec19 dummy_js = vec19::Zero(); vec19 dummy_js = vec19::Zero();
dummy_js.block<12,1>(7, 0) = m_theta; 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); dummy_js.block<4,1>(3, 0) = m_joint_state_act.block<4,1>(3, 0);
vec12 feet_pos = m_robot.forwardKinematics(m_joint_state_act); vec12 feet_pos = m_robot.forwardKinematics(dummy_js);
// XY coordinates of the virtual predicitve support polygon for each of the four legs // XY coordinates of the virtual predicitve support polygon for each of the four legs
Eigen::MatrixXd p_f_VPSP = Eigen::MatrixXd::Zero(2, 4); Eigen::MatrixXd p_f_VPSP = Eigen::MatrixXd::Zero(2, 4);
// an array for storing the adjacent legs // an array for storing the adjacent legs
Eigen::ArrayXi adj_leg = Eigen::ArrayXi::Zero(6); 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++) { for (int j = 1; j < 5; j++) {
int i = j - 1; 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); Eigen::MatrixXd p_f_VPSP = Eigen::MatrixXd::Zero(2, 4);
// an array for storing the adjacent legs // an array for storing the adjacent legs
Eigen::ArrayXi adj_leg = Eigen::ArrayXi::Zero(6); // Eigen::ArrayXi adj_leg = Eigen::ArrayXi::Zero(6);
adj_leg << 3, 0, 1, 2, 3, 0; 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++) for (int j = 1; j < 5; j++)
{ {
int i = j - 1; int i = j - 1;
int i_prev = adj_leg(j - 1); // int i_prev = adj_leg(j - 1);
int i_next = 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; 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); p_i << m_ee_state_ref(7 + 3 * i), m_ee_state_ref(7 + 3 * i + 1);

7
locomotion/src/execution/src/execute.cpp
View File

@ -9,9 +9,10 @@ void signalHandler(int signum) {
// quad_exec_ptr->~Executor(); // quad_exec_ptr->~Executor();
// Add cleanup or exit logic as needed // Add cleanup or exit logic as needed
if (quad_exec_ptr) { // if (quad_exec_ptr) {
delete quad_exec_ptr; // }
}
delete quad_exec_ptr;
exit(signum); exit(signum);
} }

9
locomotion/src/execution/src/policy_execute.py
View File

@ -43,7 +43,8 @@ class Executor:
def __init__(self, name): def __init__(self, name):
self.m_name = name self.m_name = name
self.device = "cpu" 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.policy = Policy(path_label)
self.loop_rate = 50. self.loop_rate = 50.
self.dt = 1./self.loop_rate self.dt = 1./self.loop_rate
@ -67,7 +68,7 @@ class Executor:
"trotting": [0.5, 0, 0], "trotting": [0.5, 0, 0],
"bounding": [0, 0.5, 0], "bounding": [0, 0.5, 0],
"pacing": [0, 0, 0.5]} "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.body_height_cmd = 0.34
self.step_frequency_cmd = 3.0 self.step_frequency_cmd = 3.0
self.gait = self.gaits["trotting"] self.gait = self.gaits["trotting"]
@ -75,7 +76,8 @@ class Executor:
self.pitch_cmd = 0.0 self.pitch_cmd = 0.0
self.roll_cmd = 0.0 self.roll_cmd = 0.0
self.stance_width_cmd = 0.25 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 = np.array([0.] * self.num_commands)
self.commands_np[0] = self.x_vel_cmd self.commands_np[0] = self.x_vel_cmd
self.commands_np[1] = self.y_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[11] = self.roll_cmd
self.commands_np[12] = self.stance_width_cmd self.commands_np[12] = self.stance_width_cmd
self.commands_np[13] = self.stance_length_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.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_pos_scale = 1.0
self.joint_vel_scale = 0.05 self.joint_vel_scale = 0.05