运动学解算正确

.vscode/settings.json

@@ -2,6 +2,8 @@
     "files.associations": {
         "deque": "cpp",
         "string": "cpp",
-        "vector": "cpp"
+        "vector": "cpp",
+        "chrono": "cpp",
+        "array": "cpp"
     }
 }

src/controllers/mdog_simple_controller/src/FSM/state_balance.cpp

@@ -19,9 +19,9 @@ void StateBalance::enter(MdogSimpleController* ctrl) {
 void StateBalance::run(MdogSimpleController* ctrl) {
     // 贝塞尔控制点
     std::vector<std::array<double, 3>> control_points = {
-        {0.0, 0.0, -0.30},      // 起点
+        {0.30, 0.0, 0.15},      // 起点
-        {0.05, 0.0, 0.05},     // 抬腿中点
+        {0.10, 0.0, 0.0},     // 抬腿中点
-        {0.10, 0.0, -0.30}      // 落点
+        {0.30, 0.0, -0.15},     // 落点
     };
     // 起点和终点
     const auto& p0 = control_points.front();
@@ -59,6 +59,10 @@ void StateBalance::run(MdogSimpleController* ctrl) {
         std::array<double, 3> 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;

src/controllers/mdog_simple_controller/src/common/kinematics.cpp

@@ -4,41 +4,48 @@
 namespace mdog_simple_controller {
 namespace kinematics {
 
+// 正运动学：已知关节角，求末端位置
 void forward_kinematics(const std::array<double, 3>& theta,
                         const std::array<double, 3>& link,
                         std::array<double, 3>& pos) {
+    // theta: [hip_yaw, hip_pitch, knee_pitch]
+    // link:  [hip_offset(y方向), thigh_length(x方向), shank_length(x方向)]
     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 = (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1);
+    double y = l1 + (l2 * cos(t2) + l3 * cos(t2 + t3)) * sin(t1);
-    double z = l1 + l2 * sin(t2) + l3 * sin(t2 + t3);
+    double z = l2 * sin(t2) + l3 * sin(t2 + t3);
 
     pos[0] = x;
     pos[1] = y;
     pos[2] = z;
 }
 
+// 逆运动学：已知末端位置，求关节角
 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 l1 = link[0], l2 = link[1], l3 = link[2];
 
-    theta[0] = atan2(y, x);
+    double y1 = y - l1;
+    double t1 = atan2(y1, x);
 
-    double d = sqrt(x * x + y * y);
+    double d = sqrt(x * x + y1 * y1);
-    double z1 = z - l1;
+    double z1 = z;
 
     double D = (d * d + z1 * z1 - l2 * l2 - l3 * l3) / (2 * l2 * l3);
-    if (D < -1.0 || D > 1.0) return false; // 不可达
+    if (D < -1.0 || D > 1.0) return false;
 
-    theta[2] = atan2(-sqrt(1 - D * D), D);
+    double t3 = atan2(-sqrt(1 - D * D), D);
-
+    double k1 = l2 + l3 * cos(t3);
-    double phi = atan2(z1, d);
+    double k2 = l3 * sin(t3);
-    double psi = atan2(l3 * sin(theta[2]), l2 + l3 * cos(theta[2]));
+    double t2 = atan2(z1, d) - atan2(k2, k1);
-    theta[1] = phi - psi;
 
+    theta[0] = t1;
+    theta[1] = t2;
+    theta[2] = t3;
     return true;
 }
 

src/controllers/mdog_simple_controller/src/mdog_simple_controller.cpp

@@ -18,7 +18,7 @@ MdogSimpleController::MdogSimpleController() : Node("mdog_simple_controller") {
     joint_state_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
 
     if_no_hardware_ = this->declare_parameter<bool>("if_no_hardware", false);
-
+    memset(&data_, 0, sizeof(data_));
     current_state_ = &state_zero_;
     current_state_->enter(this);
     timer_ = this->create_wall_timer(
@@ -77,9 +77,11 @@ 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处理，也可调用
         feedback_to_real(data_, data_);
     }
 
@@ -113,6 +115,7 @@ void MdogSimpleController::control_loop() {
     }
     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;
     js_msg.header.stamp = this->now();
     js_msg.name = {

src/robot_descriptions/qut/mdog_description/xacro/leg.xacro

@@ -35,8 +35,8 @@
 
     <!-- 定义thigh_joint -->
     <joint name="${name}_thigh_joint" type="revolute">
-      <!-- <origin rpy="0 -0.7854 0" xyz="0 ${mirror * 0.05} 0"/> -->
+      <origin rpy="0 -1.5708 0" xyz="0 ${mirror * 0.05} 0"/>
-      <origin rpy="0 0 0" xyz="0 ${mirror * 0.05} 0"/>
+      <!-- <origin rpy="0 0 0" xyz="0 ${mirror * 0.05} 0"/> -->
       <parent link="${name}_hip"/>
       <child link="${name}_thigh"/>
       <axis xyz="0 1 0"/>