diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h index 1287eba..24a8dab 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h @@ -20,6 +20,7 @@ public: FSMStateName checkChange() override; private: + QuadrupedRobot &robot_model_; void calc_body_target(float row, float pitch, float yaw, float height); float row_max_, row_min_; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h index 7086e29..05dd83d 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h @@ -10,7 +10,7 @@ class StateSwingTest final : public FSMState { public: - explicit StateSwingTest(CtrlComponent &ctrlComp); + explicit StateSwingTest(CtrlComponent &ctrl_component); void enter() override; @@ -21,11 +21,11 @@ public: FSMStateName checkChange() override; private: - void positionCtrl(); void torqueCtrl() const; + QuadrupedRobot &robot_model_; float _xMin, _xMax; float _yMin, _yMax; float _zMin, _zMax; diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h index f1023f7..e78bb7e 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h @@ -42,20 +42,15 @@ struct CtrlComponent { std::reference_wrapper control_inputs_; int frequency_{}; - QuadrupedRobot default_robot_model_; - std::reference_wrapper robot_model_; + QuadrupedRobot robot_model_; + Estimator estimator_; + BalanceCtrl balance_ctrl_; - Estimator default_estimator_; - std::reference_wrapper estimator_; + WaveGenerator wave_generator_; - BalanceCtrl default_balance_ctrl_; - std::reference_wrapper balance_ctrl_; - - WaveGenerator default_wave_generator_; - std::reference_wrapper wave_generator_; - - CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_), - estimator_(default_estimator_), balance_ctrl_(default_balance_ctrl_), wave_generator_(default_wave_generator_) { + CtrlComponent() : control_inputs_(default_inputs_), + robot_model_(*this), + estimator_(*this) { } void clear() { 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 3a3d197..0a0c7d9 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 @@ -10,11 +10,12 @@ #include "LowPassFilter.h" +class QuadrupedRobot; struct CtrlComponent; class Estimator { public: - Estimator(); + explicit Estimator(CtrlComponent &ctrl_component); ~Estimator() = default; @@ -68,9 +69,12 @@ public: return rotation_ * gyro_; } - void update(const CtrlComponent &ctrlComp); + void update(); private: + CtrlComponent &ctrl_component_; + QuadrupedRobot &robot_model_; + Eigen::Matrix x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4) Eigen::Matrix _u; // The input of estimator diff --git a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h index b593547..73ca436 100644 --- a/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h +++ b/controllers/unitree_guide_controller/include/unitree_guide_controller/robot/QuadrupedRobot.h @@ -16,7 +16,8 @@ struct CtrlComponent; class QuadrupedRobot { public: - explicit QuadrupedRobot() = default; + explicit QuadrupedRobot(CtrlComponent &ctrl_component): ctrl_component_(ctrl_component) { + } ~QuadrupedRobot() = default; @@ -84,9 +85,10 @@ public: std::vector current_joint_pos_; std::vector current_joint_vel_; - void update(const CtrlComponent &ctrlComp); + void update(); private: + CtrlComponent &ctrl_component_; std::vector > robot_legs_; KDL::Chain fr_chain_; diff --git a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp index cfae069..18514b5 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp @@ -4,13 +4,13 @@ #include "unitree_guide_controller/FSM/StateBalanceTest.h" -#include +#include "unitree_guide_controller/common/mathTools.h" StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateName::BALANCETEST, "balance test", - ctrlComp), - estimator_(ctrlComp.estimator_.get()), - robot_model_(ctrlComp.robot_model_.get()), - balance_ctrl_(ctrlComp.balance_ctrl_.get()) { + ctrlComp), + estimator_(ctrlComp.estimator_), + robot_model_(ctrlComp.robot_model_), + balance_ctrl_(ctrlComp.balance_ctrl_) { _xMax = 0.05; _xMin = -_xMax; _yMax = 0.05; diff --git a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp index 229da8c..974e37f 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp @@ -6,7 +6,8 @@ #include "unitree_guide_controller/common/mathTools.h" StateFreeStand::StateFreeStand(CtrlComponent &ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand", - ctrl_component) { + ctrl_component), + robot_model_(ctrl_component.robot_model_) { row_max_ = 20 * M_PI / 180; row_min_ = -row_max_; pitch_max_ = 15 * M_PI / 180; @@ -23,8 +24,8 @@ void StateFreeStand::enter() { ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5); } - init_joint_pos_ = ctrl_comp_.robot_model_.get().current_joint_pos_; - init_foot_pos_ = ctrl_comp_.robot_model_.get().getFeet2BPositions(); + init_joint_pos_ = robot_model_.current_joint_pos_; + init_foot_pos_ = robot_model_.getFeet2BPositions(); fr_init_pos_ = init_foot_pos_[0]; @@ -35,7 +36,6 @@ void StateFreeStand::enter() { } void StateFreeStand::run() { - ctrl_comp_.robot_model_.get().update(ctrl_comp_); calc_body_target(invNormalize(ctrl_comp_.control_inputs_.get().lx, row_min_, row_max_), invNormalize(ctrl_comp_.control_inputs_.get().ly, pitch_min_, pitch_max_), invNormalize(ctrl_comp_.control_inputs_.get().rx, yaw_min_, yaw_max_), @@ -68,7 +68,7 @@ void StateFreeStand::calc_body_target(const float row, const float pitch, for (int i = 0; i < 4; i++) { goal_pos[i] = body_2_fr_pos * init_foot_pos_[i]; } - target_joint_pos_ = ctrl_comp_.robot_model_.get().getQ(goal_pos); + target_joint_pos_ = robot_model_.getQ(goal_pos); for (int i = 0; i < 4; i++) { ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); diff --git a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp index 80187f5..87d41ce 100644 --- a/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp +++ b/controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp @@ -6,8 +6,8 @@ #include "unitree_guide_controller/FSM/StateSwingTest.h" #include "unitree_guide_controller/common/mathTools.h" -StateSwingTest::StateSwingTest(CtrlComponent &ctrlComp): FSMState( - FSMStateName::SWINGTEST, "swing test", ctrlComp) { +StateSwingTest::StateSwingTest(CtrlComponent &ctrl_component): FSMState( + FSMStateName::SWINGTEST, "swing test", ctrl_component), robot_model_(ctrl_component.robot_model_) { _xMin = -0.15; _xMax = 0.10; _yMin = -0.15; @@ -29,8 +29,8 @@ void StateSwingTest::enter() { Kp = KDL::Vector(20, 20, 50); Kd = KDL::Vector(5, 5, 20); - init_joint_pos_ = ctrl_comp_.robot_model_.get().current_joint_pos_; - init_foot_pos_ = ctrl_comp_.robot_model_.get().getFeet2BPositions(); + init_joint_pos_ = robot_model_.current_joint_pos_; + init_foot_pos_ = robot_model_.getFeet2BPositions(); target_foot_pos_ = init_foot_pos_; fr_init_pos_ = init_foot_pos_[0]; @@ -60,7 +60,6 @@ void StateSwingTest::run() { fr_init_pos_.p.z(), -1, 0)); } - ctrl_comp_.robot_model_.get().update(ctrl_comp_); positionCtrl(); torqueCtrl(); } @@ -81,7 +80,7 @@ FSMStateName StateSwingTest::checkChange() { void StateSwingTest::positionCtrl() { target_foot_pos_[0] = fr_goal_pos_; - target_joint_pos_ = ctrl_comp_.robot_model_.get().getQ(target_foot_pos_); + target_joint_pos_ = robot_model_.getQ(target_foot_pos_); for (int i = 0; i < 4; i++) { ctrl_comp_.joint_position_command_interface_[i * 3].get().set_value(target_joint_pos_[i](0)); ctrl_comp_.joint_position_command_interface_[i * 3 + 1].get().set_value(target_joint_pos_[i](1)); @@ -90,14 +89,14 @@ void StateSwingTest::positionCtrl() { } void StateSwingTest::torqueCtrl() const { - const KDL::Frame fr_current_pos = ctrl_comp_.robot_model_.get().getFeet2BPositions(0); + const KDL::Frame fr_current_pos = robot_model_.getFeet2BPositions(0); const KDL::Vector pos_goal = fr_goal_pos_.p; const KDL::Vector pos0 = fr_current_pos.p; - const KDL::Vector vel0 = ctrl_comp_.robot_model_.get().getFeet2BVelocities(0); + const KDL::Vector vel0 = robot_model_.getFeet2BVelocities(0); const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * -vel0; - KDL::JntArray torque0 = ctrl_comp_.robot_model_.get().getTorque(force0, 0); + KDL::JntArray torque0 = robot_model_.getTorque(force0, 0); for (int i = 0; i < 3; i++) { ctrl_comp_.joint_effort_command_interface_[i].get().set_value(torque0(i)); diff --git a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp index c672749..87d3b5c 100644 --- a/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp +++ b/controllers/unitree_guide_controller/src/UnitreeGuideController.cpp @@ -41,7 +41,6 @@ namespace unitree_guide_controller { controller_interface::return_type UnitreeGuideController:: update(const rclcpp::Time &time, const rclcpp::Duration &period) { - // auto now = std::chrono::steady_clock::now(); // std::chrono::duration time_diff = now - last_update_time_; // last_update_time_ = now; @@ -51,8 +50,8 @@ namespace unitree_guide_controller { // RCLCPP_INFO(get_node()->get_logger(), "Update frequency: %f Hz", update_frequency_); - ctrl_comp_.robot_model_.get().update(ctrl_comp_); - ctrl_comp_.estimator_.get().update(ctrl_comp_); + ctrl_comp_.robot_model_.update(); + ctrl_comp_.estimator_.update(); if (mode_ == FSMMode::NORMAL) { current_state_->run(); @@ -108,8 +107,8 @@ namespace unitree_guide_controller { robot_description_subscription_ = get_node()->create_subscription( "~/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(), [this](const std_msgs::msg::String::SharedPtr msg) { - ctrl_comp_.robot_model_.get().init(msg->data); - ctrl_comp_.balance_ctrl_.get().init(ctrl_comp_.robot_model_.get()); + ctrl_comp_.robot_model_.init(msg->data); + ctrl_comp_.balance_ctrl_.init(ctrl_comp_.robot_model_); }); get_node()->get_parameter("update_rate", ctrl_comp_.frequency_); diff --git a/controllers/unitree_guide_controller/src/control/Estimator.cpp b/controllers/unitree_guide_controller/src/control/Estimator.cpp index 1c423cc..111c717 100644 --- a/controllers/unitree_guide_controller/src/control/Estimator.cpp +++ b/controllers/unitree_guide_controller/src/control/Estimator.cpp @@ -6,7 +6,8 @@ #include "unitree_guide_controller/control/CtrlComponent.h" #include "unitree_guide_controller/common/mathTools.h" -Estimator::Estimator() { +Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_component), + robot_model_(ctrl_component.robot_model_) { g_ = KDL::Vector(0, 0, -9.81); _dt = 0.002; _largeVariance = 100; @@ -138,14 +139,14 @@ Estimator::Estimator() { low_pass_filters_[2] = std::make_shared(0.02, 3.0); } -void Estimator::update(const CtrlComponent &ctrlComp) { - if (ctrlComp.robot_model_.get().mass_ == 0) return; +void Estimator::update() { + if (robot_model_.mass_ == 0) return; Q = QInit_; R = RInit_; - foot_poses_ = ctrlComp.robot_model_.get().getFeet2BPositions(); - foot_vels_ = ctrlComp.robot_model_.get().getFeet2BVelocities(); + foot_poses_ = robot_model_.getFeet2BPositions(); + foot_vels_ = robot_model_.getFeet2BVelocities(); _feetH.setZero(); const std::vector contact(4, 1); @@ -175,18 +176,18 @@ void Estimator::update(const CtrlComponent &ctrlComp) { } // Acceleration from imu as system input - rotation_ = KDL::Rotation::Quaternion(ctrlComp.imu_state_interface_[1].get().get_value(), - ctrlComp.imu_state_interface_[2].get().get_value(), - ctrlComp.imu_state_interface_[3].get().get_value(), - ctrlComp.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(ctrlComp.imu_state_interface_[4].get().get_value(), - ctrlComp.imu_state_interface_[5].get().get_value(), - ctrlComp.imu_state_interface_[6].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()); - acceleration_ = KDL::Vector(ctrlComp.imu_state_interface_[7].get().get_value(), - ctrlComp.imu_state_interface_[8].get().get_value(), - ctrlComp.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 = Vec3((rotation_ * acceleration_ + g_).data); x_hat_ = A * x_hat_ + B * _u; diff --git a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp index 5b9e87c..f1f5f52 100644 --- a/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp +++ b/controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp @@ -83,19 +83,19 @@ std::vector QuadrupedRobot::getFeet2BVelocities() const { return result; } -void QuadrupedRobot::update(const CtrlComponent &ctrlComp) { +void QuadrupedRobot::update() { if (mass_ == 0) return; for (int i = 0; i < 4; i++) { KDL::JntArray pos_array(3); - pos_array(0) = ctrlComp.joint_position_state_interface_[i * 3].get().get_value(); - pos_array(1) = ctrlComp.joint_position_state_interface_[i * 3 + 1].get().get_value(); - pos_array(2) = ctrlComp.joint_position_state_interface_[i * 3 + 2].get().get_value(); + pos_array(0) = ctrl_component_.joint_position_state_interface_[i * 3].get().get_value(); + pos_array(1) = ctrl_component_.joint_position_state_interface_[i * 3 + 1].get().get_value(); + pos_array(2) = ctrl_component_.joint_position_state_interface_[i * 3 + 2].get().get_value(); current_joint_pos_[i] = pos_array; KDL::JntArray vel_array(3); - vel_array(0) = ctrlComp.joint_velocity_state_interface_[i * 3].get().get_value(); - vel_array(1) = ctrlComp.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); - vel_array(2) = ctrlComp.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); + vel_array(0) = ctrl_component_.joint_velocity_state_interface_[i * 3].get().get_value(); + vel_array(1) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 1].get().get_value(); + vel_array(2) = ctrl_component_.joint_velocity_state_interface_[i * 3 + 2].get().get_value(); current_joint_vel_[i] = vel_array; } }