From abfa985bd11ebeaacab06f0b74cf0da257fe00c0 Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Mon, 19 May 2025 16:29:35 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=A0=E4=B8=80=E4=B8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../mdog_simple_controller/CMakeLists.txt | 1 + .../mdog_simple_controller/FSM/state_walk.hpp | 13 ++ .../common/kinematics.hpp | 42 ++++++ .../common/user_math.hpp | 2 +- .../mdog_simple_controller.hpp | 2 + .../src/FSM/state_balance.cpp | 48 +++++- .../src/FSM/state_troting.cpp | 16 +- .../src/FSM/state_walk.cpp | 142 ++++++++++++++++++ .../src/common/kinematics.cpp | 105 ++++++++++++- .../src/mdog_simple_controller.cpp | 3 +- .../src/unitree_leg_serial.cpp | 2 +- 11 files changed, 361 insertions(+), 15 deletions(-) create mode 100644 src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_walk.hpp create mode 100644 src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp diff --git a/src/controllers/mdog_simple_controller/CMakeLists.txt b/src/controllers/mdog_simple_controller/CMakeLists.txt index 5dcdac0..5e5fb46 100644 --- a/src/controllers/mdog_simple_controller/CMakeLists.txt +++ b/src/controllers/mdog_simple_controller/CMakeLists.txt @@ -23,6 +23,7 @@ add_executable(mdog_simple_controller src/mdog_simple_controller.cpp src/FSM/state_safe.cpp src/FSM/state_zero.cpp + src/FSM/state_walk.cpp src/FSM/state_balance.cpp src/FSM/state_troting.cpp src/common/kinematics.cpp diff --git a/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_walk.hpp b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_walk.hpp new file mode 100644 index 0000000..2fbb01a --- /dev/null +++ b/src/controllers/mdog_simple_controller/include/mdog_simple_controller/FSM/state_walk.hpp @@ -0,0 +1,13 @@ +#pragma once +#include "mdog_simple_controller/FSM/fsm_state.hpp" + +namespace mdog_simple_controller { + +class StateWalk : public FSMState { +public: + void enter(MdogSimpleController*) override; + void run(MdogSimpleController*) override; + void exit(MdogSimpleController*) override; +}; + +} \ No newline at end of file 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 b591db7..f8af0de 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 @@ -13,6 +13,10 @@ const std::array& get_leg_link_length(); // 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL） const std::array& get_hip_signs(); +// 四足hip在body系下的固定偏移（单位：米，顺序: FR, FL, RR, RL） +const std::array, 4>& get_hip_offsets(); + + // 正运动学：已知关节角，求末端位置 void forward_kinematics(const std::array& theta, const std::array& link, @@ -23,5 +27,43 @@ bool inverse_kinematics(const std::array& pos, const std::array& link, std::array& theta); +// 躯干->单腿坐标变换 +void foot_body_to_leg(const std::array& foot_body, + const std::array& hip_offset, + std::array& foot_leg); + +// 单腿->躯干坐标变换 +void foot_leg_to_body(const std::array& foot_leg, + const std::array& hip_offset, + std::array& foot_body); + +// 躯干姿态变换（欧拉角+高度） body->world +void body_to_world(const std::array& body_pos, + const std::array& eulr, + const std::array& foot_body, + std::array& foot_world); + +// world->body +void world_to_body(const std::array& body_pos, + const std::array& eulr, + const std::array& foot_world, + std::array& foot_body); + +// 综合正运动学（关节角->世界坐标） +void leg_forward_kinematics_world(int leg_index, + const std::array& theta, + const std::array& link, + const std::array& body_pos, + const std::array& eulr, + std::array& foot_world); + +// 综合逆运动学（世界坐标->关节角） +bool leg_inverse_kinematics_world(int leg_index, + const std::array& foot_world, + const std::array& link, + const std::array& body_pos, + const std::array& eulr, + std::array& theta); + } // namespace kinematics } // namespace mdog_simple_controller \ 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 0f359ae..afcec06 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 @@ -6,7 +6,7 @@ namespace mdog_simple_controller { // 关节0点位置offset和减速比定义 constexpr float JOINT_OFFSET[4][3] = { - {-6.349f, -2.856f, 22.610f}, // 可根据实际填写 + {-5.149f, -5.8456f, 21.230f}, // 可根据实际填写 {-3.156f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, // {0.2f, 0.393f, 0.5507f} 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 f5cabb3..79a7719 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 @@ -8,6 +8,7 @@ #include "geometry_msgs/msg/vector3.hpp" #include "mdog_simple_controller/FSM/state_safe.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" #include "mdog_simple_controller/FSM/state_troting.hpp" @@ -75,6 +76,7 @@ private: FSMState* current_state_{nullptr}; StateSafe state_safe_; StateZero state_zero_; + StateWalk state_walk_; StateBalance state_balance_; StateTroting state_troting_; 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 55695c6..caf96ab 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp @@ -12,7 +12,53 @@ void StateBalance::enter(MdogSimpleController* ctrl) { } void StateBalance::run(MdogSimpleController* ctrl) { - + using namespace kinematics; + const auto& link = get_leg_link_length(); + const auto& hip_signs = get_hip_signs(); + + // 获取当前躯干高度和欧拉角 + double height = ctrl->input_cmd_.hight > 0.1 ? ctrl->input_cmd_.hight : 0.35; + std::array body_pos = {0.0, 0.0, height}; + std::array eulr = { + ctrl->data_.imu_eulr.rol, + ctrl->data_.imu_eulr.pit, + ctrl->data_.imu_eulr.yaw + }; + + // 原地平衡：足端目标点固定在body系下 + std::array, 4> foot_body_targets; + for (int leg = 0; leg < 4; ++leg) { + // 可加扰动补偿：如roll/pitch影响下的z修正 + double dz = 0.0; + // dz = -body_pos[2] * std::sin(eulr[0]); // roll补偿示例 + foot_body_targets[leg] = {0.0, 0.0, -height + dz}; + } + + // 计算足端世界坐标 + std::array, 4> foot_world_targets; + for (int leg = 0; leg < 4; ++leg) { + std::array foot_leg; + foot_body_to_leg(foot_body_targets[leg], get_hip_offsets()[leg], foot_leg); + body_to_world(body_pos, eulr, foot_leg, foot_world_targets[leg]); + } + + // 逆运动学求解每条腿的关节角 + for (int leg = 0; leg < 4; ++leg) { + std::array theta; + bool ok = leg_inverse_kinematics_world( + leg, foot_world_targets[leg], link, body_pos, eulr, theta); + if (ok) { + theta[0] *= hip_signs[leg]; + 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] = 1.0; + ctrl->data_.command_real[leg].kd[j] = 0.02; + } + } + } + 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 f8ff538..94d03d1 100644 --- a/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp +++ b/src/controllers/mdog_simple_controller/src/FSM/state_troting.cpp @@ -19,14 +19,14 @@ void StateTroting::enter(MdogSimpleController* ctrl) { void StateTroting::run(MdogSimpleController* ctrl) { //设置所有cmd数据为0 std::vector> control_points = { - // {0.40, 0.0, 0.15}, // 起点 - // {0.10, 0.0, 0.0}, // 抬腿中点 - // {0.40, 0.0, -0.05}, // 落点 + {0.25, 0.0, -0.35}, // 起点 + {0.1, 0.0, -0.25}, // 抬腿中点 + {-0.05, 0.0, -0.35}, // 落点 // {0.40, 0.0, 0.05}, // 起点 - {0.40, 0.0, 0.05}, // 抬腿中点 + // {0.40, 0.0, 0.05}, // 抬腿中点 - {0.40, 0.0, 0.05}, // 抬腿中点 - {0.40, 0.0, 0.05}, // 抬腿中点 + // {0.40, 0.0, 0.05}, // 抬腿中点 + // {0.40, 0.0, 0.05}, // 抬腿中点 // {0.40, 0.0, 0.05}, // 落点 }; // 起点和终点 @@ -73,8 +73,8 @@ void StateTroting::run(MdogSimpleController* ctrl) { 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; + ctrl->data_.command_real[leg].kp[j] = 1.2; + ctrl->data_.command_real[leg].kd[j] = 0.02; } } } diff --git a/src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp b/src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp new file mode 100644 index 0000000..aef2a6a --- /dev/null +++ b/src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp @@ -0,0 +1,142 @@ +#include "mdog_simple_controller/FSM/state_walk.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 + +// ...existing code... +namespace mdog_simple_controller +{ + +namespace { +double bezier_time = 0.0; +const double bezier_period = 2; +constexpr double swing_ratio = 0.25; // 摆动相占整个周期的比例 +const double phase_offset[4] = {0.0, 0.25, 0.5, 0.75}; // 四条腿交错 + +// 新增：回位阶段相关变量 +bool homing_done = false; +double homing_time = 0.0; +constexpr double homing_duration = 3.0; // 回位持续3秒 +} + +void StateWalk::enter(MdogSimpleController* ctrl) { + homing_done = false; + homing_time = 0.0; + bezier_time = 0.0; + // 记录进入回位时每条腿的当前位置 + for (int leg = 0; leg < 4; ++leg) { + for (int j = 0; j < 3; ++j) { + // 保存初始关节角度 + ctrl->data_.command_real[leg].pos_des[j] = ctrl->data_.feedback_real[leg].pos[j]; + } + } +} + +void StateWalk::run(MdogSimpleController* ctrl) { + static rclcpp::Time last_time = ctrl->now(); + rclcpp::Time now = ctrl->now(); + 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}; + const auto& link = kinematics::get_leg_link_length(); + const auto& hip_signs = kinematics::get_hip_signs(); + + std::array target_theta; + kinematics::inverse_kinematics(home_pos, link, target_theta); + + 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; + 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_); + + if (homing_time >= homing_duration) { + homing_done = true; + start_theta_set = false; // 下次回位可重新记录 + } + return; + } + + // ...existing code... + std::vector> control_points = { + {0.15, 0.0, -0.40}, // 起点 + {0.10, 0.0, -0.2}, // 抬腿中点 + {-0.15, 0.0, -0.40}, // 落点 + }; + const auto& p0 = control_points.front(); + const auto& p1 = control_points.back(); + + bezier_time += dt; + if (bezier_time > bezier_period) bezier_time -= bezier_period; + double t = bezier_time / bezier_period; // [0,1] + + const auto& link = kinematics::get_leg_link_length(); + const auto& hip_signs = kinematics::get_hip_signs(); + + for (int leg = 0; leg < 4; ++leg) { + double phase = std::fmod(t + phase_offset[leg], 1.0); + std::array foot_pos; + if (phase < swing_ratio) { + // 摆动相：贝塞尔曲线 + double swing_phase = phase / swing_ratio; // 归一化到[0,1] + foot_pos = bezier::bezier_curve_3d(control_points, swing_phase); + } else { + // 支撑相：直线滑动 + double stance_phase = (phase - swing_ratio) / (1.0 - swing_ratio); // 归一化到[0,1] + for (int i = 0; i < 3; ++i) { + foot_pos[i] = p1[i] + (p0[i] - p1[i]) * stance_phase; + } + } + + std::array theta; + bool ok = kinematics::inverse_kinematics(foot_pos, link, theta); + if (ok) { + theta[0] *= hip_signs[leg]; + 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_); +} + +// ...existing code... + +void StateWalk::exit(MdogSimpleController* ctrl) { + // 可选：状态退出时清理 +} + +} \ No newline at end of file diff --git a/src/controllers/mdog_simple_controller/src/common/kinematics.cpp b/src/controllers/mdog_simple_controller/src/common/kinematics.cpp index 5b5cfc5..0ec9a41 100644 --- a/src/controllers/mdog_simple_controller/src/common/kinematics.cpp +++ b/src/controllers/mdog_simple_controller/src/common/kinematics.cpp @@ -15,6 +15,19 @@ const std::array& get_hip_signs() { return HIP_SIGNS; } +// 四足hip在body系下的固定偏移（单位：米，顺序: FR, FL, RR, RL） +const std::array, 4>& get_hip_offsets() { + // 以body中心为原点，x前后，y左右，z向上 + static constexpr std::array, 4> HIP_OFFSETS = { + std::array{ 0.20, -0.10, 0.0 }, // FR + std::array{ 0.20, 0.10, 0.0 }, // FL + std::array{-0.20, -0.10, 0.0 }, // RR + std::array{-0.20, 0.10, 0.0 } // RL + }; + return HIP_OFFSETS; +} + + // 正运动学：已知关节角，求末端位置 void forward_kinematics(const std::array& theta, const std::array& link, @@ -24,20 +37,23 @@ void forward_kinematics(const std::array& theta, double t1 = theta[0], t2 = theta[1], t3 = theta[2]; double l1 = link[0], l2 = link[1], l3 = link[2]; + // 与逆运动学严格对应 double x = (l2 * cos(t2) + l3 * cos(t2 + t3)) * cos(t1); double y = l1 + (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1); double z = l2 * sin(t2) + l3 * sin(t2 + t3); - pos[0] = x; + // 逆运动学中 x = -pos[2], y = pos[1], z = pos[0] + // 所以正运动学输出应为 pos[0]=z, pos[1]=y, pos[2]=-x + pos[0] = z; pos[1] = y; - pos[2] = z; + pos[2] = -x; } // 逆运动学：已知末端位置，求关节角 bool inverse_kinematics(const std::array& pos, const std::array& link, std::array& theta) { - double x = pos[0], y = pos[1], z = pos[2]; + double x = -pos[2], y = pos[1], z = pos[0]; double l1 = link[0], l2 = link[1], l3 = link[2]; double y1 = y - l1; @@ -60,6 +76,89 @@ bool inverse_kinematics(const std::array& pos, return true; } +// 躯干->单腿坐标变换 +void foot_body_to_leg(const std::array& foot_body, + const std::array& hip_offset, + std::array& foot_leg) { + for (int i = 0; i < 3; ++i) + foot_leg[i] = foot_body[i] - hip_offset[i]; +} + +// 单腿->躯干坐标变换 +void foot_leg_to_body(const std::array& foot_leg, + const std::array& hip_offset, + std::array& foot_body) { + for (int i = 0; i < 3; ++i) + foot_body[i] = foot_leg[i] + hip_offset[i]; +} + +// 躯干->世界坐标变换 +void body_to_world(const std::array& body_pos, + const std::array& eulr, + const std::array& foot_body, + std::array& foot_world) { + double cr = cos(eulr[0]), sr = sin(eulr[0]); + double cp = cos(eulr[1]), sp = sin(eulr[1]); + double cy = cos(eulr[2]), sy = sin(eulr[2]); + double R[3][3] = { + {cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr}, + {sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr}, + {-sp, cp*sr, cp*cr} + }; + for (int i = 0; i < 3; ++i) { + foot_world[i] = body_pos[i]; + for (int j = 0; j < 3; ++j) + foot_world[i] += R[i][j] * foot_body[j]; + } +} + +// 世界->躯干坐标变换 +void world_to_body(const std::array& body_pos, + const std::array& eulr, + const std::array& foot_world, + std::array& foot_body) { + std::array delta; + for (int i = 0; i < 3; ++i) + delta[i] = foot_world[i] - body_pos[i]; + double cr = cos(eulr[0]), sr = sin(eulr[0]); + double cp = cos(eulr[1]), sp = sin(eulr[1]); + double cy = cos(eulr[2]), sy = sin(eulr[2]); + double R[3][3] = { + {cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr}, + {sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr}, + {-sp, cp*sr, cp*cr} + }; + for (int i = 0; i < 3; ++i) { + foot_body[i] = 0; + for (int j = 0; j < 3; ++j) + foot_body[i] += R[j][i] * delta[j]; + } +} + +// 综合正运动学（关节角->世界坐标） +void leg_forward_kinematics_world(int leg_index, + const std::array& theta, + const std::array& link, + const std::array& body_pos, + const std::array& eulr, + std::array& foot_world) { + std::array foot_leg, foot_body; + forward_kinematics(theta, link, foot_leg); + foot_leg_to_body(foot_leg, get_hip_offsets()[leg_index], foot_body); + body_to_world(body_pos, eulr, foot_body, foot_world); +} + +bool leg_inverse_kinematics_world(int leg_index, + const std::array& foot_world, + const std::array& link, + const std::array& body_pos, + const std::array& eulr, + std::array& theta) { + std::array foot_body, foot_leg; + world_to_body(body_pos, eulr, foot_world, foot_body); + foot_body_to_leg(foot_body, get_hip_offsets()[leg_index], foot_leg); + return inverse_kinematics(foot_leg, link, theta); +} } // namespace kinematics } // 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 f2236fb..6cb40fb 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.5; + input_cmd_.hight = 0.35; input_cmd_.status = msg->command; } @@ -64,6 +64,7 @@ void MdogSimpleController::control_loop() { 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; default: target_state = &state_safe_; break; } if (target_state != current_state_) { diff --git a/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp b/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp index bc5d1ae..e9743dd 100644 --- a/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp +++ b/src/hardware/unitree_leg_serial_driver/src/unitree_leg_serial.cpp @@ -9,7 +9,7 @@ UnitreeLegSerial::UnitreeLegSerial(const rclcpp::NodeOptions &options) : Node("unitree_leg_serial", options) { // 串口名和波特率初始化 - serial_port_name_ = {"/dev/ttyACM1", "/dev/ttyACM2", "/dev/ttyACM3", "/dev/ttyACM4"}; + serial_port_name_ = {"/dev/ttyACM0", "/dev/ttyACM1", "/dev/ttyACM2", "/dev/ttyACM3"}; baud_rate_ = {4000000, 4000000, 4000000, 4000000}; last_freq_time_ = this->now();