From 493e8082bee93cc287c03e7cae722067df5fce23 Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Tue, 20 May 2025 16:50:36 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8A=BD=E8=B1=A1=E5=8A=9B=E6=8E=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../FSM/state_stand.hpp | 22 +++++++- .../common/user_math.hpp | 4 +- .../mdog_simple_controller.hpp | 5 +- .../src/FSM/state_stand.cpp | 55 ++++++++++++++++++- .../src/common/user_math.cpp | 37 ++++++++++++- .../src/mdog_simple_controller.cpp | 48 ++++++---------- 6 files changed, 130 insertions(+), 41 deletions(-) diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp index 796763e..f83281e 100644 --- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp @@ -1,5 +1,6 @@ #pragma once #include +#include #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 kp = {4.5, 4.5, 4.5}; + std::array kd = {0.05, 0.05, 0.05}; - std::array, 4> stand_pos; + std::array, 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, 4> start_pos; + std::array, 4> target_joint_pos; + std::array, 4> start_joint_pos; + std::array, 4> last_joint_pos; }; diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp index faadd38..a18c4e6 100644 --- a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/user_math.hpp +++ b/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& kp, const std::array& kd); - +double limit_speed(double current, double target, double max_speed); +int8_t get_direction(double target, double current); } // namespace mdog_simple_controller \ No newline at end of file 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 b9ff349..33eadb7 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 @@ -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::SharedPtr fdb_sub_; rclcpp::Subscription::SharedPtr input_sub_; diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp index 74998a3..add84b0 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp +++ b/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) { diff --git a/src/controllers/mdog_simple_controller/src/common/user_math.cpp b/src/controllers/mdog_simple_controller/src/common/user_math.cpp index c16a208..07e867f 100644 --- a/src/controllers/mdog_simple_controller/src/common/user_math.cpp +++ b/src/controllers/mdog_simple_controller/src/common/user_math.cpp @@ -47,9 +47,40 @@ void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) { } } -// 插补函数 -double interpolate(double start, double end, double alpha) { - return (1 - alpha) * start + alpha * end; +void set_pd_config(MdogControllerData& data, const std::array& kp, const std::array& kd) { + for (int leg = 0; leg < 4; ++leg) { + 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 \ 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 bc6341a..ac8da74 100644 --- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp +++ b/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("joint_states", 10); if_no_hardware_ = this->declare_parameter("if_no_hardware", false); + if_debug_ = this->declare_parameter("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 7: target_state = &state_troting_; break; - case 8: target_state = &state_walk_; break; + // case 5: target_state = &state_balance_; break; + // case 6: target_state = &state_troting_; 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(), "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(), "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; + // RCLCPP_INFO(this->get_logger(), "cmd: %f %f %f %f %f %f %f %f %f %f %f %f", + // data_.command[0].pos_des[0], data_.command[0].pos_des[1], data_.command[0].pos_des[2], + // data_.command[1].pos_des[0], data_.command[1].pos_des[1], data_.command[1].pos_des[2], + // data_.command[2].pos_des[0], data_.command[2].pos_des[1], data_.command[2].pos_des[2], + // data_.command[3].pos_des[0], data_.command[3].pos_des[1], data_.command[3].pos_des[2]); + + 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]); - // } - // } }