can walk but cannot turn on go2 robot

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
 
 )

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.00571868, 0.608813, -1.21763,
-        0.608813, -1.21763, 0.00571868, 0.608813,
+        -0.00571868, 0.608813, -1.21763,
-        -1.21763, -0.00571868, 0.608813, -1.21763
+        0.00571868, 0.608813, -1.21763,
+        -0.00571868, 0.608813, -1.21763
     };
 
     double start_pos_[12] = {};

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 yaw_cmd_{}, d_yaw_cmd_{}, d_yaw_cmd_past_{};
-    double _dYawCmdPast{};
+    Vec3 w_cmd_global_;
-    Vec3 _wCmdGlobal;
     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_;
 };
 
 

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<double, 3, 1> u_; // The input of estimator
 
-    Eigen::Matrix<double, 28, 1> _y; // The measurement value of output y
+    Eigen::Matrix<double, 28, 1> y_; // The measurement value of output y
     Eigen::Matrix<double, 28, 1> y_hat_; // The prediction of output y
     Eigen::Matrix<double, 18, 18> A; // The transtion matrix of estimator
     Eigen::Matrix<double, 18, 3> B; // The input matrix
@@ -130,9 +130,9 @@ private:
     Eigen::Matrix<double, 3, 3> Cu; // The covariance of system input u
 
     // Output Measurement
-    Eigen::Matrix<double, 12, 1> _feetPos2Body; // The feet positions to body, in the global coordinate
+    Eigen::Matrix<double, 12, 1> feet_pos_body_; // The feet positions to body, in the global coordinate
-    Eigen::Matrix<double, 12, 1> _feetVel2Body; // The feet velocity to body, in the global coordinate
+    Eigen::Matrix<double, 12, 1> feet_vel_body_; // The feet velocity to body, in the global coordinate
-    Eigen::Matrix<double, 4, 1> _feetH; // The Height of each foot, in the global coordinate
+    Eigen::Matrix<double, 4, 1> feet_h_; // The Height of each foot, in the global coordinate
 
     Eigen::Matrix<double, 28, 28> S; // _S = C*P*C.T + R
     Eigen::PartialPivLU<Eigen::Matrix<double, 28, 28> > Slu; // _S.lu()
@@ -143,7 +143,7 @@ private:
     Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC
 
     Vec3 g_;
-    double _dt;
+    double dt_;
 
     RotMat rotation_;
     Vec3 acceleration_;
@@ -153,7 +153,7 @@ private:
     std::vector<KDL::Vector> foot_vels_;
     std::vector<std::shared_ptr<LowPassFilter> > low_pass_filters_;
 
-    double _largeVariance;
+    double large_variance_;
 };
 
 

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);
 

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h

@@ -7,7 +7,7 @@
 #define GAITGENERATOR_H
 #include <unitree_guide_controller/common/mathTypes.h>
 
-#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_;

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();

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;
+    v_x_limit_ << -0.4, 0.4;
-    _vyLim << -0.3, 0.3;
+    v_y_limit_ << -0.3, 0.3;
-    _wyawLim << -0.5, 0.5;
+    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);
+    pcd_(2) = -estimator_.getFeetPos2Body()(2, 0);
-    _vCmdBody.setZero();
+    v_cmd_body_.setZero();
-    _yawCmd = estimator_.getYaw();
+    yaw_cmd_ = estimator_.getYaw();
-    Rd = rotz(_yawCmd);
+    Rd = rotz(yaw_cmd_);
-    _wCmdGlobal.setZero();
+    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));
+    v_cmd_body_(0) = invNormalize(ctrl_comp_.control_inputs_.get().ly, v_x_limit_(0), v_x_limit_(1));
-    _vCmdBody(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, _vyLim(0), _vyLim(1));
+    v_cmd_body_(1) = -invNormalize(ctrl_comp_.control_inputs_.get().lx, v_y_limit_(0), v_y_limit_(1));
-    _vCmdBody(2) = 0;
+    v_cmd_body_(2) = 0;
 
     /* Turning */
-    _dYawCmd = -invNormalize(ctrl_comp_.control_inputs_.get().rx, _wyawLim(0), _wyawLim(1));
+    d_yaw_cmd_ = -invNormalize(ctrl_comp_.control_inputs_.get().rx, w_yaw_limit_(0), w_yaw_limit_(1));
-    _dYawCmd = 0.9 * _dYawCmdPast + (1 - 0.9) * _dYawCmd;
+    d_yaw_cmd_ = 0.9 * d_yaw_cmd_past_ + (1 - 0.9) * d_yaw_cmd_;
-    _dYawCmdPast = _dYawCmd;
+    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_;
+    yaw_cmd_ = yaw_cmd_ + d_yaw_cmd_ * dt_;
-    Rd = rotz(_yawCmd);
+    Rd = rotz(yaw_cmd_);
-    _wCmdGlobal(2) = _dYawCmd;
+    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,6 +135,7 @@ 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) =
@@ -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;

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;
+    dt_ = 0.002;
-    _largeVariance = 100;
+    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_.resize(3);
+    // low_pass_filters_[0] = std::make_shared<LowPassFilter>(dt_, 3.0);
-    low_pass_filters_[0] = std::make_shared<LowPassFilter>(0.02, 3.0);
+    // low_pass_filters_[1] = std::make_shared<LowPassFilter>(dt_, 3.0);
-    low_pass_filters_[1] = std::make_shared<LowPassFilter>(0.02, 3.0);
+    // low_pass_filters_[2] = std::make_shared<LowPassFilter>(dt_, 3.0);
-    low_pass_filters_[2] = std::make_shared<LowPassFilter>(0.02, 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);
+            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) = _largeVariance * 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) = _largeVariance;
+            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<Eigen::Vector3d>(foot_poses_[i].p.data);
+        feet_pos_body_.segment(3 * i, 3) = Vec3(foot_poses_[i].p.data);
-        _feetVel2Body.segment(3 * i, 3) = Eigen::Map<Eigen::Vector3d>(foot_vels_[i].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
+    // // Using low pass filter to smooth the velocity
-    low_pass_filters_[0]->addValue(x_hat_(3));
+    // low_pass_filters_[0]->addValue(x_hat_(3));
-    low_pass_filters_[1]->addValue(x_hat_(4));
+    // low_pass_filters_[1]->addValue(x_hat_(4));
-    low_pass_filters_[2]->addValue(x_hat_(5));
+    // low_pass_filters_[2]->addValue(x_hat_(5));
-    x_hat_(3) = low_pass_filters_[0]->getValue();
+    // x_hat_(3) = low_pass_filters_[0]->getValue();
-    x_hat_(4) = low_pass_filters_[1]->getValue();
+    // x_hat_(4) = low_pass_filters_[1]->getValue();
-    x_hat_(5) = low_pass_filters_[2]->getValue();
+    // x_hat_(5) = low_pass_filters_[2]->getValue();
 }

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 = estimator_.getFeetPos2Body();
-    Vec34 feet_pos_body = robot_model_.feet_pos_normal_stand_;
+    // 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) +=

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();
 }
 
 

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<KDL::Frame> &pos, const Vec34 &vel) {
     Vec12 qd;
-    std::vector<KDL::JntArray> q = getQ(pos);
+    const std::vector<KDL::JntArray> 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<double, 3, Eigen::Dynamic> jacobian = getJacobian(index).data.topRows(3);
+    const Mat3 jacobian = getJacobian(index).data.topRows(3);
-    Eigen::VectorXd foot_velocity = jacobian * current_joint_vel_[index].data;
+    Vec3 foot_velocity = jacobian * current_joint_vel_[index].data;
     return {foot_velocity(0), foot_velocity(1), foot_velocity(2)};
 }
 

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;
 }
 

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
+        X: 0.011728689074516296
-        Y: -0.002330765128135681
+        Y: 0.011509218253195286
-        Z: -0.15908166766166687
+        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
+  Width: 626
-  X: 1327
+  X: 635
-  Y: 195
+  Y: 1328