抽象力控

src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp

@@ -1,5 +1,6 @@
 #pragma once
 #include <array>
+#include <cstdint>
 #include "mdog_simple_controller/FSM/fsm_state.hpp"
 
 namespace mdog_simple_controller {
@@ -12,10 +13,27 @@ public:
 
 private:
     bool inited = false;
+    bool if_stand = false;
+    uint16_t joint_reached = 0;
+    float max_speed = 0.001;
+    float pos_threshold = 0.01;
+    float speed_threshold = 0.01;
+    // 默认PD参数
+    std::array<double, 3> kp = {4.5, 4.5, 4.5};
+    std::array<double, 3> kd = {0.05, 0.05, 0.05};
 
-    std::array<std::array<double, 3>, 4> stand_pos;
+    std::array<std::array<double, 3>, 4> stand_foot_pos = {
+        {
+            {0.0, 0.05, -0.25}, // FR
+            {0.0, 0.05, -0.25}, // FL
+            {0.0, 0.05, -0.25}, // RR
+            {0.0, 0.05, -0.25}  // RL
+        }
+    };
 
-    std::array<std::array<double, 3>, 4> start_pos;
+    std::array<std::array<double, 3>, 4> target_joint_pos;
+    std::array<std::array<double, 3>, 4> start_joint_pos;
+    std::array<std::array<double, 3>, 4> last_joint_pos;
 
 };
 

src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp

@@ -29,6 +29,8 @@ void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst);
 // real cmd → 输出 cmd
 void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst);
 
+void set_pd_config(MdogControllerData& data, const std::array<float, 3>& kp, const std::array<float, 3>& kd);
 
-
+double limit_speed(double current, double target, double max_speed);
+int8_t get_direction(double target, double current);
 } // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp

@@ -68,6 +68,9 @@ public:
 
     MdogControllerData data_;
     InputCmd input_cmd_;
+
+    bool if_no_hardware_{false};
+    bool if_debug_{false};
 private:
     void fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg);
     void input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg);
@@ -82,7 +85,7 @@ private:
     StateBalance state_balance_;
     StateTroting state_troting_;
 
-    bool if_no_hardware_{false};
+
 
     rclcpp::Subscription<rc_msgs::msg::LegFdb>::SharedPtr fdb_sub_;
     rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr input_sub_;

src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp

@@ -1,14 +1,65 @@
 #include "mdog_simple_controller/FSM/state_stand.hpp"
 #include "mdog_simple_controller/mdog_simple_controller.hpp"
-
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
 namespace mdog_simple_controller {
 
 void StateStand::enter(MdogSimpleController* ctrl) {
+    inited = false;
+    if_stand = false;
+    max_speed = 0.04;
+    joint_reached = 0;
+    pos_threshold = 0.01;
+    speed_threshold = 0.01;
+
+    const auto& hip_signs = kinematics::get_hip_signs();
+
+    for (int leg = 0; leg < 4; ++leg) {
+        kinematics::inverse_kinematics(stand_foot_pos[leg], kinematics::get_leg_link_length(), target_joint_pos[leg]);
+        for (int j = 0; j < 3; ++j) {
+            start_joint_pos[leg][j] = ctrl->data_.feedback_real[leg].pos[j];
+            last_joint_pos[leg][j] = start_joint_pos[leg][j];
+            ctrl->data_.command_real[leg].pos_des[j] = 0;
+            ctrl->data_.command_real[leg].speed_des[j] = 0;
+            ctrl->data_.command_real[leg].torque_des[j] = 0;
+            ctrl->data_.command_real[leg].kp[j] = kp[j];
+            ctrl->data_.command_real[leg].kd[j] = kd[j];
+
+
+        }
+        target_joint_pos[leg][0] *= hip_signs[leg];
+    }
 
 }
 
 void StateStand::run(MdogSimpleController* ctrl) {
-    // SAFE状态下的控制逻辑
+
+    if (!if_stand) {
+        for (int leg = 0; leg < 4; ++leg) {
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = limit_speed(ctrl->data_.feedback_real[leg].pos[j], target_joint_pos[leg][j], max_speed);
+                if (!ctrl->if_no_hardware_){
+                    if (std::abs(ctrl->data_.feedback_real[leg].pos[j] - target_joint_pos[leg][j]) > pos_threshold) {
+                        if (std::abs(ctrl->data_.feedback_real[leg].speed[j]) < speed_threshold) {
+                            ctrl->data_.command_real[leg].torque_des[j] += 0.01*get_direction(target_joint_pos[leg][j], ctrl->data_.feedback_real[leg].pos[j]);
+                            RCLCPP_INFO(ctrl->get_logger(), "leg %d joint %d torque_des: %f", leg, j, ctrl->data_.command_real[leg].torque_des[j]);
+
+                    } else {
+                        ctrl->data_.command_real[leg].speed_des[j] = 0;
+                    }
+                }
+                if (std::abs(ctrl->data_.feedback_real[leg].pos[j] - target_joint_pos[leg][j]) < 0.01) {
+                    //将对应标志位置为1
+                    joint_reached |= (1 << (leg * 3 + j));
+                }
+            }
+        }
+        if (joint_reached == 0b0000111111111111) {
+            if_stand = true;
+        }
+    }
+    }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
 }
 
 void StateStand::exit(MdogSimpleController* ctrl) {

src/controllers/mdog_simple_controller/src/common/user_math.cpp

@@ -47,9 +47,40 @@ void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) {
     }
 }
 
-// 插补函数
+void set_pd_config(MdogControllerData& data, const std::array<float, 3>& kp, const std::array<float, 3>& kd) {
-double interpolate(double start, double end, double alpha) {
+    for (int leg = 0; leg < 4; ++leg) {
-    return (1 - alpha) * start + alpha * end;
+        for (int j = 0; j < 3; ++j) {
+            data.command[leg].kp[j] = kp[j];
+            data.command[leg].kd[j] = kd[j];
+        }
+    }
+}
+
+//pd计算公式：
+// u =du + kp * (target - current) + kd * (target_speed - current_speed)
+
+// double simple_torque_ctrl(double detal_pos, )
+
+//计算方向
+int8_t get_direction(double target, double current) {
+    if (target > current) {
+        return 1;
+    } else if (target < current) {
+        return -1;
+    }
+    return 0;
+}
+
+// 移动速度最大限制器
+double limit_speed(double current, double target, double max_speed) {
+    double diff = target - current;
+    if (diff > max_speed) {
+        return current + max_speed;
+    } else if (diff < -max_speed) {
+        return current - max_speed;
+    }
+    return target;
+
 }
 
 } // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -18,6 +18,8 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
     joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
 
     if_no_hardware_ = this->declare_parameter<bool>("if_no_hardware", false);
+    if_debug_ = this->declare_parameter<bool>("if_debug", false);
+
     memset(&data_, 0, sizeof(data_));
     current_state_ = &state_zero_;
     current_state_->enter(this);
@@ -48,7 +50,7 @@ void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs:
     input_cmd_.voltage.vx = msg->lx;
     input_cmd_.voltage.vy = msg->ly;
     input_cmd_.voltage.wz = msg->rx;
-    // input_cmd_.hight = 0.35;
+    input_cmd_.hight = 0.35;
     input_cmd_.status = msg->command;
 }
 
@@ -62,9 +64,10 @@ void MdogSimpleController::control_loop() {
     FSMState* target_state = nullptr;
     switch (input_cmd_.status) {
         case 0: target_state = &state_safe_; break;
-        case 6: target_state = &state_balance_; break;
+        // case 5: target_state = &state_balance_; break;
-        case 7: target_state = &state_troting_; break;
+        // case 6: target_state = &state_troting_; break;
-        case 8: target_state = &state_walk_; break;
+        // case 7: target_state = &state_walk_; break;
+        case 8: target_state = &state_stand_; break;
         default: target_state = &state_safe_; break;
     }
     if (target_state != current_state_) {
@@ -75,9 +78,6 @@ void MdogSimpleController::control_loop() {
     if (if_no_hardware_) {
         for (int leg = 0; leg < 4; ++leg) {
             for (int j = 0; j < 3; ++j) {
-                // data_.feedback[leg].torque[j] = data_.command[leg].torque_des[j];
-                // data_.feedback[leg].speed[j]  = data_.command[leg].speed_des[j];
-                // data_.feedback[leg].pos[j]    = data_.command[leg].pos_des[j];
                 data_.feedback_real[leg].torque[j] = data_.command_real[leg].torque_des[j];
                 data_.feedback_real[leg].speed[j]  = data_.command_real[leg].speed_des[j];
                 data_.feedback_real[leg].pos[j]    = data_.command_real[leg].pos_des[j];
@@ -85,8 +85,7 @@ void MdogSimpleController::control_loop() {
                 data_.feedback[leg].speed[j]  = data_.command_real[leg].speed_des[j];
             }
         }
-        // feedback_to_real(data_, data_);
+        real_to_feedback(data_, data_);
-        // real_to_feedback(data_, data_);
     }
 
     rc_msgs::msg::LegCmd msg;
@@ -102,23 +101,15 @@ void MdogSimpleController::control_loop() {
     msg.header.stamp = this->now();
     msg.header.frame_id = "leg_cmd";
     cmd_pub_->publish(msg);
+    //打印调试信息
 
-    if (0){
+        // RCLCPP_INFO(this->get_logger(), "cmd: %f %f %f %f %f %f %f %f %f %f %f %f",
-        RCLCPP_INFO(this->get_logger(), "MdogControllerData:");
+        //     data_.command[0].pos_des[0], data_.command[0].pos_des[1], data_.command[0].pos_des[2],
-        for (int leg = 0; leg < 4; ++leg) {
+        //     data_.command[1].pos_des[0], data_.command[1].pos_des[1], data_.command[1].pos_des[2],
-            RCLCPP_INFO(this->get_logger(), "Leg %d:", leg);
+        //     data_.command[2].pos_des[0], data_.command[2].pos_des[1], data_.command[2].pos_des[2],
-            for (int j = 0; j < 3; ++j) {
+        //     data_.command[3].pos_des[0], data_.command[3].pos_des[1], data_.command[3].pos_des[2]);
-                RCLCPP_INFO(this->get_logger(), "Joint %d: Torque: %f, Speed: %f, Pos: %f",
+
-                    j, data_.feedback[leg].torque[j], data_.feedback[leg].speed[j], data_.feedback[leg].pos[j]);
+    sensor_msgs::msg::JointState js_msg;
-            }
-        }
-        RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d",
-            input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status);
-    }
-    RCLCPP_INFO(this->get_logger(), "InputCmd: Voltage: [%f, %f, %f], Height: %f, Status: %d",
-        input_cmd_.voltage.vx, input_cmd_.voltage.vy, input_cmd_.voltage.wz, input_cmd_.hight, input_cmd_.status);
-   
-        sensor_msgs::msg::JointState js_msg;
     js_msg.header.stamp = this->now();
     js_msg.name = {
         "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
@@ -134,13 +125,6 @@ void MdogSimpleController::control_loop() {
         }
     }
     joint_state_pub_->publish(js_msg);
-    //打印真实数据
-    // for (int leg = 0; leg < 4; ++leg) {
-    //     for (int j = 0; j < 3; ++j) {
-    //         RCLCPP_INFO(this->get_logger(), "Leg %d Joint %d: Torque: %f, Speed: %f, Pos: %f",
-    //             leg, j, data_.feedback_real[leg].torque[j], data_.feedback_real[leg].speed[j], data_.feedback_real[leg].pos[j]);
-    //     }
-    // }
 }