传一下

2025-05-19 16:29:35 +08:00 · 2025-05-19 16:29:35 +08:00 · abfa985bd1
commit abfa985bd1
parent 6b2f364d0e
11 changed files with 361 additions and 15 deletions
--- 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
--- 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
@ -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;
+};
+
+}
--- 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<double, 3>& get_leg_link_length();
 // 获取左右腿hip方向（右腿为1，左腿为-1，顺序: FR, FL, RR, RL）
 const std::array<int, 4>& get_hip_signs();

+// 四足hip在body系下的固定偏移（单位：米，顺序: FR, FL, RR, RL）
+const std::array<std::array<double, 3>, 4>& get_hip_offsets();
+
+
 // 正运动学：已知关节角，求末端位置
 void forward_kinematics(const std::array<double, 3>& theta,
                        const std::array<double, 3>& link,
@ -23,5 +27,43 @@ bool inverse_kinematics(const std::array<double, 3>& pos,
                        const std::array<double, 3>& link,
                        std::array<double, 3>& theta);

+// 躯干->单腿坐标变换
+void foot_body_to_leg(const std::array<double, 3>& foot_body,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_leg);
+
+// 单腿->躯干坐标变换
+void foot_leg_to_body(const std::array<double, 3>& foot_leg,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& foot_body);
+
+// 躯干姿态变换（欧拉角+高度） body->world
+void body_to_world(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_body,
+                   std::array<double, 3>& foot_world);
+
+// world->body
+void world_to_body(const std::array<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_world,
+                   std::array<double, 3>& foot_body);
+
+// 综合正运动学（关节角->世界坐标）
+void leg_forward_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& theta,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& foot_world);
+
+// 综合逆运动学（世界坐标->关节角）
+bool leg_inverse_kinematics_world(int leg_index,
+                                  const std::array<double, 3>& foot_world,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& theta);
+                                  
 } // namespace kinematics
 } // namespace mdog_simple_controller
--- 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}
--- 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_;

--- 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<double, 3> body_pos = {0.0, 0.0, height};
+    std::array<double, 3> eulr = {
+        ctrl->data_.imu_eulr.rol,
+        ctrl->data_.imu_eulr.pit,
+        ctrl->data_.imu_eulr.yaw
+    };
+
+    // 原地平衡：足端目标点固定在body系下
+    std::array<std::array<double, 3>, 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<std::array<double, 3>, 4> foot_world_targets;
+    for (int leg = 0; leg < 4; ++leg) {
+        std::array<double, 3> 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<double, 3> 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) {
--- 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<std::array<double, 3>> 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;
            }
        }
    }
--- a/src/controllers/mdog_simple_controller/src/FSM/state_walk.cpp
+++ 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 <cmath>
+
+// ...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<std::array<double, 3>, 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<double, 3> 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<double, 3> target_theta;
+        kinematics::inverse_kinematics(home_pos, link, target_theta);
+
+        for (int leg = 0; leg < 4; ++leg) {
+            std::array<double, 3> 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<std::array<double, 3>> 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<double, 3> 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<double, 3> 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) {
+    // 可选：状态退出时清理
+}
+
+}
--- 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<int, 4>& get_hip_signs() {
    return HIP_SIGNS;
 }

+// 四足hip在body系下的固定偏移（单位：米，顺序: FR, FL, RR, RL）
+const std::array<std::array<double, 3>, 4>& get_hip_offsets() {
+    // 以body中心为原点，x前后，y左右，z向上
+    static constexpr std::array<std::array<double, 3>, 4> HIP_OFFSETS = {
+        std::array<double, 3>{ 0.20, -0.10, 0.0 }, // FR
+        std::array<double, 3>{ 0.20,  0.10, 0.0 }, // FL
+        std::array<double, 3>{-0.20, -0.10, 0.0 }, // RR
+        std::array<double, 3>{-0.20,  0.10, 0.0 }  // RL
+    };
+    return HIP_OFFSETS;
+}
+
+
 // 正运动学：已知关节角，求末端位置
 void forward_kinematics(const std::array<double, 3>& theta,
                        const std::array<double, 3>& link,
@ -24,20 +37,23 @@ void forward_kinematics(const std::array<double, 3>& 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<double, 3>& pos,
                        const std::array<double, 3>& link,
                        std::array<double, 3>& 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<double, 3>& pos,
    return true;
 }

+// 躯干->单腿坐标变换
+void foot_body_to_leg(const std::array<double, 3>& foot_body,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& 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<double, 3>& foot_leg,
+                      const std::array<double, 3>& hip_offset,
+                      std::array<double, 3>& 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<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_body,
+                   std::array<double, 3>& 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<double, 3>& body_pos,
+                   const std::array<double, 3>& eulr,
+                   const std::array<double, 3>& foot_world,
+                   std::array<double, 3>& foot_body) {
+    std::array<double, 3> 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<double, 3>& theta,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& foot_world) {
+    std::array<double, 3> 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<double, 3>& foot_world,
+                                  const std::array<double, 3>& link,
+                                  const std::array<double, 3>& body_pos,
+                                  const std::array<double, 3>& eulr,
+                                  std::array<double, 3>& theta) {
+    std::array<double, 3> 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
--- 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_) {
--- 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();