From 657029864e5a0a4134532adefdf7e2a2dfbea768 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Tue, 17 Sep 2024 19:42:21 +0800 Subject: [PATCH] using kdl::RPY --- readme.md => README.md | 0 .../FSM/StateBalanceTest.h | 2 +- .../common/mathTools.h | 34 ------------------- .../src/FSM/StateBalanceTest.cpp | 6 ++-- .../src/control/BalanceCtrl.cpp | 4 --- .../{readme.md => README.md} | 0 6 files changed, 4 insertions(+), 42 deletions(-) rename readme.md => README.md (100%) rename hardwares/hardware_unitree_mujoco/{readme.md => README.md} (100%) diff --git a/readme.md b/README.md similarity index 100% rename from readme.md rename to README.md diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h index 465f68c..32d011b 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateBalanceTest.h +++ b/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_; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h index a76bdb3..b91f312 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/common/mathTools.h +++ b/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 diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index c0b4270..3523949 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/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); diff --git a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp index 34da15d..7dbb5cd 100644 --- a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp +++ b/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 &feetPos2B, const std::vector &contact) { - std::cout << "ddPcd: " << ddPcd.transpose() << std::endl; - std::cout << "dWbd: " << dWbd.transpose() << std::endl; - calMatrixA(feetPos2B, rotM); calVectorBd(ddPcd, dWbd, rotM); calConstraints(contact); diff --git a/hardwares/hardware_unitree_mujoco/readme.md b/hardwares/hardware_unitree_mujoco/README.md similarity index 100% rename from hardwares/hardware_unitree_mujoco/readme.md rename to hardwares/hardware_unitree_mujoco/README.md