起码好用

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

@@ -2,21 +2,29 @@
 
 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(double kp, double ki, double kd, double dt, double max_out = 1e6, double min_out = -1e6);
+    PID(const PIDParam& param);
 
-    void set_gains(double kp, double ki, double kd);
+    void set_param(const PIDParam& param);
-    void set_limits(double min_out, double max_out);
     void reset();
     double compute(double setpoint, double measurement);
 
+    const PIDParam& get_param() const { return param_; }
+
 private:
-    double kp_, ki_, kd_;
+    PIDParam param_;
-    double dt_;
+    double prev_error_{0.0};
-    double max_out_, min_out_;
+    double integral_{0.0};
-    double prev_error_;
-    double integral_;
 };
 
 } // 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}, // 可根据实际填写
+    {-6.349f, -2.856f, 22.610f}, // 可根据实际填写
-    {0.0f, 0.0f, 0.0f},
+    {-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 eulr;
-    AHRS_Eulr_t machine_euler;
     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

@@ -1,5 +1,9 @@
 #include "mdog_simple_controller/FSM/state_balance.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 {
 
@@ -8,7 +12,6 @@ void StateBalance::enter(MdogSimpleController* ctrl) {
 }
 
 void StateBalance::run(MdogSimpleController* ctrl) {
-    // 贝塞尔控制点
     
 }
 

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

@@ -9,7 +9,7 @@ namespace mdog_simple_controller {
 
 namespace {
 double bezier_time = 0.0;
-const double bezier_period = 1.0;
+const double bezier_period = 1.2;
 }
 
 void StateTroting::enter(MdogSimpleController* ctrl) {
@@ -19,9 +19,15 @@ void StateTroting::enter(MdogSimpleController* ctrl) {
 void StateTroting::run(MdogSimpleController* ctrl) {
     //设置所有cmd数据为0
     std::vector<std::array<double, 3>> control_points = {
-        {0.30, 0.0, 0.15},      // 起点
+        // {0.40, 0.0, 0.15},      // 起点
-        {0.10, 0.0, 0.0},     // 抬腿中点
+        // {0.10, 0.0, 0.0},     // 抬腿中点
-        {0.30, 0.0, -0.15},     // 落点
+        // {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();
@@ -36,6 +42,9 @@ void StateTroting::run(MdogSimpleController* ctrl) {
     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) {
         // FR(0) & RL(3) 同步，FL(1) & RR(2) 同步
@@ -56,13 +65,10 @@ void StateTroting::run(MdogSimpleController* ctrl) {
         }
 
         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)为左腿
+            // 使用参数化的hip方向
-            if (leg == 1 || leg == 3) { // 右腿
+            theta[0] *= hip_signs[leg];
-                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;

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

@@ -2,16 +2,12 @@
 
 namespace mdog_simple_controller {
 
-PID::PID(double kp, double ki, double kd, double dt, double max_out, double min_out)
+PID::PID(const PIDParam& param)
-    : kp_(kp), ki_(ki), kd_(kd), dt_(dt), max_out_(max_out), min_out_(min_out),
+    : param_(param), prev_error_(0.0), integral_(0.0) {}
-      prev_error_(0.0), integral_(0.0) {}
 
-void PID::set_gains(double kp, double ki, double kd) {
+void PID::set_param(const PIDParam& param) {
-    kp_ = kp; ki_ = ki; kd_ = kd;
+    param_ = param;
-}
+    reset();
-
-void PID::set_limits(double min_out, double max_out) {
-    min_out_ = min_out; max_out_ = max_out;
 }
 
 void PID::reset() {
@@ -21,11 +17,11 @@ void PID::reset() {
 
 double PID::compute(double setpoint, double measurement) {
     double error = setpoint - measurement;
-    integral_ += error * dt_;
+    integral_ += error * param_.dt;
-    double derivative = (error - prev_error_) / dt_;
+    double derivative = (error - prev_error_) / param_.dt;
-    double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
+    double output = param_.kp * error + param_.ki * integral_ + param_.kd * derivative;
-    if (output > max_out_) output = max_out_;
+    if (output > param_.max_out) output = param_.max_out;
-    if (output < min_out_) output = min_out_;
+    if (output < param_.min_out) output = param_.min_out;
     prev_error_ = error;
     return output;
 }

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;
+    data_.imu_eulr.yaw = msg->z;
-    input_cmd_.imu_euler.rol = msg->x;
+    data_.imu_eulr.rol = msg->x;
-    input_cmd_.imu_euler.pit = msg->y;
+    data_.imu_eulr.pit = msg->y;
 }
 
 void MdogSimpleController::control_loop() {
@@ -107,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->declare_parameter<std::string>("Eulr_angles_topic","/eulr_angles");
-  this->get_parameter("Euler_angles_topic", Euler_angles_topic);
+  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;  
+      geometry_msgs::msg::Vector3 Eulr_angles_2d,Magnetic;  
-      Euler_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
+      Eulr_angles_2d.x = ahrs_frame_.frame.data.data_pack.Roll;
-      Euler_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
+      Eulr_angles_2d.y = ahrs_frame_.frame.data.data_pack.Pitch;
-      Euler_angles_2d.z = ahrs_frame_.frame.data.data_pack.Heading;
+      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);
 
     }