using kdl::RPY

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

@@ -25,7 +25,7 @@ private:
     KDL::Vector pcd_;
     KDL::Vector pcdInit_;
     RotMat Rd_;
-    RotMat init_rotation_;
+    KDL::Rotation init_rotation_;
 
     KDL::Vector pose_body_, vel_body_;
 

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

@@ -61,39 +61,5 @@ inline Vec3 rotMatToExp(const RotMat &rm) {
     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/src/FSM/StateBalanceTest.cpp

@@ -27,7 +27,7 @@ StateBalanceTest::StateBalanceTest(CtrlComponent ctrlComp) : FSMState(FSMStateNa
 void StateBalanceTest::enter() {
     pcdInit_ = ctrl_comp_.estimator_.get().getPosition();
     pcd_ = pcdInit_;
-    init_rotation_ = Eigen::Matrix3d(ctrl_comp_.estimator_.get().getRotation().data);
+    init_rotation_ = ctrl_comp_.estimator_.get().getRotation();
 }
 
 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_ = rpyToRotMat(0, 0, yaw) * init_rotation_;
+    Rd_ = Mat3((KDL::Rotation::RPY(0, 0, yaw).Inverse() * init_rotation_).data);
 
     pose_body_ = ctrl_comp_.estimator_.get().getPosition();
     vel_body_ = ctrl_comp_.estimator_.get().getVelocity();
@@ -71,7 +71,7 @@ void StateBalanceTest::calcTorque() {
 
     // expected body angular acceleration
     d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) +
-             Kd_w_ * (Vec3(0,0,0) - Vec3((-ctrl_comp_.estimator_.get().getGlobalGyro()).data));
+             Kd_w_ * (Vec3(0, 0, 0) - Vec3((-ctrl_comp_.estimator_.get().getGlobalGyro()).data));
 
     // calculate foot force
     const std::vector contact(4, 1);

controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp

@@ -28,7 +28,6 @@ void BalanceCtrl::init(const QuadrupedRobot &robot) {
     Vec12 w, u;
     w << 10, 10, 4, 10, 10, 4, 10, 10, 4, 10, 10, 4;
     u << 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3;
-
     s << 20, 20, 50, 450, 450, 450;
 
     S_ = s.asDiagonal();
@@ -40,9 +39,6 @@ void BalanceCtrl::init(const QuadrupedRobot &robot) {
 
 Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rotM,
                         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);