used share_ptr rather than reference in CtrlComponent

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h

@@ -7,7 +7,6 @@
 
 #include <string>
 #include <utility>
-#include <rclcpp/time.hpp>
 #include <unitree_guide_controller/common/enumClass.h>
 #include <unitree_guide_controller/control/CtrlComponent.h>
 

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

@@ -22,10 +22,10 @@ public:
 private:
     void calcTorque();
 
-    Estimator &estimator_;
+    std::shared_ptr<Estimator> &estimator_;
-    QuadrupedRobot &robot_model_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
-    BalanceCtrl &balance_ctrl_;
+    std::shared_ptr<BalanceCtrl> &balance_ctrl_;
-    WaveGenerator &wave_generator_;
+    std::shared_ptr<WaveGenerator> &wave_generator_;
 
     Vec3 pcd_, pcd_init_;
     RotMat Rd_;

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h

@@ -5,6 +5,8 @@
 #ifndef STATEFIXEDDOWN_H
 #define STATEFIXEDDOWN_H
 
+#include <rclcpp/time.hpp>
+
 #include "FSMState.h"
 
 class StateFixedDown final : public FSMState {
@@ -18,6 +20,7 @@ public:
     void exit() override;
 
     FSMStateName checkChange() override;
+
 private:
     double target_pos_[12] = {
         0.0473455, 1.22187, -2.44375, -0.0473455,

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h

@@ -5,6 +5,8 @@
 #ifndef STATEFIXEDSTAND_H
 #define STATEFIXEDSTAND_H
 
+#include <rclcpp/time.hpp>
+
 #include "FSMState.h"
 
 class StateFixedStand final : public FSMState {

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFreeStand.h

@@ -20,7 +20,7 @@ public:
     FSMStateName checkChange() override;
 
 private:
-    QuadrupedRobot &robot_model_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
     void calc_body_target(float row, float pitch, float yaw, float height);
 
     float row_max_, row_min_;

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateSwingTest.h

@@ -25,7 +25,7 @@ private:
 
     void torqueCtrl() const;
 
-    QuadrupedRobot &robot_model_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
     float _xMin, _xMax;
     float _yMin, _yMax;
     float _zMin, _zMax;

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateTrotting.h

@@ -47,10 +47,11 @@ private:
      */
     bool checkStepOrNot();
 
-    Estimator &estimator_;
+    std::shared_ptr<Estimator> &estimator_;
-    QuadrupedRobot &robot_model_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
-    BalanceCtrl &balance_ctrl_;
+    std::shared_ptr<BalanceCtrl> &balance_ctrl_;
-    WaveGenerator &wave_generator_;
+    std::shared_ptr<WaveGenerator> &wave_generator_;
+
     GaitGenerator gait_generator_;
 
     // Robot State

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

@@ -5,17 +5,17 @@
 #ifndef BALANCECTRL_H
 #define BALANCECTRL_H
 
+#include <memory>
+
 #include "unitree_guide_controller/common/mathTypes.h"
 class QuadrupedRobot;
 
 class BalanceCtrl {
 public:
-    explicit BalanceCtrl();
+    explicit BalanceCtrl(const std::shared_ptr<QuadrupedRobot>& robot);
 
     ~BalanceCtrl() = default;
 
-    void init(const QuadrupedRobot &robot);
-
     /**
      * Calculate the desired feet end force
      * @param ddPcd desired body acceleration

controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h

@@ -41,15 +41,13 @@ struct CtrlComponent {
     control_input_msgs::msg::Inputs control_inputs_;
     int frequency_{};
 
-    QuadrupedRobot robot_model_;
+    std::shared_ptr<QuadrupedRobot> robot_model_;
-    Estimator estimator_;
+    std::shared_ptr<Estimator> estimator_;
-    BalanceCtrl balance_ctrl_;
 
-    WaveGenerator wave_generator_;
+    std::shared_ptr<BalanceCtrl> balance_ctrl_;
+    std::shared_ptr<WaveGenerator> wave_generator_;
 
-    CtrlComponent() : robot_model_(*this),
+    CtrlComponent() = default;
-                      estimator_(*this) {
-    }
 
     void clear() {
         joint_torque_command_interface_.clear();

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

@@ -63,7 +63,7 @@ public:
      * @return feet velocity in world frame
      */
     Vec34 getFeetVel() {
-        const std::vector<KDL::Vector> feet_vel = robot_model_.getFeet2BVelocities();
+        const std::vector<KDL::Vector> feet_vel = robot_model_->getFeet2BVelocities();
         Vec34 result;
         for (int i(0); i < 4; ++i) {
             result.col(i) = Vec3(feet_vel[i].data) + getVelocity();
@@ -106,8 +106,8 @@ public:
 
 private:
     CtrlComponent &ctrl_component_;
-    QuadrupedRobot &robot_model_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
-    WaveGenerator &wave_generator_;
+    std::shared_ptr<WaveGenerator> &wave_generator_;
 
     Eigen::Matrix<double, 18, 1> x_hat_; // The state of estimator, position(3)+velocity(3)+feet position(3x4)
 

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/FeetEndCalc.h

@@ -5,6 +5,7 @@
 
 #ifndef FOOTENDCTRL_H
 #define FOOTENDCTRL_H
+#include <memory>
 #include <unitree_guide_controller/common/mathTypes.h>
 
 
@@ -24,14 +25,13 @@ public:
 
 private:
     CtrlComponent &ctrl_component_;
-    QuadrupedRobot &robot_model_;
+    std::shared_ptr<QuadrupedRobot> &robot_model_;
-    Estimator &estimator_;
+    std::shared_ptr<Estimator> &estimator_;
 
     Vec4 feet_radius_, feet_init_angle_;
 
     double k_x_, k_y_, k_yaw_;
     double t_stance_{}, t_swing_{};
-
 };
 
 

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/GaitGenerator.h

@@ -5,6 +5,7 @@
 
 #ifndef GAITGENERATOR_H
 #define GAITGENERATOR_H
+#include <memory>
 #include <unitree_guide_controller/common/mathTypes.h>
 
 #include "FeetEndCalc.h"
@@ -66,8 +67,8 @@ private:
      */
     [[nodiscard]] double cycloidZVelocity(double height, double phase) const;
 
-    WaveGenerator &wave_generator_;
+    std::shared_ptr<WaveGenerator> &wave_generator_;
-    Estimator &estimator_;
+    std::shared_ptr<Estimator> &estimator_;
     FeetEndCalc feet_end_calc_;
 
     double gait_height_{};

controllers/unitree_guide_controller/include/unitree_guide_controller/gait/WaveGenerator.h

@@ -17,12 +17,10 @@ inline long long getSystemTime() {
 
 class WaveGenerator {
 public:
-    WaveGenerator();
+    WaveGenerator(double period, double st_ratio, const Vec4 &bias);
 
     ~WaveGenerator() = default;
 
-    void init(double period, double st_ratio, const Vec4 &bias);
-
     void update();
 
     [[nodiscard]] double get_t_stance() const { return period_ * st_ratio_; }

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

@@ -16,13 +16,11 @@ struct CtrlComponent;
 
 class QuadrupedRobot {
 public:
-    explicit QuadrupedRobot(CtrlComponent &ctrl_component): ctrl_component_(ctrl_component) {
+    explicit QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
-    }
+                            const std::vector<std::string> &feet_names, const std::string &base_name);
 
     ~QuadrupedRobot() = default;
 
-    void init(const std::string &robot_description, const std::vector<std::string> &feet_names, const std::string& base_name);
-
     /**
      * Calculate the joint positions based on the foot end position
      * @param pEe_list vector of foot-end position

controllers/unitree_guide_controller/src/FSM/StateBalanceTest.cpp

@@ -29,10 +29,10 @@ StateBalanceTest::StateBalanceTest(CtrlComponent &ctrlComp) : FSMState(FSMStateN
 }
 
 void StateBalanceTest::enter() {
-    pcd_init_ = estimator_.getPosition();
+    pcd_init_ = estimator_->getPosition();
     pcd_ = pcd_init_;
-    init_rotation_ = estimator_.getRotation();
+    init_rotation_ = estimator_->getRotation();
-    wave_generator_.status_ = WaveStatus::STANCE_ALL;
+    wave_generator_->status_ = WaveStatus::STANCE_ALL;
 }
 
 void StateBalanceTest::run() {
@@ -52,7 +52,7 @@ void StateBalanceTest::run() {
 }
 
 void StateBalanceTest::exit() {
-    wave_generator_.status_ = WaveStatus::SWING_ALL;
+    wave_generator_->status_ = WaveStatus::SWING_ALL;
 }
 
 FSMStateName StateBalanceTest::checkChange() {
@@ -67,28 +67,28 @@ FSMStateName StateBalanceTest::checkChange() {
 }
 
 void StateBalanceTest::calcTorque() {
-    const auto B2G_Rotation = estimator_.getRotation();
+    const auto B2G_Rotation = estimator_->getRotation();
     const RotMat G2B_Rotation = B2G_Rotation.transpose();
 
-    const Vec3 pose_body = estimator_.getPosition();
+    const Vec3 pose_body = estimator_->getPosition();
-    const Vec3 vel_body = estimator_.getVelocity();
+    const Vec3 vel_body = estimator_->getVelocity();
 
     // expected body acceleration
     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_ * (Vec3(0, 0, 0) - estimator_.getGyroGlobal());
+             Kd_w_ * (Vec3(0, 0, 0) - estimator_->getGyroGlobal());
 
     // calculate foot force
-    const Vec34 pos_feet_2_body_global = estimator_.getFeetPos2Body();
+    const Vec34 pos_feet_2_body_global = estimator_->getFeetPos2Body();
-    const Vec34 force_feet_global = -balance_ctrl_.calF(dd_pcd_, d_wbd_, B2G_Rotation,
+    const Vec34 force_feet_global = -balance_ctrl_->calF(dd_pcd_, d_wbd_, B2G_Rotation,
-                                                        pos_feet_2_body_global, wave_generator_.contact_);
+                                                        pos_feet_2_body_global, wave_generator_->contact_);
     const Vec34 force_feet_body = G2B_Rotation * force_feet_global;
 
-    std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
+    std::vector<KDL::JntArray> current_joints = robot_model_->current_joint_pos_;
     for (int i = 0; i < 4; i++) {
-        KDL::JntArray torque = robot_model_.getTorque(force_feet_body.col(i), i);
+        KDL::JntArray torque = robot_model_->getTorque(force_feet_body.col(i), i);
         for (int j = 0; j < 3; j++) {
             ctrl_comp_.joint_torque_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/FSM/StateFreeStand.cpp

@@ -24,8 +24,8 @@ void StateFreeStand::enter() {
         ctrl_comp_.joint_kd_command_interface_[i].get().set_value(5);
     }
 
-    init_joint_pos_ = robot_model_.current_joint_pos_;
+    init_joint_pos_ = robot_model_->current_joint_pos_;
-    init_foot_pos_ = robot_model_.getFeet2BPositions();
+    init_foot_pos_ = robot_model_->getFeet2BPositions();
 
 
     fr_init_pos_ = init_foot_pos_[0];
@@ -69,7 +69,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_ = robot_model_.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));

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -7,7 +7,9 @@
 #include "unitree_guide_controller/common/mathTools.h"
 
 StateSwingTest::StateSwingTest(CtrlComponent &ctrl_component): FSMState(
-    FSMStateName::SWINGTEST, "swing test", ctrl_component), robot_model_(ctrl_component.robot_model_) {
+                                                                   FSMStateName::SWINGTEST, "swing test",
+                                                                   ctrl_component),
+                                                               robot_model_(ctrl_component.robot_model_) {
     _xMin = -0.15;
     _xMax = 0.10;
     _yMin = -0.15;
@@ -29,8 +31,8 @@ void StateSwingTest::enter() {
     Kp = KDL::Vector(20, 20, 50);
     Kd = KDL::Vector(5, 5, 20);
 
-    init_joint_pos_ = robot_model_.current_joint_pos_;
+    init_joint_pos_ = robot_model_->current_joint_pos_;
-    init_foot_pos_ = robot_model_.getFeet2BPositions();
+    init_foot_pos_ = robot_model_->getFeet2BPositions();
 
     target_foot_pos_ = init_foot_pos_;
     fr_init_pos_ = init_foot_pos_[0];
@@ -80,7 +82,7 @@ FSMStateName StateSwingTest::checkChange() {
 
 void StateSwingTest::positionCtrl() {
     target_foot_pos_[0] = fr_goal_pos_;
-    target_joint_pos_ = robot_model_.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));
@@ -89,14 +91,14 @@ void StateSwingTest::positionCtrl() {
 }
 
 void StateSwingTest::torqueCtrl() const {
-    const KDL::Frame fr_current_pos = robot_model_.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 = robot_model_.getFeet2BVelocities(0);
+    const KDL::Vector vel0 = robot_model_->getFeet2BVelocities(0);
 
     const KDL::Vector force0 = Kp * (pos_goal - pos0) + Kd * -vel0;
-    KDL::JntArray torque0 = robot_model_.getTorque(force0, 0);
+    KDL::JntArray torque0 = robot_model_->getTorque(force0, 0);
 
     for (int i = 0; i < 3; i++) {
         ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque0(i));

controllers/unitree_guide_controller/src/FSM/StateTrotting.cpp

@@ -27,10 +27,10 @@ StateTrotting::StateTrotting(CtrlComponent &ctrlComp) : FSMState(FSMStateName::T
 }
 
 void StateTrotting::enter() {
-    pcd_ = estimator_.getPosition();
+    pcd_ = estimator_->getPosition();
-    pcd_(2) = -estimator_.getFeetPos2Body()(2, 0);
+    pcd_(2) = -estimator_->getFeetPos2Body()(2, 0);
     v_cmd_body_.setZero();
-    yaw_cmd_ = estimator_.getYaw();
+    yaw_cmd_ = estimator_->getYaw();
     Rd = rotz(yaw_cmd_);
     w_cmd_global_.setZero();
 
@@ -39,10 +39,10 @@ void StateTrotting::enter() {
 }
 
 void StateTrotting::run() {
-    pos_body_ = estimator_.getPosition();
+    pos_body_ = estimator_->getPosition();
-    vel_body_ = estimator_.getVelocity();
+    vel_body_ = estimator_->getVelocity();
 
-    B2G_RotMat = estimator_.getRotation();
+    B2G_RotMat = estimator_->getRotation();
     G2B_RotMat = B2G_RotMat.transpose();
 
     getUserCmd();
@@ -55,16 +55,16 @@ void StateTrotting::run() {
     calcQQd();
 
     if (checkStepOrNot()) {
-        wave_generator_.status_ = WaveStatus::WAVE_ALL;
+        wave_generator_->status_ = WaveStatus::WAVE_ALL;
     } else {
-        wave_generator_.status_ = WaveStatus::STANCE_ALL;
+        wave_generator_->status_ = WaveStatus::STANCE_ALL;
     }
 
     calcGain();
 }
 
 void StateTrotting::exit() {
-    wave_generator_.status_ = WaveStatus::SWING_ALL;
+    wave_generator_->status_ = WaveStatus::SWING_ALL;
 }
 
 FSMStateName StateTrotting::checkChange() {
@@ -118,7 +118,7 @@ void StateTrotting::calcTau() {
 
     Vec3 dd_pcd = Kpp * pos_error_ + Kdp * vel_error_;
     Vec3 d_wbd = kp_w_ * rotMatToExp(Rd * G2B_RotMat) +
-                 Kd_w_ * (w_cmd_global_ - estimator_.getGyroGlobal());
+                 Kd_w_ * (w_cmd_global_ - estimator_->getGyroGlobal());
 
     dd_pcd(0) = saturation(dd_pcd(0), Vec2(-3, 3));
     dd_pcd(1) = saturation(dd_pcd(1), Vec2(-3, 3));
@@ -128,16 +128,16 @@ void StateTrotting::calcTau() {
     d_wbd(1) = saturation(d_wbd(1), Vec2(-40, 40));
     d_wbd(2) = saturation(d_wbd(2), Vec2(-10, 10));
 
-    const Vec34 pos_feet_body_global = estimator_.getFeetPos2Body();
+    const Vec34 pos_feet_body_global = estimator_->getFeetPos2Body();
     Vec34 force_feet_global =
-            -balance_ctrl_.calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet_body_global, wave_generator_.contact_);
+            -balance_ctrl_->calF(dd_pcd, d_wbd, B2G_RotMat, pos_feet_body_global, wave_generator_->contact_);
 
 
-    Vec34 pos_feet_global = estimator_.getFeetPos();
+    Vec34 pos_feet_global = estimator_->getFeetPos();
-    Vec34 vel_feet_global = estimator_.getFeetVel();
+    Vec34 vel_feet_global = estimator_->getFeetVel();
 
     for (int i(0); i < 4; ++i) {
-        if (wave_generator_.contact_(i) == 0) {
+        if (wave_generator_->contact_(i) == 0) {
             force_feet_global.col(i) =
                 Kp_swing_ * (pos_feet_global_goal_.col(i) - pos_feet_global.col(i)) +
                 Kd_swing_ * (vel_feet_global_goal_.col(i) - vel_feet_global.col(i));
@@ -146,9 +146,9 @@ void StateTrotting::calcTau() {
 
     Vec34 force_feet_body_ = G2B_RotMat * force_feet_global;
 
-    std::vector<KDL::JntArray> current_joints = robot_model_.current_joint_pos_;
+    std::vector<KDL::JntArray> current_joints = robot_model_->current_joint_pos_;
     for (int i = 0; i < 4; i++) {
-        KDL::JntArray torque = robot_model_.getTorque(force_feet_body_.col(i), i);
+        KDL::JntArray torque = robot_model_->getTorque(force_feet_body_.col(i), i);
         for (int j = 0; j < 3; j++) {
             ctrl_comp_.joint_torque_command_interface_[i * 3 + j].get().set_value(torque(j));
         }
@@ -156,7 +156,7 @@ void StateTrotting::calcTau() {
 }
 
 void StateTrotting::calcQQd() {
-    const std::vector<KDL::Frame> pos_feet_body = robot_model_.getFeet2BPositions();
+    const std::vector<KDL::Frame> pos_feet_body = robot_model_->getFeet2BPositions();
 
     Vec34 pos_feet_target, vel_feet_target;
     for (int i(0); i < 4; ++i) {
@@ -164,8 +164,8 @@ void StateTrotting::calcQQd() {
         vel_feet_target.col(i) = G2B_RotMat * (vel_feet_global_goal_.col(i) - vel_body_);
     }
 
-    Vec12 q_goal = robot_model_.getQ(pos_feet_target);
+    Vec12 q_goal = robot_model_->getQ(pos_feet_target);
-    Vec12 qd_goal = robot_model_.getQd(pos_feet_body, vel_feet_target);
+    Vec12 qd_goal = robot_model_->getQd(pos_feet_body, vel_feet_target);
     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));
@@ -174,7 +174,7 @@ void StateTrotting::calcQQd() {
 
 void StateTrotting::calcGain() const {
     for (int i(0); i < 4; ++i) {
-        if (wave_generator_.contact_(i) == 0) {
+        if (wave_generator_->contact_(i) == 0) {
             // swing gain
             for (int j = 0; j < 3; j++) {
                 ctrl_comp_.joint_kp_command_interface_[i * 3 + j].get().set_value(3);

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -44,7 +44,7 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::return_type UnitreeGuideController::
-    update(const rclcpp::Time &time, const rclcpp::Duration &period) {
+    update(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
         // auto now = std::chrono::steady_clock::now();
         // std::chrono::duration<double> time_diff = now - last_update_time_;
         // last_update_time_ = now;
@@ -53,9 +53,13 @@ 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();
+        if (ctrl_comp_.robot_model_ == nullptr) {
-        ctrl_comp_.wave_generator_.update();
+            return controller_interface::return_type::OK;
-        ctrl_comp_.estimator_.update();
+        }
+
+        ctrl_comp_.robot_model_->update();
+        ctrl_comp_.wave_generator_->update();
+        ctrl_comp_.estimator_->update();
 
         if (mode_ == FSMMode::NORMAL) {
             current_state_->run();
@@ -92,6 +96,11 @@ namespace unitree_guide_controller {
             command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
             feet_names_ =
                     auto_declare<std::vector<std::string> >("feet_names", feet_names_);
+
+            get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
+            RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
+
+            ctrl_comp_.estimator_ = std::make_shared<Estimator>(ctrl_comp_);
         } catch (const std::exception &e) {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
@@ -101,7 +110,7 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::CallbackReturn UnitreeGuideController::on_configure(
-        const rclcpp_lifecycle::State &previous_state) {
+        const rclcpp_lifecycle::State & /*previous_state*/) {
         control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
             "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
                 // Handle message
@@ -115,20 +124,18 @@ namespace unitree_guide_controller {
         robot_description_subscription_ = get_node()->create_subscription<std_msgs::msg::String>(
             "/robot_description", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(),
             [this](const std_msgs::msg::String::SharedPtr msg) {
-                ctrl_comp_.robot_model_.init(msg->data, feet_names_, base_name_);
+                ctrl_comp_.robot_model_ = std::make_shared<QuadrupedRobot>(
-                ctrl_comp_.balance_ctrl_.init(ctrl_comp_.robot_model_);
+                    ctrl_comp_, msg->data, feet_names_, base_name_);
+                ctrl_comp_.balance_ctrl_ = std::make_shared<BalanceCtrl>(ctrl_comp_.robot_model_);
             });
 
-        get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
+        ctrl_comp_.wave_generator_ = std::make_shared<WaveGenerator>(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
-        RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
-
-        ctrl_comp_.wave_generator_.init(0.45, 0.5, Vec4(0, 0.5, 0.5, 0));
 
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_activate(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) {
         // clear out vectors in case of restart
         ctrl_comp_.clear();
 
@@ -171,27 +178,27 @@ namespace unitree_guide_controller {
     }
 
     controller_interface::CallbackReturn UnitreeGuideController::on_deactivate(
-        const rclcpp_lifecycle::State &previous_state) {
+        const rclcpp_lifecycle::State & /*previous_state*/) {
         release_interfaces();
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_error(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_error(const rclcpp_lifecycle::State & /*previous_state*/) {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn
-    UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State &previous_state) {
+    UnitreeGuideController::on_shutdown(const rclcpp_lifecycle::State & /*previous_state*/) {
         return CallbackReturn::SUCCESS;
     }
 
-    std::shared_ptr<FSMState> UnitreeGuideController::getNextState(FSMStateName stateName) const {
+    std::shared_ptr<FSMState> UnitreeGuideController::getNextState(const FSMStateName stateName) const {
         switch (stateName) {
             case FSMStateName::INVALID:
                 return state_list_.invalid;

controllers/unitree_guide_controller/src/control/BalanceCtrl.cpp

@@ -9,18 +9,16 @@
 
 #include "quadProgpp/QuadProg++.hh"
 
-BalanceCtrl::BalanceCtrl() {
+BalanceCtrl::BalanceCtrl(const std::shared_ptr<QuadrupedRobot> &robot) {
-    mass_ = 15;
+    mass_ = robot->mass_;
+
     alpha_ = 0.001;
     beta_ = 0.1;
     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_;
     pcb_ = Vec3(0.0, 0.0, 0.0);
     Ib_ = Vec3(0.0792, 0.2085, 0.2265).asDiagonal();
 

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -12,7 +12,9 @@ Estimator::Estimator(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_compo
                                                       robot_model_(ctrl_component.robot_model_),
                                                       wave_generator_(ctrl_component.wave_generator_) {
     g_ << 0, 0, -9.81;
-    dt_ = 0.002;
+    dt_ = 1.0 / ctrl_component_.frequency_;
+
+    std::cout<<"dt: "<<dt_<<std::endl;
     large_variance_ = 100;
     for (int i(0); i < Qdig.rows(); ++i) {
         Qdig(i) = i < 6 ? 0.0003 : 0.01;
@@ -146,25 +148,25 @@ double Estimator::getYaw() const {
 }
 
 void Estimator::update() {
-    if (robot_model_.mass_ == 0) return;
+    if (robot_model_->mass_ == 0) return;
 
     Q = QInit_;
     R = RInit_;
 
-    foot_poses_ = robot_model_.getFeet2BPositions();
+    foot_poses_ = robot_model_->getFeet2BPositions();
-    foot_vels_ = robot_model_.getFeet2BVelocities();
+    foot_vels_ = robot_model_->getFeet2BVelocities();
     feet_h_.setZero();
 
     // Adjust the covariance based on foot contact and phase.
     for (int i(0); i < 4; ++i) {
-        if (wave_generator_.contact_[i] == 0) {
+        if (wave_generator_->contact_[i] == 0) {
             // foot not contact
             Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
             R.block(12 + 3 * i, 12 + 3 * i, 3, 3) = large_variance_ * Eigen::MatrixXd::Identity(3, 3);
             R(24 + i, 24 + i) = large_variance_;
         } else {
             // foot contact
-            const double trust = windowFunc(wave_generator_.phase_[i], 0.2);
+            const double trust = windowFunc(wave_generator_->phase_[i], 0.2);
             Q.block(6 + 3 * i, 6 + 3 * i, 3, 3) =
                     (1 + (1 - trust) * large_variance_) *
                     QInit_.block(6 + 3 * i, 6 + 3 * i, 3, 3);

controllers/unitree_guide_controller/src/gait/FeetEndCalc.cpp

@@ -15,10 +15,10 @@ FeetEndCalc::FeetEndCalc(CtrlComponent &ctrl_component) : ctrl_component_(ctrl_c
 }
 
 void FeetEndCalc::init() {
-    t_stance_ = ctrl_component_.wave_generator_.get_t_stance();
+    t_stance_ = ctrl_component_.wave_generator_->get_t_stance();
-    t_swing_ = ctrl_component_.wave_generator_.get_t_swing();
+    t_swing_ = ctrl_component_.wave_generator_->get_t_swing();
 
-    Vec34 feet_pos_body = estimator_.getFeetPos2Body();
+    Vec34 feet_pos_body = estimator_->getFeetPos2Body();
     // Vec34 feet_pos_body = robot_model_.feet_pos_normal_stand_;
     for (int i(0); i < 4; ++i) {
         feet_radius_(i) =
@@ -28,7 +28,7 @@ void FeetEndCalc::init() {
 }
 
 Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const double d_yaw_global, const double phase) {
-    Vec3 body_vel_global = estimator_.getVelocity();
+    Vec3 body_vel_global = estimator_->getVelocity();
     Vec3 next_step;
 
     next_step(0) = body_vel_global(0) * (1 - phase) * t_swing_ +
@@ -39,8 +39,8 @@ Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const doubl
                    k_y_ * (body_vel_global(1) - vxy_goal_global(1));
     next_step(2) = 0;
 
-    const double yaw = estimator_.getYaw();
+    const double yaw = estimator_->getYaw();
-    const double d_yaw = estimator_.getDYaw();
+    const double d_yaw = estimator_->getDYaw();
     const double next_yaw = d_yaw * (1 - phase) * t_swing_ + d_yaw * t_stance_ / 2 +
                             k_yaw_ * (d_yaw_global - d_yaw);
 
@@ -49,7 +49,7 @@ Vec3 FeetEndCalc::calcFootPos(const int index, Vec2 vxy_goal_global, const doubl
     next_step(1) +=
             feet_radius_(index) * sin(yaw + feet_init_angle_(index) + next_yaw);
 
-    Vec3 foot_pos = estimator_.getPosition() + next_step;
+    Vec3 foot_pos = estimator_->getPosition() + next_step;
     foot_pos(2) = 0.0;
 
     return foot_pos;

controllers/unitree_guide_controller/src/gait/GaitGenerator.cpp

@@ -22,21 +22,21 @@ void GaitGenerator::setGait(Vec2 vxy_goal_global, const double d_yaw_goal, const
 
 void GaitGenerator::generate(Vec34 &feet_pos, Vec34 &feet_vel) {
     if (first_run_) {
-        start_p_ = estimator_.getFeetPos();
+        start_p_ = estimator_->getFeetPos();
         first_run_ = false;
     }
 
     for (int i = 0; i < 4; i++) {
-        if (wave_generator_.contact_(i) == 1) {
+        if (wave_generator_->contact_(i) == 1) {
-            if (wave_generator_.phase_(i) < 0.5) {
+            if (wave_generator_->phase_(i) < 0.5) {
                 // foot contact the ground
-                start_p_.col(i) = estimator_.getFootPos(i);
+                start_p_.col(i) = estimator_->getFootPos(i);
             }
             feet_pos.col(i) = start_p_.col(i);
             feet_vel.col(i).setZero();
         } else {
             // foot not contact, swing
-            end_p_.col(i) = feet_end_calc_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_.phase_(i));
+            end_p_.col(i) = feet_end_calc_.calcFootPos(i, vxy_goal_, d_yaw_goal_, wave_generator_->phase_(i));
             feet_pos.col(i) = getFootPos(i);
             feet_vel.col(i) = getFootVel(i);
         }
@@ -54,10 +54,10 @@ Vec3 GaitGenerator::getFootPos(const int i) {
     Vec3 foot_pos;
 
     foot_pos(0) =
-            cycloidXYPosition(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
+            cycloidXYPosition(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_->phase_(i));
     foot_pos(1) =
-            cycloidXYPosition(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
+            cycloidXYPosition(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_->phase_(i));
-    foot_pos(2) = cycloidZPosition(start_p_.col(i)(2), gait_height_, wave_generator_.phase_(i));
+    foot_pos(2) = cycloidZPosition(start_p_.col(i)(2), gait_height_, wave_generator_->phase_(i));
 
     return foot_pos;
 }
@@ -66,10 +66,10 @@ Vec3 GaitGenerator::getFootVel(const int i) {
     Vec3 foot_vel;
 
     foot_vel(0) =
-            cycloidXYVelocity(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_.phase_(i));
+            cycloidXYVelocity(start_p_.col(i)(0), end_p_.col(i)(0), wave_generator_->phase_(i));
     foot_vel(1) =
-            cycloidXYVelocity(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_.phase_(i));
+            cycloidXYVelocity(start_p_.col(i)(1), end_p_.col(i)(1), wave_generator_->phase_(i));
-    foot_vel(2) = cycloidZVelocity(gait_height_, wave_generator_.phase_(i));
+    foot_vel(2) = cycloidZVelocity(gait_height_, wave_generator_->phase_(i));
 
     return foot_vel;
 }
@@ -86,10 +86,10 @@ double GaitGenerator::cycloidZPosition(const double startZ, const double height,
 
 double GaitGenerator::cycloidXYVelocity(const double startXY, const double endXY, const double phase) const {
     const double phase_pi = 2 * M_PI * phase;
-    return (endXY - startXY) * (1 - cos(phase_pi)) / wave_generator_.get_t_swing();
+    return (endXY - startXY) * (1 - cos(phase_pi)) / wave_generator_->get_t_swing();
 }
 
 double GaitGenerator::cycloidZVelocity(const double height, const double phase) const {
     const double phase_pi = 2 * M_PI * phase;
-    return height * M_PI * sin(phase_pi) / wave_generator_.get_t_swing();
+    return height * M_PI * sin(phase_pi) / wave_generator_->get_t_swing();
 }

controllers/unitree_guide_controller/src/gait/WaveGenerator.cpp

@@ -8,14 +8,13 @@
 
 #include <iostream>
 
-WaveGenerator::WaveGenerator() {
+WaveGenerator::WaveGenerator(double period, double st_ratio, const Vec4 &bias) {
+
     phase_past_ << 0.5, 0.5, 0.5, 0.5;
     contact_past_.setZero();
     status_past_ = WaveStatus::SWING_ALL;
     status_ = WaveStatus::SWING_ALL;
-}
 
-void WaveGenerator::init(const double period, const double st_ratio, const Vec4 &bias) {
     period_ = period;
     st_ratio_ = st_ratio;
     bias_ = bias;

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -6,8 +6,9 @@
 #include "unitree_guide_controller/control/CtrlComponent.h"
 #include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
-void QuadrupedRobot::init(const std::string &robot_description, const std::vector<std::string> &feet_names,
+QuadrupedRobot::QuadrupedRobot(CtrlComponent &ctrl_component, const std::string &robot_description,
-                          const std::string& base_name) {
+                               const std::vector<std::string> &feet_names,
+                               const std::string &base_name) : ctrl_component_(ctrl_component) {
     KDL::Tree robot_tree;
     kdl_parser::treeFromString(robot_description, robot_tree);