From cca513ae0bf47dca9c76916d1c937c3aa23893d7 Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Sat, 17 May 2025 17:57:38 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B4=9D=E5=A1=9E=E5=B0=94=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mdog_simple_controller/CMakeLists.txt | 24 +-- .../common/kinematics.hpp | 2 +- .../mdog_simple_controller.hpp | 3 + .../launch/mdog_simple_control_real.launch.py | 13 ++ .../launch/no_hardware.launch.py | 13 ++ .../src/FSM/state_balance.cpp | 6 +- .../src/mdog_sim_controller.cpp | 142 ------------------ .../src/mdog_simple_controller.cpp | 15 ++ 8 files changed, 60 insertions(+), 158 deletions(-) create mode 100644 src/controllers/mdog_simple_controller/launch/mdog_simple_control_real.launch.py create mode 100644 src/controllers/mdog_simple_controller/launch/no_hardware.launch.py delete mode 100644 src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp diff --git a/src/controllers/mdog_simple_controller/CMakeLists.txt b/src/controllers/mdog_simple_controller/CMakeLists.txt index dcfbbb1..ca35ba8 100644 --- a/src/controllers/mdog_simple_controller/CMakeLists.txt +++ b/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 - src/mdog_sim_controller.cpp - src/FSM/state_safe.cpp - src/FSM/state_zero.cpp - src/FSM/state_balance.cpp - src/FSM/state_troting.cpp - src/common/kinematics.cpp - src/common/user_math.cpp - src/common/bezier_curve.cpp -) -ament_target_dependencies(mdog_sim_controller rclcpp rc_msgs std_msgs control_input_msgs geometry_msgs sensor_msgs) +# add_executable(mdog_sim_controller +# src/mdog_sim_controller.cpp +# src/FSM/state_safe.cpp +# src/FSM/state_zero.cpp +# src/FSM/state_balance.cpp +# src/FSM/state_troting.cpp +# src/common/kinematics.cpp +# src/common/user_math.cpp +# src/common/bezier_curve.cpp +# ) +# 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} ) diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp index 5ed1ec0..7ee3898 100644 --- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/kinematics.hpp +++ b/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 LEG_LINK_LENGTH = {0.05, 0.30, 0.30}; +constexpr std::array LEG_LINK_LENGTH = {0.08, 0.25, 0.25}; // 正运动学：已知关节角，求末端位置 void forward_kinematics(const std::array& theta, diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp index e6d28d5..a87372f 100644 --- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/mdog_simple_controller.hpp +++ b/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::SharedPtr fdb_sub_; rclcpp::Subscription::SharedPtr input_sub_; rclcpp::Subscription::SharedPtr ahrs_sub_; diff --git a/src/controllers/mdog_simple_controller/launch/mdog_simple_control_real.launch.py b/src/controllers/mdog_simple_controller/launch/mdog_simple_control_real.launch.py new file mode 100644 index 0000000..514453e --- /dev/null +++ b/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}] + ) + ]) \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/launch/no_hardware.launch.py b/src/controllers/mdog_simple_controller/launch/no_hardware.launch.py new file mode 100644 index 0000000..1b6564a --- /dev/null +++ b/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}] + ) + ]) \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp index c6bfb6c..4530634 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp @@ -20,8 +20,8 @@ void StateBalance::run(MdogSimpleController* ctrl) { // 贝塞尔控制点 std::vector> control_points = { {0.0, 0.0, -0.30}, // 起点 - {0.01, 0.0, -0.05}, // 抬腿中点 - {0.02, 0.0, -0.30} // 落点 + {0.05, 0.0, 0.05}, // 抬腿中点 + {0.10, 0.0, -0.30} // 落点 }; // 起点和终点 const auto& p0 = control_points.front(); @@ -56,7 +56,7 @@ void StateBalance::run(MdogSimpleController* ctrl) { } std::array theta; - std::array link = {0.05, 0.25, 0.25}; // 根据实际参数 + std::array 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) { diff --git a/src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp b/src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp deleted file mode 100644 index f868bbc..0000000 --- a/src/controllers/mdog_simple_controller/src/mdog_sim_controller.cpp +++ /dev/null @@ -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( - "/leg_fdb", 10, - std::bind(&MdogSimpleController::fdb_callback, this, std::placeholders::_1)); - input_sub_ = this->create_subscription( - "/control_input", 10, - std::bind(&MdogSimpleController::input_callback, this, std::placeholders::_1)); - ahrs_sub_ = this->create_subscription( - "/euler_angles", 10, - std::bind(&MdogSimpleController::ahrs_callback, this, std::placeholders::_1)); - cmd_pub_ = this->create_publisher("/leg_cmd", 10); - joint_state_pub_ = this->create_publisher("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()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp index b63f837..7898781 100644 --- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp +++ b/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("/leg_cmd", 10); joint_state_pub_ = this->create_publisher("joint_states", 10); + if_no_hardware_ = this->declare_parameter("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) {