diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index 88b4561..4fc1da0 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/controllers/unitree_guide_controller/CMakeLists.txt @@ -47,7 +47,7 @@ add_library(${PROJECT_NAME} SHARED src/quadProgpp/QuadProg++.cc src/gait/WaveGenerator.cpp - src/gait/FeetEndCtrl.cpp + src/gait/FeetEndCalc.cpp src/gait/GaitGenerator.cpp ) diff --git a/controllers/unitree_guide_controller/readme.md b/controllers/unitree_guide_controller/README.md similarity index 100% rename from controllers/unitree_guide_controller/readme.md rename to controllers/unitree_guide_controller/README.md diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h index fbf17d3..473cbd5 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h @@ -21,9 +21,10 @@ public: private: double target_pos_[12] = { - 0.00571868, 0.608813, -1.21763, -0.00571868, - 0.608813, -1.21763, 0.00571868, 0.608813, - -1.21763, -0.00571868, 0.608813, -1.21763 + 0.00571868, 0.608813, -1.21763, + -0.00571868, 0.608813, -1.21763, + 0.00571868, 0.608813, -1.21763, + -0.00571868, 0.608813, -1.21763 }; double start_pos_[12] = {}; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h index 7595984..df62b47 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h @@ -26,10 +26,19 @@ private: void calcCmd(); + /** + * Calculate the torque command + */ void calcTau(); + /** + * Calculate the joint space velocity and acceleration + */ void calcQQd(); + /** + * Calculate the PD gain for the joints + */ void calcGain() const; /** @@ -48,14 +57,12 @@ private: Vec3 pos_body_, vel_body_; RotMat B2G_RotMat, G2B_RotMat; - // Robot command Vec3 pcd_; - Vec3 vel_target_, _vCmdBody; + Vec3 vel_target_, v_cmd_body_; double dt_; - double _yawCmd{}, _dYawCmd{}; - double _dYawCmdPast{}; - Vec3 _wCmdGlobal; + double yaw_cmd_{}, d_yaw_cmd_{}, d_yaw_cmd_past_{}; + Vec3 w_cmd_global_; Vec34 pos_feet_global_goal_, vel_feet_global_goal_; RotMat Rd; @@ -65,7 +72,7 @@ private: Mat3 Kpp, Kdp, Kd_w_; double kp_w_; Mat3 Kp_swing_, Kd_swing_; - Vec2 _vxLim, _vyLim, _wyawLim; + Vec2 v_x_limit_, v_y_limit_, w_yaw_limit_; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h index f8f206c..fc7cddf 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h @@ -92,14 +92,14 @@ public: return gyro_; } - [[nodiscard]] Vec3 getGlobalGyro() const { + [[nodiscard]] Vec3 getGyroGlobal() const { return rotation_ * gyro_; } [[nodiscard]] double getYaw() const; [[nodiscard]] double getDYaw() const { - return getGlobalGyro()(2); + return getGyroGlobal()(2); } void update(); @@ -113,7 +113,7 @@ private: Eigen::Matrix u_; // The input of estimator - Eigen::Matrix _y; // The measurement value of output y + Eigen::Matrix y_; // The measurement value of output y Eigen::Matrix y_hat_; // The prediction of output y Eigen::Matrix A; // The transtion matrix of estimator Eigen::Matrix B; // The input matrix @@ -130,9 +130,9 @@ private: Eigen::Matrix Cu; // The covariance of system input u // Output Measurement - Eigen::Matrix _feetPos2Body; // The feet positions to body, in the global coordinate - Eigen::Matrix _feetVel2Body; // The feet velocity to body, in the global coordinate - Eigen::Matrix _feetH; // The Height of each foot, in the global coordinate + Eigen::Matrix feet_pos_body_; // The feet positions to body, in the global coordinate + Eigen::Matrix feet_vel_body_; // The feet velocity to body, in the global coordinate + Eigen::Matrix feet_h_; // The Height of each foot, in the global coordinate Eigen::Matrix S; // _S = C*P*C.T + R Eigen::PartialPivLU > Slu; // _S.lu() @@ -143,7 +143,7 @@ private: Eigen::Matrix IKC; // _IKC = I - KC Vec3 g_; - double _dt; + double dt_; RotMat rotation_; Vec3 acceleration_; @@ -153,7 +153,7 @@ private: std::vector foot_vels_; std::vector > low_pass_filters_; - double _largeVariance; + double large_variance_; }; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCtrl.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCalc.h similarity index 84% rename from controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCtrl.h rename to controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCalc.h index ca69a01..78d37d4 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCtrl.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCalc.h @@ -12,13 +12,13 @@ class Estimator; struct CtrlComponent; class QuadrupedRobot; -class FeetEndCtrl { +class FeetEndCalc { public: - explicit FeetEndCtrl(CtrlComponent &ctrl_component); + explicit FeetEndCalc(CtrlComponent &ctrl_component); void init(); - ~FeetEndCtrl() = default; + ~FeetEndCalc() = default; Vec3 calcFootPos(int index, Vec2 vxy_goal_global, double d_yaw_global, double phase); diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h index c61cb4a..5a039a1 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h @@ -7,7 +7,7 @@ #define GAITGENERATOR_H #include -#include "FeetEndCtrl.h" +#include "FeetEndCalc.h" class Estimator; @@ -68,7 +68,7 @@ private: WaveGenerator &wave_generator_; Estimator &estimator_; - FeetEndCtrl feet_end_ctrl_; + FeetEndCalc feet_end_calc_; double gait_height_{}; Vec2 vxy_goal_; diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index 5bbcd46..ac425e8 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -77,7 +77,7 @@ void StateBalanceTest::calcTorque() { // expected body angular acceleration d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) + - Kd_w_ * (Vec3(0, 0, 0) - estimator_.getGlobalGyro()); + Kd_w_ * (Vec3(0, 0, 0) - estimator_.getGyroGlobal()); // calculate foot force const Vec34 pos_feet_2_body_global = estimator_.getFeetPos2Body(); diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp index 3e20dda..0645601 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp @@ -17,22 +17,22 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T Kdp = Vec3(10, 10, 10).asDiagonal(); kp_w_ = 780; Kd_w_ = Vec3(70, 70, 70).asDiagonal(); - Kp_swing_ = Vec3(300,300,300).asDiagonal(); + Kp_swing_ = Vec3(400, 400, 400).asDiagonal(); Kd_swing_ = Vec3(10, 10, 10).asDiagonal(); - _vxLim << -0.4, 0.4; - _vyLim << -0.3, 0.3; - _wyawLim << -0.5, 0.5; + v_x_limit_ << -0.4, 0.4; + v_y_limit_ << -0.3, 0.3; + w_yaw_limit_ << -0.5, 0.5; dt_ = 1.0 / ctrl_comp_.frequency_; } void StateTrotting::enter() { pcd_ = estimator_.getPosition(); - pcd_(2) = -robot_model_.feet_pos_normal_stand_(2, 0); - _vCmdBody.setZero(); - _yawCmd = estimator_.getYaw(); - Rd = rotz(_yawCmd); - _wCmdGlobal.setZero(); + pcd_(2) = -estimator_.getFeetPos2Body()(2, 0); + v_cmd_body_.setZero(); + yaw_cmd_ = estimator_.getYaw(); + Rd = rotz(yaw_cmd_); + w_cmd_global_.setZero(); ctrl_comp_.control_inputs_.get().command = 0; gait_generator_.restart(); @@ -48,7 +48,7 @@ void StateTrotting::run() { getUserCmd(); calcCmd(); - gait_generator_.setGait(vel_target_.segment(0, 2), _wCmdGlobal(2), gait_height_); + gait_generator_.setGait(vel_target_.segment(0, 2), w_cmd_global_(2), gait_height_); gait_generator_.generate(pos_feet_global_goal_, vel_feet_global_goal_); calcTau(); @@ -80,19 +80,19 @@ FSMStateName StateTrotting::checkChange() { void StateTrotting::getUserCmd() { /* Movement */ - _vCmdBody(0) = invNormalize(ctrl_comp_.control_inputs_.get().ly, _vxLim(0), _vxLim(1)); - _vCmdBody(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, _vyLim(0), _vyLim(1)); - _vCmdBody(2) = 0; + v_cmd_body_(0) = invNormalize(ctrl_comp_.control_inputs_.get().ly, v_x_limit_(0), v_x_limit_(1)); + v_cmd_body_(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, v_y_limit_(0), v_y_limit_(1)); + v_cmd_body_(2) = 0; /* Turning */ - _dYawCmd = -invNormalize(ctrl_comp_.control_inputs_.get().rx, _wyawLim(0), _wyawLim(1)); - _dYawCmd = 0.9 * _dYawCmdPast + (1 - 0.9) * _dYawCmd; - _dYawCmdPast = _dYawCmd; + d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.get().rx, w_yaw_limit_(0), w_yaw_limit_(1)); + d_yaw_cmd_ = 0.9 * d_yaw_cmd_past_ + (1 - 0.9) * d_yaw_cmd_; + d_yaw_cmd_past_ = d_yaw_cmd_; } void StateTrotting::calcCmd() { /* Movement */ - vel_target_ = B2G_RotMat * _vCmdBody; + vel_target_ = B2G_RotMat * v_cmd_body_; vel_target_(0) = saturation(vel_target_(0), Vec2(vel_body_(0) - 0.2, vel_body_(0) + 0.2)); @@ -107,9 +107,9 @@ void StateTrotting::calcCmd() { vel_target_(2) = 0; /* Turning */ - _yawCmd = _yawCmd + _dYawCmd * dt_; - Rd = rotz(_yawCmd); - _wCmdGlobal(2) = _dYawCmd; + yaw_cmd_ = yaw_cmd_ + d_yaw_cmd_ * dt_; + Rd = rotz(yaw_cmd_); + w_cmd_global_(2) = d_yaw_cmd_; } void StateTrotting::calcTau() { @@ -118,7 +118,7 @@ void StateTrotting::calcTau() { Vec3 dd_pcd = Kpp * pos_error_ + Kdp * vel_error_; Vec3 d_wbd = kp_w_ * rotMatToExp(Rd * G2B_RotMat) + - Kd_w_ * (_wCmdGlobal - estimator_.getGlobalGyro()); + Kd_w_ * (w_cmd_global_ - estimator_.getGyroGlobal()); dd_pcd(0) = saturation(dd_pcd(0), Vec2(-3, 3)); dd_pcd(1) = saturation(dd_pcd(1), Vec2(-3, 3)); @@ -135,11 +135,12 @@ void StateTrotting::calcTau() { Vec34 pos_feet_global = estimator_.getFeetPos(); Vec34 vel_feet_global = estimator_.getFeetVel(); + for (int i(0); i < 4; ++i) { if (wave_generator_.contact_(i) == 0) { force_feet_global.col(i) = - Kp_swing_ * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) + - Kd_swing_ * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i)); + Kp_swing_ * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) + + Kd_swing_ * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i)); } } @@ -161,9 +162,6 @@ void StateTrotting::calcQQd() { for (int i(0); i < 4; ++i) { pos_feet_target.col(i) = G2B_RotMat * (pos_feet_global_goal_.col(i) - pos_body_); vel_feet_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_); - // vel_feet2_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_ - B2G_RotMat * skew( - // estimator_.getGyro()) * Vec3(pos_feet_body[i].p.data) - // ); // c.f formula (6.12) } Vec12 q_goal = robot_model_.getQ(pos_feet_target); @@ -193,10 +191,10 @@ void StateTrotting::calcGain() const { } bool StateTrotting::checkStepOrNot() { - if (fabs(_vCmdBody(0)) > 0.03 || fabs(_vCmdBody(1)) > 0.03 || + if (fabs(v_cmd_body_(0)) > 0.03 || fabs(v_cmd_body_(1)) > 0.03 || fabs(pos_error_(0)) > 0.08 || fabs(pos_error_(1)) > 0.08 || fabs(vel_error_(0)) > 0.05 || fabs(vel_error_(1)) > 0.05 || - fabs(_dYawCmd) > 0.20) { + fabs(d_yaw_cmd_) > 0.20) { return true; } return false; diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index b86429d..1a24f17 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -12,8 +12,8 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo robot_model_(ctrl_component.robot_model_), wave_generator_(ctrl_component.wave_generator_) { g_ << 0, 0, -9.81; - _dt = 0.002; - _largeVariance = 100; + dt_ = 0.002; + large_variance_ = 100; for (int i(0); i < Qdig.rows(); ++i) { Qdig(i) = i < 6 ? 0.0003 : 0.01; } @@ -23,12 +23,12 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo A.setZero(); A.block(0, 0, 3, 3) = I3; - A.block(0, 3, 3, 3) = I3 * _dt; + A.block(0, 3, 3, 3) = I3 * dt_; A.block(3, 3, 3, 3) = I3; A.block(6, 6, 12, 12) = I12; B.setZero(); - B.block(3, 0, 3, 3) = I3 * _dt; + B.block(3, 0, 3, 3) = I3 * dt_; C.setZero(); C.block(0, 0, 3, 3) = -I3; @@ -46,7 +46,7 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo C(27, 17) = 1; P.setIdentity(); - P = _largeVariance * P; + P = large_variance_ * P; RInit_ << 0.008, 0.012, -0.000, -0.009, 0.012, 0.000, 0.009, -0.009, -0.000, -0.009, -0.009, 0.000, -0.000, 0.000, -0.000, 0.000, -0.000, -0.001, @@ -135,11 +135,10 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo QInit_ = Qdig.asDiagonal(); QInit_ += B * Cu * B.transpose(); - - low_pass_filters_.resize(3); - low_pass_filters_[0] = std::make_shared(0.02, 3.0); - low_pass_filters_[1] = std::make_shared(0.02, 3.0); - low_pass_filters_[2] = std::make_shared(0.02, 3.0); + // low_pass_filters_.resize(3); + // low_pass_filters_[0] = std::make_shared(dt_, 3.0); + // low_pass_filters_[1] = std::make_shared(dt_, 3.0); + // low_pass_filters_[2] = std::make_shared(dt_, 3.0); } double Estimator::getYaw() const { @@ -154,29 +153,29 @@ void Estimator::update() { foot_poses_ = robot_model_.getFeet2BPositions(); foot_vels_ = robot_model_.getFeet2BVelocities(); - _feetH.setZero(); + feet_h_.setZero(); // Adjust the covariance based on foot contact and phase. for (int i(0); i < 4; ++i) { if (wave_generator_.contact_[i] == 0) { // foot not contact - Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3); - R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = _largeVariance * Eigen::MatrixXd::Identity(3, 3); - R(24 + i, 24 + i) = _largeVariance; + Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3); + R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3); + R(24 + i, 24 + i) = large_variance_; } else { // foot contact const double trust = windowFunc(wave_generator_.phase_[i], 0.2); Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = - (1 + (1 - trust) * _largeVariance) * + (1 + (1 - trust) * large_variance_) * QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3); R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = - (1 + (1 - trust) * _largeVariance) * + (1 + (1 - trust) * large_variance_) * RInit_.block(12 + 3 * i, 12 + 3 * i, 3, 3); R(24 + i, 24 + i) = - (1 + (1 - trust) * _largeVariance) * RInit_(24 + i, 24 + i); + (1 + (1 - trust) * large_variance_) * RInit_(24 + i, 24 + i); } - _feetPos2Body.segment(3 * i, 3) = Eigen::Map(foot_poses_[i].p.data); - _feetVel2Body.segment(3 * i, 3) = Eigen::Map(foot_vels_[i].data); + feet_pos_body_.segment(3 * i, 3) = Vec3(foot_poses_[i].p.data); + feet_vel_body_.segment(3 * i, 3) = Vec3(foot_vels_[i].data); } // Acceleration from imu as system input @@ -212,13 +211,13 @@ void Estimator::update() { y_hat_ = C * x_hat_; // Update the measurement value - _y << _feetPos2Body, _feetVel2Body, _feetH; + y_ << feet_pos_body_, feet_vel_body_, feet_h_; // Update the covariance matrix Ppriori = A * P * A.transpose() + Q; S = R + C * Ppriori * C.transpose(); Slu = S.lu(); - Sy = Slu.solve(_y - y_hat_); + Sy = Slu.solve(y_ - y_hat_); Sc = Slu.solve(C); SR = Slu.solve(R); STC = S.transpose().lu().solve(C); @@ -229,11 +228,11 @@ void Estimator::update() { P = IKC * Ppriori * IKC.transpose() + Ppriori * C.transpose() * SR * STC * Ppriori.transpose(); - // Using low pass filter to smooth the velocity - low_pass_filters_[0]->addValue(x_hat_(3)); - low_pass_filters_[1]->addValue(x_hat_(4)); - low_pass_filters_[2]->addValue(x_hat_(5)); - x_hat_(3) = low_pass_filters_[0]->getValue(); - x_hat_(4) = low_pass_filters_[1]->getValue(); - x_hat_(5) = low_pass_filters_[2]->getValue(); + // // Using low pass filter to smooth the velocity + // low_pass_filters_[0]->addValue(x_hat_(3)); + // low_pass_filters_[1]->addValue(x_hat_(4)); + // low_pass_filters_[2]->addValue(x_hat_(5)); + // x_hat_(3) = low_pass_filters_[0]->getValue(); + // x_hat_(4) = low_pass_filters_[1]->getValue(); + // x_hat_(5) = low_pass_filters_[2]->getValue(); } diff --git a/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp b/controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp similarity index 84% rename from controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp rename to controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp index 37f1686..bd0c12e 100644 --- a/controllers/unitree_guide_controller/src/gait/FeetEndCtrl.cpp +++ b/controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp @@ -2,11 +2,11 @@ // Created by biao on 24-9-18. // -#include "unitree_guide_controller/gait/FeetEndCtrl.h" +#include "unitree_guide_controller/gait/FeetEndCalc.h" #include "unitree_guide_controller/control/CtrlComponent.h" -FeetEndCtrl::FeetEndCtrl(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component), +FeetEndCalc::FeetEndCalc(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component), robot_model_(ctrl_component.robot_model_), estimator_(ctrl_component.estimator_) { k_x_ = 0.005; @@ -14,12 +14,12 @@ FeetEndCtrl::FeetEndCtrl(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_c k_yaw_ = 0.005; } -void FeetEndCtrl::init() { +void FeetEndCalc::init() { t_stance_ = ctrl_component_.wave_generator_.get_t_stance(); t_swing_ = ctrl_component_.wave_generator_.get_t_swing(); - // Vec34 feet_pos_body = estimator_.getFeetPos2Body(); - Vec34 feet_pos_body = robot_model_.feet_pos_normal_stand_; + Vec34 feet_pos_body = estimator_.getFeetPos2Body(); + // Vec34 feet_pos_body = robot_model_.feet_pos_normal_stand_; for (int i(0); i < 4; ++i) { feet_radius_(i) = sqrt(pow(feet_pos_body(0, i), 2) + pow(feet_pos_body(1, i), 2)); @@ -27,7 +27,7 @@ void FeetEndCtrl::init() { } } -Vec3 FeetEndCtrl::calcFootPos(const int index, Vec2 vxy_goal_global, const double d_yaw_global, const double phase) { +Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const double d_yaw_global, const double phase) { Vec3 body_vel_global = estimator_.getVelocity(); Vec3 next_step; @@ -44,7 +44,6 @@ Vec3 FeetEndCtrl::calcFootPos(const int index, Vec2 vxy_goal_global, const doubl const double next_yaw = d_yaw * (1 - phase) * t_swing_ + d_yaw * t_stance_ / 2 + k_yaw_ * (d_yaw_global - d_yaw); - next_step(0) += feet_radius_(index) * cos(yaw + feet_init_angle_(index) + next_yaw); next_step(1) += diff --git a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp index ea508cb..49ff0ee 100644 --- a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp +++ b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp @@ -10,9 +10,8 @@ GaitGenerator::GaitGenerator(CtrlComponent &ctrl_component) : wave_generator_(ctrl_component.wave_generator_), estimator_(ctrl_component.estimator_), - feet_end_ctrl_(ctrl_component) { + feet_end_calc_(ctrl_component) { first_run_ = true; - feet_end_ctrl_.init(); } void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const double gait_height) { @@ -37,7 +36,7 @@ void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) { feet_vel.col(i).setZero(); } else { // foot not contact, swing - end_p_.col(i) = feet_end_ctrl_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_.phase_(i)); + end_p_.col(i) = feet_end_calc_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_.phase_(i)); feet_pos.col(i) = getFootPos(i); feet_vel.col(i) = getFootVel(i); } @@ -47,6 +46,7 @@ void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) { void GaitGenerator::restart() { first_run_ = true; vxy_goal_.setZero(); + feet_end_calc_.init(); } diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index 8d34482..508ac71 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -49,6 +49,7 @@ Vec12 QuadrupedRobot::getQ(const Vec34 &vecP) const { for (int i(0); i < 4; ++i) { KDL::Frame frame; frame.p = KDL::Vector(vecP.col(i)[0], vecP.col(i)[1], vecP.col(i)[2]); + frame.M = KDL::Rotation::Identity(); q.segment(3 * i, 3) = robot_legs_[i]->calcQ(frame, current_joint_pos_[i]).data; } return q; @@ -56,7 +57,7 @@ Vec12 QuadrupedRobot::getQ(const Vec34 &vecP) const { Vec12 QuadrupedRobot::getQd(const std::vector &pos, const Vec34 &vel) { Vec12 qd; - std::vector q = getQ(pos); + const std::vector q = getQ(pos); for (int i(0); i < 4; ++i) { Mat3 jacobian = robot_legs_[i]->calcJaco(q[i]).data.topRows(3); qd.segment(3 * i, 3) = jacobian.inverse() * vel.col(i); @@ -91,8 +92,8 @@ KDL::JntArray QuadrupedRobot::getTorque(const KDL::Vector &force, int index) con } KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const { - const Eigen::Matrix jacobian = getJacobian(index).data.topRows(3); - Eigen::VectorXd foot_velocity = jacobian * current_joint_vel_[index].data; + const Mat3 jacobian = getJacobian(index).data.topRows(3); + Vec3 foot_velocity = jacobian * current_joint_vel_[index].data; return {foot_velocity(0), foot_velocity(1), foot_velocity(2)}; } diff --git a/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp index 331d97f..aad5e07 100644 --- a/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp +++ b/controllers/unitree_guide_controller/src/robot/RobotLeg.cpp @@ -18,6 +18,7 @@ RobotLeg::RobotLeg(const KDL::Chain &chain) { KDL::Frame RobotLeg::calcPEe2B(const KDL::JntArray &joint_positions) const { KDL::Frame pEe; fk_pose_solver_->JntToCart(joint_positions, pEe); + pEe.M = KDL::Rotation::Identity(); return pEe; } diff --git a/descriptions/go2_description/config/visualize_urdf.rviz b/descriptions/go2_description/config/visualize_urdf.rviz index 454f381..eb7b57c 100644 --- a/descriptions/go2_description/config/visualize_urdf.rviz +++ b/descriptions/go2_description/config/visualize_urdf.rviz @@ -8,7 +8,7 @@ Panels: - /Status1 - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 832 + Tree Height: 301 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -215,16 +215,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.469738483428955 + Distance: 0.9871627688407898 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.0716833621263504 - Y: -0.002330765128135681 - Z: -0.15908166766166687 + X: 0.011728689074516296 + Y: 0.011509218253195286 + Z: -0.10348724573850632 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -238,10 +238,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: true - Height: 1194 + Height: 593 Hide Left Dock: true Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001f40000043cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000043c0000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006180000005efc0100000002fb0000000800540069006d00650100000000000006180000047a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000618000003a000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001f40000043cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000043c000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000043cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000700000043c000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002720000005efc0100000002fb0000000800540069006d00650100000000000002720000026f00fffffffb0000000800540069006d00650100000000000004500000000000000000000002720000018f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -250,6 +250,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1560 - X: 1327 - Y: 195 + Width: 626 + X: 635 + Y: 1328