贝塞尔控制

src/controllers/mdog_simple_controller/CMakeLists.txt

@@ -31,21 +31,21 @@ add_executable(mdog_simple_controller
 )
 ament_target_dependencies(mdog_simple_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs )
 
-add_executable(mdog_sim_controller
+# add_executable(mdog_sim_controller
-  src/mdog_sim_controller.cpp
+#   src/mdog_sim_controller.cpp
-  src/FSM/state_safe.cpp
+#   src/FSM/state_safe.cpp
-  src/FSM/state_zero.cpp
+#   src/FSM/state_zero.cpp
-  src/FSM/state_balance.cpp
+#   src/FSM/state_balance.cpp
-  src/FSM/state_troting.cpp
+#   src/FSM/state_troting.cpp
-  src/common/kinematics.cpp
+#   src/common/kinematics.cpp
-  src/common/user_math.cpp
+#   src/common/user_math.cpp
-  src/common/bezier_curve.cpp
+#   src/common/bezier_curve.cpp
-)
+# )
-ament_target_dependencies(mdog_sim_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs)
+# ament_target_dependencies(mdog_sim_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs)
 
 install(TARGETS
   mdog_simple_controller
-  mdog_sim_controller
+  # mdog_sim_controller
   DESTINATION lib/${PROJECT_NAME}
 )
 

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

@@ -5,7 +5,7 @@ namespace mdog_simple_controller {
 namespace kinematics {
 
 // 固定三连杆长度（单位：米，可根据实际机器人参数修改）
-constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.05, 0.30, 0.30};
+constexpr std::array<double, 3> LEG_LINK_LENGTH = {0.08, 0.25, 0.25};
 
 // 正运动学：已知关节角，求末端位置
 void forward_kinematics(const std::array<double, 3>& theta,

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

@@ -72,10 +72,13 @@ private:
     void control_loop();
 
     FSMState* current_state_{nullptr};
+    StateSafe state_safe_;
     StateZero state_zero_;
     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_;
     rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr ahrs_sub_;

src/controllers/mdog_simple_controller/launch/mdog_simple_control_real.launch.py

@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='mdog_simple_controller',
+            executable='mdog_simple_controller',
+            name='mdog_simple_controller',
+            output='screen',
+            parameters=[{'if_no_hardware': False}]
+        )
+    ])

src/controllers/mdog_simple_controller/launch/no_hardware.launch.py

@@ -0,0 +1,13 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='mdog_simple_controller',
+            executable='mdog_simple_controller',
+            name='mdog_simple_controller',
+            output='screen',
+            parameters=[{'if_no_hardware': True}]
+        )
+    ])

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

@@ -20,8 +20,8 @@ void StateBalance::run(MdogSimpleController* ctrl) {
     // 贝塞尔控制点
     std::vector<std::array<double, 3>> control_points = {
         {0.0, 0.0, -0.30},      // 起点
-        {0.01, 0.0, -0.05},     // 抬腿中点
+        {0.05, 0.0, 0.05},     // 抬腿中点
-        {0.02, 0.0, -0.30}      // 落点
+        {0.10, 0.0, -0.30}      // 落点
     };
     // 起点和终点
     const auto& p0 = control_points.front();
@@ -56,7 +56,7 @@ void StateBalance::run(MdogSimpleController* ctrl) {
         }
 
         std::array<double, 3> theta;
-        std::array<double, 3> link = {0.05, 0.25, 0.25}; // 根据实际参数
+        std::array<double, 3> link = {0.08, 0.25, 0.25}; // 根据实际参数
         bool ok = kinematics::inverse_kinematics(foot_pos, link, theta);
         if (ok) {
             for (int j = 0; j < 3; ++j) {

src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp

@@ -1,142 +0,0 @@
-#include "mdog_simple_controller/mdog_simple_controller.hpp"
-#include "mdog_simple_controller/common/user_math.hpp"
-
-namespace mdog_simple_controller
-{
-
-MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
-    fdb_sub_ = this->create_subscription<rc_msgs::msg::LegFdb>(
-        "/leg_fdb", 10,
-        std::bind(&MdogSimpleController::fdb_callback, this, std::placeholders::_1));
-    input_sub_ = this->create_subscription<control_input_msgs::msg::Inputs>(
-        "/control_input", 10,
-        std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1));
-    ahrs_sub_ = this->create_subscription<geometry_msgs::msg::Vector3>(
-        "/euler_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);
-
-    current_state_ = &state_zero_;
-    current_state_->enter(this);
-    timer_ = this->create_wall_timer(
-        std::chrono::microseconds(2000),
-        std::bind(&MdogSimpleController::control_loop, this));
-}
-
-void MdogSimpleController::change_state(FSMState* new_state) {
-    if (current_state_) current_state_->exit(this);
-    current_state_ = new_state;
-    if (current_state_) current_state_->enter(this);
-}
-
-void MdogSimpleController::fdb_callback(const rc_msgs::msg::LegFdb::SharedPtr msg) {
-    // for (int leg = 0; leg < 4; ++leg) {
-    //     for (int j = 0; j < 3; ++j) {
-    //         int idx = leg * 3 + j;
-    //         data_.feedback[leg].torque[j] = msg->leg_fdb[idx].torque;
-    //         data_.feedback[leg].speed[j]  = msg->leg_fdb[idx].speed;
-    //         data_.feedback[leg].pos[j]    = msg->leg_fdb[idx].pos;
-    //     }
-    // }
-    feedback_to_real(data_, data_);
-}
-
-void MdogSimpleController::input_callback(const control_input_msgs::msg::Inputs::SharedPtr msg){
-    input_cmd_.voltage.vx = msg->lx;
-    input_cmd_.voltage.vy = msg->ly;
-    input_cmd_.voltage.wz = msg->rx;
-    input_cmd_.hight = 0.5;
-    input_cmd_.status = msg->command;
-}
-
-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;
-}
-
-void MdogSimpleController::control_loop() {
-    FSMState* target_state = nullptr;
-    switch (input_cmd_.status) {
-        case 0: target_state = &state_zero_; break;
-        case 6: target_state = &state_balance_; break;
-        case 7: target_state = &state_troting_; break;
-        default: target_state = &state_zero_; break;
-    }
-    if (target_state != current_state_) {
-        change_state(target_state);
-    }
-    if (current_state_) current_state_->run(this);
-
-    rc_msgs::msg::LegCmd msg;
-    for (int leg = 0; leg < 4; ++leg) {
-        for (int j = 0; j < 3; ++j) {
-            msg.leg_cmd[leg * 3 + j].torque_des = data_.command[leg].torque_des[j];
-            msg.leg_cmd[leg * 3 + j].speed_des = data_.command[leg].speed_des[j];
-            msg.leg_cmd[leg * 3 + j].pos_des = data_.command[leg].pos_des[j];
-            msg.leg_cmd[leg * 3 + j].kp = data_.command[leg].kp[j];
-            msg.leg_cmd[leg * 3 + j].kd = data_.command[leg].kd[j];
-        }
-    }
-    msg.header.stamp = this->now();
-    msg.header.frame_id = "leg_cmd";
-    cmd_pub_->publish(msg);
-
-    if (0){
-        RCLCPP_INFO(this->get_logger(), "MdogControllerData:");
-        for (int leg = 0; leg < 4; ++leg) {
-            RCLCPP_INFO(this->get_logger(), "Leg %d:", leg);
-            for (int j = 0; j < 3; ++j) {
-                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]);
-            }
-        }
-        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);
-    sensor_msgs::msg::JointState js_msg;
-    js_msg.header.stamp = this->now();
-    js_msg.name = {
-        "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
-        "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
-        "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint",
-        "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint"
-    };
-    for (int leg = 0; leg < 4; ++leg) {
-        for (int j = 0; j < 3; ++j) {
-            js_msg.position.push_back(data_.feedback_real[leg].pos[j]);
-            js_msg.velocity.push_back(data_.feedback_real[leg].speed[j]);
-            js_msg.effort.push_back(data_.feedback_real[leg].torque[j]);
-        }
-    }
-    joint_state_pub_->publish(js_msg);
-
-    //将publish的数据直接赋值给feedback
-    for (int leg = 0; leg < 4; ++leg) {
-        for (int j = 0; j < 3; ++j) {
-            data_.feedback_real[leg].pos[j] = data_.command_real[leg].pos_des[j];
-        }
-    }
-
-    //打印真实数据
-    // 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]);
-    //     }
-    // }
-}
-
-} // namespace mdog_simple_controller
-
-int main(int argc, char **argv) {
-    rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<mdog_simple_controller::MdogSimpleController>());
-    rclcpp::shutdown();
-    return 0;
-}

src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -17,6 +17,8 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
     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);
 
+    if_no_hardware_ = this->declare_parameter<bool>("if_no_hardware", false);
+
     current_state_ = &state_zero_;
     current_state_->enter(this);
     timer_ = this->create_wall_timer(
@@ -69,6 +71,18 @@ void MdogSimpleController::control_loop() {
     }
     if (current_state_) current_state_->run(this);
 
+    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];
+            }
+        }
+        // 若有feedback_to_real处理，也可调用
+        feedback_to_real(data_, data_);
+    }
+
     rc_msgs::msg::LegCmd msg;
     for (int leg = 0; leg < 4; ++leg) {
         for (int j = 0; j < 3; ++j) {
@@ -124,6 +138,7 @@ void MdogSimpleController::control_loop() {
     // }
 }
 
+
 } // namespace mdog_simple_controller
 
 int main(int argc, char **argv) {