起码好用

src/controllers/mdog_simple_controller/CMakeLists.txt

@@ -28,6 +28,7 @@ add_executable(mdog_simple_controller
   src/common/kinematics.cpp
   src/common/user_math.cpp
   src/common/bezier_curve.cpp
+  src/common/pid.cpp
 )
 ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs )
 

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

@@ -10,4 +10,4 @@ public:
     void exit(MdogSimpleController*) override;
 };
 
-}
+} // namespace mdog_simple_controller

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

@@ -7,6 +7,12 @@ namespace kinematics {
 // 固定三连杆长度（单位：米，可根据实际机器人参数修改）
 constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.08, 0.25, 0.25};
 
+// 获取默认三连杆长度
+const std::array<double, 3>& get_leg_link_length();
+
+// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
+const std::array<int, 4>& get_hip_signs();
+
 // 正运动学：已知关节角，求末端位置
 void forward_kinematics(const std::array<double, 3>& theta,
                         const std::array<double, 3>& link,

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

@@ -0,0 +1,30 @@
+#pragma once
+
+namespace mdog_simple_controller {
+
+struct PIDParam {
+    double kp{0.0};
+    double ki{0.0};
+    double kd{0.0};
+    double dt{0.01};
+    double max_out{1e6};
+    double min_out{-1e6};
+};
+
+class PID {
+public:
+    PID(const PIDParam& param);
+
+    void set_param(const PIDParam& param);
+    void reset();
+    double compute(double setpoint, double measurement);
+
+    const PIDParam& get_param() const { return param_; }
+
+private:
+    PIDParam param_;
+    double prev_error_{0.0};
+    double integral_{0.0};
+};
+
+} // namespace mdog_simple_controller

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

@@ -6,11 +6,11 @@ namespace mdog_simple_controller {
 
 // 关节0点位置offset和减速比定义
 constexpr float JOINT_OFFSET[4][3] = {
-    {0.0f, 0.0f, 0.0f}, // 可根据实际填写
-    {0.0f, 0.0f, 0.0f},
+    {-6.349f, -2.856f, 22.610f}, // 可根据实际填写
+    {-3.156f, 0.0f, 0.0f},
     {0.0f, 0.0f, 0.0f},
     // {0.2f, 0.393f, 0.5507f}
-    {5.63f, 5.29f, 4.34f}
+    {5.63f, 2.9f, 0.0f}
 };
 
 constexpr float JOINT_RATIO[4][3] = {

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

@@ -43,8 +43,7 @@ struct AHRS_Eulr_t {
 
 struct InputCmd {
     Voltage voltage;
-    AHRS_Eulr_t imu_euler;
-    AHRS_Eulr_t machine_euler;
+    AHRS_Eulr_t eulr;
     float hight;
     int8_t status;
 };
@@ -54,6 +53,8 @@ struct MdogControllerData {
     LegJointData feedback_real[4];
     LegJointCmd command[4];
     LegJointCmd command_real[4];
+    AHRS_Eulr_t imu_eulr;
+    AHRS_Eulr_t cmd_eulr;
 };
 
 

src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp

@@ -7,72 +7,12 @@
 
 namespace mdog_simple_controller {
 
-namespace {
-double bezier_time = 0.0;
-const double bezier_period = 1.0;
-}
-
 void StateBalance::enter(MdogSimpleController* ctrl) {
     // 进入BALANCE状态时的初始化
 }
 
 void StateBalance::run(MdogSimpleController* ctrl) {
-    // 贝塞尔控制点
-    std::vector<std::array<double, 3>> control_points = {
-        {0.30, 0.0, 0.15},      // 起点
-        {0.10, 0.0, 0.0},     // 抬腿中点
-        {0.30, 0.0, -0.15},     // 落点
-    };
-    // 起点和终点
-    const auto& p0 = control_points.front();
-    const auto& p1 = control_points.back();
-
-    // 时间推进
-    static rclcpp::Time last_time = ctrl->now();
-    rclcpp::Time now = ctrl->now();
-    double dt = (now - last_time).seconds();
-    last_time = now;
-    bezier_time += dt;
-    if (bezier_time > bezier_period) bezier_time -= bezier_period;
-    double t = bezier_time / bezier_period; // [0,1]
-
-    // 对角腿同步，交错运动，相位差0.5
-    for (int leg = 0; leg < 4; ++leg) {
-        // FR(0) & RL(3) 同步，FL(1) & RR(2) 同步
-        double phase_offset = (leg == 0 || leg == 3) ? 0.0 : 0.5;
-        double phase = std::fmod(t + phase_offset, 1.0);
-
-        std::array<double, 3> foot_pos;
-        if (phase < 0.5) {
-            // 摆动相：贝塞尔曲线
-            double swing_t = phase / 0.5; // 归一化到[0,1]
-            foot_pos = bezier::bezier_curve_3d(control_points, swing_t);
-        } else {
-            // 支撑相：直线连接首尾
-            double stance_t = (phase - 0.5) / 0.5; // 归一化到[0,1]
-            for (int i = 0; i < 3; ++i) {
-                foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_t;
-            }
-        }
-
-        std::array<double, 3> theta;
-        std::array<double, 3> link = {0.08, 0.25, 0.25}; // 根据实际参数
-        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
-        if (ok) {
-            // 左右腿hip方向相反，FR(0) RR(2)为右腿，FL(1) RL(3)为左腿
-            if (leg == 1 || leg == 3) { // 右腿
-                theta[0] = -theta[0];
-            }
-            for (int j = 0; j < 3; ++j) {
-                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
-                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] = 0.8;
-                ctrl->data_.command_real[leg].kd[j] = 0.01;
-            }
-        }
-    }
-    realcmd_to_cmd(ctrl->data_, ctrl->data_);
+    
 }
 
 void StateBalance::exit(MdogSimpleController* ctrl) {

src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp

@@ -1,23 +1,84 @@
 #include "mdog_simple_controller/FSM/state_troting.hpp"
 #include "mdog_simple_controller/mdog_simple_controller.hpp"
+#include "mdog_simple_controller/common/user_math.hpp"
+#include "mdog_simple_controller/common/kinematics.hpp"
+#include "mdog_simple_controller/common/bezier_curve.hpp"
+#include <cmath>
 
 namespace mdog_simple_controller {
 
+namespace {
+double bezier_time = 0.0;
+const double bezier_period = 1.2;
+}
+
 void StateTroting::enter(MdogSimpleController* ctrl) {
     // 进入TROTING状态时的初始化
 }
 
 void StateTroting::run(MdogSimpleController* ctrl) {
     //设置所有cmd数据为0
+    std::vector<std::array<double, 3>> control_points = {
+        // {0.40, 0.0, 0.15},      // 起点
+        // {0.10, 0.0, 0.0},     // 抬腿中点
+        // {0.40, 0.0, -0.05},     // 落点
+        // {0.40, 0.0, 0.05},      // 起点
+        {0.40, 0.0, 0.05},     // 抬腿中点
+
+        {0.40, 0.0, 0.05},     // 抬腿中点
+        {0.40, 0.0, 0.05},     // 抬腿中点
+        // {0.40, 0.0, 0.05},     // 落点
+    };
+    // 起点和终点
+    const auto& p0 = control_points.front();
+    const auto& p1 = control_points.back();
+
+    // 时间推进
+    static rclcpp::Time last_time = ctrl->now();
+    rclcpp::Time now = ctrl->now();
+    double dt = (now - last_time).seconds();
+    last_time = now;
+    bezier_time += dt;
+    if (bezier_time > bezier_period) bezier_time -= bezier_period;
+    double t = bezier_time / bezier_period; // [0,1]
+
+    const auto& link = kinematics::get_leg_link_length();
+    const auto& hip_signs = kinematics::get_hip_signs();
+
+    // 对角腿同步，交错运动，相位差0.5
     for (int leg = 0; leg < 4; ++leg) {
-        for (int j = 0; j < 3; ++j) {
-            ctrl->data_.command[leg].torque_des[j] = 0;
-            ctrl->data_.command[leg].speed_des[j] = 0;
-            ctrl->data_.command[leg].pos_des[j] = 0;
-            ctrl->data_.command[leg].kp[j] = 0;
-            ctrl->data_.command[leg].kd[j] = 0.05;
+        // FR(0) & RL(3) 同步，FL(1) & RR(2) 同步
+        double phase_offset = (leg == 0 || leg == 3) ? 0.0 : 0.5;
+        double phase = std::fmod(t + phase_offset, 1.0);
+
+        std::array<double, 3> foot_pos;
+        if (phase < 0.5) {
+            // 摆动相：贝塞尔曲线
+            double swing_t = phase / 0.5; // 归一化到[0,1]
+            foot_pos = bezier::bezier_curve_3d(control_points, swing_t);
+        } else {
+            // 支撑相：直线连接首尾
+            double stance_t = (phase - 0.5) / 0.5; // 归一化到[0,1]
+            for (int i = 0; i < 3; ++i) {
+                foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_t;
+            }
+        }
+
+        std::array<double, 3> theta;
+        bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
+        if (ok) {
+            // 使用参数化的hip方向
+            theta[0] *= hip_signs[leg];
+            for (int j = 0; j < 3; ++j) {
+                ctrl->data_.command_real[leg].pos_des[j] = theta[j];
+                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] = 0.8;
+                ctrl->data_.command_real[leg].kd[j] = 0.01;
+            }
         }
     }
+    realcmd_to_cmd(ctrl->data_, ctrl->data_);
 }
 
 void StateTroting::exit(MdogSimpleController* ctrl) {

src/controllers/mdog_simple_controller/src/common/kinematics.cpp

@@ -4,6 +4,17 @@
 namespace mdog_simple_controller {
 namespace kinematics {
 
+// 获取默认三连杆长度
+const std::array<double, 3>& get_leg_link_length() {
+    return LEG_LINK_LENGTH;
+}
+
+// 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
+const std::array<int, 4>& get_hip_signs() {
+    static constexpr std::array<int, 4> HIP_SIGNS = {1, -1, 1, -1};
+    return HIP_SIGNS;
+}
+
 // 正运动学：已知关节角，求末端位置
 void forward_kinematics(const std::array<double, 3>& theta,
                         const std::array<double, 3>& link,
@@ -49,5 +60,6 @@ bool inverse_kinematics(const std::array<double, 3>& pos,
     return true;
 }
 
+
 } // namespace kinematics
 } // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/common/pid.cpp

@@ -0,0 +1,29 @@
+#include "mdog_simple_controller/common/pid.hpp"
+
+namespace mdog_simple_controller {
+
+PID::PID(const PIDParam& param)
+    : param_(param), prev_error_(0.0), integral_(0.0) {}
+
+void PID::set_param(const PIDParam& param) {
+    param_ = param;
+    reset();
+}
+
+void PID::reset() {
+    prev_error_ = 0.0;
+    integral_ = 0.0;
+}
+
+double PID::compute(double setpoint, double measurement) {
+    double error = setpoint - measurement;
+    integral_ += error * param_.dt;
+    double derivative = (error - prev_error_) / param_.dt;
+    double output = param_.kp * error + param_.ki * integral_ + param_.kd * derivative;
+    if (output > param_.max_out) output = param_.max_out;
+    if (output < param_.min_out) output = param_.min_out;
+    prev_error_ = error;
+    return output;
+}
+
+} // namespace mdog_simple_controller

src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -12,7 +12,7 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
         "/control_input", 10,
         std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1));
     ahrs_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>(
-        "/euler_angles", 10,
+        "/eulr_angles", 10,
         std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1));
     cmd_pub_ = this->create_publisher<rc_msgs::msg::LegCmd>("/leg_cmd", 10);
     joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
@@ -53,9 +53,9 @@ void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs:
 }
 
 void MdogSimpleController::ahrs_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
-    input_cmd_.imu_euler.yaw = msg->z;
-    input_cmd_.imu_euler.rol = msg->x;
-    input_cmd_.imu_euler.pit = msg->y;
+    data_.imu_eulr.yaw = msg->z;
+    data_.imu_eulr.rol = msg->x;
+    data_.imu_eulr.pit = msg->y;
 }
 
 void MdogSimpleController::control_loop() {
@@ -77,9 +77,6 @@ void MdogSimpleController::control_loop() {
                 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[leg].torque_des[j];
-                // data_.feedback_real[leg].speed[j]  = data_.command[leg].speed_des[j];
-                // data_.feedback_real[leg].pos[j]    = data_.command[leg].pos_des[j];
             }
         }
         feedback_to_real(data_, data_);
@@ -110,8 +107,6 @@ void MdogSimpleController::control_loop() {
         }
         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(), "AHRS: Yaw: %f, Roll: %f, Pitch: %f",
-            input_cmd_.imu_euler.yaw, input_cmd_.imu_euler.rol, input_cmd_.imu_euler.pit);
     }
     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);

src/hardware/fdilink_ahrs_ROS2/include/ahrs_driver.h

@@ -87,7 +87,7 @@ private:
   //topic
   std::string imu_topic="/imu", mag_pose_2d_topic="/mag_pose_2d";
   std::string latlon_topic="latlon";
-  std::string Euler_angles_topic="/Euler_angles", Magnetic_topic="/Magnetic";
+  std::string Eulr_angles_topic="/Eulr_angles", Magnetic_topic="/Magnetic";
   std::string gps_topic="/gps/fix",twist_topic="/system_speed",NED_odom_topic="/NED_odometry";
 
 
@@ -97,7 +97,7 @@ private:
 
   rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gps_pub_;
   
-  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Euler_angles_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Eulr_angles_pub_;
   rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr Magnetic_pub_;
 
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr  twist_pub_;

src/hardware/fdilink_ahrs_ROS2/launch/ahrs_driver.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
             'imu_frame_id_':'gyro_link',
             'mag_pose_2d_topic':'/mag_pose_2d',
             'Magnetic_topic':'/magnetic',
-            'Euler_angles_topic':'/euler_angles',
+            'Eulr_angles_topic':'/eulr_angles',
             'gps_topic':'/gps/fix',
             'twist_topic':'/system_speed',
             'NED_odom_topic':'/NED_odometry',

src/hardware/fdilink_ahrs_ROS2/src/ahrs_driver.cpp

@@ -24,8 +24,8 @@ ahrsBringup::ahrsBringup()
   this->declare_parameter<std::string>("mag_pose_2d_topic","/mag_pose_2d");
   this->get_parameter("mag_pose_2d_topic", mag_pose_2d_topic);
 
-  this->declare_parameter<std::string>("Euler_angles_topic","/euler_angles");
-  this->get_parameter("Euler_angles_topic", Euler_angles_topic);
+  this->declare_parameter<std::string>("Eulr_angles_topic","/eulr_angles");
+  this->get_parameter("Eulr_angles_topic", Eulr_angles_topic);
 
   this->declare_parameter<std::string>("gps_topic","/gps/fix");
   this->get_parameter("gps_topic", gps_topic);
@@ -51,7 +51,7 @@ ahrsBringup::ahrsBringup()
   // pravite_nh.param("imu_topic", imu_topic_, std::string("/imu"));
   // pravite_nh.param("imu_frame", imu_frame_id_, std::string("imu"));
   // pravite_nh.param("mag_pose_2d_topic", mag_pose_2d_topic_, std::string("/mag_pose_2d"));
-  // pravite_nh.param("Euler_angles_pub_", Euler_angles_topic_, std::string("/euler_angles"));
+  // pravite_nh.param("Eulr_angles_pub_", Eulr_angles_topic_, std::string("/eulr_angles"));
   // pravite_nh.param("Magnetic_pub_", Magnetic_topic_, std::string("/magnetic"));
  
   //serial                                                 
@@ -64,7 +64,7 @@ ahrsBringup::ahrsBringup()
 
   mag_pose_pub_ = create_publisher<geometry_msgs::msg::Pose2D>(mag_pose_2d_topic.c_str(), 10);
 
-  Euler_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Euler_angles_topic.c_str(), 10);
+  Eulr_angles_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Eulr_angles_topic.c_str(), 10);
   Magnetic_pub_ = create_publisher<geometry_msgs::msg::Vector3>(Magnetic_topic.c_str(), 10);
  
   twist_pub_ = create_publisher<geometry_msgs::msg::Twist>(twist_topic.c_str(), 10);
@@ -500,15 +500,15 @@ void ahrsBringup::processLoop()  // 数据处理过程
       pose_2d.theta = ahrs_frame_.frame.data.data_pack.Heading;
       mag_pose_pub_->publish(pose_2d);
       //std::cout << "YAW: " << pose_2d.theta << std::endl;
-      geometry_msgs::msg::Vector3 Euler_angles_2d,Magnetic;  
-      Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
-      Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
-      Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
+      geometry_msgs::msg::Vector3 Eulr_angles_2d,Magnetic;  
+      Eulr_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
+      Eulr_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
+      Eulr_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
       Magnetic.x = imu_frame_.frame.data.data_pack.magnetometer_x;
       Magnetic.y = imu_frame_.frame.data.data_pack.magnetometer_y;
       Magnetic.z = imu_frame_.frame.data.data_pack.magnetometer_z;
 
-      Euler_angles_pub_->publish(Euler_angles_2d);
+      Eulr_angles_pub_->publish(Eulr_angles_2d);
       Magnetic_pub_->publish(Magnetic);
 
     }