balance control achieved

controllers/unitree_guide_controller/CMakeLists.txt

@@ -49,7 +49,9 @@ add_library(${PROJECT_NAME} SHARED
 target_include_directories(${PROJECT_NAME}
         PUBLIC
         "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+        "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+        PRIVATE
+        src)
 ament_target_dependencies(
         ${PROJECT_NAME} PUBLIC
         ${CONTROLLER_INCLUDE_DEPENDS}

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h

@@ -24,14 +24,14 @@ private:
 
     KDL::Vector pcd_;
     KDL::Vector pcdInit_;
-    KDL::Rotation Rd_;
+    RotMat Rd_;
-    KDL::Rotation RdInit_;
+    RotMat init_rotation_;
 
     KDL::Vector pose_body_, vel_body_;
 
     double kp_w_;
     Mat3 Kp_p_, Kd_p_, Kd_w_;
-    Vec3 _ddPcd, _dWbd;
+    Vec3 dd_pcd_, d_wbd_;
 
     float _xMax, _xMin;
     float _yMax, _yMin;

controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h

@@ -33,17 +33,13 @@ T windowFunc(const T x, const T windowRatio, const T xRange = 1.0,
     return yRange;
 }
 
-inline Eigen::Matrix3d skew(const KDL::Vector &vec) {
+inline Mat3 skew(const Vec3 &v) {
-    Eigen::Matrix3d skewMat;
+    Mat3 m;
-    skewMat << 0, -vec.z(), vec.y(),
+    m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
-            vec.z(), 0, -vec.x(),
+    return m;
-            -vec.y(), vec.x(), 0;
-    return skewMat;
 }
 
-
+inline Vec3 rotMatToExp(const RotMat &rm) {
-inline Vec3 rotationToExp(const KDL::Rotation &rotation) {
-    auto rm = Eigen::Matrix3d(rotation.data);
     double cosValue = rm.trace() / 2.0 - 1 / 2.0;
     if (cosValue > 1.0f) {
         cosValue = 1.0f;
@@ -51,7 +47,7 @@ inline Vec3 rotationToExp(const KDL::Rotation &rotation) {
         cosValue = -1.0f;
     }
 
-    double angle = acos(cosValue);
+    const double angle = acos(cosValue);
     Vec3 exp;
     if (fabs(angle) < 1e-5) {
         exp = Vec3(0, 0, 0);
@@ -65,5 +61,39 @@ inline Vec3 rotationToExp(const KDL::Rotation &rotation) {
     return exp;
 }
 
+inline RotMat rotx(const double &theta) {
+    double s = std::sin(theta);
+    double c = std::cos(theta);
+
+    RotMat R;
+    R << 1, 0, 0, 0, c, -s, 0, s, c;
+    return R;
+}
+
+inline RotMat roty(const double &theta) {
+    double s = std::sin(theta);
+    double c = std::cos(theta);
+
+    RotMat R;
+    R << c, 0, s, 0, 1, 0, -s, 0, c;
+    return R;
+}
+
+inline RotMat rotz(const double &theta) {
+    double s = std::sin(theta);
+    double c = std::cos(theta);
+
+    RotMat R;
+    R << c, -s, 0, s, c, 0, 0, 0, 1;
+    return R;
+}
+
+inline RotMat rpyToRotMat(const double &row, const double &pitch,
+                          const double &yaw) {
+    RotMat m = rotz(yaw) * roty(pitch) * rotx(row);
+    return m;
+}
+
+
 
 #endif //MATHTOOLS_H

controllers/unitree_guide_controller/include/unitree_guide_controller/control/BalanceCtrl.h

@@ -18,30 +18,29 @@ public:
 
     void init(const QuadrupedRobot &robot);
 
-    std::vector<KDL::Vector> calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM,
+    Vec34 calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
-                                  const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact);
+               const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact);
 
 private:
-    void calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const KDL::Rotation &rotM);
+    void calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const RotMat &rotM);
 
-    void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM);
+    void calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM);
 
     void calConstraints(const std::vector<int> &contact);
 
     void solveQP();
 
-    Mat12 _G, _W, _U;
+    Mat12 G_, W_, U_;
-    Mat6 _S;
+    Mat6 S_;
-    Mat3 _Ib;
+    Mat3 Ib_;
-    Vec6 _bd;
+    Vec6 bd_;
-    KDL::Vector _pcb;
+    Vec3 _g, _pcb;
-    Vec3 _g;
+    Vec12 F_, F_prev_, g0T_;
-    Vec12 _F, _Fprev, _g0T;
+    double mass_, alpha_, beta_, friction_ratio_;
-    double _mass, _alpha, _beta, _fricRatio;
+    Eigen::MatrixXd CE_, CI_;
-    Eigen::MatrixXd _CE, _CI;
+    Eigen::VectorXd ce0_, ci0_;
-    Eigen::VectorXd _ce0, _ci0;
+    Eigen::Matrix<double, 6, 12> A_;
-    Eigen::Matrix<double, 6, 12> _A;
+    Eigen::Matrix<double, 5, 3> friction_mat_;
-    Eigen::Matrix<double, 5, 3> _fricMat;
 };
 
 

controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h

@@ -4,6 +4,7 @@
 
 #ifndef ESTIMATOR_H
 #define ESTIMATOR_H
+#include <iostream>
 #include <memory>
 #include <eigen3/Eigen/Dense>
 #include <kdl/frames.hpp>
@@ -23,7 +24,7 @@ public:
      * @return robot central position
      */
     KDL::Vector getPosition() {
-        return {_xhat(0), _xhat(1), _xhat(2)};
+        return {x_hat_(0), x_hat_(1), x_hat_(2)};
     }
 
     /**
@@ -31,7 +32,7 @@ public:
      * @return robot central velocity
      */
     KDL::Vector getVelocity() {
-        return {_xhat(3), _xhat(4), _xhat(5)};
+        return {x_hat_(3), x_hat_(4), x_hat_(5)};
     }
 
     /**
@@ -85,38 +86,38 @@ public:
     void update(const CtrlComponent &ctrlComp);
 
 private:
-    Eigen::Matrix<double, 18, 1> _xhat;     // The state of estimator, position(3)+velocity(3)+feet position(3x4)
+    Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
 
-    Eigen::Matrix<double, 3, 1> _u;         // The input of estimator
+    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> _yhat;     // The prediction 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, 18> A; // The transtion matrix of estimator
-    Eigen::Matrix<double, 18, 3> B;         // The input matrix
+    Eigen::Matrix<double, 18, 3> B; // The input matrix
-    Eigen::Matrix<double, 28, 18> C;        // The output matrix
+    Eigen::Matrix<double, 28, 18> C; // The output matrix
 
     // Covariance Matrix
-    Eigen::Matrix<double, 18, 18> P;        // Prediction covariance
+    Eigen::Matrix<double, 18, 18> P; // Prediction covariance
-    Eigen::Matrix<double, 18, 18> Ppriori;  // Priori prediction covariance
+    Eigen::Matrix<double, 18, 18> Ppriori; // Priori prediction covariance
-    Eigen::Matrix<double, 18, 18> Q;        // Dynamic simulation covariance
+    Eigen::Matrix<double, 18, 18> Q; // Dynamic simulation covariance
-    Eigen::Matrix<double, 28, 28> R;        // Measurement covariance
+    Eigen::Matrix<double, 28, 28> R; // Measurement covariance
-    Eigen::Matrix<double, 18, 18> QInit_;   // Initial value of Dynamic simulation covariance
+    Eigen::Matrix<double, 18, 18> QInit_; // Initial value of Dynamic simulation covariance
-    Eigen::Matrix<double, 28, 28> RInit_;   // Initial value of Measurement covariance
+    Eigen::Matrix<double, 28, 28> RInit_; // Initial value of Measurement covariance
-    Eigen::Matrix<double, 18, 1> Qdig;      // adjustable process noise covariance
+    Eigen::Matrix<double, 18, 1> Qdig; // adjustable process noise covariance
-    Eigen::Matrix<double, 3, 3> Cu;         // The covariance of system input u
+    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> _feetPos2Body; // 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> _feetVel2Body; // 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> _feetH; // The Height of each foot, in the global coordinate
 
-    Eigen::Matrix<double, 28, 28> S;        // _S = C*P*C.T + R
+    Eigen::Matrix<double, 28, 28> S; // _S = C*P*C.T + R
     Eigen::PartialPivLU<Eigen::Matrix<double, 28, 28> > Slu; // _S.lu()
-    Eigen::Matrix<double, 28, 1> Sy;        // _Sy = _S.inv() * (y - yhat)
+    Eigen::Matrix<double, 28, 1> Sy; // _Sy = _S.inv() * (y - yhat)
-    Eigen::Matrix<double, 28, 18> Sc;       // _Sc = _S.inv() * C
+    Eigen::Matrix<double, 28, 18> Sc; // _Sc = _S.inv() * C
-    Eigen::Matrix<double, 28, 28> SR;       // _SR = _S.inv() * R
+    Eigen::Matrix<double, 28, 28> SR; // _SR = _S.inv() * R
-    Eigen::Matrix<double, 28, 18> STC;      // _STC = (_S.transpose()).inv() * C
+    Eigen::Matrix<double, 28, 18> STC; // _STC = (_S.transpose()).inv() * C
-    Eigen::Matrix<double, 18, 18> IKC;      // _IKC = I - KC
+    Eigen::Matrix<double, 18, 18> IKC; // _IKC = I - KC
 
     KDL::Vector g_;
     double _dt;

controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h

@@ -7,6 +7,7 @@
 #define QUADRUPEDROBOT_H
 #include <string>
 #include <kdl_parser/kdl_parser.hpp>
+#include <unitree_guide_controller/common/mathTypes.h>
 
 #include "RobotLeg.h"
 
@@ -54,6 +55,15 @@ public:
      * @param index leg index
      * @return torque
      */
+    [[nodiscard]] KDL::JntArray getTorque(
+        const Vec3 &force, int index) const;
+
+    /**
+    * Calculate the torque based on joint positions, joint velocities and external force
+    * @param force external force
+    * @param index leg index
+    * @return torque
+    */
     [[nodiscard]] KDL::JntArray getTorque(
         const KDL::Vector &force, int index) const;
 

controllers/unitree_guide_controller/include/unitree_guide_controller/robot/RobotLeg.h

@@ -11,6 +11,7 @@
 #include <kdl/chainjnttojacsolver.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
+#include <unitree_guide_controller/common/mathTypes.h>
 
 class RobotLeg {
 public:
@@ -46,7 +47,7 @@ public:
      * @param force foot end force
      * @return joint torque
      */
-    [[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const KDL::Vector &force) const;
+    [[nodiscard]] KDL::JntArray calcTorque(const KDL::JntArray &joint_positions, const Vec3 &force) const;
 
 protected:
     KDL::Chain chain_;

controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp

@@ -25,9 +25,9 @@ StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateNa
 }
 
 void StateBalanceTest::enter() {
-    pcd_ = ctrl_comp_.estimator_.get().getPosition();
+    pcdInit_ = ctrl_comp_.estimator_.get().getPosition();
-    pcdInit_ = pcd_;
+    pcd_ = pcdInit_;
-    RdInit_ = ctrl_comp_.estimator_.get().getRotation();
+    init_rotation_ = Eigen::Matrix3d(ctrl_comp_.estimator_.get().getRotation().data);
 }
 
 void StateBalanceTest::run() {
@@ -35,7 +35,7 @@ void StateBalanceTest::run() {
     pcd_(1) = pcdInit_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax);
     pcd_(2) = pcdInit_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax);
     const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax);
-    Rd_ = KDL::Rotation::RPY(0, 0, yaw) * RdInit_;
+    Rd_ = rpyToRotMat(0, 0, yaw) * init_rotation_;
 
     pose_body_ = ctrl_comp_.estimator_.get().getPosition();
     vel_body_ = ctrl_comp_.estimator_.get().getVelocity();
@@ -63,26 +63,25 @@ FSMStateName StateBalanceTest::checkChange() {
 }
 
 void StateBalanceTest::calcTorque() {
+    const auto B2G_Rotation = Eigen::Matrix3d(ctrl_comp_.estimator_.get().getRotation().data);
+    const RotMat G2B_Rotation = B2G_Rotation.transpose();
+
     // expected body acceleration
-    _ddPcd = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3(
+    dd_pcd_ = Kp_p_ * Vec3((pcd_ - pose_body_).data) + Kd_p_ * Vec3((-vel_body_).data);
-                 (KDL::Vector(0, 0, 0) - vel_body_).data);
 
     // expected body angular acceleration
-    const KDL::Rotation B2G_Rotation = ctrl_comp_.estimator_.get().getRotation();
+    d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
-    const KDL::Rotation G2B_Rotation = B2G_Rotation.Inverse();
+             Kd_w_ * (Vec3(0,0,0) - Vec3((-ctrl_comp_.estimator_.get().getGlobalGyro()).data));
-    _dWbd = kp_w_ * rotationToExp(Rd_ * G2B_Rotation) +
-            Kd_w_ * (Vec3(0, 0, 0) - Vec3(ctrl_comp_.estimator_.get().getGlobalGyro().data));
 
     // calculate foot force
     const std::vector contact(4, 1);
-    const std::vector<KDL::Vector> foot_force = ctrl_comp_.balance_ctrl_.get().calF(_ddPcd, _dWbd, B2G_Rotation,
+    const Vec34 foot_force = G2B_Rotation * ctrl_comp_.balance_ctrl_.get().calF(dd_pcd_, -d_wbd_, B2G_Rotation,
-        ctrl_comp_.estimator_.get().
+                                 ctrl_comp_.estimator_.get().
-        getFootPos2Body(), contact);
+                                 getFootPos2Body(), contact);
 
     std::vector<KDL::JntArray> current_joints = ctrl_comp_.robot_model_.get().current_joint_pos_;
     for (int i = 0; i < 4; i++) {
-        std::cout<<Vec3(foot_force[i].data).transpose()<<std::endl;
+        KDL::JntArray torque = ctrl_comp_.robot_model_.get().getTorque(-foot_force.col(i), i);
-        KDL::JntArray torque = ctrl_comp_.robot_model_.get().getTorque(B2G_Rotation*(-foot_force[i]), i);
         for (int j = 0; j < 3; j++) {
             ctrl_comp_.joint_effort_command_interface_[i * 3 + j].get().set_value(torque(j));
             ctrl_comp_.joint_position_command_interface_[i * 3 + j].get().set_value(current_joints[i](j));

controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp

@@ -5,21 +5,24 @@
 #include "unitree_guide_controller/control/BalanceCtrl.h"
 
 #include <unitree_guide_controller/common/mathTools.h>
-#include <unitree_guide_controller/quadProgpp/QuadProg++.hh>
 #include <unitree_guide_controller/robot/QuadrupedRobot.h>
 
+#include "quadProgpp/QuadProg++.hh"
+
 BalanceCtrl::BalanceCtrl() {
-    _mass = 15;
+    mass_ = 15;
-    _alpha = 0.001;
+    alpha_ = 0.001;
-    _beta = 0.1;
+    beta_ = 0.1;
-    _fricRatio = 0.4;
     _g << 0, 0, -9.81;
+    friction_ratio_ = 0.4;
+    friction_mat_ << 1, 0, friction_ratio_, -1, 0, friction_ratio_, 0, 1, friction_ratio_, 0, -1,
+            friction_ratio_, 0, 0, 1;
 }
 
 void BalanceCtrl::init(const QuadrupedRobot &robot) {
-    _mass = robot.mass_;
+    mass_ = robot.mass_;
-    _pcb = KDL::Vector(0.0, 0.0, 0.0);
+    _pcb = Vec3(0.0, 0.0, 0.0);
-    _Ib = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
+    Ib_ = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
 
     Vec6 s;
     Vec12 w, u;
@@ -27,46 +30,42 @@ void BalanceCtrl::init(const QuadrupedRobot &robot) {
     u << 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3;
 
     s << 20, 20, 50, 450, 450, 450;
-    _S = s.asDiagonal();
-    _W = w.asDiagonal();
-    _U = u.asDiagonal();
 
-    _Fprev.setZero();
+    S_ = s.asDiagonal();
-    _fricMat << 1, 0, _fricRatio, -1, 0, _fricRatio, 0, 1, _fricRatio, 0, -1,
+    W_ = w.asDiagonal();
-            _fricRatio, 0, 0, 1;
+    U_ = u.asDiagonal();
+
+    F_prev_.setZero();
 }
 
-std::vector<KDL::Vector> BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM,
+Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
-                                           const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact) {
+                        const std::vector<KDL::Vector> &feetPos2B, const std::vector<int> &contact) {
+    std::cout << "ddPcd: " << ddPcd.transpose() << std::endl;
+    std::cout << "dWbd: " << dWbd.transpose() << std::endl;
+
     calMatrixA(feetPos2B, rotM);
     calVectorBd(ddPcd, dWbd, rotM);
     calConstraints(contact);
 
-    _G = _A.transpose() * _S * _A + _alpha * _W + _beta * _U;
+    G_ = A_.transpose() * S_ * A_ + alpha_ * W_ + beta_ * U_;
-    _g0T = -_bd.transpose() * _S * _A - _beta * _Fprev.transpose() * _U;
+    g0T_ = -bd_.transpose() * S_ * A_ - beta_ * F_prev_.transpose() * U_;
 
     solveQP();
-    _Fprev = _F;
+    F_prev_ = F_;
-    std::vector<KDL::Vector> res;
+
-    res.resize(4);
+    return vec12ToVec34(F_);
-    for (int i = 0; i < 4; ++i) {
-        res[i] = KDL::Vector(_F(i * 3), _F(i * 3 + 1), _F(i * 3 + 2));
-    }
-    return res;
 }
 
-void BalanceCtrl::calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const KDL::Rotation &rotM) {
+void BalanceCtrl::calMatrixA(const std::vector<KDL::Vector> &feetPos2B, const RotMat &rotM) {
     for (int i = 0; i < 4; ++i) {
-        _A.block(0, 3 * i, 3, 3) = I3;
+        A_.block(0, 3 * i, 3, 3) = I3;
-        KDL::Vector tempVec = feetPos2B[i] - rotM * _pcb;
+        A_.block(3, 3 * i, 3, 3) = skew(Vec3(feetPos2B[i].data) - rotM * _pcb);
-        _A.block(3, 3 * i, 3, 3) = skew(tempVec);
     }
 }
 
-void BalanceCtrl::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const KDL::Rotation &rotM) {
+void BalanceCtrl::calVectorBd(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM) {
-    _bd.head(3) = _mass * (ddPcd - _g);
+    bd_.head(3) = mass_ * (ddPcd - _g);
-    _bd.tail(3) = Eigen::Matrix3d(rotM.data) * _Ib * Eigen::Matrix3d(rotM.data).transpose() *
+    bd_.tail(3) = rotM * Ib_ * rotM.transpose() * dWbd;
-                  dWbd;
 }
 
 void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
@@ -76,33 +75,33 @@ void BalanceCtrl::calConstraints(const std::vector<int> &contact) {
             contactLegNum += 1;
         }
     }
-    _CI.resize(5 * contactLegNum, 12);
+    CI_.resize(5 * contactLegNum, 12);
-    _ci0.resize(5 * contactLegNum);
+    ci0_.resize(5 * contactLegNum);
-    _CE.resize(3 * (4 - contactLegNum), 12);
+    CE_.resize(3 * (4 - contactLegNum), 12);
-    _ce0.resize(3 * (4 - contactLegNum));
+    ce0_.resize(3 * (4 - contactLegNum));
 
-    _CI.setZero();
+    CI_.setZero();
-    _ci0.setZero();
+    ci0_.setZero();
-    _CE.setZero();
+    CE_.setZero();
-    _ce0.setZero();
+    ce0_.setZero();
 
     int ceID = 0;
     int ciID = 0;
     for (int i(0); i < 4; ++i) {
         if (contact[i] == 1) {
-            _CI.block(5 * ciID, 3 * i, 5, 3) = _fricMat;
+            CI_.block(5 * ciID, 3 * i, 5, 3) = friction_mat_;
             ++ciID;
         } else {
-            _CE.block(3 * ceID, 3 * i, 3, 3) = I3;
+            CE_.block(3 * ceID, 3 * i, 3, 3) = I3;
             ++ceID;
         }
     }
 }
 
 void BalanceCtrl::solveQP() {
-    const long n = _F.size();
+    const long n = F_.size();
-    const long m = _ce0.size();
+    const long m = ce0_.size();
-    const long p = _ci0.size();
+    const long p = ci0_.size();
 
     quadprogpp::Matrix<double> G, CE, CI;
     quadprogpp::Vector<double> g0, ce0, ci0, x;
@@ -117,37 +116,37 @@ void BalanceCtrl::solveQP() {
 
     for (int i = 0; i < n; ++i) {
         for (int j = 0; j < n; ++j) {
-            G[i][j] = _G(i, j);
+            G[i][j] = G_(i, j);
         }
     }
 
     for (int i = 0; i < n; ++i) {
         for (int j = 0; j < m; ++j) {
-            CE[i][j] = (_CE.transpose())(i, j);
+            CE[i][j] = CE_.transpose()(i, j);
         }
     }
 
     for (int i = 0; i < n; ++i) {
         for (int j = 0; j < p; ++j) {
-            CI[i][j] = (_CI.transpose())(i, j);
+            CI[i][j] = CI_.transpose()(i, j);
         }
     }
 
     for (int i = 0; i < n; ++i) {
-        g0[i] = _g0T[i];
+        g0[i] = g0T_[i];
     }
 
     for (int i = 0; i < m; ++i) {
-        ce0[i] = _ce0[i];
+        ce0[i] = ce0_[i];
     }
 
     for (int i = 0; i < p; ++i) {
-        ci0[i] = _ci0[i];
+        ci0[i] = ci0_[i];
     }
 
     solve_quadprog(G, g0, CE, ce0, CI, ci0, x);
 
     for (int i = 0; i < n; ++i) {
-        _F[i] = x[i];
+        F_[i] = x[i];
     }
 }

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -6,14 +6,6 @@
 #include "unitree_guide_controller/control/CtrlComponent.h"
 #include "unitree_guide_controller/common/mathTools.h"
 
-#define I3 Eigen::MatrixXd::Identity(3, 3)
-
-// 12x12 Identity Matrix
-#define I12 Eigen::MatrixXd::Identity(12, 12)
-
-// 18x18 Identity Matrix
-#define I18 Eigen::MatrixXd::Identity(18, 18)
-
 Estimator::Estimator() {
     g_ = KDL::Vector(0, 0, -9.81);
     _dt = 0.002;
@@ -22,7 +14,7 @@ Estimator::Estimator() {
         Qdig(i) = i < 6 ? 0.0003 : 0.01;
     }
 
-    _xhat.setZero();
+    x_hat_.setZero();
     _u.setZero();
 
     A.setZero();
@@ -197,8 +189,8 @@ void Estimator::update(const CtrlComponent &ctrlComp) {
                                 ctrlComp.imu_state_interface_[9].get().get_value());
 
     _u = Vec3((rotation_ * acceleration_ + g_).data);
-    _xhat = A * _xhat + B * _u;
+    x_hat_ = A * x_hat_ + B * _u;
-    _yhat = C * _xhat;
+    y_hat_ = C * x_hat_;
 
     // Update the measurement value
     _y << _feetPos2Body, _feetVel2Body, _feetH;
@@ -207,22 +199,22 @@ void Estimator::update(const CtrlComponent &ctrlComp) {
     Ppriori = A * P * A.transpose() + Q;
     S = R + C * Ppriori * C.transpose();
     Slu = S.lu();
-    Sy = Slu.solve(_y - _yhat);
+    Sy = Slu.solve(_y - y_hat_);
     Sc = Slu.solve(C);
     SR = Slu.solve(R);
     STC = S.transpose().lu().solve(C);
     IKC = Eigen::MatrixXd::Identity(18, 18) - Ppriori * C.transpose() * Sc;
 
     // Update the state and covariance matrix
-    _xhat += Ppriori * C.transpose() * Sy;
+    x_hat_ += Ppriori * C.transpose() * Sy;
     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(_xhat(3));
+    low_pass_filters_[0]->addValue(x_hat_(3));
-    low_pass_filters_[1]->addValue(_xhat(4));
+    low_pass_filters_[1]->addValue(x_hat_(4));
-    low_pass_filters_[2]->addValue(_xhat(5));
+    low_pass_filters_[2]->addValue(x_hat_(5));
-    _xhat(3) = low_pass_filters_[0]->getValue();
+    x_hat_(3) = low_pass_filters_[0]->getValue();
-    _xhat(4) = low_pass_filters_[1]->getValue();
+    x_hat_(4) = low_pass_filters_[1]->getValue();
-    _xhat(5) = low_pass_filters_[2]->getValue();
+    x_hat_(5) = low_pass_filters_[2]->getValue();
 }

controllers/unitree_guide_controller/src/quadProgpp/Array.cc

@@ -5,24 +5,26 @@
 // This software may be modified and distributed under the terms
 // of the MIT license.  See the LICENSE file for details.
 
-#include "unitree_guide_controller/quadProgpp/Array.hh"
+#include "Array.hh"
 
 /**
   Index utilities
  */
 
 namespace quadprogpp {
-    std::set<unsigned int> seq(unsigned int s, unsigned int e) {
-        std::set<unsigned int> tmp;
-        for (unsigned int i = s; i <= e; i++) tmp.insert(i);
 
-        return tmp;
+std::set<unsigned int> seq(unsigned int s, unsigned int e) {
-    }
+  std::set<unsigned int> tmp;
+  for (unsigned int i = s; i <= e; i++) tmp.insert(i);
 
-    std::set<unsigned int> singleton(unsigned int i) {
+  return tmp;
-        std::set<unsigned int> tmp;
+}
-        tmp.insert(i);
 
-        return tmp;
+std::set<unsigned int> singleton(unsigned int i) {
-    }
+  std::set<unsigned int> tmp;
-} // namespace quadprogpp
+  tmp.insert(i);
+
+  return tmp;
+}
+
+}  // namespace quadprogpp

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -60,10 +60,14 @@ KDL::Jacobian QuadrupedRobot::getJacobian(const int index) const {
 }
 
 KDL::JntArray QuadrupedRobot::getTorque(
-    const KDL::Vector &force, const int index) const {
+    const Vec3 &force, const int index) const {
     return robot_legs_[index]->calcTorque(current_joint_pos_[index], force);
 }
 
+KDL::JntArray QuadrupedRobot::getTorque(const KDL::Vector &force, int index) const {
+    return robot_legs_[index]->calcTorque(current_joint_pos_[index], Vec3(force.data));
+}
+
 KDL::Vector QuadrupedRobot::getFeet2BVelocities(const int index) const {
     const Eigen::Matrix<double, 3, Eigen::Dynamic> jacobian = getJacobian(index).data.topRows(3);
     Eigen::VectorXd foot_velocity = jacobian * current_joint_vel_[index].data;

controllers/unitree_guide_controller/src/robot/RobotLeg.cpp

@@ -33,9 +33,9 @@ KDL::Jacobian RobotLeg::calcJaco(const KDL::JntArray &joint_positions) const {
     return jacobian;
 }
 
-KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const KDL::Vector &force) const {
+KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const Vec3 &force) const {
     const Eigen::Matrix<double, 3, Eigen::Dynamic> jacobian = calcJaco(joint_positions).data.topRows(3);
-    Eigen::VectorXd torque_eigen = jacobian.transpose() * Vec3(force.data);
+    Eigen::VectorXd torque_eigen = jacobian.transpose() * force;
     KDL::JntArray torque(chain_.getNrOfJoints());
     for (unsigned int i = 0; i < chain_.getNrOfJoints(); ++i) {
         torque(i) = torque_eigen(i);