From a1a29818e82dab2927177b805035cc846217cb70 Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Sun, 18 May 2025 01:43:28 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86balance=E5=92=8Ctrot?= =?UTF-8?q?=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mdog_simple_controller/CMakeLists.txt | 1 + .../mdog_simple_controller/common/pid.hpp | 22 ++++++ .../src/FSM/state_balance.cpp | 63 ----------------- .../src/FSM/state_troting.cpp | 67 +++++++++++++++++-- .../mdog_simple_controller/src/common/pid.cpp | 33 +++++++++ .../src/mdog_simple_controller.cpp | 3 - 6 files changed, 117 insertions(+), 72 deletions(-) create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp create mode 100644 src/controllers/mdog_simple_controller/src/common/pid.cpp diff --git a/src/controllers/mdog_simple_controller/CMakeLists.txt b/src/controllers/mdog_simple_controller/CMakeLists.txt index ca35ba8..5dcdac0 100644 --- a/src/controllers/mdog_simple_controller/CMakeLists.txt +++ b/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 ) diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp new file mode 100644 index 0000000..1b56dfe --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/common/pid.hpp @@ -0,0 +1,22 @@ +#pragma once + +namespace mdog_simple_controller { + +class PID { +public: + PID(double kp, double ki, double kd, double dt, double max_out = 1e6, double min_out = -1e6); + + void set_gains(double kp, double ki, double kd); + void set_limits(double min_out, double max_out); + void reset(); + double compute(double setpoint, double measurement); + +private: + double kp_, ki_, kd_; + double dt_; + double max_out_, min_out_; + double prev_error_; + double integral_; +}; + +} // namespace mdog_simple_controller \ 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 b33b079..1b4098a 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp @@ -1,78 +1,15 @@ #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 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> 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 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 theta; - std::array 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) { diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp index 6b8e96f..f19515b 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp @@ -1,23 +1,78 @@ #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 namespace mdog_simple_controller { +namespace { +double bezier_time = 0.0; +const double bezier_period = 1.0; +} + void StateTroting::enter(MdogSimpleController* ctrl) { // 进入TROTING状态时的初始化 } void StateTroting::run(MdogSimpleController* ctrl) { //设置所有cmd数据为0 + std::vector> 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) { - 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 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 theta; + std::array 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 StateTroting::exit(MdogSimpleController* ctrl) { diff --git a/src/controllers/mdog_simple_controller/src/common/pid.cpp b/src/controllers/mdog_simple_controller/src/common/pid.cpp new file mode 100644 index 0000000..c5e94d2 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/common/pid.cpp @@ -0,0 +1,33 @@ +#include "mdog_simple_controller/common/pid.hpp" + +namespace mdog_simple_controller { + +PID::PID(double kp, double ki, double kd, double dt, double max_out, double min_out) + : kp_(kp), ki_(ki), kd_(kd), dt_(dt), max_out_(max_out), min_out_(min_out), + prev_error_(0.0), integral_(0.0) {} + +void PID::set_gains(double kp, double ki, double kd) { + kp_ = kp; ki_ = ki; kd_ = kd; +} + +void PID::set_limits(double min_out, double max_out) { + min_out_ = min_out; max_out_ = max_out; +} + +void PID::reset() { + prev_error_ = 0.0; + integral_ = 0.0; +} + +double PID::compute(double setpoint, double measurement) { + double error = setpoint - measurement; + integral_ += error * dt_; + double derivative = (error - prev_error_) / dt_; + double output = kp_ * error + ki_ * integral_ + kd_ * derivative; + if (output > max_out_) output = max_out_; + if (output < min_out_) output = min_out_; + prev_error_ = error; + return output; +} + +} // 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 f83c942..0f5efdc 100644 --- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp +++ b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp @@ -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_);