From af6599e904f901a13ac45dbfd670f6b0378d5cb6 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Wed, 18 Sep 2024 21:48:14 +0800 Subject: [PATCH] use unitree rotation calculation --- .../FSM/StateBalanceTest.h | 4 +- .../FSM/StateTrotting.h | 19 ++-- .../common/mathTools.h | 31 +++++++ .../control/Estimator.h | 42 +++++---- .../gait/GaitGenerator.h | 2 +- .../src/FSM/StateBalanceTest.cpp | 17 ++-- .../src/FSM/StateFreeStand.cpp | 2 +- .../src/FSM/StateTrotting.cpp | 86 ++++++++++--------- .../src/UnitreeGuideController.cpp | 1 - .../src/control/BalanceCtrl.cpp | 3 +- .../src/control/Estimator.cpp | 46 +++++++--- .../src/gait/GaitGenerator.cpp | 2 +- 12 files changed, 155 insertions(+), 100 deletions(-) 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 4dcc673..64e0f77 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 @@ -29,9 +29,7 @@ private: Vec3 pcd_, pcd_init_; RotMat Rd_; - KDL::Rotation init_rotation_; - - Vec3 pose_body_, vel_body_; + RotMat init_rotation_; double kp_w_; Mat3 Kp_p_, Kd_p_, Kd_w_; 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 d542ac6..e1d2ef9 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 @@ -42,34 +42,25 @@ private: QuadrupedRobot &robot_model_; BalanceCtrl &balance_ctrl_; WaveGenerator &wave_generator_; - GaitGenerator gait_generator_; // Robot State - Vec3 _posBody, _velBody; - double _yaw{}, _dYaw{}; - Vec34 _posFeetGlobal, _velFeetGlobal; - Vec34 _posFeet2BGlobal; + Vec3 pos_body_, vel_body_; RotMat B2G_RotMat, G2B_RotMat; - Vec12 _q; // Robot command - Vec3 _pcd; - Vec3 _vCmdGlobal, _vCmdBody; + Vec3 pcd_; + Vec3 vel_target_, _vCmdBody; double dt_; double _yawCmd{}, _dYawCmd{}; double _dYawCmdPast{}; Vec3 _wCmdGlobal; - Vec34 _posFeetGlobalGoal, _velFeetGlobalGoal; - Vec34 _posFeet2BGoal, _velFeet2BGoal; + Vec34 pos_feet_global_goal_, vel_feet_global_goal_; RotMat Rd; - Vec3 _ddPcd, _dWbd; - Vec34 force_feet_global_, force_feet_body_; - Vec12 _tau; // Control Parameters - double _gaitHeight; + double gait_height_; Vec3 pos_error_, vel_error_; Mat3 Kpp, Kdp, Kdw; double _kpw; 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 13bb47e..32ce1e4 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 @@ -81,5 +81,36 @@ T saturation(const T a, Vec2 limits) { return a; } +inline RotMat quatToRotMat(const Quat &q) { + double e0 = q(0); + double e1 = q(1); + double e2 = q(2); + double e3 = q(3); + + RotMat R; + R << 1 - 2 * (e2 * e2 + e3 * e3), 2 * (e1 * e2 - e0 * e3), + 2 * (e1 * e3 + e0 * e2), 2 * (e1 * e2 + e0 * e3), + 1 - 2 * (e1 * e1 + e3 * e3), 2 * (e2 * e3 - e0 * e1), + 2 * (e1 * e3 - e0 * e2), 2 * (e2 * e3 + e0 * e1), + 1 - 2 * (e1 * e1 + e2 * e2); + return R; +} + +inline Vec3 rotMatToRPY(const Mat3 &R) { + Vec3 rpy; + rpy(0) = atan2(R(2, 1), R(2, 2)); + rpy(1) = asin(-R(2, 0)); + rpy(2) = atan2(R(1, 0), R(0, 0)); + return rpy; +} + +inline RotMat rotz(const double &theta) { + const double s = std::sin(theta); + const double c = std::cos(theta); + + RotMat R; + R << c, -s, 0, s, c, 0, 0, 0, 1; + return R; +} #endif //MATHTOOLS_H 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 b0ad6ed..3d51587 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 @@ -7,6 +7,7 @@ #include #include #include +#include #include "LowPassFilter.h" @@ -41,7 +42,7 @@ public: * @return foot position in world frame */ Vec3 getFootPos(const int index) { - return getPosition() + Vec3((rotation_ * foot_poses_[index].p).data); + return getPosition() + rotation_ * Vec3(foot_poses_[index].p.data); } /** @@ -49,11 +50,24 @@ public: * @return feet velocity in world frame */ Vec34 getFeetPos() { - Vec34 feetPos; + Vec34 feet_pos; for (int i(0); i < 4; ++i) { - feetPos.col(i) = getFootPos(i); + feet_pos.col(i) = getFootPos(i); } - return feetPos; + return feet_pos; + } + + /** + * Get the estimated feet velocity in world frame + * @return feet velocity in world frame + */ + Vec34 getFeetVel() { + const std::vector feet_vel = robot_model_.getFeet2BVelocities(); + Vec34 result; + for (int i(0); i < 4; ++i) { + result.col(i) = Vec3(feet_vel[i].data) + getVelocity(); + } + return result; } /** @@ -69,23 +83,19 @@ public: return foot_pos; } - KDL::Rotation getRotation() { + RotMat getRotation() { return rotation_; } - KDL::Vector getGyro() { + Vec3 getGyro() { return gyro_; } [[nodiscard]] Vec3 getGlobalGyro() const { - return Vec3((rotation_ * gyro_).data); + return rotation_ * gyro_; } - [[nodiscard]] double getYaw() const { - double roll, pitch, yaw; - rotation_.GetRPY(roll, pitch, yaw); - return yaw; - } + [[nodiscard]] double getYaw() const; [[nodiscard]] double getDYaw() const { return getGlobalGyro()(2); @@ -130,12 +140,12 @@ private: Eigen::Matrix STC; // _STC = (_S.transpose()).inv() * C Eigen::Matrix IKC; // _IKC = I - KC - KDL::Vector g_; + Vec3 g_; double _dt; - KDL::Rotation rotation_; - KDL::Vector acceleration_; - KDL::Vector gyro_; + RotMat rotation_; + Vec3 acceleration_; + Vec3 gyro_; std::vector foot_poses_; std::vector foot_vels_; 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 14661e7..c61cb4a 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 @@ -22,7 +22,7 @@ public: void setGait(Vec2 vxy_goal_global, double d_yaw_goal, double gait_height); - void run(Vec34 &feet_pos, Vec34 &feet_vel); + void generate(Vec34 &feet_pos, Vec34 &feet_vel); void restart(); diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index 41ccee3..7ec83dd 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -39,11 +39,9 @@ void StateBalanceTest::run() { pcd_(0) = pcd_init_(0) + invNormalize(ctrl_comp_.control_inputs_.get().ly, _xMin, _xMax); pcd_(1) = pcd_init_(1) - invNormalize(ctrl_comp_.control_inputs_.get().lx, _yMin, _yMax); pcd_(2) = pcd_init_(2) + invNormalize(ctrl_comp_.control_inputs_.get().ry, _zMin, _zMax); - const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax); - Rd_ = Mat3((KDL::Rotation::RPY(0, 0, yaw).Inverse() * init_rotation_).data); - pose_body_ = estimator_.getPosition(); - vel_body_ = estimator_.getVelocity(); + const float yaw = invNormalize(ctrl_comp_.control_inputs_.get().rx, _yawMin, _yawMax); + Rd_ = rotz(yaw) * init_rotation_; for (int i = 0; i < 12; i++) { ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.8); @@ -68,15 +66,18 @@ FSMStateName StateBalanceTest::checkChange() { } void StateBalanceTest::calcTorque() { - const auto B2G_Rotation = Eigen::Matrix3d(estimator_.getRotation().data); + const auto B2G_Rotation = estimator_.getRotation(); const RotMat G2B_Rotation = B2G_Rotation.transpose(); + const Vec3 pose_body = estimator_.getPosition(); + const Vec3 vel_body = estimator_.getVelocity(); + // expected body acceleration - dd_pcd_ = Kp_p_ * (pcd_ - pose_body_) + Kd_p_ * -vel_body_; + dd_pcd_ = Kp_p_ * (pcd_ - pose_body) + Kd_p_ * (Vec3(0, 0, 0) - vel_body); // expected body angular acceleration - d_wbd_ = -kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) + - Kd_w_ * -estimator_.getGlobalGyro(); + d_wbd_ = kp_w_ * rotMatToExp(Rd_ * G2B_Rotation) + + Kd_w_ * (Vec3(0, 0, 0) - estimator_.getGlobalGyro()); // calculate foot force const Vec34 foot_force = G2B_Rotation * balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation, diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp index 3c1e13f..1efaa69 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp @@ -62,7 +62,7 @@ void StateFreeStand::calc_body_target(const float row, const float pitch, KDL::Frame fr_2_body_pos; fr_2_body_pos.p = -fr_init_pos_.p; fr_2_body_pos.p.z(fr_2_body_pos.p.z() + height); - fr_2_body_pos.M = KDL::Rotation::RPY(row, pitch, yaw); + fr_2_body_pos.M = KDL::Rotation::RPY(row, pitch, -yaw); const KDL::Frame body_2_fr_pos = fr_2_body_pos.Inverse(); std::vector goal_pos(4, KDL::Frame::Identity()); diff --git a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp index 86c92df..4a774d3 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp @@ -12,7 +12,7 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T balance_ctrl_(ctrlComp.balance_ctrl_), wave_generator_(ctrl_comp_.wave_generator_), gait_generator_(ctrlComp) { - _gaitHeight = 0.08; + gait_height_ = 0.08; Kpp = Vec3(70, 70, 70).asDiagonal(); Kdp = Vec3(10, 10, 10).asDiagonal(); _kpw = 780; @@ -27,7 +27,7 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T } void StateTrotting::enter() { - _pcd = estimator_.getPosition(); + pcd_ = estimator_.getPosition(); _vCmdBody.setZero(); _yawCmd = estimator_.getYaw(); Rd = Mat3(KDL::Rotation::RPY(0, 0, _yawCmd).Inverse().data); @@ -38,18 +38,17 @@ void StateTrotting::enter() { } void StateTrotting::run() { - _posBody = estimator_.getPosition(); - _velBody = estimator_.getVelocity(); - _posFeet2BGlobal = estimator_.getFeetPos2Body(); - _posFeetGlobal = estimator_.getFeetPos(); - B2G_RotMat = Eigen::Matrix3d(estimator_.getRotation().data); + pos_body_ = estimator_.getPosition(); + vel_body_ = estimator_.getVelocity(); + + B2G_RotMat = estimator_.getRotation(); G2B_RotMat = B2G_RotMat.transpose(); getUserCmd(); calcCmd(); - gait_generator_.setGait(_vCmdGlobal.segment(0, 2), _wCmdGlobal(2), _gaitHeight); - gait_generator_.run(_posFeetGlobalGoal, _velFeetGlobalGoal); + gait_generator_.setGait(vel_target_.segment(0, 2), _wCmdGlobal(2), gait_height_); + gait_generator_.generate(pos_feet_global_goal_, vel_feet_global_goal_); calcTau(); calcQQd(); @@ -92,19 +91,19 @@ void StateTrotting::getUserCmd() { void StateTrotting::calcCmd() { /* Movement */ - _vCmdGlobal = B2G_RotMat * _vCmdBody; + vel_target_ = B2G_RotMat * _vCmdBody; - _vCmdGlobal(0) = - saturation(_vCmdGlobal(0), Vec2(_velBody(0) - 0.2, _velBody(0) + 0.2)); - _vCmdGlobal(1) = - saturation(_vCmdGlobal(1), Vec2(_velBody(1) - 0.2, _velBody(1) + 0.2)); + vel_target_(0) = + saturation(vel_target_(0), Vec2(vel_body_(0) - 0.2, vel_body_(0) + 0.2)); + vel_target_(1) = + saturation(vel_target_(1), Vec2(vel_body_(1) - 0.2, vel_body_(1) + 0.2)); - _pcd(0) = saturation(_pcd(0) + _vCmdGlobal(0) * dt_, - Vec2(_posBody(0) - 0.05, _posBody(0) + 0.05)); - _pcd(1) = saturation(_pcd(1) + _vCmdGlobal(1) * dt_, - Vec2(_posBody(1) - 0.05, _posBody(1) + 0.05)); + pcd_(0) = saturation(pcd_(0) + vel_target_(0) * dt_, + Vec2(pos_body_(0) - 0.05, pos_body_(0) + 0.05)); + pcd_(1) = saturation(pcd_(1) + vel_target_(1) * dt_, + Vec2(pos_body_(1) - 0.05, pos_body_(1) + 0.05)); - _vCmdGlobal(2) = 0; + vel_target_(2) = 0; /* Turning */ _yawCmd = _yawCmd + _dYawCmd * dt_; @@ -113,33 +112,37 @@ void StateTrotting::calcCmd() { } void StateTrotting::calcTau() { - pos_error_ = _pcd - _posBody; - vel_error_ = _vCmdGlobal - _velBody; + pos_error_ = pcd_ - pos_body_; + vel_error_ = vel_target_ - vel_body_; - _ddPcd = Kpp * pos_error_ + Kdp * vel_error_; - _dWbd = _kpw * rotMatToExp(Rd * G2B_RotMat) + - Kdw * (_wCmdGlobal - estimator_.getGlobalGyro()); + Vec3 dd_pcd = Kpp * pos_error_ + Kdp * vel_error_; + Vec3 d_wbd = _kpw * rotMatToExp(Rd * G2B_RotMat) + + Kdw * (_wCmdGlobal - estimator_.getGlobalGyro()); - _ddPcd(0) = saturation(_ddPcd(0), Vec2(-3, 3)); - _ddPcd(1) = saturation(_ddPcd(1), Vec2(-3, 3)); - _ddPcd(2) = saturation(_ddPcd(2), Vec2(-5, 5)); + dd_pcd(0) = saturation(dd_pcd(0), Vec2(-3, 3)); + dd_pcd(1) = saturation(dd_pcd(1), Vec2(-3, 3)); + dd_pcd(2) = saturation(dd_pcd(2), Vec2(-5, 5)); - _dWbd(0) = saturation(_dWbd(0), Vec2(-40, 40)); - _dWbd(1) = saturation(_dWbd(1), Vec2(-40, 40)); - _dWbd(2) = saturation(_dWbd(2), Vec2(-10, 10)); + d_wbd(0) = saturation(d_wbd(0), Vec2(-40, 40)); + d_wbd(1) = saturation(d_wbd(1), Vec2(-40, 40)); + d_wbd(2) = saturation(d_wbd(2), Vec2(-10, 10)); - force_feet_global_ = - -balance_ctrl_.calF(_ddPcd, _dWbd, B2G_RotMat, _posFeet2BGlobal, wave_generator_.contact_); + const Vec34 pos_feet2_b_global = estimator_.getFeetPos2Body(); + Vec34 force_feet_global = + -balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet2_b_global, wave_generator_.contact_); + + 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) = - KpSwing * (_posFeetGlobalGoal.col(i) - _posFeetGlobal.col(i)) + - KdSwing * (_velFeetGlobalGoal.col(i) - _velFeetGlobal.col(i)); + force_feet_global.col(i) = + KpSwing * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) + + KdSwing * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i)); } } - force_feet_body_ = G2B_RotMat * force_feet_global_; + Vec34 force_feet_body_ = G2B_RotMat * force_feet_global; std::vector current_joints = robot_model_.current_joint_pos_; for (int i = 0; i < 4; i++) { @@ -151,18 +154,19 @@ void StateTrotting::calcTau() { } void StateTrotting::calcQQd() { - const std::vector _posFeet2B = robot_model_.getFeet2BPositions(); + const std::vector pos_feet2_b = robot_model_.getFeet2BPositions(); + Vec34 pos_feet2_b_goal, vel_feet2_b_goal; for (int i(0); i < 4; ++i) { - _posFeet2BGoal.col(i) = G2B_RotMat * (_posFeetGlobalGoal.col(i) - _posBody); - _velFeet2BGoal.col(i) = G2B_RotMat * (_velFeetGlobalGoal.col(i) - _velBody); + pos_feet2_b_goal.col(i) = G2B_RotMat * (pos_feet_global_goal_.col(i) - pos_body_); + vel_feet2_b_goal.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_); // _velFeet2BGoal.col(i) = _G2B_RotMat * (_velFeetGlobalGoal.col(i) - // _velBody - _B2G_RotMat * (skew(_lowState->getGyro()) * _posFeet2B.col(i)) // ); // c.f formula (6.12) } - Vec12 q_goal = robot_model_.getQ(_posFeet2BGoal); - Vec12 qd_goal = robot_model_.getQd(_posFeet2B, _velFeet2BGoal); + Vec12 q_goal = robot_model_.getQ(pos_feet2_b_goal); + Vec12 qd_goal = robot_model_.getQd(pos_feet2_b, vel_feet2_b_goal); for (int i = 0; i < 12; i++) { ctrl_comp_.joint_position_command_interface_[i].get().set_value(q_goal(i)); ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(qd_goal(i)); diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index b30bbc8..c4bd976 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -49,7 +49,6 @@ namespace unitree_guide_controller { // update_frequency_ = 1.0 / time_diff.count(); // RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_); - ctrl_comp_.robot_model_.update(); ctrl_comp_.wave_generator_.update(); ctrl_comp_.estimator_.update(); diff --git a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp index ae147ae..23bce34 100644 --- a/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp +++ b/controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp @@ -47,8 +47,8 @@ Vec34 BalanceCtrl::calF(const Vec3 &ddPcd, const Vec3 &dWbd, const RotMat &rot_m g0T_ = -bd_.transpose() * S_ * A_ - beta_ * F_prev_.transpose() * U_; solveQP(); - F_prev_ = F_; + F_prev_ = F_; return vec12ToVec34(F_); } @@ -71,6 +71,7 @@ void BalanceCtrl::calConstraints(const VecInt4 &contact) { contactLegNum += 1; } } + CI_.resize(5 * contactLegNum, 12); ci0_.resize(5 * contactLegNum); CE_.resize(3 * (4 - contactLegNum), 12); diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index 111c717..a3ed982 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -3,12 +3,14 @@ // #include "unitree_guide_controller/control/Estimator.h" + +#include + #include "unitree_guide_controller/control/CtrlComponent.h" -#include "unitree_guide_controller/common/mathTools.h" Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component), robot_model_(ctrl_component.robot_model_) { - g_ = KDL::Vector(0, 0, -9.81); + g_ << 0, 0, -9.81; _dt = 0.002; _largeVariance = 100; for (int i(0); i < Qdig.rows(); ++i) { @@ -139,6 +141,10 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo low_pass_filters_[2] = std::make_shared(0.02, 3.0); } +double Estimator::getYaw() const { + return rotMatToRPY(rotation_)(2); +} + void Estimator::update() { if (robot_model_.mass_ == 0) return; @@ -176,20 +182,34 @@ void Estimator::update() { } // Acceleration from imu as system input - rotation_ = KDL::Rotation::Quaternion(ctrl_component_.imu_state_interface_[1].get().get_value(), - ctrl_component_.imu_state_interface_[2].get().get_value(), - ctrl_component_.imu_state_interface_[3].get().get_value(), - ctrl_component_.imu_state_interface_[0].get().get_value()); + // rotation_ = KDL::Rotation::Quaternion(ctrl_component_.imu_state_interface_[1].get().get_value(), + // ctrl_component_.imu_state_interface_[2].get().get_value(), + // ctrl_component_.imu_state_interface_[3].get().get_value(), + // ctrl_component_.imu_state_interface_[0].get().get_value()); - gyro_ = KDL::Vector(ctrl_component_.imu_state_interface_[4].get().get_value(), - ctrl_component_.imu_state_interface_[5].get().get_value(), - ctrl_component_.imu_state_interface_[6].get().get_value()); + Quat quat; + quat << ctrl_component_.imu_state_interface_[0].get().get_value(), + ctrl_component_.imu_state_interface_[1].get().get_value(), + ctrl_component_.imu_state_interface_[2].get().get_value(), + ctrl_component_.imu_state_interface_[3].get().get_value(); + rotation_ = quatToRotMat(quat); - acceleration_ = KDL::Vector(ctrl_component_.imu_state_interface_[7].get().get_value(), - ctrl_component_.imu_state_interface_[8].get().get_value(), - ctrl_component_.imu_state_interface_[9].get().get_value()); + gyro_ << ctrl_component_.imu_state_interface_[4].get().get_value(), + ctrl_component_.imu_state_interface_[5].get().get_value(), + ctrl_component_.imu_state_interface_[6].get().get_value(); - _u = Vec3((rotation_ * acceleration_ + g_).data); + // gyro_ = KDL::Vector(ctrl_component_.imu_state_interface_[4].get().get_value(), + // ctrl_component_.imu_state_interface_[5].get().get_value(), + // ctrl_component_.imu_state_interface_[6].get().get_value()); + + acceleration_ << ctrl_component_.imu_state_interface_[7].get().get_value(), + ctrl_component_.imu_state_interface_[8].get().get_value(), + ctrl_component_.imu_state_interface_[9].get().get_value(); + // acceleration_ = KDL::Vector(ctrl_component_.imu_state_interface_[7].get().get_value(), + // ctrl_component_.imu_state_interface_[8].get().get_value(), + // ctrl_component_.imu_state_interface_[9].get().get_value()); + + _u = rotation_ * acceleration_ + g_; x_hat_ = A * x_hat_ + B * _u; y_hat_ = C * x_hat_; diff --git a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp index 6b3ec77..463f33b 100644 --- a/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp +++ b/controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp @@ -20,7 +20,7 @@ void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const gait_height_ = gait_height; } -void GaitGenerator::run(Vec34 &feet_pos, Vec34 &feet_vel) { +void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) { if (first_run_) { start_p_ = estimator_.getFeetPos(); first_run_ = false;