From 5f7c7042bf9a31340a1edf02dfd9744be36ba497 Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Tue, 20 May 2025 10:50:04 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=AD=A3=E9=81=A5=E6=8E=A7=E5=99=A8?= =?UTF-8?q?=E6=98=A0=E5=B0=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- build.sh | 2 ++ .../joystick_input/src/JoystickInput.cpp | 32 +++++++++--------- .../mdog_simple_controller/CMakeLists.txt | 1 + .../FSM/state_stand.hpp | 22 +++++++++++++ .../common/user_math.hpp | 5 +++ .../mdog_simple_controller.hpp | 2 ++ .../src/FSM/state_stand.cpp | 18 ++++++++++ .../src/FSM/state_walk.cpp | 33 +++++-------------- .../src/common/user_math.cpp | 19 +++++++++++ .../src/mdog_simple_controller.cpp | 16 ++++++--- 10 files changed, 104 insertions(+), 46 deletions(-) create mode 100644 build.sh create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp create mode 100644 src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp diff --git a/build.sh b/build.sh new file mode 100644 index 0000000..73fb450 --- /dev/null +++ b/build.sh @@ -0,0 +1,2 @@ +source install/setup.bash +colcon build \ No newline at end of file diff --git a/src/commands/joystick_input/src/JoystickInput.cpp b/src/commands/joystick_input/src/JoystickInput.cpp index b282738..5dc1209 100644 --- a/src/commands/joystick_input/src/JoystickInput.cpp +++ b/src/commands/joystick_input/src/JoystickInput.cpp @@ -13,24 +13,22 @@ JoystickInput::JoystickInput() : Node("joysick_input_node") { } void JoystickInput::joy_callback(sensor_msgs::msg::Joy::SharedPtr msg) { - if (msg->buttons[1] && msg->buttons[4]) { - inputs_.command = 1; // LB + B - } else if (msg->buttons[0] && msg->buttons[4]) { - inputs_.command = 2; // LB + A - } else if (msg->buttons[2] && msg->buttons[4]) { - inputs_.command = 3; // LB + X - } else if (msg->buttons[3] && msg->buttons[4]) { - inputs_.command = 4; // LB + Y - } else if (msg->axes[2] != 1 && msg->buttons[1]) { - inputs_.command = 5; // LT + B - } else if (msg->axes[2] != 1 && msg->buttons[0]) { - inputs_.command = 6; // LT + A - } else if (msg->axes[2] != 1 && msg->buttons[3]) { - inputs_.command = 7; // LT + X - } else if (msg->axes[2] != 1 && msg->buttons[4]) { - inputs_.command = 8; // LT + Y + if (msg->buttons[0]) { + inputs_.command = 1; // BACK + } else if (msg->buttons[1]) { + inputs_.command = 2; // RB + } else if (msg->buttons[3]) { + inputs_.command = 3; // LB + } else if (msg->buttons[4]) { + inputs_.command = 4; // Y + } else if (msg->buttons[6]) { + inputs_.command = 5; // X } else if (msg->buttons[7]) { - inputs_.command = 9; // START + inputs_.command = 6; // B + } else if (msg->buttons[8]) { + inputs_.command = 7; // A + } else if (msg->buttons[9]) { + inputs_.command = 8; } else { inputs_.command = 0; inputs_.lx = -msg->axes[0]; diff --git a/src/controllers/mdog_simple_controller/CMakeLists.txt b/src/controllers/mdog_simple_controller/CMakeLists.txt index 5e5fb46..f58d911 100644 --- a/src/controllers/mdog_simple_controller/CMakeLists.txt +++ b/src/controllers/mdog_simple_controller/CMakeLists.txt @@ -22,6 +22,7 @@ include_directories( add_executable(mdog_simple_controller src/mdog_simple_controller.cpp src/FSM/state_safe.cpp + src/FSM/state_stand.cpp src/FSM/state_zero.cpp src/FSM/state_walk.cpp src/FSM/state_balance.cpp 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 new file mode 100644 index 0000000..796763e --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_stand.hpp @@ -0,0 +1,22 @@ +#pragma once +#include +#include "mdog_simple_controller/FSM/fsm_state.hpp" + +namespace mdog_simple_controller { + +class StateStand : public FSMState { +public: + void enter(MdogSimpleController*) override; + void run(MdogSimpleController*) override; + void exit(MdogSimpleController*) override; + +private: + bool inited = false; + + std::array, 4> stand_pos; + + std::array, 4> start_pos; + +}; + +} \ No newline at end of file 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 afcec06..faadd38 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 @@ -23,7 +23,12 @@ constexpr float JOINT_RATIO[4][3] = { // feedback → real void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst); +// real → feedback +void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst); + // real cmd → 输出 cmd void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst); + + } // 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 79a7719..b9ff349 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 @@ -7,6 +7,7 @@ #include "sensor_msgs/msg/joint_state.hpp" #include "geometry_msgs/msg/vector3.hpp" #include "mdog_simple_controller/FSM/state_safe.hpp" +#include "mdog_simple_controller/FSM/state_stand.hpp" #include "mdog_simple_controller/FSM/state_zero.hpp" #include "mdog_simple_controller/FSM/state_walk.hpp" #include "mdog_simple_controller/FSM/state_balance.hpp" @@ -75,6 +76,7 @@ private: FSMState* current_state_{nullptr}; StateSafe state_safe_; + StateStand state_stand_; StateZero state_zero_; StateWalk state_walk_; StateBalance state_balance_; diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp new file mode 100644 index 0000000..74998a3 --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/FSM/state_stand.cpp @@ -0,0 +1,18 @@ +#include "mdog_simple_controller/FSM/state_stand.hpp" +#include "mdog_simple_controller/mdog_simple_controller.hpp" + +namespace mdog_simple_controller { + +void StateStand::enter(MdogSimpleController* ctrl) { + +} + +void StateStand::run(MdogSimpleController* ctrl) { + // SAFE状态下的控制逻辑 +} + +void StateStand::exit(MdogSimpleController* ctrl) { + // 离开SAFE状态时的清理 +} + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp index aef2a6a..00844d6 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp @@ -40,24 +40,11 @@ void StateWalk::run(MdogSimpleController* ctrl) { double dt = (now - last_time).seconds(); last_time = now; - static std::array, 4> start_theta; // 回位起点 - static bool start_theta_set = false; - if (!homing_done) { - if (!start_theta_set) { - // 只在回位开始时记录一次 - for (int leg = 0; leg < 4; ++leg) { - for (int j = 0; j < 3; ++j) { - start_theta[leg][j] = ctrl->data_.feedback_real[leg].pos[j]; - } - } - start_theta_set = true; - } - homing_time += dt; double alpha = std::min(homing_time / homing_duration, 1.0); // 插值系数 - std::array home_pos = {0.0, 0.0, -0.4}; + std::array home_pos = {0.0, 0.0, -0.35}; const auto& link = kinematics::get_leg_link_length(); const auto& hip_signs = kinematics::get_hip_signs(); @@ -67,9 +54,10 @@ void StateWalk::run(MdogSimpleController* ctrl) { for (int leg = 0; leg < 4; ++leg) { std::array theta; for (int j = 0; j < 3; ++j) { - // 插值到目标关节角度 - double tgt = target_theta[j] * hip_signs[leg]; - theta[j] = (1 - alpha) * start_theta[leg][j] + alpha * tgt; + // 每次都用当前位置做起点 + double start = ctrl->data_.feedback_real[leg].pos[j]; + double tgt = (j == 0) ? (target_theta[j] * hip_signs[leg]) : target_theta[j]; + theta[j] = (1 - alpha) * start + alpha * tgt; 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; @@ -81,16 +69,15 @@ void StateWalk::run(MdogSimpleController* ctrl) { if (homing_time >= homing_duration) { homing_done = true; - start_theta_set = false; // 下次回位可重新记录 } return; } - // ...existing code... + // 以下内容必须在 run 函数体内 std::vector> control_points = { - {0.15, 0.0, -0.40}, // 起点 - {0.10, 0.0, -0.2}, // 抬腿中点 - {-0.15, 0.0, -0.40}, // 落点 + {0.25, 0.0, -0.35}, // 起点 + {0.1, 0.0, -0.25}, // 抬腿中点 + {-0.05, 0.0, -0.35}, // 落点 }; const auto& p0 = control_points.front(); const auto& p1 = control_points.back(); @@ -133,8 +120,6 @@ void StateWalk::run(MdogSimpleController* ctrl) { realcmd_to_cmd(ctrl->data_, ctrl->data_); } -// ...existing code... - void StateWalk::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 f738b54..c16a208 100644 --- a/src/controllers/mdog_simple_controller/src/common/user_math.cpp +++ b/src/controllers/mdog_simple_controller/src/common/user_math.cpp @@ -16,6 +16,20 @@ void feedback_to_real(const MdogControllerData& src, MdogControllerData& dst) { } } +// real -> feedback +void real_to_feedback(const MdogControllerData& src, MdogControllerData& dst) { + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + // 位置：乘以减速比，加offset + dst.feedback[leg].pos[j] = src.feedback_real[leg].pos[j] * JOINT_RATIO[leg][j] + JOINT_OFFSET[leg][j]; + // 速度：乘以减速比 + dst.feedback[leg].speed[j] = src.feedback_real[leg].speed[j] * JOINT_RATIO[leg][j]; + // 力矩：除以减速比 + dst.feedback[leg].torque[j] = src.feedback_real[leg].torque[j] / JOINT_RATIO[leg][j]; + } + } +} + // real cmd → 输出 cmd void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) { for (int leg = 0; leg < 4; ++leg) { @@ -33,4 +47,9 @@ void realcmd_to_cmd(const MdogControllerData& src, MdogControllerData& dst) { } } +// 插补函数 +double interpolate(double start, double end, double alpha) { + return (1 - alpha) * start + alpha * end; +} + } // 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 6cb40fb..bc6341a 100644 --- a/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp +++ b/src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp @@ -48,7 +48,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; } @@ -75,12 +75,18 @@ 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[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]; + data_.feedback[leg].torque[j] = data_.command_real[leg].torque_des[j]; + data_.feedback[leg].speed[j] = data_.command_real[leg].speed_des[j]; } } - feedback_to_real(data_, data_); + // feedback_to_real(data_, data_); + // real_to_feedback(data_, data_); } rc_msgs::msg::LegCmd msg;