fix motion action issues

.gitignore

@@ -0,0 +1,8 @@
+# 忽略所有层级的 build 文件夹及其内容
+**/build/
+
+# 忽略所有层级的 install 文件夹及其内容
+**/install/
+
+# 忽略所有层级的 log 文件夹及其内容
+**/log/

.vscode/c_cpp_properties.json

@@ -0,0 +1,18 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/opt/ros/humble/include/**",
+                "/usr/include/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-x64"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,82 @@
+{
+    "files.associations": {
+        "*.launch": "python",
+        "*.world": "xml",
+        "*.xacro": "xml",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "codecvt": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "regex": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "semaphore": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "variant": "cpp",
+        "dense": "cpp"
+    }
+}

src/robot_control/CMakeLists.txt

@@ -0,0 +1,69 @@
+cmake_minimum_required(VERSION 3.16)  # 提升最低版本以匹配ROS 2推荐版本
+project(robot_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# 依赖
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)  # 更精确的Eigen3查找方式
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(action_msgs REQUIRED) 
+find_package(rosidl_default_generators REQUIRED)
+find_package(interfaces REQUIRED)
+
+# 头文件目录
+include_directories(
+  include
+  include/${PROJECT_NAME}
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+# 源文件列表
+set(SOURCES
+  src/trapezoidal_trajectory.cpp
+  src/robot_control_manager.cpp  
+  src/robot_control_node.cpp
+  src/arm_control.cpp
+  src/leg_control.cpp
+  src/robot_model.cpp
+  src/wheel_control.cpp
+  src/waist_control.cpp
+  src/control_base.cpp
+  src/main.cpp
+)
+
+# 构建可执行文件
+add_executable(robot_control_node ${SOURCES})
+
+# 链接Eigen3库
+target_link_libraries(robot_control_node Eigen3::Eigen)
+
+# 链接ROS依赖
+ament_target_dependencies(robot_control_node 
+  rclcpp 
+  geometry_msgs 
+  sensor_msgs
+  trajectory_msgs
+  control_msgs
+  rclcpp_action
+  interfaces
+)
+
+
+# 安装可执行文件和launch目录
+install(TARGETS
+  robot_control_node
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

src/robot_control/LICENSE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

src/robot_control/action/MoveHome.action

@@ -0,0 +1,8 @@
+# 目标 回零点
+---
+# 结果：运动成功，或执行完毕
+bool success
+string message
+---
+# 反馈：各关节位置，运动进度
+int64[] joint_values

src/robot_control/action/MoveLeg.action

@@ -0,0 +1,9 @@
+# 目标 腿伸长或缩短运动
+float32 move_up_distance
+---
+# 结果：运动成功，或执行完毕
+bool success
+string message
+---
+# 反馈：各关节角度
+int64[] joint_values

src/robot_control/action/MoveWaist.action

@@ -0,0 +1,10 @@
+# 目标 腰部运动
+float32 move_pitch_degree
+float32 move_yaw_degree
+---
+# 结果：运动成功，或执行完毕
+bool success
+string message
+---
+# 反馈：各关节角度
+int64[] joint_values

src/robot_control/action/MoveWheel.action

@@ -0,0 +1,11 @@
+# 目标 底盘运动
+float32 move_distance
+float32 move_angle
+---
+# 结果：运动成功，或执行完毕
+bool success
+string message
+---
+# 反馈：当前位置，当前角度，运动进度
+float32 current_pos
+float32 current_angle

src/robot_control/include/arm_control.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include "control_base.hpp"  // 包含父类头文件
+
+namespace robot_control
+{
+    class ArmControl : public ControlBase
+    {
+        public:
+            ArmControl(
+                size_t total_joints, 
+                const std::vector<double>& lengthParameters, 
+                const std::vector<double>& maxSpeed,
+                const std::vector<double>& maxAcc,
+                const std::vector<double>& minLimits, 
+                const std::vector<double>& maxLimits, 
+                const std::vector<double>& home_positions,
+                const std::vector<double>& zero_positions,
+                const std::vector<int>& joint_directions
+            );
+
+            ~ArmControl() override;
+
+            bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
+
+            bool MoveUp(std::vector<double>& joint_positions, double dt);
+
+            bool MoveDown(std::vector<double>& joint_positions, double dt);
+    };
+
+}

src/robot_control/include/common_enum.hpp

@@ -0,0 +1,14 @@
+#pragma once
+
+namespace robot_control
+{
+    /**
+     * @brief 运动模块枚举
+     */
+    enum class MovementPart {
+        LEG,
+        WAIST,
+        WHEEL,
+        ALL
+    };
+}

src/robot_control/include/control_base.hpp

@@ -0,0 +1,77 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include "rclcpp/rclcpp.hpp"
+#include "trapezoidal_trajectory.hpp"
+#include "motion_parameters.hpp"
+
+namespace robot_control
+{
+    class ControlBase
+    {
+        // 重要：将成员变量改为 protected，子类可访问但外部不可见
+        protected:
+            size_t total_joints_;                               // 总关节数
+            std::vector<double> lengthParameters_;              // 机械参数（子类可复用）
+            std::vector<double> maxSpeed_;                      // 各关节最大速度
+            std::vector<double> maxAcc_;                        // 各关节最大加速度
+
+            double stopDurationTime_;
+            double movingDurationTime_;
+
+            TrapezoidalTrajectory* trapezoidalTrajectory_;      // 轨迹规划器（子类直接用）
+
+            std::vector<double> joint_home_positions_;          // 回home点位置
+            std::vector<double> joint_zero_positions_;          // 零点位置
+            std::vector<int> joint_directions_;                 // 关节运动方向
+            std::vector<double> joint_position_;                // 当前关节位置
+            std::vector<double> joint_velocity_;                // 当前关节速度
+            std::vector<double> joint_acceleration_;            // 当前关节加速度
+            std::vector<double> joint_torque_;                  // 当前关节力矩
+            std::vector<double> minLimits_;                     // 关节位置下限
+            std::vector<double> maxLimits_;                     // 关节位置上限
+            std::vector<double> joint_position_desired_;        // 期望关节位置
+            
+            bool is_moving_;                                    // 是否运动中
+            bool is_stopping_;                                  // 是否停止中
+            bool is_target_set_;                                // 是否已设置目标点
+
+        public:
+            // 构造函数（子类需调用此构造函数初始化父类）
+            ControlBase(
+                size_t total_joints, 
+                const std::vector<double>& lengthParameters, 
+                const std::vector<double>& maxSpeed,
+                const std::vector<double>& maxAcc,
+                const std::vector<double>& minLimits, 
+                const std::vector<double>& maxLimits, 
+                const std::vector<double>& home_positions,
+                const std::vector<double>& zero_positions,
+                const std::vector<int>& joint_directions
+            );
+
+            virtual ~ControlBase();
+
+            // 需子类重写的函数：声明为 virtual（纯虚函数/普通虚函数）
+            // 1. 笛卡尔空间目标点运动（机械臂需解算运动学，子类重写）
+            virtual bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) = 0;
+
+            // 2. 关节空间目标运动（通用逻辑，父类可实现，子类可重写）
+            virtual bool MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double duration);
+
+            // 3. 回零点（通用逻辑，父类实现）
+            virtual bool GoHome(std::vector<double>& joint_positions, double dt);
+
+            // 4. 停止运动（通用逻辑，父类实现）
+            virtual bool Stop(std::vector<double>& joint_positions, double dt);
+
+            // 5. 更新关节状态（通用逻辑，父类实现）
+            virtual void UpdateJointStates(const std::vector<double>& joint_positions, const std::vector<double>& joint_velocities, const std::vector<double>& joint_torques);
+
+            // 6. 判断是否运动中（通用逻辑，父类实现）
+            virtual bool IsMoving();
+
+            bool checkJointLimits(const std::vector<double>& target_joint);
+    };
+
+}

src/robot_control/include/deceleration_planner.hpp

@@ -0,0 +1,105 @@
+#pragma once
+
+#include <vector>
+#include <functional>
+#include <cmath>
+#include <stdexcept>
+
+inline double calculate_decel_time(double initial_velocity, double max_decel) {
+    if (std::fabs(initial_velocity) < 1e-9) { // 速度为0，无需减速
+        return 0.0;
+    }
+    return std::fabs(initial_velocity) / max_decel; // 时间 = 速度 / 减速度
+}
+
+/**
+ * @brief 机械臂多轴同步减速插补函数
+ * @details 计算多轴从当前位置、速度以最大减速度同步减速至停止的轨迹，确保所有轴同时停止
+ * 
+ * @param current_positions 各轴当前位置 (单位: m 或 rad，与机械臂配置一致)
+ * @param current_velocities 各轴当前速度 (单位: m/s 或 rad/s)
+ * @param max_deceleration 最大减速度 (绝对值，单位: m/s² 或 rad/s²)
+ * @param time_step 插补时间步长 (单位: s，建议0.001~0.01)
+ * 
+ * @note 1. 所有轴必须具有相同数量的元素
+ *       2. 最大减速度必须为正数
+ *       3. 若某轴初始速度为0，则该轴保持静止
+ */
+void plan_deceleration_trajectory(
+    const std::vector<double>& current_positions,
+    const std::vector<double>& current_velocities,
+    double max_deceleration,
+    double time_step,
+    const std::function<void(double, const std::vector<double>&, const std::vector<double>&)>& callback
+) {
+    // 输入合法性检查
+    if (current_positions.size() != current_velocities.size()) {
+        throw std::invalid_argument("位置和速度数组长度必须相同");
+    }
+    if (max_deceleration <= 0) {
+        throw std::invalid_argument("最大减速度必须为正数");
+    }
+    if (time_step <= 0 || time_step > 1.0) {
+        throw std::invalid_argument("时间步长必须在(0, 1]范围内");
+    }
+    if (!callback) {
+        throw std::invalid_argument("回调函数不能为空");
+    }
+
+    const size_t dof = current_positions.size(); // 自由度数量
+    std::vector<double> decel_times(dof);       // 各轴理论减速时间
+    std::vector<double> actual_decel(dof);      // 各轴实际减速度（确保同步停止）
+
+    // 第一步：计算各轴理论减速时间和总减速时间（取最大值，确保所有轴同时停止）
+    double total_time = 0.0;
+    for (size_t i = 0; i < dof; ++i) {
+        decel_times[i] = calculate_decel_time(current_velocities[i], max_deceleration);
+        if (decel_times[i] > total_time) {
+            total_time = decel_times[i]; // 总时间取最长减速时间
+        }
+    }
+
+    // 特殊情况：所有轴已静止
+    if (total_time < 1e-9) {
+        callback(0.0, current_positions, current_velocities);
+        return;
+    }
+
+    // 第二步：计算各轴实际减速度（确保在总时间内减速到0）
+    for (size_t i = 0; i < dof; ++i) {
+        if (std::fabs(current_velocities[i]) < 1e-9) {
+            actual_decel[i] = 0.0; // 静止轴减速度为0
+        } else {
+            // 实际减速度 = 初始速度 / 总时间（方向与速度相反）
+            actual_decel[i] = -std::copysign(max_deceleration, current_velocities[i]) 
+                            * (decel_times[i] / total_time);
+        }
+    }
+
+    // 第三步：时间插补计算（逐步生成轨迹）
+    for (double t = 0.0; t <= total_time + 1e-9; t += time_step) {
+        // 确保最后一步精确到达总时间
+        const double current_t = std::min(t, total_time);
+        
+        std::vector<double> positions(dof);
+        std::vector<double> velocities(dof);
+
+        for (size_t i = 0; i < dof; ++i) {
+            // 计算当前速度：v(t) = v0 + a*t
+            velocities[i] = current_velocities[i] + actual_decel[i] * current_t;
+            
+            // 计算当前位置：s(t) = s0 + v0*t + 0.5*a*t²
+            positions[i] = current_positions[i] 
+                         + current_velocities[i] * current_t 
+                         + 0.5 * actual_decel[i] * current_t * current_t;
+
+            // 数值稳定性处理（避免因浮点误差出现微小负速度）
+            if (std::fabs(velocities[i]) < 1e-9) {
+                velocities[i] = 0.0;
+            }
+        }
+
+        // 回调输出当前轨迹点
+        callback(current_t, positions, velocities);
+    }
+}

src/robot_control/include/leg_control.hpp

@@ -0,0 +1,39 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "trapezoidal_trajectory.hpp"
+#include "control_base.hpp"
+
+namespace robot_control
+{
+
+    /**
+     * @brief LegControl 类用于控制腿部运动
+     * 
+     * 该类提供了腿部关节的控制功能，包括关节重置、移动和计算目标位置等操作。
+     */
+    class LegControl : public ControlBase
+    {
+        public:
+            LegControl(
+                size_t total_joints, 
+                const std::vector<double>& lengthParameters, 
+                const std::vector<double>& maxSpeed,
+                const std::vector<double>& maxAcc,
+                const std::vector<double>& minLimits, 
+                const std::vector<double>& maxLimits, 
+                const std::vector<double>& home_positions,
+                const std::vector<double>& zero_positions,
+                const std::vector<int>& joint_directions
+            );
+
+            ~LegControl() override;
+
+            bool SetMoveLegParametersInternal(double moveDistance);
+
+            bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
+
+            bool MoveUp(std::vector<double>& joint_positions, double dt);
+    };
+
+}  // namespace robot_control

src/robot_control/include/motion_parameters.hpp

@@ -0,0 +1,310 @@
+#pragma once
+
+#include <vector>
+#include <iostream>
+#include <map>
+#include <string>
+
+
+#define CYCLE_TIME 4    // 插补周期（毫秒）
+
+#ifndef DEG2RAD
+#define DEG2RAD(x) ((x)*0.017453293)
+#endif
+
+#ifndef RAD2DEG
+#define RAD2DEG(x) ((x)*57.29578)
+#endif
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace robot_control 
+{
+    enum class LimitType {
+        POSITION,   // 位置限制（单位：度/弧度）
+        VELOCITY,   // 速度限制（单位：度/秒/弧度/秒）
+        EFFORT      // 力矩限制（单位：N·m）
+    };
+
+    // 单个关节的限制参数：包含 max（最大值）、min（最小值）、限制类型
+    struct JointLimit {
+        int index;          // 关节索引
+        double max;          // 关节运动范围上限（如位置最大角度）
+        double min;          // 关节运动范围下限（如位置最小角度）
+        LimitType limit_type;// 限制类型（区分位置/速度/力矩）
+
+        // 构造函数：默认初始化（避免未定义值）
+        JointLimit() : index(0), max(0.0), min(0.0), limit_type(LimitType::POSITION) {}
+
+        // 带参构造函数：快速初始化
+        JointLimit(int i, double max_val, double min_val, LimitType type) 
+            :index(i), max(max_val), min(min_val), limit_type(type) {
+            if (max < min) {
+                std::cerr << "[Warning] JointLimit: max (" << max << ") < min (" << min << "), swapping values!" << std::endl;
+                std::swap(max, min);
+            }
+        }
+    };
+
+    class MotionParameters 
+    {
+        public:
+            MotionParameters()
+            {
+                // 初始化结构参数 unit m
+                //TODO: 修改为实际参数
+                wheel_radius_ = 0.09;
+                wheel_separation_ = 0.26;
+
+                wheel_length_ = {
+                    wheel_radius_,
+                    wheel_radius_
+                };
+
+                // 腿长参数 unit m
+                leg_length_ = {
+                    0.70,
+                    0.66
+                };
+
+                waist_length_ = {
+                    0.1,
+                };
+ 
+                // 轮子速度参数
+                max_linear_speed_x_ = 100;
+                max_linear_speed_z_ = 10;
+                max_angular_speed_z_ = 50;
+
+                // 关节速度参数
+                max_joint_velocity_ = {
+                    15,
+                    15,
+                    15,
+                    15,
+                    15,
+                    15
+                }; 
+
+                max_joint_acceleration_ = {
+                    50,
+                    50,
+                    50,
+                    50,
+                    50,
+                    50
+                };
+
+                // 初始化关节索引
+                wheel_joint_indices_ = {0, 1}; 
+
+                waist_joint_indices_ = {0, 1}; 
+
+                leg_joint_indices_ = {2, 3, 4, 5};
+
+                total_joints_ = 6; 
+
+                // 初始化关节方向
+                wheel_joint_directions_ = {1, -1};  
+
+                waist_joint_directions_ = {1, 1};
+
+                leg_joint_directions_ = {1, -1, -1, 1};  
+
+                //TODO: check the ratio
+                joint_gear_ratio_ = {100.0, 100.0, 100.0, 100.0, 100.0, 100.0};  // 传动比 都一样
+                
+                joint_resolution_ = {524288, 131072, 131072 , 131072, 131072, 131072};  // 分辨率 都一样
+
+
+                //TODO: 限位需要修改
+                joint_limits_ = {
+                    JointLimit(0, 30.0,     -30.0, LimitType::POSITION),
+                    JointLimit(1, 180.0,    -180.0, LimitType::POSITION),
+                    JointLimit(2, 80.0,     0.0, LimitType::POSITION),
+                    // JointLimit(3, 5.0,     -5.0, LimitType::POSITION),
+                    // JointLimit(4, 50.0,      -50.0, LimitType::POSITION),
+                    JointLimit(3, 0.0,     -60.0, LimitType::POSITION),
+                    JointLimit(4, 0,     -80.0, LimitType::POSITION),
+                    // JointLimit(7, 5.0,     -5.0, LimitType::POSITION),
+                    // JointLimit(8, 50.0,     -50.0, LimitType::POSITION),
+                    JointLimit(5, 60.0,     0.0, LimitType::POSITION),
+                };
+
+                // 初始化限制参数
+                wheel_max_velocity_.resize(wheel_joint_indices_.size());
+                wheel_max_acceleration_.resize(wheel_joint_indices_.size());
+                wheel_max_limit_.resize(wheel_joint_indices_.size());
+                wheel_min_limit_.resize(wheel_joint_indices_.size());
+
+                wheel_max_velocity_ = {
+                    5,
+                    5
+                };
+                wheel_max_acceleration_ = {
+                    25,
+                    25
+                };
+
+                //There is no limit for wheel
+                wheel_max_limit_ = {
+                    0.0,
+                    0.0
+                };
+                wheel_min_limit_ = {
+                    0.0,
+                    0.0
+                };
+
+                waist_min_limit_.resize(waist_joint_indices_.size());
+                waist_max_limit_.resize(waist_joint_indices_.size());
+                waist_max_velocity_.resize(waist_joint_indices_.size());
+                waist_max_acceleration_.resize(waist_joint_indices_.size());
+
+                leg_max_limit_.resize(leg_joint_indices_.size());
+                leg_min_limit_.resize(leg_joint_indices_.size());
+                leg_max_velocity_.resize(leg_joint_indices_.size());
+                leg_max_acceleration_.resize(leg_joint_indices_.size());
+                
+                for (size_t i = 0; i < waist_joint_indices_.size(); i++)
+                {
+                    waist_max_limit_[i] = joint_limits_[waist_joint_indices_[i]].max;
+                    waist_min_limit_[i] = joint_limits_[waist_joint_indices_[i]].min;
+                    waist_max_velocity_[i] = max_joint_velocity_[waist_joint_indices_[i]];
+                    waist_max_acceleration_[i] = max_joint_acceleration_[waist_joint_indices_[i]];
+                }
+
+                for (size_t i = 0; i < leg_joint_indices_.size(); i++)
+                {
+                    leg_max_limit_[i] = joint_limits_[leg_joint_indices_[i]].max;
+                    leg_min_limit_[i] = joint_limits_[leg_joint_indices_[i]].min;
+                    leg_max_velocity_[i] = max_joint_velocity_[leg_joint_indices_[i]];
+                    leg_max_acceleration_[i] = max_joint_acceleration_[leg_joint_indices_[i]];
+                }
+                
+                waist_zero_positions_ = {
+                    181.0, 
+                    158.5
+                };
+
+                leg_zero_positions_ = {
+                    -18.36, 
+                    // 129.71, 
+                    // 45.02,
+                    25.54, 
+                    -21.54, 
+                    // 167.96, 
+                    // 16.26, 
+                    -127.24
+                };
+
+                waist_home_positions_ = {
+                    181.0, 
+                    158.5
+                };
+
+
+                // 后腿零点为站直时候的状态, 然后 + 25度, 后腿长 0.66m, 25度为 0.6m
+                // 前腿零点为站直时候的状态, 然后 髋部 pitch + 49度, 大腿长 0.38m, 38度为 0.25m , 小腿长0.36  膝盖 pitch 再收回 35.5 度, 剩余 13.5度 长度为 0.35m
+                // 初始化初始位置
+                leg_home_positions_ = {
+                    30.64, 
+                    // 129.71, 
+                    // 9.52,
+                    0.54, 
+                    -70.54, 
+                    // 167.96, 
+                    // 51.76, 
+                    -102.24
+                };
+
+                // 初始化零点位置
+                wheel_home_positions_ = {
+                    0.0,
+                    0.0
+                };
+
+                wheel_zero_positions_ = {
+                    0.0,
+                    0.0
+                };
+
+                pulse_to_degree_.resize(joint_resolution_.size());
+                degree_to_pulse_.resize(joint_resolution_.size());
+
+                for (size_t i = 0; i < joint_resolution_.size(); i++)
+                {
+                    degree_to_pulse_[i] = joint_resolution_[i] / 360.0;
+                    pulse_to_degree_[i] = 360.0 / joint_resolution_[i];
+                }
+
+                jog_step_size_ = 10.0/ (1000.0 / CYCLE_TIME) ;     // 5度每秒
+
+            };
+
+            // 运动参数
+            size_t total_joints_;           // 总关节数
+
+            // 关节索引
+            std::vector<int> leg_joint_indices_;    // 腿部关节索引
+            std::vector<int> wheel_joint_indices_;  // 轮子关节索引
+            std::vector<int> waist_joint_indices_;   // 腰部关节索引
+            std::vector<int> real_waist_joint_indices_;  // 实际腰部关节索引
+            std::vector<int> real_leg_joint_indices_;     // 实际腿部关节索引
+
+            std::vector<int> wheel_joint_directions_; // 轮子关节方向
+            std::vector<int> waist_joint_directions_; // 身体关节方向
+            std::vector<int> leg_joint_directions_; // 腿部关节方向
+
+            std::vector<double> leg_home_positions_; // 左腿初始位置
+            std::vector<double> waist_home_positions_; // 身体初始位置
+            std::vector<double> wheel_home_positions_; // 轮子零点位置
+
+            std::vector<double> waist_zero_positions_; // 身体零点位置
+            std::vector<double> leg_zero_positions_; // 左腿零点位置
+            std::vector<double> wheel_zero_positions_; // 轮子零点位置
+
+            // 限制参数
+            std::vector<JointLimit> joint_limits_; // 关节限制
+            std::vector<double> waist_min_limit_;
+            std::vector<double> waist_max_limit_;
+            std::vector<double> leg_min_limit_;
+            std::vector<double> leg_max_limit_;
+            std::vector<double> wheel_min_limit_;
+            std::vector<double> wheel_max_limit_;
+
+            std::vector<double> max_joint_velocity_;
+            std::vector<double> waist_max_velocity_;
+            std::vector<double> leg_max_velocity_;
+            std::vector<double> wheel_max_velocity_;
+
+            std::vector<double> max_joint_acceleration_;
+            std::vector<double> waist_max_acceleration_;
+            std::vector<double> leg_max_acceleration_;
+            std::vector<double> wheel_max_acceleration_;
+
+            // 轮子控制相关
+            double wheel_radius_;         // 轮子半径（米）
+            double wheel_separation_;     // 轮距（米）
+            double max_linear_speed_x_;       // 线速度（m/s）
+            double max_linear_speed_z_;       // 线速度（m/s）
+            double max_angular_speed_z_;      // 角速度（rad/s）
+
+            // 尺寸相关
+            std::vector<double> leg_length_; 
+            std::vector<double> waist_length_;
+            std::vector<double> wheel_length_;
+
+            //关节传动比和分辨率
+            std::vector<double> joint_gear_ratio_;
+            std::vector<double> joint_resolution_;
+            
+            std::vector<double> degree_to_pulse_; // 角度转脉冲的转换因子
+            std::vector<double> pulse_to_degree_; // 脉冲转角度的转换因子
+
+            double jog_step_size_; // Jog步长
+
+    };
+}

src/robot_control/include/robot_control_manager.hpp

@@ -0,0 +1,99 @@
+#pragma once
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "motion_parameters.hpp"
+#include "arm_control.hpp"
+#include "leg_control.hpp"
+#include "wheel_control.hpp"
+#include "robot_model.hpp"
+#include "common_enum.hpp"
+#include "waist_control.hpp"
+
+#include "interfaces/msg/motor_pos.hpp"
+
+using MotorPos = interfaces::msg::MotorPos;
+
+namespace robot_control
+{
+    class RobotControlManager
+    {
+        public:
+            RobotControlManager();
+            ~RobotControlManager();
+
+            // 控制机器人运动
+            bool MoveLeg(double dt);
+            bool MoveWaist(double dt);
+            bool MoveWheel();
+            bool Stop(double dt);
+            bool GoHome(double dt);
+            void JogAxis(size_t axis, int dir);
+
+            // 检查参数是否合理
+            bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle){return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);};
+            bool SetMoveLegParameters(double moveLegDistance){return legController_->SetMoveLegParametersInternal(moveLegDistance);};
+            bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle){return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);};
+            
+            // 机器人关节指令
+            std_msgs::msg::Float64MultiArray GetJointCommands();
+            std_msgs::msg::Float64MultiArray GetJointFeedback();
+            std_msgs::msg::Float64MultiArray GetWheelCommands();
+            std_msgs::msg::Float64MultiArray GetWheelFeedback();
+
+            void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
+            void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
+            void UpdateWheelStates(const MotorPos::SharedPtr msg);
+
+            MotionParameters GetMotionParameters();
+
+            bool IsMoving(MovementPart part);
+
+            bool RobotInitFinished();
+            
+        private:
+            void init();
+
+            MotionParameters motionParams_;
+
+            bool isWaistHomed_;
+            bool isLegHomed_;
+
+            // 控制器
+            LegControl* legController_;
+            WheelControl* wheelController_;
+            WaistControl* waistController_;
+            
+            // 运动状态
+            std::vector<double> wheelPositions_;
+            std::vector<double> jointPositions_; // 关节位置（弧度）
+
+            std::vector<double> jointVelocities_; // 关节速度（弧度/秒）
+            std::vector<double> wheelVelocities_; // 关节速度（弧度/秒）
+
+            std::vector<double> jointAccelerations_; // 关节加速度（弧度/秒^2）
+            std::vector<double> wheelAccelerations_; // 关节加速度（弧度/秒^2）
+
+            std::vector<double> jointEfforts_; // 关节力矩（牛顿米）
+            std::vector<double> wheelEfforts_; // 关节力矩（牛顿米）
+
+            std::vector<bool> jointInited_; // 机器人是否已经初始化
+            bool isJointInitValueSet_;
+            
+            // 临时变量
+            std::vector<double> tempWaistCmd_;
+            std::vector<double> tempLegCmd_;
+            std::vector<double> tempWheelCmd_;
+
+            MotorPos motorPos_;
+            sensor_msgs::msg::JointState jointStates_;
+            std_msgs::msg::Float64MultiArray jointCommands_; 
+            std_msgs::msg::Float64MultiArray wheelCommands_;
+
+            void AssignTempValues();
+            void UpdateJointCommands();
+    };
+
+} // namespace robot_control_manager

src/robot_control/include/robot_control_node.hpp

@@ -0,0 +1,108 @@
+#pragma once
+
+#include <filesystem>
+#include <fstream>  // 添加这行来支持文件流操作
+#include <time.h>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "robot_control_manager.hpp"
+
+#include "interfaces/action/move_home.hpp"
+#include "interfaces/action/move_leg.hpp"
+#include "interfaces/action/move_waist.hpp"
+#include "interfaces/action/move_wheel.hpp"
+#include "interfaces/msg/motor_cmd.hpp"
+
+
+using MoveHome = interfaces::action::MoveHome;
+using GoalHandleMoveHome = rclcpp_action::ServerGoalHandle<MoveHome>;
+
+using MoveLeg = interfaces::action::MoveLeg;
+using GoalHandleMoveLeg = rclcpp_action::ServerGoalHandle<MoveLeg>;
+
+using MoveWaist = interfaces::action::MoveWaist;
+using GoalHandleMoveWaist = rclcpp_action::ServerGoalHandle<MoveWaist>;
+
+using MoveWheel = interfaces::action::MoveWheel;
+using GoalHandleMoveWheel = rclcpp_action::ServerGoalHandle<MoveWheel>;
+
+using MotorCmd = interfaces::msg::MotorCmd;
+
+namespace robot_control
+{
+    class RobotControlNode : public rclcpp::Node 
+    {
+        public:
+            RobotControlNode();
+            ~RobotControlNode();
+            
+            // 状态机主循环
+            void ControlLoop();
+
+            rclcpp_action::GoalResponse handle_move_home_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveHome::Goal> goal);
+            rclcpp_action::CancelResponse handle_move_home_cancel(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
+            void handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
+            void move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle);
+            
+            rclcpp_action::GoalResponse handle_move_leg_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveLeg::Goal> goal);
+            rclcpp_action::CancelResponse handle_move_leg_cancel(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
+            void handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
+            void move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle);
+            
+            rclcpp_action::GoalResponse handle_move_waist_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveWaist::Goal> goal);
+            rclcpp_action::CancelResponse handle_move_waist_cancel(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
+            void handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
+            void move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle);
+
+            rclcpp_action::GoalResponse handle_move_wheel_goal(const rclcpp_action::GoalUUID & uuid,std::shared_ptr<const MoveWheel::Goal> goal);
+            rclcpp_action::CancelResponse handle_move_wheel_cancel(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
+            void handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
+            void move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle);
+
+        private:
+
+            rclcpp_action::Server<MoveHome>::SharedPtr move_home_action_server_;
+            bool move_home_executing_;
+            rclcpp_action::Server<MoveLeg>::SharedPtr move_leg_action_server_;
+            bool move_leg_executing_;
+            rclcpp_action::Server<MoveWaist>::SharedPtr move_waist_action_server_;
+            bool move_waist_executing_;
+            rclcpp_action::Server<MoveWheel>::SharedPtr move_wheel_action_server_;
+            bool move_wheel_executing_;
+
+            bool isStopping_;
+
+            rclcpp::Publisher<std_msgs::msg::String>::SharedPtr ethercatSetPub_;
+            rclcpp::Subscription<std_msgs::msg::String>::SharedPtr joyCommandSub_;
+            rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
+            rclcpp::Publisher<MotorCmd>::SharedPtr motorCmdPub_;
+            rclcpp::Subscription<MotorPos>::SharedPtr wheelStatesSub_;
+
+            RobotControlManager robotControlManager_;
+
+             // 文件流相关
+            std::ofstream data_file_;  // 用于写入数据的文件流
+            std::string data_file_path_;  // 数据文件路径
+
+            rclcpp::TimerBase::SharedPtr controlTimer_;
+            rclcpp::Time lastTime_;  // 移至类成员
+
+            bool isJogMode_;
+
+            int jogDirection_;
+            size_t jogIndex_;
+            double jogValue_;
+
+            void Publish_joint_trajectory();
+
+            void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
+            void JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg);
+            void WheelStatesCallback(const MotorPos::SharedPtr cmd_msg);
+    };
+
+} // namespace robot_control
+

src/robot_control/include/robot_kinematics.hpp

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "robot_model.hpp"
+#include <Eigen/Dense>
+#include <vector>
+#include <memory>
+
+namespace robot_control
+{
+
+/**
+ * @class RobotKinematics
+ * @brief 机器人运动学计算类，处理手臂和腿部的正逆运动学计算
+ */
+    class RobotKinematics
+    {
+    public:
+        /**
+         * @brief 构造函数
+         * @param robot_model 机器人模型指针
+         */
+        explicit RobotKinematics(std::shared_ptr<RobotModel> robot_model);
+
+        /**
+         * @brief 析构函数
+         */
+        ~RobotKinematics() = default;
+
+        // 手臂运动学
+
+        /**
+         * @brief 计算手臂正运动学
+         * @param joint_angles 关节角度数组
+         * @return 末端执行器位置 (x, y, z)
+         */
+        Eigen::Vector3d arm_forward_kinematics(const std::vector<double>& joint_angles);
+
+        /**
+         * @brief 计算手臂逆运动学
+         * @param target_pos 目标位置 (x, y, z)
+         * @param[out] joint_angles 计算得到的关节角度
+         * @return 成功返回true，失败返回false
+         */
+        bool arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);
+
+        // 腿部运动学
+
+        /**
+         * @brief 计算腿部正运动学
+         * @param leg_index 腿的索引 (0-3)
+         * @param joint_angles 关节角度数组
+         * @return 足部位置 (x, y, z)
+         */
+        Eigen::Vector3d leg_forward_kinematics(size_t leg_index, const std::vector<double>& joint_angles);
+
+        /**
+         * @brief 计算腿部逆运动学
+         * @param leg_index 腿的索引 (0-3)
+         * @param target_pos 目标位置 (x, y, z)
+         * @param[out] joint_angles 计算得到的关节角度
+         * @return 成功返回true，失败返回false
+         */
+        bool leg_inverse_kinematics(size_t leg_index, const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles);
+
+    private:
+        /**
+         * @brief 创建旋转矩阵 (绕Z轴)
+         * @param angle 旋转角度(弧度)
+         * @return 旋转矩阵
+         */
+        Eigen::Matrix3d rotate_z(double angle);
+
+        /**
+         * @brief 创建旋转矩阵 (绕Y轴)
+         * @param angle 旋转角度(弧度)
+         * @return 旋转矩阵
+         */
+        Eigen::Matrix3d rotate_y(double angle);
+
+        /**
+         * @brief 创建旋转矩阵 (绕X轴)
+         * @param angle 旋转角度(弧度)
+         * @return 旋转矩阵
+         */
+        Eigen::Matrix3d rotate_x(double angle);
+
+        std::shared_ptr<RobotModel> robot_model_;  // 机器人模型指针
+    };
+
+}  // namespace robot_model

src/robot_control/include/robot_model.hpp

@@ -0,0 +1,111 @@
+#pragma once
+
+#include <vector>
+#include <string>
+#include <memory>
+
+namespace robot_control
+{
+
+    class UrdfParser;
+
+    // 关节结构体
+    struct Joint
+    {
+        std::string name;          // 关节名称
+        double angle;              // 关节角度（弧度）
+        double min_angle;          // 最小角度限制
+        double max_angle;          // 最大角度限制
+        double effort_limit;       // 力/力矩限制
+
+        Joint(const std::string & joint_name, double min, double max, double effort)
+            : name(joint_name), angle(0.0), min_angle(min), max_angle(max), effort_limit(effort) {}
+    };
+
+    // 连杆结构体
+    struct Link
+    {
+        std::string name;          // 连杆名称
+        double length;             // 连杆长度
+        double mass;               // 连杆质量
+
+        Link(const std::string & link_name, double len, double m)
+            : name(link_name), length(len), mass(m) {}
+    };
+
+    // 机械臂结构体
+    struct Arm
+    {
+        std::vector<std::shared_ptr<Joint>> joints;  // 关节
+        std::vector<std::shared_ptr<Link>> links;    // 连杆
+        std::string side;                            // 机械臂位置（如"left", "right"）
+    };
+
+    // 腿部结构体
+    struct Leg
+    {
+        std::vector<std::shared_ptr<Joint>> joints;  // 关节
+        std::vector<std::shared_ptr<Link>> links;    // 连杆
+        std::string side;                            // 腿部位置（如"front_left", "front_right", "rear_left", "rear_right"）
+    };
+
+    class RobotModel
+    {
+        // 声明UrdfParser为友元类，使其可以访问私有成员
+        friend class UrdfParser;
+        
+    public:
+        /**
+         * @brief 构造函数
+         */
+        RobotModel();
+
+        /**
+         * @brief 析构函数
+         */
+        ~RobotModel() = default;
+
+        /**
+         * @brief 获取机械臂
+         * @return 机械臂对象
+         */
+        const Arm & get_arm(size_t index) const;
+
+        /**
+         * @brief 获取腿部
+         * @param index 腿的索引（0-3）
+         * @return 腿部对象
+         */
+        const Leg & get_leg(size_t index) const;
+
+        /**
+         * @brief 设置关节角度
+         * @param joint_name 关节名称
+         * @param angle 目标角度（弧度）
+         * @return 成功返回true，失败返回false
+         */
+        bool set_joint_angle(const std::string & joint_name, double angle);
+
+        /**
+         * @brief 获取关节角度
+         * @param joint_name 关节名称
+         * @return 关节角度（弧度），未找到返回0.0
+         */
+        double get_joint_angle(const std::string & joint_name) const;
+
+        /**
+         * @brief 打印机器人模型信息
+         */
+        void print_model_info() const;
+
+    private:
+        /**
+         * @brief 初始化机器人模型
+         */
+        void initialize_model();
+
+        std::vector<Arm> arms_;            // 机械臂集合 （2个机械臂）
+        std::vector<Leg> legs_;            // 腿部集合（4条腿）
+    };
+
+}  // namespace robot_CONTROL

src/robot_control/include/trapezoidal_trajectory.hpp

@@ -0,0 +1,172 @@
+#pragma once
+
+#include <vector>
+#include <Eigen/Dense>
+
+namespace robot_control
+{
+
+/**
+ * @brief 多轴梯形速度轨迹规划器（支持各轴独立速度/加速度约束）
+ * 保证所有轴同时启动、同时结束，加速/匀速/减速阶段时间完全同步
+ */
+class TrapezoidalTrajectory
+{
+public:
+    /**
+     * @brief 构造函数（多轴版本）
+     * @param max_velocities 各轴最大速度约束（向量，长度=轴数）
+     * @param max_accelerations 各轴最大加速度约束（向量，长度=轴数）
+     */
+    TrapezoidalTrajectory(const std::vector<double>& max_velocities, 
+                         const std::vector<double>& max_accelerations);
+    
+    /**
+     * @brief 构造函数（单轴版本）
+     * @param max_velocity 最大速度约束
+     * @param max_acceleration 最大加速度约束
+     */
+    TrapezoidalTrajectory(double max_velocity = 1.0, double max_acceleration = 0.5);
+    
+    /**
+     * @brief 析构函数
+     */
+    ~TrapezoidalTrajectory() = default;
+    
+    /**
+     * @brief 初始化多轴轨迹规划
+     * @param start_pos 起始位置（向量，长度=轴数）
+     * @param target_pos 目标位置（向量，长度=轴数）
+     * @param duration 期望运动时间（若>0且>最小时间则强制使用该时间）
+     */
+    void init(const std::vector<double>& start_pos, const std::vector<double>& target_pos);
+    
+    /**
+     * @brief 初始化单轴轨迹规划
+     * @param start_pos 起始位置
+     * @param target_pos 目标位置
+     * @param duration 期望运动时间（若>0且>最小时间则强制使用该时间）
+     */
+    void init(double start_pos, double target_pos);
+    
+    /**
+     * @brief 更新多轴轨迹，计算当前时间的位置
+     * @param time 已运动时间（s）
+     * @return 当前位置向量
+     */
+    std::vector<double> update(double time);
+    
+    /**
+     * @brief 更新单轴轨迹，计算当前时间的位置
+     * @param time 已运动时间（s）
+     * @return 当前位置
+     */
+    double updateSingle(double time);
+    
+    /**
+     * @brief 获取当前速度（多轴）
+     */
+    std::vector<double> getVelocity() const { return current_velocity_; }
+    
+    /**
+     * @brief 获取当前速度（单轴）
+     */
+    double getSingleVelocity() const { return current_velocity_[0]; }
+    
+    /**
+     * @brief 获取当前加速度（多轴）
+     */
+    std::vector<double> getAcceleration() const { return current_acceleration_; }
+    
+    /**
+     * @brief 获取当前加速度（单轴）
+     */
+    double getSingleAcceleration() const { return current_acceleration_[0]; }
+    
+    /**
+     * @brief 检查轨迹是否已完成
+     * @param time 已运动时间（s）
+     * @return 完成返回true
+     */
+    bool isFinished(double time) const { return time >= total_time_; }
+
+    /**
+     * @brief 设置各轴最大速度
+     * @param max_velocities 新的最大速度向量（长度=轴数）
+     */
+    void setMaxVelocities(const std::vector<double>& max_velocities);
+    
+    /**
+     * @brief 设置单轴最大速度
+     * @param max_velocity 新的最大速度
+     */
+    void setMaxVelocity(double max_velocity);
+    
+    /**
+     * @brief 设置各轴最大加速度
+     * @param max_accelerations 新的最大加速度向量（长度=轴数）
+     */
+    void setMaxAccelerations(const std::vector<double>& max_accelerations);
+    
+    /**
+     * @brief 设置单轴最大加速度
+     * @param max_acceleration 新的最大加速度
+     */
+    void setMaxAcceleration(double max_acceleration);
+
+    /**
+     * @brief 计算急停轨迹参数（当前状态下减速到停止）
+     */
+    void calculateStopTrajectory();
+    
+    /**
+     * @brief 更新急停轨迹
+     * @param dt 从急停开始的时间（s）
+     * @return 当前位置
+     */
+    std::vector<double> updateStop(double dt);
+    
+    /**
+     * @brief 检查急停是否完成
+     * @param dt 从急停开始的时间（s）
+     * @return 完成返回true
+     */
+    bool isStopFinished(double dt) const { return dt >= stop_total_time_; }
+
+private:
+    /**
+     * @brief 计算轨迹核心参数（统一时间+各轴巡航速度）
+     */
+    void calculateTrajectoryParams();
+
+    std::vector<double> start_pos_;         // 起始位置（各轴）
+    std::vector<double> target_pos_;        // 目标位置（各轴）
+    std::vector<double> total_distance_;    // 总位移绝对值（各轴）
+    std::vector<double> current_pos_;       // 当前位置（各轴）
+    std::vector<double> current_velocity_;  // 当前速度（各轴）
+    std::vector<double> current_acceleration_; // 当前加速度（各轴）
+
+    // 轨迹参数（各轴独立）
+    std::vector<double> max_velocity_;      // 最大速度（各轴独立）
+    std::vector<double> max_acceleration_;  // 最大加速度（各轴独立）
+    std::vector<double> cruise_velocity_;   // 巡航速度（各轴独立）
+    std::vector<double> acceleration_;      // 加速度（各轴独立）
+    std::vector<double> deceleration_;      // 减速度（各轴独立）
+
+    // 统一时间参数（所有轴共用）
+    double total_time_;         // 总运动时间
+    double acceleration_time_;  // 加速阶段时间
+    double deceleration_time_;  // 减速阶段时间
+    double cruise_time_;        // 匀速阶段时间
+
+    // 急停相关参数
+    bool is_stopping_;              // 是否处于急停状态
+    double stop_total_time_;        // 急停总时间（统一）
+    std::vector<double> stop_start_pos_;  // 急停起始位置
+    std::vector<double> stop_start_vel_;  // 急停起始速度
+    std::vector<double> stop_decel_;      // 急停减速度（各轴独立）
+
+    bool is_single_joint_;  // 是否为单轴模式
+};
+
+}  // namespace robot_control

src/robot_control/include/urdf_parser.hpp

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <string>
+#include <memory>
+#include <urdf/model.h>
+
+#include "robot_model.hpp"
+
+namespace robot_control
+{
+
+    class UrdfParser
+    {
+    public:
+        /**
+         * @brief 构造函数
+         */
+        UrdfParser() = default;
+
+        /**
+         * @brief 析构函数
+         */
+        ~UrdfParser() = default;
+
+        /**
+         * @brief 从文件解析URDF并构建RobotModel
+         * @param urdf_file_path URDF文件路径
+         * @return 构建的RobotModel共享指针，失败返回nullptr
+         */
+        std::shared_ptr<RobotModel> parse_from_file(const std::string& urdf_file_path);
+
+        /**
+         * @brief 从XML字符串解析URDF并构建RobotModel
+         * @param urdf_xml URDF XML字符串
+         * @return 构建的RobotModel共享指针，失败返回nullptr
+         */
+        std::shared_ptr<RobotModel> parse_from_string(const std::string& urdf_xml);
+
+    private:
+        /**
+         * @brief 解析URDF模型并构建RobotModel
+         * @param model URDF模型
+         * @return 构建的RobotModel共享指针
+         */
+        std::shared_ptr<RobotModel> build_robot_model(const urdf::Model& model);
+
+        /**
+         * @brief 解析连杆信息
+         * @param link URDF连杆
+         * @return 构建的Link共享指针
+         */
+        std::shared_ptr<Link> parse_link(const urdf::LinkConstSharedPtr& link);
+
+        /**
+         * @brief 解析关节信息
+         * @param joint URDF关节
+         * @return 构建的Joint共享指针
+         */
+        std::shared_ptr<Joint> parse_joint(const urdf::JointConstSharedPtr& joint);
+
+        /**
+         * @brief 识别关节所属部分（手臂、腿部等）
+         * @param joint_name 关节名称
+         * @return 关节所属部分的字符串标识
+         */
+        std::string identify_joint_part(const std::string& joint_name);
+
+        /**
+         * @brief 识别腿部位置（前左、前右、后左、后右）
+         * @param joint_name 关节名称
+         * @return 腿部位置字符串
+         */
+        std::string identify_leg_side(const std::string& joint_name);
+    };
+
+} // namespace robot_control

src/robot_control/include/waist_control.hpp

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include "control_base.hpp"  // 包含父类头文件
+
+namespace robot_control
+{
+    class WaistControl : public ControlBase
+    {
+        public:
+            WaistControl(
+                size_t total_joints, 
+                const std::vector<double>& lengthParameters, 
+                const std::vector<double>& maxSpeed,
+                const std::vector<double>& maxAcc,
+                const std::vector<double>& minLimits, 
+                const std::vector<double>& maxLimits, 
+                const std::vector<double>& home_positions,
+                const std::vector<double>& zero_positions,
+                const std::vector<int>& joint_directions
+            );
+
+            ~WaistControl() override;
+
+            bool SetMoveWaistParametersInternal(double movepitch, double moveyaw);
+
+            bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
+
+            bool MoveWaist(std::vector<double>& joint_positions, double dt);
+    };
+
+}

src/robot_control/include/wheel_control.hpp

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <Eigen/Dense>
+#include "control_base.hpp"
+
+namespace robot_control
+{
+    class WheelControl : public ControlBase
+    {
+        public:
+            WheelControl(
+                size_t total_joints, 
+                const std::vector<double>& lengthParameters, 
+                const std::vector<double>& maxSpeed,
+                const std::vector<double>& maxAcc,
+                const std::vector<double>& minLimits, 
+                const std::vector<double>& maxLimits, 
+                const std::vector<double>& home_positions,
+                const std::vector<double>& zero_positions,
+                const std::vector<int>& joint_directions
+            );
+
+            ~WheelControl() override;
+
+            bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
+
+            bool MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double duration) override;
+
+            bool MoveWheel(std::vector<double>& joint_positions);
+
+            bool GoHome(std::vector<double>& joint_positions, double dt) override;
+    };
+} // namespace robot_control

src/robot_control/launch/robot_control.launch.py

@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import TimerAction
+
+def generate_launch_description():
+    # 创建节点启动描述
+    robot_control_node = Node(
+        package='robot_control',  # 功能包名称
+        executable='robot_control_node',  # 可执行文件名称
+        name='robot_control_node',  # 节点名称（可自定义）
+        output='screen',  # 输出到屏幕
+        parameters=[{"use_sim_time": False}]  # 启用模拟时间
+    )
+
+    start_robot_control_node = TimerAction(
+        period=1.0,
+        actions=[robot_control_node],
+    )
+    
+    # 组装launch描述
+    return LaunchDescription([
+        start_robot_control_node
+    ])

src/robot_control/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>robot_control</name>
+  <version>1.0.0</version>
+  <description>Robot control package</description>
+  <maintainer email="Ray@email.com">Your Name</maintainer>
+  <license>Apache-2.0</license>
+
+  <!-- 编译时依赖 -->
+  <build_depend>ament_cmake</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <build_depend>action_msgs</build_depend>
+  <build_depend>rclcpp</build_depend>  <!-- 编译时也需依赖，避免链接错误 -->
+  <build_depend>rclcpp_action</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>trajectory_msgs</build_depend>
+  <build_depend>control_msgs</build_depend>
+  <build_depend>Eigen3</build_depend>  <!-- 对应CMake中的Eigen3依赖 -->
+  <build_depend>interfaces</build_depend>
+
+
+  <!-- 运行时依赖（必须完整，否则类型无法识别） -->
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_action</exec_depend>  <!-- 关键：Action通信核心库 -->
+  <exec_depend>action_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>interfaces</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robot_control/src/arm_control.cpp

@@ -0,0 +1,112 @@
+#include "arm_control.hpp"
+#include <stdexcept>
+#include <vector>
+
+namespace robot_control
+{
+
+ArmControl::ArmControl(
+    size_t total_joints, 
+    const std::vector<double>& lengthParameters, 
+    const std::vector<double>& maxSpeed,
+    const std::vector<double>& maxAcc,
+    const std::vector<double>& minLimits, 
+    const std::vector<double>& maxLimits, 
+    const std::vector<double>& home_positions,
+    const std::vector<double>& zero_positions,
+    const std::vector<int>& joint_directions
+): ControlBase(
+    total_joints, 
+    lengthParameters, 
+    maxSpeed, maxAcc, 
+    minLimits, 
+    maxLimits, 
+    home_positions, 
+    zero_positions,
+    joint_directions)
+{
+    std::cout << "ArmControl Constructor" << std::endl;
+    if (total_joints_ <= 0)
+        throw std::invalid_argument("Total joints must be positive");
+    
+    // 初始化关节状态向量
+    joint_position_.resize(total_joints_, 0.0);
+    joint_velocity_.resize(total_joints_, 0.0);
+    joint_acceleration_.resize(total_joints_, 0.0);
+    joint_torque_.resize(total_joints_, 0.0);
+    joint_position_desired_.resize(total_joints_, 0.0);
+    
+    // 初始化梯形轨迹规划器
+    trapezoidalTrajectory_ = new TrapezoidalTrajectory(10.0, 100);
+
+    is_moving_ = false;
+    is_stopping_ = false;
+    stopDurationTime_ = 0.0;
+    movingDurationTime_ = 0.0;
+}
+
+ArmControl::~ArmControl()
+{
+    delete trapezoidalTrajectory_;
+}
+
+
+bool ArmControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
+{
+    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
+    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
+    std::vector<double> joint_target(total_joints_, 0.0);
+    
+    // 实际应用中应在此处添加逆运动学求解
+    // joint_target = inverse_kinematics(target_pos);
+    
+    for (size_t i = 0; i < target_pos.size(); i++)
+    {
+        std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
+    }
+
+    // 使用梯形轨迹规划移动到目标关节位置
+    return MoveToTargetJoint(joint_positions, joint_target, dt);
+}
+
+bool ArmControl::MoveUp(std::vector<double>& joint_positions, double dt)
+{    
+    if (!is_moving_)
+    {
+        for (size_t i = 0; i < joint_position_.size(); i++)
+        {
+            if (joint_directions_[i] == 1)
+            {
+                joint_position_desired_[i] = joint_position_[i] + 20;
+            }
+            else
+            {
+                joint_position_desired_[i] = joint_position_[i] - 20;
+            }
+        }
+    }
+
+    return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+}
+
+bool ArmControl::MoveDown(std::vector<double>& joint_positions, double dt)
+{
+    if (!is_moving_)
+    {
+        for (size_t i = 0; i < joint_position_.size(); i++)
+        {
+            if (joint_directions_[i] == 1)
+            {
+                joint_position_desired_[i] = joint_position_[i] - 20;
+            }
+            else
+            {
+                joint_position_desired_[i] = joint_position_[i] + 20;
+            }
+        }
+    }
+
+    return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+}
+
+}  // namespace robot_control

src/robot_control/src/control_base.cpp

@@ -0,0 +1,182 @@
+#include "control_base.hpp"
+#include <stdexcept>
+#include <vector>
+
+namespace robot_control
+{
+
+ControlBase::ControlBase(
+    size_t total_joints, 
+    const std::vector<double>& lengthParameters, 
+    const std::vector<double>& maxSpeed, 
+    const std::vector<double>& maxAcc,  
+    const std::vector<double>& minLimits, 
+    const std::vector<double>& maxLimits, 
+    const std::vector<double>& home_positions,
+    const std::vector<double>& zero_positions,
+    const std::vector<int>& joint_directions
+): 
+    total_joints_(total_joints),
+    lengthParameters_(lengthParameters),
+    maxSpeed_(maxSpeed),
+    maxAcc_(maxAcc),
+    joint_home_positions_(home_positions),
+    joint_zero_positions_(zero_positions),
+    joint_directions_(joint_directions),
+    minLimits_(minLimits),
+    maxLimits_(maxLimits)
+{
+    if (total_joints_ <= 0)
+        throw std::invalid_argument("Total joints must be positive");
+    
+    // 初始化关节状态向量
+    joint_position_.resize(total_joints_, 0.0);
+    joint_velocity_.resize(total_joints_, 0.0);
+    joint_acceleration_.resize(total_joints_, 0.0);
+    joint_torque_.resize(total_joints_, 0.0);
+    joint_position_desired_.resize(total_joints_, 0.0);
+    
+    // 初始化梯形轨迹规划器
+    trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
+
+    is_moving_ = false;
+    is_stopping_ = false;
+    is_target_set_ = false;
+
+    stopDurationTime_ = 0.0;
+    movingDurationTime_ = 0.0;
+}
+
+ControlBase::~ControlBase()
+{
+    delete trapezoidalTrajectory_;
+}
+
+bool ControlBase::MoveToTargetJoint(std::vector<double>& joint_positions, const std::vector<double>& target_joint, double dt)
+{
+    if (joint_position_.size() != target_joint.size())
+    {
+        throw std::invalid_argument("Joint position and target joint size do not match.");
+    }
+
+    if (!is_moving_)
+    {
+        bool all_joints_reached = true;  
+        const double position_tolerance = 1e-3; 
+
+        for (size_t i = 0; i < target_joint.size(); i++)
+        {
+            double position_error = std::fabs(joint_position_[i] - target_joint[i]);
+            if (position_error > position_tolerance)
+            {
+                all_joints_reached = false;  
+                break;
+            }
+        }
+
+        if (all_joints_reached)
+        {
+            joint_positions = target_joint;
+
+            return true; 
+        }
+        
+        // 从当前关节位置开始规划轨迹
+        trapezoidalTrajectory_->init(joint_position_, target_joint);
+        movingDurationTime_ = 0.0;       
+        is_moving_ = true;     
+    }
+
+    movingDurationTime_ += dt;
+
+    std::vector<double> desired_pos = trapezoidalTrajectory_->update(movingDurationTime_);
+
+    // 检查是否到达目标位置
+    if (trapezoidalTrajectory_->isFinished(movingDurationTime_))
+    {
+        joint_positions = target_joint;
+        is_moving_ = false;
+        movingDurationTime_ = 0.0;
+
+        return true;
+    }
+
+    joint_positions = desired_pos;
+    joint_velocity_ = trapezoidalTrajectory_->getVelocity();
+    joint_acceleration_ = trapezoidalTrajectory_->getAcceleration();
+
+    return false;
+}
+
+bool ControlBase::GoHome(std::vector<double>& joint_positions, double dt)
+{
+    if (!is_moving_)
+    {
+        joint_position_desired_ = joint_home_positions_;
+    }
+
+    return MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+}
+
+
+bool ControlBase::Stop(std::vector<double>& joint_positions, double dt)
+{    
+    if (is_moving_ && !is_stopping_)
+    {
+        is_stopping_ = true;
+        trapezoidalTrajectory_->calculateStopTrajectory();
+        stopDurationTime_ = 0.0;
+    }
+
+    if (is_stopping_)
+    {
+        stopDurationTime_ += dt;
+        joint_positions = trapezoidalTrajectory_->updateStop(stopDurationTime_);
+        if (trapezoidalTrajectory_->isStopFinished(stopDurationTime_))
+        {
+            is_moving_ = false;
+            is_stopping_ = false;
+            stopDurationTime_ = 0.0;
+            return true;
+        }
+        return false;
+    }
+
+    return true;
+}
+
+void ControlBase::UpdateJointStates(
+    const std::vector<double>& joint_positions,
+    const std::vector<double>& joint_velocities,
+    const std::vector<double>& joint_torques)
+{
+    // 2. 更新关节位置（同时同步关节角度，假设位置与角度单位一致）
+    joint_position_ = joint_positions;
+
+    // 3. 更新关节速度
+    joint_velocity_ = joint_velocities;
+
+    // 4. 更新关节力矩
+    joint_torque_ = joint_torques;
+}
+
+bool ControlBase::IsMoving()
+{
+    return is_moving_;
+}
+
+bool ControlBase::checkJointLimits(const std::vector<double>& target_joint)
+{
+    for (size_t i = 0; i < target_joint.size(); i++)
+    {
+        if (target_joint[i] < minLimits_[i] || target_joint[i] > maxLimits_[i])
+        {
+            printf("Joint %zu target position %f is out of limits [%f, %f]", i, target_joint[i], minLimits_[i], maxLimits_[i]);
+            return false;
+        }
+    }
+
+    return true;
+}
+
+}  // namespace robot_control

src/robot_control/src/leg_control.cpp

@@ -0,0 +1,142 @@
+#include "leg_control.hpp"
+#include <stdexcept>
+#include <vector>
+
+namespace robot_control
+{
+
+LegControl::LegControl(
+    size_t total_joints, 
+    const std::vector<double>& lengthParameters, 
+    const std::vector<double>& maxSpeed,
+    const std::vector<double>& maxAcc,
+    const std::vector<double>& minLimits, 
+    const std::vector<double>& maxLimits, 
+    const std::vector<double>& home_positions,
+    const std::vector<double>& zero_positions,
+    const std::vector<int>& joint_directions
+) : ControlBase(
+    total_joints, 
+    lengthParameters, 
+    maxSpeed, 
+    maxAcc, 
+    minLimits, 
+    maxLimits, 
+    home_positions, 
+    zero_positions,
+    joint_directions)
+{
+    std::cout << "LegControl Constructor" << std::endl;
+}
+
+LegControl::~LegControl() 
+{
+    delete trapezoidalTrajectory_;
+}
+
+bool LegControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
+{
+    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
+    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
+    std::vector<double> joint_target(total_joints_, 0.0);
+    
+    // 实际应用中应在此处添加逆运动学求解
+    // joint_target = inverse_kinematics(target_pos);
+    
+    for (size_t i = 0; i < target_pos.size(); i++)
+    {
+        std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
+    }
+
+    // 使用梯形轨迹规划移动到目标关节位置
+    return MoveToTargetJoint(joint_positions, joint_target, dt);
+}
+
+bool LegControl::MoveUp(std::vector<double>& joint_positions, double dt)
+{    
+    if (!is_moving_)
+    {
+        if (!is_target_set_)
+        {
+            {
+                std::cout << "Target position not set!" << std::endl;
+                return true;
+            }
+        }
+    }
+
+    bool result = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+
+    if (result)
+    {
+        is_target_set_ = false;
+    }
+
+    return result;
+}
+
+bool LegControl::SetMoveLegParametersInternal(double moveDistance)
+{
+    std::vector<double> tempPosition;
+    tempPosition.resize(total_joints_, 0.0);
+
+    double currentFrountLegLength = lengthParameters_[0] * abs(std::cos(DEG2RAD(joint_position_[0] - joint_zero_positions_[0])));
+
+    double currentBackLegLength = lengthParameters_[1] * abs(std::cos(DEG2RAD(joint_position_[1] - joint_zero_positions_[1])));
+
+    if (moveDistance > 0)
+    {
+        if ((moveDistance + currentFrountLegLength) > (lengthParameters_[0] - 0.02) || (moveDistance + currentBackLegLength) > (lengthParameters_[1] - 0.03))
+        {
+            std::cout << "Move distance is too large! " << std::endl;
+            std::cout << " Current Front Leg Length: " << currentFrountLegLength << std::endl;
+            std::cout << " Current Back Leg Length: " << currentBackLegLength << std::endl;
+            std::cout << " Move Distance: " << moveDistance << std::endl;
+            return false;
+        }
+    }
+    else
+    {
+        if (moveDistance < (-currentFrountLegLength + 0.03) || moveDistance < (-currentBackLegLength + 0.03))
+        {
+            std::cout << "Move distance is too large!" << std::endl;
+            std::cout << " Current Front Leg Length: " << currentFrountLegLength << std::endl;
+            std::cout << " Current Back Leg Length: " << currentBackLegLength << std::endl;
+            std::cout << " Move Distance: " << moveDistance << std::endl;
+            return false;
+        }
+    }
+
+    double finalFrountLegLength = currentFrountLegLength + moveDistance;
+    double finalBackLegLength = currentBackLegLength + moveDistance;
+    
+    double frontFinalAngle = RAD2DEG(std::acos(finalFrountLegLength / lengthParameters_[0]));
+    double backFinalAngle = RAD2DEG(std::acos(finalBackLegLength / lengthParameters_[1]));
+
+    tempPosition[0] = joint_directions_[0] * frontFinalAngle;
+    tempPosition[1] = joint_directions_[1] * backFinalAngle;
+    tempPosition[2] = joint_directions_[2] * frontFinalAngle;
+    tempPosition[3] = joint_directions_[3] * backFinalAngle;
+
+    if (!checkJointLimits(tempPosition))
+    {
+        std::cout << "Joint limits exceeded!" << std::endl;
+        return false;
+    }
+
+    for (size_t i = 0; i < joint_position_desired_.size(); i++)
+    {
+        joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
+    }
+
+    std::cout << "Target Joint Position: " 
+    << joint_position_desired_[0] 
+    << ", " << joint_position_desired_[1] << ", " 
+    <<  joint_position_desired_[2] << ", " << joint_position_desired_[3] << std::endl;
+
+    is_target_set_ = true;
+
+    return true;
+}
+
+}  // namespace robot_control

src/robot_control/src/main.cpp

@@ -0,0 +1,26 @@
+#include <rclcpp/rclcpp.hpp>
+#include <iostream>
+
+#include "robot_control_node.hpp"
+
+/**
+ * @brief 程序入口函数
+ * 功能：初始化ROS 2，创建状态机节点，启动自旋
+ */
+
+ using namespace robot_control;
+
+int main(int argc, char *argv[]) {
+    // 初始化ROS 2
+    rclcpp::init(argc, argv);
+
+    // 创建状态机节点（智能指针管理）
+    auto robot_control_node = std::make_shared<RobotControlNode>();
+
+    // 启动节点自旋（阻塞，处理回调和定时器）
+    rclcpp::spin(robot_control_node);
+
+    // 关闭ROS 2
+    rclcpp::shutdown();
+    return 0;
+}

src/robot_control/src/plot_joint_trajectpry_multi.py

@@ -0,0 +1,65 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+
+def plot_joint_trajectories(file_path):
+    # 检查文件是否存在
+    if not os.path.exists(file_path):
+        print(f"错误：文件不存在 - {file_path}")
+        return
+
+    # 读取数据
+    timestamps = []
+    joint_data = []  # 存储所有关节数据，格式：[ [关节1数据], [关节2数据], ... ]
+
+    with open(file_path, 'r') as f:
+        for line in f:
+            line = line.strip()
+            if not line:
+                continue
+            
+            # 解析一行数据（时间戳 + 所有关节值）
+            parts = list(map(float, line.split(',')))
+            timestamp = parts[0]
+            joints = parts[1:]  # 第0个是时间戳，后面是关节数据
+            
+            timestamps.append(timestamp)
+            
+            # 初始化关节数据列表（首次读取时）
+            if not joint_data:
+                joint_data = [[] for _ in range(len(joints))]
+            
+            # 按关节索引存储数据
+            for i, val in enumerate(joints):
+                if i < len(joint_data):  # 避免索引越界
+                    joint_data[i].append(val)
+
+    # 转换为相对时间（以第一个时间戳为起点）
+    if not timestamps:
+        print("警告：未读取到有效数据")
+        return
+    start_time = timestamps[0]
+    relative_times = [t - start_time for t in timestamps]
+
+    # 绘制所有关节轨迹（最多显示12个关节，避免图过于拥挤）
+    num_joints = len(joint_data)
+    num_plots = min(num_joints, 12)  # 超过12个关节只显示前12个
+
+    plt.figure(figsize=(12, 8))
+    for i in range(num_plots):
+        plt.plot(joint_data[i], label=f'关节 {i+1}')
+
+    # 图形配置
+    plt.xlabel('时间 (秒)')
+    plt.ylabel('关节位置')
+    plt.title('所有关节轨迹曲线')
+    plt.grid(True, linestyle='--', alpha=0.7)
+    plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left')  # 图例放右侧
+    plt.tight_layout()  # 自动调整布局
+    plt.show()
+
+if __name__ == "__main__":
+    # 数据文件路径（与C++代码中的路径一致）
+    data_file = "/home/demo/ros2_joint_data.txt"
+    plot_joint_trajectories(data_file)
+    

src/robot_control/src/robot_control_manager.cpp

@@ -0,0 +1,414 @@
+#include "robot_control_manager.hpp"
+
+using namespace robot_control;
+using namespace std;
+using namespace std_msgs::msg;
+
+
+RobotControlManager::RobotControlManager()
+{
+    this->init();
+}
+
+void RobotControlManager::init()
+{
+    // jointInited_.resize(motionParams_.total_joints_, false);
+    jointInited_.resize(3, false);
+
+    // Initialize the joint commands
+    for (size_t i = 0; i < motionParams_.total_joints_; i++)
+    {
+        jointCommands_.data.push_back(0.0);
+        wheelCommands_.data.push_back(0.0);
+    }
+
+    legController_ = new LegControl(
+        motionParams_.leg_joint_indices_.size(), 
+        motionParams_.leg_length_,
+        motionParams_.leg_max_velocity_,
+        motionParams_.leg_max_acceleration_,
+        motionParams_.leg_min_limit_,
+        motionParams_.leg_max_limit_,
+        motionParams_.leg_home_positions_,
+        motionParams_.leg_zero_positions_,
+        motionParams_.leg_joint_directions_
+    );
+
+    waistController_ = new WaistControl(
+        motionParams_.waist_joint_indices_.size(), 
+        motionParams_.waist_length_,
+        motionParams_.waist_max_velocity_,
+        motionParams_.waist_max_acceleration_,
+        motionParams_.waist_min_limit_,
+        motionParams_.waist_max_limit_,
+        motionParams_.waist_home_positions_,
+        motionParams_.waist_zero_positions_,
+        motionParams_.waist_joint_directions_
+    );
+
+    wheelController_ = new WheelControl(
+        motionParams_.wheel_joint_indices_.size(), 
+        motionParams_.wheel_length_, 
+        motionParams_.wheel_max_velocity_,
+        motionParams_.wheel_max_acceleration_,
+        motionParams_.wheel_min_limit_,
+        motionParams_.wheel_max_limit_,
+        motionParams_.wheel_home_positions_,
+        motionParams_.wheel_zero_positions_,
+        motionParams_.wheel_joint_directions_
+    );
+
+    isWaistHomed_ = false;
+    isLegHomed_ = false;
+
+    isJointInitValueSet_ = false;
+}
+
+RobotControlManager::~RobotControlManager()
+{
+    delete waistController_;
+    delete legController_;
+    delete wheelController_;
+}
+
+Float64MultiArray RobotControlManager::GetWheelCommands()
+{
+    return wheelCommands_;
+}
+
+Float64MultiArray RobotControlManager::GetJointFeedback()
+{
+    Float64MultiArray tempValues;
+    tempValues.data.resize(motionParams_.total_joints_);
+
+    for (size_t i = 0; i < jointPositions_.size(); i++)
+    {
+        tempValues.data[i] = jointPositions_[i];
+    }
+
+    return tempValues;
+}
+
+Float64MultiArray RobotControlManager::GetWheelFeedback()
+{
+    Float64MultiArray tempValues;
+    tempValues.data.resize(motionParams_.wheel_joint_indices_.size());
+
+    for (size_t i = 0; i < wheelPositions_.size(); i++)
+    {
+        double angle = wheelPositions_[i];
+
+        tempValues.data[i] = angle;
+    }
+
+    return tempValues;
+}
+
+Float64MultiArray RobotControlManager::GetJointCommands()
+{
+    Float64MultiArray tempValues;
+    tempValues.data.resize(motionParams_.total_joints_);
+
+    // Convert the joint commands to motor values and limit to encoder range
+    for (size_t i = 0; i < jointCommands_.data.size(); i++)
+    {
+        double angle = jointCommands_.data[i];
+
+        double normalizedAngle = angle ; // TODO :  check angle fmod(angle, 360.0);
+
+        // 计算原始脉冲值
+        double pulseValue = normalizedAngle * motionParams_.degree_to_pulse_[i];
+        
+        tempValues.data[i] = pulseValue;
+    }
+
+    return tempValues;
+}
+
+
+void RobotControlManager::CheckJointLimits(Float64MultiArray &tempJointValues)
+{
+    // Check the joint limits
+    for (size_t i = 0; i < tempJointValues.data.size(); i++)
+    {
+        if (tempJointValues.data[i] > motionParams_.joint_limits_[i].max)
+        {
+            tempJointValues.data[i] = motionParams_.joint_limits_[i].max;
+        }
+        
+        if (tempJointValues.data[i] < motionParams_.joint_limits_[i].min)
+        {
+            tempJointValues.data[i] = motionParams_.joint_limits_[i].min;
+        }
+    }
+}
+
+bool isAllTrueManual(const std::vector<bool>& vec) {
+    for (bool element : vec) {  // 范围 for 循环遍历每个元素
+        if (!element) {  // 若存在 false，直接返回 false
+            return false;
+        }
+    }
+    return true;  // 所有元素都是 true
+}
+
+void RobotControlManager::UpdateWheelStates(const MotorPos::SharedPtr msg) 
+{
+    motorPos_ = *msg;
+    wheelPositions_.resize(motionParams_.wheel_joint_indices_.size());
+    wheelVelocities_.resize(motionParams_.wheel_joint_indices_.size());
+    wheelEfforts_.resize(motionParams_.wheel_joint_indices_.size());
+
+    if (motorPos_.motor_angle.size() != motionParams_.wheel_joint_indices_.size())
+    {
+        std::cout << "wheel states size is not equal to wheel numbles" << std::endl;
+        return;
+    }
+
+    for (size_t i = 0; i < motorPos_.motor_angle.size(); i++)
+    {
+        wheelPositions_[i] = motorPos_.motor_angle[i];
+        wheelVelocities_[i] = 0.0;
+        wheelEfforts_[i] = 0.0;
+    }
+
+    wheelController_->UpdateJointStates(wheelPositions_, wheelVelocities_, wheelEfforts_);
+}
+
+void RobotControlManager::UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg) 
+{
+    jointStates_ = *msg;
+    jointPositions_.resize(motionParams_.total_joints_);
+    jointVelocities_.resize(motionParams_.total_joints_);
+    jointEfforts_.resize(motionParams_.total_joints_);
+
+    if (jointStates_.position.size() != motionParams_.total_joints_)
+    {
+        std::cout << "Joint states size is not equal to total joints" << std::endl;
+        return;
+    }
+    
+    for (size_t i = 0; i < jointStates_.position.size(); i++)
+    {
+        jointPositions_[i] = jointStates_.position[i] * motionParams_.pulse_to_degree_[i];
+        jointVelocities_[i] = jointStates_.velocity[i] * motionParams_.pulse_to_degree_[i];
+        jointEfforts_[i] = jointStates_.effort[i];
+    }
+
+    // TODO: This block can be deleted
+    for (size_t i = 0; i < motionParams_.total_joints_; i++)
+    {
+       if(!jointInited_[i])
+       {
+            if(jointPositions_[i] != 0)
+            {
+                jointInited_[i] = true;
+                std::cout << "get joint feedback, joint" << i + 1 << "  " << jointPositions_[i] << std::endl;
+            }
+       }
+    }
+
+    static int count = 0;
+    if (isAllTrueManual(jointInited_) && !isJointInitValueSet_)
+    {
+        if (count > 50)
+        {
+            jointCommands_.data = jointPositions_;
+            isJointInitValueSet_ = true;
+            count = 0;
+            std::cout << "Joint commands set to joint positions" << std::endl;
+            std::cout << "All joints are initialized" << std::endl;
+            for (size_t i = 0; i < motionParams_.total_joints_; i++)
+            {
+                std::cout << "Joint positions: " << i + 1 << "  " << jointCommands_.data[i] << std::endl;
+            }
+        }
+        count++;
+    }
+
+    // Update the waist controller
+    size_t waistStartIndex = motionParams_.waist_joint_indices_[0];
+    size_t waistJointSize = motionParams_.waist_joint_indices_.size();
+
+    std::vector<double> waistPositions(
+        jointPositions_.begin() + waistStartIndex, 
+        jointPositions_.begin() + waistStartIndex + waistJointSize
+    );
+
+    std::vector<double> waistVelocities(
+        jointVelocities_.begin() + waistStartIndex, 
+        jointVelocities_.begin() + waistStartIndex + waistJointSize
+    );
+
+    std::vector<double> waistEfforts(
+        jointEfforts_.begin() + waistStartIndex, 
+        jointEfforts_.begin() + waistStartIndex + waistJointSize
+    );
+    waistController_->UpdateJointStates(waistPositions, waistVelocities, waistEfforts);
+
+    // Update the leg controller
+    size_t legStartIndex = motionParams_.leg_joint_indices_[0];
+    size_t legJointSize = motionParams_.leg_joint_indices_.size();
+
+    std::vector<double> legPositions(
+        jointPositions_.begin() + legStartIndex, 
+        jointPositions_.begin() + legStartIndex + legJointSize
+    );
+
+    std::vector<double> legVelocities(
+        jointVelocities_.begin() + legStartIndex, 
+        jointVelocities_.begin() + legStartIndex + legJointSize
+    );
+
+    std::vector<double> legEfforts(
+        jointEfforts_.begin() + legStartIndex, 
+        jointEfforts_.begin() + legStartIndex + legJointSize
+    );
+    legController_->UpdateJointStates(legPositions, legVelocities, legEfforts);
+}
+
+MotionParameters RobotControlManager::GetMotionParameters()
+{
+    return motionParams_;
+}
+
+bool RobotControlManager::IsMoving(MovementPart part)
+{
+    switch (part)
+    {
+        case MovementPart::WHEEL:
+            return wheelController_->IsMoving();
+        case MovementPart::LEG:
+            return legController_->IsMoving();
+        case MovementPart::WAIST:
+            return waistController_->IsMoving();
+        case MovementPart::ALL:
+            return wheelController_->IsMoving() && legController_->IsMoving() && waistController_->IsMoving();
+        default:
+            return false;
+    }
+}
+
+bool RobotControlManager::RobotInitFinished()
+{
+    return isJointInitValueSet_;
+}
+
+bool RobotControlManager::Stop(double dt)
+{
+    AssignTempValues();
+
+    bool waistStopped = waistController_->Stop(tempWaistCmd_, dt);
+    bool legStopped = legController_->Stop(tempLegCmd_, dt);
+    bool wheelStopped = wheelController_->Stop(tempWheelCmd_, dt);
+
+    UpdateJointCommands();
+
+    return legStopped  && waistStopped && wheelStopped;
+}
+
+bool RobotControlManager::MoveLeg(double dt)
+{
+    AssignTempValues();
+
+    bool result = legController_ -> MoveUp(tempLegCmd_, dt);
+
+    UpdateJointCommands();
+
+    return result;
+}
+
+bool RobotControlManager::MoveWaist(double dt)
+{
+    AssignTempValues();
+
+    bool result = waistController_ -> MoveWaist(tempWaistCmd_, dt);
+
+    UpdateJointCommands();
+
+    return result;
+}
+
+bool RobotControlManager::MoveWheel()
+{
+    tempWheelCmd_.resize(motionParams_.wheel_joint_indices_.size());
+
+    bool result = wheelController_ -> MoveWheel(tempWheelCmd_);
+
+    for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
+    {
+        wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] = tempWheelCmd_[i];
+        // std::cout << "Wheel commands: " << i << " " << wheelCommands_.data[motionParams_.wheel_joint_indices_[i]] <<  std::endl;
+    }
+
+    return result;
+}
+
+
+bool RobotControlManager::GoHome(double dt) 
+{
+    AssignTempValues();
+
+    if (!isWaistHomed_)
+    {
+        isWaistHomed_ = waistController_->GoHome(tempWaistCmd_, dt);
+    }
+    
+    if (!isLegHomed_)
+    {
+        isLegHomed_ = legController_->GoHome(tempLegCmd_, dt);
+    }
+
+    UpdateJointCommands();
+
+    if (isWaistHomed_ && isLegHomed_)
+    {
+        isWaistHomed_ = false;
+        isLegHomed_ = false;
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+void RobotControlManager::JogAxis(size_t axis, int direction)
+{
+    if(axis > motionParams_.total_joints_)
+    {
+        return;
+    }
+
+    jointCommands_.data[axis] += direction * motionParams_.jog_step_size_;
+}
+
+void RobotControlManager::AssignTempValues()
+{
+    tempWaistCmd_.resize(motionParams_.waist_joint_indices_.size());
+
+    for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++)
+    {
+        tempWaistCmd_[i] = jointCommands_.data[motionParams_.waist_joint_indices_[i]];
+    }
+
+    tempLegCmd_.resize(motionParams_.leg_joint_indices_.size());
+
+    for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
+    {
+        tempLegCmd_[i] = jointCommands_.data[motionParams_.leg_joint_indices_[i]];
+    }
+}
+
+void RobotControlManager::UpdateJointCommands()
+{
+    for (size_t i = 0; i < motionParams_.waist_joint_indices_.size(); i++)
+    {
+        jointCommands_.data[motionParams_.waist_joint_indices_[i]] = tempWaistCmd_[i];
+    }
+
+    for (size_t i = 0; i < motionParams_.leg_joint_indices_.size(); i++)
+    {
+        jointCommands_.data[motionParams_.leg_joint_indices_[i]] = tempLegCmd_[i];
+    }
+}

src/robot_control/src/robot_control_node.cpp

@@ -0,0 +1,695 @@
+#include <thread>
+
+#include "rclcpp/rclcpp.hpp"
+#include "robot_control_node.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+using namespace robot_control;
+
+using namespace std;
+using namespace std_msgs::msg;
+
+namespace fs = std::filesystem;
+
+RobotControlNode::RobotControlNode() : Node("robot_control_node")
+{
+    RCLCPP_INFO(this->get_logger(), "robot_control_node Node is creating");
+    isJogMode_ = false;
+    jogDirection_ = 0;
+    jogIndex_ = 0;
+ 
+    // 初始化数据文件（设置路径，确保目录存在）
+    data_file_path_ = "/home/demo/ros2_joint_data.txt";  // 数据保存路径
+    fs::path file_path(data_file_path_);
+    fs::create_directories(file_path.parent_path());  // 自动创建父目录（如果不存在）
+    data_file_.open(data_file_path_, std::ios::trunc);
+    if (!data_file_.is_open()) {
+        
+    } else {
+        // 写入表头（仅在文件为空时）
+        data_file_.seekp(0, std::ios::end);  // 移动到文件末尾
+    }
+
+    using namespace std::placeholders;
+        
+    // 创建Action服务器
+    this->move_home_action_server_ = rclcpp_action::create_server<MoveHome>(
+        this,
+        "MoveHome",
+        std::bind(&RobotControlNode::handle_move_home_goal, this, _1, _2),
+        std::bind(&RobotControlNode::handle_move_home_cancel, this, _1),
+        std::bind(&RobotControlNode::handle_move_home_accepted, this, _1));
+    move_home_executing_ = false;
+        
+    RCLCPP_INFO(this->get_logger(), "MoveHome action server is ready");
+
+    this->move_leg_action_server_ = rclcpp_action::create_server<MoveLeg>(
+        this,
+        "MoveLeg",
+        std::bind(&RobotControlNode::handle_move_leg_goal, this, _1, _2),
+        std::bind(&RobotControlNode::handle_move_leg_cancel, this, _1),
+        std::bind(&RobotControlNode::handle_move_leg_accepted, this, _1));
+    move_leg_executing_ = false;
+
+    RCLCPP_INFO(this->get_logger(), "MoveLeg action server is ready");
+
+    this->move_waist_action_server_ = rclcpp_action::create_server<MoveWaist>(
+        this,
+        "MoveWaist",
+        std::bind(&RobotControlNode::handle_move_waist_goal, this, _1, _2),
+        std::bind(&RobotControlNode::handle_move_waist_cancel, this, _1),
+        std::bind(&RobotControlNode::handle_move_waist_accepted, this, _1));
+    move_waist_executing_ = false;
+
+    RCLCPP_INFO(this->get_logger(), "MoveWaist action server is ready");
+
+    this->move_wheel_action_server_ = rclcpp_action::create_server<MoveWheel>(
+        this,
+        "MoveWheel",
+        std::bind(&RobotControlNode::handle_move_wheel_goal, this, _1, _2),
+        std::bind(&RobotControlNode::handle_move_wheel_cancel, this, _1),
+        std::bind(&RobotControlNode::handle_move_wheel_accepted, this, _1));
+    move_wheel_executing_ = false;
+
+    RCLCPP_INFO(this->get_logger(), "MoveWheel action server is ready");
+
+    motorCmdPub_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
+    ethercatSetPub_ = this->create_publisher<std_msgs::msg::String>("/ethercat/set", 10);
+    jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/ec_joint_states", 10,std::bind(&RobotControlNode::JointStatesCallback, this, std::placeholders::_1));
+    wheelStatesSub_ = this->create_subscription<MotorPos>("/motor_pos", 10,std::bind(&RobotControlNode::WheelStatesCallback, this, std::placeholders::_1));
+    joyCommandSub_ = this->create_subscription<std_msgs::msg::String>("/gamepad_msg", 10,std::bind(&RobotControlNode::JoyCommandCallback, this, std::placeholders::_1));
+    controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(CYCLE_TIME),std::bind(&RobotControlNode::ControlLoop, this));  // 绑定到新的控制函数);
+
+    lastTime_ = this->now();  // 初始化时间
+    std::cout << "RobotFsm Node is created finished!" << std::endl;
+}
+
+RobotControlNode::~RobotControlNode() 
+{
+    if (data_file_.is_open()) {
+        data_file_.close(); 
+    }
+}
+
+rclcpp_action::GoalResponse RobotControlNode::handle_move_home_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const MoveHome::Goal> goal)
+{
+    RCLCPP_INFO(this->get_logger(), "Received move home request");
+
+    (void)goal;
+
+    if (robotControlManager_.IsMoving(MovementPart::ALL))
+    {
+         RCLCPP_ERROR(this->get_logger(), "Robot is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (move_home_executing_)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Another move home goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (isJogMode_)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+    
+    (void)uuid;
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+    
+// 处理取消请求
+rclcpp_action::CancelResponse RobotControlNode::handle_move_home_cancel(
+    const std::shared_ptr<GoalHandleMoveHome> goal_handle)
+{
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+    
+// 接受目标后执行
+void RobotControlNode::handle_move_home_accepted(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
+{
+    move_home_executing_ = true;
+    RCLCPP_INFO(this->get_logger(), "Goal accepted");
+
+    using namespace std::placeholders;
+    std::thread{std::bind(&RobotControlNode::move_home_execute, this, _1), goal_handle}.detach();
+}
+
+// 执行目标计算
+void RobotControlNode::move_home_execute(const std::shared_ptr<GoalHandleMoveHome> goal_handle)
+{
+    RCLCPP_INFO(this->get_logger(), "Executing home goal");
+    rclcpp::Rate loop_rate(10); // 10Hz更新频率
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<MoveHome::Feedback>();
+    auto result = std::make_shared<MoveHome::Result>();
+
+    while (move_home_executing_ && rclcpp::ok())
+    {
+        if (goal_handle->is_canceling())
+        {
+            result->success = false;
+            goal_handle->canceled(result);
+            RCLCPP_INFO(this->get_logger(), "Move home canceled");
+            move_home_executing_ = false;
+            //TODO: ADD STOP LOGIC
+            return;
+        }
+
+        auto joint_commands = robotControlManager_.GetJointFeedback(); 
+        feedback->joint_values.clear();
+
+        for (double val : joint_commands.data) {
+            feedback->joint_values.push_back(static_cast<int64_t>(val));
+        }
+        
+        goal_handle->publish_feedback(feedback);
+        
+        loop_rate.sleep();
+    }
+    
+    // 检查目标是否仍在活跃状态
+    if (rclcpp::ok())
+    {
+        result->success = true;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(this->get_logger(), "move home succeeded.");
+    }
+}
+
+rclcpp_action::GoalResponse RobotControlNode::handle_move_leg_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const MoveLeg::Goal> goal)
+{
+    RCLCPP_INFO(this->get_logger(), "Received move leg request");
+
+    if (robotControlManager_.IsMoving(MovementPart::LEG))
+    {
+         RCLCPP_ERROR(this->get_logger(), "Robot is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (move_leg_executing_)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Another move leg goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (isJogMode_)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (!robotControlManager_.SetMoveLegParameters(goal->move_up_distance))
+    {
+        RCLCPP_ERROR(this->get_logger(), "Failed to set move leg parameters");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+    
+
+    (void)uuid;
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+// 处理取消请求
+rclcpp_action::CancelResponse RobotControlNode::handle_move_leg_cancel(
+    const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+{
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+// 接受目标后执行
+void RobotControlNode::handle_move_leg_accepted(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+{
+    move_leg_executing_ = true;
+    RCLCPP_INFO(this->get_logger(), "Goal accepted");
+
+    using namespace std::placeholders;
+    std::thread{std::bind(&RobotControlNode::move_leg_execute, this, _1), goal_handle}.detach();
+}
+
+// 执行目标计算
+void RobotControlNode::move_leg_execute(const std::shared_ptr<GoalHandleMoveLeg> goal_handle)
+{
+    RCLCPP_INFO(this->get_logger(), "Executing leg goal");
+    rclcpp::Rate loop_rate(10); // 10Hz更新频率
+    const auto goal = goal_handle->get_goal();
+    auto feedback = std::make_shared<MoveLeg::Feedback>();
+    auto result = std::make_shared<MoveLeg::Result>();
+
+    while (move_leg_executing_ && rclcpp::ok())
+    {
+        if (goal_handle->is_canceling())
+        {
+            result->success = false;
+            goal_handle->canceled(result);
+            RCLCPP_INFO(this->get_logger(), "move leg canceled");
+            move_leg_executing_ = false;
+            //TODO: ADD STOP LOGIC
+            return;
+        }
+
+        auto joint_commands = robotControlManager_.GetJointFeedback(); 
+        feedback->joint_values.clear();
+
+        for (double val : joint_commands.data) {
+            feedback->joint_values.push_back(static_cast<int64_t>(val));
+        }
+        
+        goal_handle->publish_feedback(feedback);
+        
+        loop_rate.sleep();
+    }
+    
+    // 检查目标是否仍在活跃状态
+    if (rclcpp::ok())
+    {
+        result->success = true;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(this->get_logger(), "move leg succeeded.");
+    }
+}
+
+rclcpp_action::GoalResponse RobotControlNode::handle_move_waist_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const MoveWaist::Goal> goal)
+{
+    RCLCPP_INFO(this->get_logger(), "Received move waist request");
+
+    if (robotControlManager_.IsMoving(MovementPart::WAIST))
+    {
+         RCLCPP_ERROR(this->get_logger(), "Robot is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (move_waist_executing_)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Another move waist goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (isJogMode_)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Jog mode is enabled");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (!robotControlManager_.SetMoveWaistParameters(goal->move_pitch_degree, goal->move_yaw_degree))
+    {
+        RCLCPP_ERROR(this->get_logger(), "Invalid move waist request");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+    
+
+    (void)uuid;
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+// 处理取消请求
+rclcpp_action::CancelResponse RobotControlNode::handle_move_waist_cancel(
+    const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+{
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+// 接受目标后执行
+void RobotControlNode::handle_move_waist_accepted(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+{
+    move_waist_executing_ = true;
+    RCLCPP_INFO(this->get_logger(), "Goal accepted");
+    using namespace std::placeholders;
+    std::thread{std::bind(&RobotControlNode::move_waist_execute, this, _1), goal_handle}.detach();
+}
+
+// 执行目标计算
+void RobotControlNode::move_waist_execute(const std::shared_ptr<GoalHandleMoveWaist> goal_handle)
+{
+    RCLCPP_INFO(this->get_logger(), "Executing waist goal");
+    rclcpp::Rate loop_rate(10); // 10Hz更新频率
+    const auto goal = goal_handle->get_goal(); // 获取目标
+    auto feedback = std::make_shared<MoveWaist::Feedback>();
+    auto result = std::make_shared<MoveWaist::Result>();
+
+    while (move_waist_executing_ && rclcpp::ok())
+    {
+        if (goal_handle->is_canceling())
+        {
+            result->success = false;
+            goal_handle->canceled(result);
+            RCLCPP_INFO(this->get_logger(), "move waist canceled");
+            move_waist_executing_ = false;
+            //TODO: ADD STOP LOGIC
+            return;
+        }
+
+        auto joint_commands = robotControlManager_.GetJointFeedback();
+        feedback->joint_values.clear();
+
+        for (double val : joint_commands.data) {
+            feedback->joint_values.push_back(static_cast<int64_t>(val));
+        }
+
+        goal_handle->publish_feedback(feedback);
+
+        loop_rate.sleep();
+
+    }
+    // 检查目标是否仍在活跃状态
+    if (rclcpp::ok())
+    {
+        result->success = true;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(this->get_logger(), "move waist succeeded.");
+    }
+}
+
+rclcpp_action::GoalResponse RobotControlNode::handle_move_wheel_goal(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const MoveWheel::Goal> goal)
+{
+    RCLCPP_INFO(this->get_logger(), "Received move wheel request");
+
+    if (robotControlManager_.IsMoving(MovementPart::WHEEL))
+    {
+         RCLCPP_ERROR(this->get_logger(), "Robot is moving");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (move_wheel_executing_)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Another move wheel goal is executing");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    if (!robotControlManager_.SetMoveWheelParameters(goal->move_distance, goal->move_angle))
+    {
+        RCLCPP_ERROR(this->get_logger(), "Invalid move wheel request");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+    
+
+    (void)uuid;
+    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+// 处理取消请求
+rclcpp_action::CancelResponse RobotControlNode::handle_move_wheel_cancel(
+    const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
+{
+    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
+    (void)goal_handle;
+    return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+// 接受目标后执行
+void RobotControlNode::handle_move_wheel_accepted(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
+{
+    move_wheel_executing_ = true;
+    RCLCPP_INFO(this->get_logger(), "Goal accepted");
+    using namespace std::placeholders;
+    std::thread{std::bind(&RobotControlNode::move_wheel_execute, this, _1), goal_handle}.detach();
+}
+
+// 执行目标计算
+void RobotControlNode::move_wheel_execute(const std::shared_ptr<GoalHandleMoveWheel> goal_handle)
+{
+    RCLCPP_INFO(this->get_logger(), "Executing wheel goal");
+    rclcpp::Rate loop_rate(10); // 10Hz更新频率
+    const auto goal = goal_handle->get_goal(); // 获取目标
+    auto feedback = std::make_shared<MoveWheel::Feedback>();
+    auto result = std::make_shared<MoveWheel::Result>();
+
+    robotControlManager_.MoveWheel();
+
+    std_msgs::msg::Float64MultiArray wheel_commands;
+    wheel_commands = robotControlManager_.GetWheelCommands();
+
+    MotorCmd wheel_commands_msg;
+    wheel_commands_msg.target = "rs485";
+    wheel_commands_msg.type = "bm";
+    wheel_commands_msg.position = "";
+    wheel_commands_msg.motor_id = {1,2};
+    wheel_commands_msg.motor_angle = {(float)(wheel_commands.data[0]),(float)(wheel_commands.data[1])};
+
+    motorCmdPub_->publish(wheel_commands_msg);
+
+    while (move_wheel_executing_ && rclcpp::ok())
+    {
+        if (goal_handle->is_canceling())
+        {
+            result->success = false;
+            goal_handle->canceled(result);
+            RCLCPP_INFO(this->get_logger(), "move wheel canceled");
+            move_wheel_executing_ = false;
+            //TODO: ADD STOP LOGIC
+            return;
+        }
+
+        auto joint_feedback = robotControlManager_.GetWheelFeedback();
+
+        if (joint_feedback.data[0] - wheel_commands.data[0] < 10)
+        {
+            move_wheel_executing_ = false;
+        }
+
+        feedback->current_pos = joint_feedback.data[0];
+
+        //TODO: get the angle from lidar.
+        feedback->current_angle = 0.0;
+
+        goal_handle->publish_feedback(feedback);
+
+        loop_rate.sleep();
+    }
+    // 检查目标是否仍在活跃状态
+    if (rclcpp::ok())
+    {
+        result->success = true;
+        goal_handle->succeed(result);
+        RCLCPP_INFO(this->get_logger(), "move wheel succeeded.");
+    }
+}
+
+// 状态机主循环
+void RobotControlNode::ControlLoop() {
+    // 计算时间差
+    rclcpp::Time current_time = this->now();
+    rclcpp::Duration dt = current_time - lastTime_;
+    double dt_sec = dt.seconds();
+    lastTime_ = current_time;
+
+    if (isStopping_)
+    {
+        move_home_executing_ = false;
+        move_leg_executing_ = false;
+        move_waist_executing_ = false;
+        isJogMode_ = false;
+
+        if (robotControlManager_.Stop(dt_sec))
+        {
+            isStopping_ = false;
+        }
+    }
+
+    if (move_home_executing_)
+    {
+        if(robotControlManager_.GoHome(dt_sec))
+        {
+            move_home_executing_ = false;
+        }
+    }
+
+    if (move_leg_executing_)
+    {
+        if(robotControlManager_.MoveLeg(dt_sec))
+        {
+            move_leg_executing_ = false;
+        }
+    }
+
+    if (move_waist_executing_)
+    {
+        if(robotControlManager_.MoveWaist(dt_sec))
+        {
+            move_waist_executing_ = false;
+        }
+    }
+
+    if (robotControlManager_.RobotInitFinished())
+    {
+        Publish_joint_trajectory();
+    }
+}
+
+
+void RobotControlNode::Publish_joint_trajectory() 
+{
+    std_msgs::msg::Float64MultiArray cmd_msg;
+    if (isJogMode_)
+    {
+        robotControlManager_.JogAxis(jogIndex_, jogDirection_);
+        cmd_msg = robotControlManager_.GetJointCommands();
+    }
+    else
+    {
+        cmd_msg = robotControlManager_.GetJointCommands();
+    }
+
+    data_file_ << 0;
+
+    // 2. 保存整个数组数据到txt文件
+    if (data_file_.is_open()) {
+        for (const auto& val : cmd_msg.data) {
+            data_file_ << "," << val;  // 用逗号分隔每个元素
+        }
+        
+        data_file_ << std::endl;  // 换行
+        data_file_.flush();  // 强制刷新
+    }
+
+    std_msgs::msg::String positionMsg;
+    std::stringstream msg_stream; 
+    size_t total_joints = robotControlManager_.GetMotionParameters().total_joints_;
+
+    for (size_t i = 0; i < total_joints; ++i)
+    {
+        double current_pos = cmd_msg.data[i];
+
+        msg_stream << i << " pos " << current_pos;
+
+        if (i != total_joints - 1)
+        {
+            msg_stream << "; ";
+        }
+    }
+    positionMsg.data = msg_stream.str();
+
+    // std::cout << "publishing joint trajectory: " << positionMsg.data << std::endl;
+
+    ethercatSetPub_->publish(positionMsg);
+}
+
+void RobotControlNode::JoyCommandCallback(const std_msgs::msg::String::SharedPtr msg) {
+    if (!msg) {  // 检查消息是否有效
+        RCLCPP_WARN(this->get_logger(), "收到空的joy消息,忽略");
+        return;
+    }
+
+    // 解析消息内容
+    std::string command = msg->data;
+    static std::string last_msg="";
+    if(last_msg!=command)
+    	last_msg=command;
+    else
+    	return;
+
+    if (command == "RB,1") {
+        if (isJogMode_ && jogDirection_ == 0)
+        {
+            if (jogIndex_ < robotControlManager_.GetMotionParameters().total_joints_ - 1)
+            {
+                jogIndex_ ++;
+                std::cout << "switch to jog axis: " << jogIndex_ + 1  << std::endl;
+            }
+            else
+            {
+                std::cout << "Reach the max joint, can't switch to next axis: " << jogIndex_ + 1  << std::endl;
+            }
+        }
+    }
+    else if (command == "LB,1") {
+        if (isJogMode_ && jogDirection_ == 0)
+        {
+            if (jogIndex_ > 0)
+            {
+                jogIndex_ --;
+                std::cout << "switch to jog axis: " << jogIndex_ + 1 << std::endl;
+            }
+            else
+            {
+                std::cout << "Reach the min joint, can't switch to previous axis: " << jogIndex_ - 1 << std::endl;
+            }
+        }
+    }
+    else if (command == "A,1") {
+        if (!robotControlManager_.IsMoving(MovementPart::ALL))
+        {
+            isJogMode_ = 1;
+            std::cout << "jog mode on" << std::endl;
+        }
+        else
+        {
+            std::cout << "robot is moving, can't switch jog mode" << std::endl;
+        }
+    }
+    else if (command == "B,1") {
+        if (!robotControlManager_.IsMoving(MovementPart::ALL))
+        {
+            isJogMode_ = 0;
+            std::cout << "jog mode OFF" << std::endl;
+        }
+        else
+        {
+            std::cout << "robot is moving, can't switch jog mode" << std::endl;
+        }
+    }
+    else if (command == "方向垂直,-1.0") {
+        if (isJogMode_)
+        {
+            jogDirection_ = -1;
+            std::cout << "jog axis in negative direction: " << jogIndex_ + 1 << std::endl;
+        }
+    }
+    else if (command == "方向垂直,1.0") {
+        if (isJogMode_)
+        {
+            jogDirection_ = 1;
+            std::cout << "jog axis in positive direction: " << jogIndex_ + 1 << std::endl;
+        }
+    }
+    else if (command == "方向垂直,0.0") {
+        if (isJogMode_)
+        {
+            jogDirection_ = 0;
+            std::cout << "jog axis stopped: " << jogIndex_ + 1 << std::endl;
+        }
+    }
+    else if (command == "X,1") {
+        if (!isJogMode_)
+        {
+            isStopping_ = true;
+            std::cout << "stop robot" << std::endl;
+        }
+    }
+}
+
+void RobotControlNode::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) 
+{
+    if (!msg) {  // 检查消息是否有效
+        std::cout << "get null joint states!" << std::endl;
+        return;
+    }
+
+    robotControlManager_.UpdateJointStates(msg);
+}
+
+void RobotControlNode::WheelStatesCallback(const MotorPos::SharedPtr cmd_msg)
+{
+    if (!cmd_msg) {  // 检查消息是否有效
+        std::cout << "get null wheel states!" << std::endl;
+        return;
+    }
+    
+    robotControlManager_.UpdateWheelStates(cmd_msg);
+}
+

src/robot_control/src/robot_kinematics.cpp

@@ -0,0 +1,213 @@
+#include "robot_kinematics.hpp"
+#include <cmath>
+#include <stdexcept>
+#include <iostream>
+
+using namespace robot_control;
+
+RobotKinematics::RobotKinematics(std::shared_ptr<RobotModel> robot_model)
+    : robot_model_(robot_model)
+{
+    if (!robot_model_)
+    {
+        throw std::invalid_argument("Robot model pointer cannot be null");
+    }
+}
+
+Eigen::Matrix3d RobotKinematics::rotate_z(double angle)
+{
+    Eigen::Matrix3d mat;
+    mat << std::cos(angle), -std::sin(angle), 0,
+           std::sin(angle),  std::cos(angle), 0,
+           0,                0,               1;
+    return mat;
+}
+
+Eigen::Matrix3d RobotKinematics::rotate_y(double angle)
+{
+    Eigen::Matrix3d mat;
+    mat << std::cos(angle),  0, std::sin(angle),
+           0,                1, 0,
+           -std::sin(angle), 0, std::cos(angle);
+    return mat;
+}
+
+Eigen::Matrix3d RobotKinematics::rotate_x(double angle)
+{
+    Eigen::Matrix3d mat;
+    mat << 1, 0,                 0,
+           0, std::cos(angle), -std::sin(angle),
+           0, std::sin(angle),  std::cos(angle);
+    return mat;
+}
+
+Eigen::Vector3d RobotKinematics::arm_forward_kinematics(const std::vector<double>& joint_angles)
+{
+    const auto& arm = robot_model_->get_arm(0);
+    
+    // 检查关节角度数量是否正确
+    if (joint_angles.size() != arm.joints.size())
+    {
+        throw std::invalid_argument("Invalid number of joint angles for arm");
+    }
+    
+    // 连杆长度
+    double l1 = arm.links[0]->length;  // 上臂长度
+    double l2 = arm.links[1]->length;  // 下臂长度
+    
+    // 关节角度
+    double theta1 = joint_angles[0];  // 基座关节 (绕Z轴)
+    double theta2 = joint_angles[1];  // 肩部关节 (绕Y轴)
+    double theta3 = joint_angles[2];  // 肘部关节 (绕Y轴)
+    double theta4 = joint_angles[3];  // 腕部关节 (绕Y轴) - 简化模型中不影响位置
+    
+    // 计算末端位置
+    Eigen::Vector3d end_pos;
+    
+    // 正运动学公式 (简化的3DOF模型)
+    double x = (l1 * std::cos(theta2) + l2 * std::cos(theta2 + theta3)) * std::cos(theta1);
+    double y = (l1 * std::cos(theta2) + l2 * std::cos(theta2 + theta3)) * std::sin(theta1);
+    double z = l1 * std::sin(theta2) + l2 * std::sin(theta2 + theta3);
+    
+    end_pos << x, y, z;
+    return end_pos;
+}
+
+bool RobotKinematics::arm_inverse_kinematics(const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles)
+{
+    const auto& arm = robot_model_->get_arm(0);
+    joint_angles.resize(4, 0.0);  // 4个关节
+    
+    // 连杆长度
+    double l1 = arm.links[0]->length;
+    double l2 = arm.links[1]->length;
+    
+    // 目标位置坐标
+    double x = target_pos.x();
+    double y = target_pos.y();
+    double z = target_pos.z();
+    
+    // 计算手腕位置到基座的距离
+    double r = std::sqrt(x*x + y*y);
+    double d = std::sqrt(r*r + z*z);
+    
+    // 检查工作空间
+    if (d > l1 + l2 || d < std::fabs(l1 - l2))
+    {
+        std::cerr << "Target position out of arm workspace" << std::endl;
+        return false;
+    }
+    
+    // 计算关节角度 (逆运动学解)
+    // 基座关节角度
+    joint_angles[0] = std::atan2(y, x);
+    
+    // 肩部和肘部关节角度 (使用余弦定理)
+    double cos_theta3 = (d*d - l1*l1 - l2*l2) / (2*l1*l2);
+    cos_theta3 = std::clamp(cos_theta3, -1.0, 1.0);  // 防止数值误差导致的问题
+    joint_angles[2] = std::acos(cos_theta3);
+    
+    double alpha = std::atan2(z, r);
+    double beta = std::asin((l2 * std::sin(joint_angles[2])) / d);
+    joint_angles[1] = alpha + beta;
+    
+    // 腕部关节 (简化为0)
+    joint_angles[3] = 0.0;
+    
+    // 检查关节角度限制
+    for (size_t i = 0; i < joint_angles.size(); ++i)
+    {
+        if (joint_angles[i] < arm.joints[i]->min_angle || 
+            joint_angles[i] > arm.joints[i]->max_angle)
+        {
+            std::cerr << "Joint " << i << " angle out of range" << std::endl;
+            return false;
+        }
+    }
+    
+    return true;
+}
+
+Eigen::Vector3d RobotKinematics::leg_forward_kinematics(size_t leg_index, const std::vector<double>& joint_angles)
+{
+    const auto& leg = robot_model_->get_leg(leg_index);
+    
+    // 检查关节角度数量是否正确
+    if (joint_angles.size() != leg.joints.size())
+    {
+        throw std::invalid_argument("Invalid number of joint angles for leg");
+    }
+    
+    // 连杆长度
+    double l1 = leg.links[0]->length;  // 大腿长度
+    double l2 = leg.links[1]->length;  // 小腿长度
+    
+    // 关节角度
+    double theta1 = joint_angles[0];  // 髋关节
+    double theta2 = joint_angles[1];  // 膝关节
+    
+    // 根据腿部位置调整坐标系
+    double sign = (leg.side.find("right") != std::string::npos) ? -1.0 : 1.0;
+    
+    // 计算足部位置
+    Eigen::Vector3d foot_pos;
+    
+    // 正运动学计算
+    foot_pos.x() = sign * (l1 * std::cos(theta1) + l2 * std::cos(theta1 + theta2));
+    foot_pos.y() = 0.0;  // 简化模型
+    foot_pos.z() = -(l1 * std::sin(theta1) + l2 * std::sin(theta1 + theta2));  // Z轴向下为正
+    
+    return foot_pos;
+}
+
+bool RobotKinematics::leg_inverse_kinematics(size_t leg_index, const Eigen::Vector3d& target_pos, std::vector<double>& joint_angles)
+{
+    const auto& leg = robot_model_->get_leg(leg_index);
+    joint_angles.resize(2, 0.0);  // 每条腿2个关节
+    
+    // 连杆长度
+    double l1 = leg.links[0]->length;  // 大腿长度
+    double l2 = leg.links[1]->length;  // 小腿长度
+    
+    // 根据腿部位置调整坐标系
+    double sign = (leg.side.find("right") != std::string::npos) ? -1.0 : 1.0;
+    
+    // 目标位置坐标 (调整符号)
+    double x = sign * target_pos.x();
+    double z = -target_pos.z();  // 转换为腿部坐标系
+    
+    // 计算髋关节到目标点的距离
+    double d = std::sqrt(x*x + z*z);
+    
+    // 检查工作空间
+    if (d > l1 + l2 || d < std::fabs(l1 - l2))
+    {
+        std::cerr << "Target position out of leg workspace" << std::endl;
+        return false;
+    }
+    
+    // 计算关节角度 (逆运动学解)
+    double cos_theta2 = (d*d - l1*l1 - l2*l2) / (2*l1*l2);
+    cos_theta2 = std::clamp(cos_theta2, -1.0, 1.0);  // 防止数值误差导致的问题
+    joint_angles[1] = std::acos(cos_theta2);
+    
+    double alpha = std::atan2(z, x);
+    double beta = std::asin((l2 * std::sin(joint_angles[1])) / d);
+    joint_angles[0] = alpha - beta;
+    
+    // 检查关节角度限制
+    for (size_t i = 0; i < joint_angles.size(); ++i)
+    {
+        if (joint_angles[i] < leg.joints[i]->min_angle || 
+            joint_angles[i] > leg.joints[i]->max_angle)
+        {
+            std::cerr << "Leg joint " << i << " angle out of range: " 
+                      << joint_angles[i] << " (min: " << leg.joints[i]->min_angle 
+                      << ", max: " << leg.joints[i]->max_angle << ")" << std::endl;
+            return false;
+        }
+    }
+    
+    return true;
+}
+

src/robot_control/src/robot_model.cpp

@@ -0,0 +1,184 @@
+#include "robot_model.hpp"
+#include <iostream>
+#include <cmath>
+
+using namespace robot_control;
+
+RobotModel::RobotModel()
+{
+    initialize_model();
+}
+
+void RobotModel::initialize_model()
+{
+    // 初始化机械臂（4个关节，2个连杆）
+    // 添加关节
+    arms_[0].joints.push_back(std::make_shared<Joint>("arm_base_joint", -M_PI/2, M_PI/2, 10.0));
+    arms_[0].joints.push_back(std::make_shared<Joint>("arm_shoulder_joint", -M_PI/2, M_PI/2, 8.0));
+    arms_[0].joints.push_back(std::make_shared<Joint>("arm_elbow_joint", 0, M_PI/2, 6.0));
+    arms_[0].joints.push_back(std::make_shared<Joint>("arm_wrist_joint", -M_PI/4, M_PI/4, 4.0));
+    
+    // 添加连杆
+    arms_[0].links.push_back(std::make_shared<Link>("arm_upper_link", 0.3, 1.5));
+    arms_[0].links.push_back(std::make_shared<Link>("arm_lower_link", 0.25, 1.0));
+
+    // 初始化腿部（4条腿，每条腿2个连杆）
+    std::vector<std::string> leg_sides = {"front_left", "front_right", "rear_left", "rear_right"};
+    
+    for (const auto & side : leg_sides)
+    {
+        Leg leg;
+        leg.side = side;
+        
+        // 为每条腿添加关节
+        leg.joints.push_back(std::make_shared<Joint>(side + "_hip_joint", -M_PI/4, M_PI/4, 15.0));
+        leg.joints.push_back(std::make_shared<Joint>(side + "_knee_joint", 0, M_PI/2, 12.0));
+        
+        // 为每条腿添加连杆
+        leg.links.push_back(std::make_shared<Link>(side + "_thigh_link", 0.2, 2.0));
+        leg.links.push_back(std::make_shared<Link>(side + "_shin_link", 0.2, 1.5));
+        
+        legs_.push_back(leg);
+    }
+}
+
+const Arm & RobotModel::get_arm(size_t index) const
+{
+    if (index >= arms_.size())
+    {
+        throw std::out_of_range("Arm index out of range");
+    }
+    return arms_[index];
+}
+
+const Leg & RobotModel::get_leg(size_t index) const
+{
+    if (index >= legs_.size())
+    {
+        throw std::out_of_range("Leg index out of range");
+    }
+    return legs_[index];
+}
+
+bool RobotModel::set_joint_angle(const std::string & joint_name, double angle)
+{
+    // 检查机械臂关节
+    for (auto & arm : arms_)
+    {
+        for (const auto & joint : arm.joints)
+        {
+            if (joint->name == joint_name)
+            {
+                // 检查角度限制
+                if (angle >= joint->min_angle && angle <= joint->max_angle)
+                {
+                    joint->angle = angle;
+                    return true;
+                }
+                return false;
+            }
+        }
+    }
+    
+    // 检查腿部关节
+    for (auto & leg : legs_)
+    {
+        for (const auto & joint : leg.joints)
+        {
+            if (joint->name == joint_name)
+            {
+                // 检查角度限制
+                if (angle >= joint->min_angle && angle <= joint->max_angle)
+                {
+                    joint->angle = angle;
+                    return true;
+                }
+                return false;
+            }
+        }
+    }
+    
+    // 未找到关节
+    return false;
+}
+
+double RobotModel::get_joint_angle(const std::string & joint_name) const
+{
+    // 检查机械臂关节
+    for (auto & arm : arms_)
+    {
+        for (const auto & joint : arm.joints)
+        {
+            if (joint->name == joint_name)
+            {
+                return joint->angle;
+            }
+        }
+    }
+    
+    // 检查腿部关节
+    for (const auto & leg : legs_)
+    {
+        for (const auto & joint : leg.joints)
+        {
+            if (joint->name == joint_name)
+            {
+                return joint->angle;
+            }
+        }
+    }
+    
+    // 未找到关节
+    return 0.0;
+}
+
+void RobotModel::print_model_info() const
+{
+    std::cout << "=== Robot Model Information ===" << std::endl;
+    
+    // 打印机械臂信息
+    std::cout << "\nArm:" << std::endl;
+    std::cout << "  Joints:" << std::endl;
+    for (size_t i = 0; i < arms_.size(); ++i)
+    {
+        const auto & arm = arms_[i];
+        std::cout << "  Leg " << i << " (" << arm.side << "):" << std::endl;
+        std::cout << "    Joints:" << std::endl;
+        for (const auto & joint : arm.joints)
+        {
+            std::cout << "    " << joint->name << ": "
+                    << "Angle=" << joint->angle << " rad, "
+                    << "Range=[" << joint->min_angle << ", " << joint->max_angle << "] rad" << std::endl;
+        }
+        std::cout << "  Links:" << std::endl;
+        for (const auto & link : arm.links)
+        {
+            std::cout << "    " << link->name << ": "
+                    << "Length=" << link->length << " m, "
+                    << "Mass=" << link->mass << " kg" << std::endl;
+        }
+    }
+    
+    // 打印腿部信息
+    std::cout << "\nLegs:" << std::endl;
+    for (size_t i = 0; i < legs_.size(); ++i)
+    {
+        const auto & leg = legs_[i];
+        std::cout << "  Leg " << i << " (" << leg.side << "):" << std::endl;
+        std::cout << "    Joints:" << std::endl;
+        for (const auto & joint : leg.joints)
+        {
+            std::cout << "      " << joint->name << ": "
+                    << "Angle=" << joint->angle << " rad, "
+                    << "Range=[" << joint->min_angle << ", " << joint->max_angle << "] rad" << std::endl;
+        }
+        std::cout << "    Links:" << std::endl;
+        for (const auto & link : leg.links)
+        {
+            std::cout << "      " << link->name << ": "
+                    << "Length=" << link->length << " m, "
+                    << "Mass=" << link->mass << " kg" << std::endl;
+        }
+    }
+}
+

src/robot_control/src/rs485_driver.cpp

@@ -0,0 +1,419 @@
+#include "rs485_driver.hpp"
+#include <fcntl.h>
+#include <unistd.h>
+#include <cstring>
+#include <iostream>
+#include <chrono>
+#include <thread>
+
+namespace robot_control
+{
+
+    const unsigned char CRC8Table[]={  
+        0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,  
+        157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,  
+        35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,  
+        190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,  
+        70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,  
+        219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,  
+        101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,  
+        248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,  
+        140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,  
+        17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,  
+        175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,  
+        50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,  
+        202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,  
+        87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,  
+        233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,  
+        116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53  
+    }; 
+    /**
+        *	@name		CRC8_Table
+    * @brief  This function is used to Get CRC8 check value.
+        * @param  p :Array pointer. counter :Number of array elements.
+    * @retval crc8
+    */
+    unsigned char CRC8_Table(unsigned char *p, int counter)  
+    {  
+        unsigned char crc8 = 0;  
+    
+        for( ; counter > 0; counter--)
+        {  
+        crc8 = CRC8Table[crc8^*p];  
+        p++;  
+        }  
+        return(crc8);  
+    }
+
+    RS485Driver::RS485Driver(size_t maxMotors) : max_motors_(maxMotors), serial_port_(-1), is_open_(false)
+    {
+        control_index_ = 0;
+    }
+    
+    RS485Driver::~RS485Driver()
+    {
+        closePort();
+    }
+    
+    speed_t RS485Driver::convertBaudRate(int baud_rate)
+    {
+        switch (baud_rate)
+        {
+            case 9600: return B9600;
+            case 19200: return B19200;
+            case 38400: return B38400;
+            case 57600: return B57600;
+            case 115200: return B115200;
+            case 230400: return B230400;
+            case 460800: return B460800;
+            case 500000: return B500000;
+            case 576000: return B576000;
+            case 921600: return B921600;
+            default: return B0;
+        }
+    }
+    
+    bool RS485Driver::openPort(const std::string &port_name, int baud_rate)
+    {
+        // 关闭已打开的端口
+        if (is_open_)
+        {
+            closePort();
+        }
+        
+        // 打开串口
+        serial_port_ = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
+        if (serial_port_ == -1)
+        {
+            std::cerr << "无法打开串口: " << port_name << std::endl;
+            return false;
+        }
+        
+        // 保存原始配置
+        if (tcgetattr(serial_port_, &original_termios_) == -1)
+        {
+            std::cerr << "无法获取串口属性" << std::endl;
+            close(serial_port_);
+            serial_port_ = -1;
+            return false;
+        }
+        
+        // 配置串口
+        struct termios tty;
+        memset(&tty, 0, sizeof(tty));
+        
+        // 设置波特率
+        speed_t speed = convertBaudRate(baud_rate);
+        if (speed == B0)
+        {
+            std::cerr << "不支持的波特率: " << baud_rate << std::endl;
+            closePort();
+            return false;
+        }
+        
+        cfsetospeed(&tty, speed);
+        cfsetispeed(&tty, speed);
+        
+        // 8位数据位，无奇偶校验，1位停止位
+        tty.c_cflag &= ~PARENB;  // 无奇偶校验
+        tty.c_cflag &= ~CSTOPB;  // 1位停止位
+        tty.c_cflag &= ~CSIZE;   // 清除数据位设置
+        tty.c_cflag |= CS8;      // 8位数据位
+        
+        // 禁用硬件流控制
+        tty.c_cflag &= ~CRTSCTS;
+        
+        // 启用接收器，设置本地模式
+        tty.c_cflag |= (CLOCAL | CREAD);
+        
+        // 禁用软件流控制
+        tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+        
+        // 原始输入模式
+        tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+        
+        // 原始输出模式
+        tty.c_oflag &= ~OPOST;
+        
+        // 设置读取超时
+        tty.c_cc[VTIME] = 10;  // 1秒超时
+        tty.c_cc[VMIN] = 0;
+        
+        // 应用配置
+        if (tcsetattr(serial_port_, TCSANOW, &tty) != 0)
+        {
+            std::cerr << "无法设置串口属性" << std::endl;
+            closePort();
+            return false;
+        }
+        
+        is_open_ = true;
+        std::cout << "串口 " << port_name << " 已打开，波特率: " << baud_rate << std::endl;
+        return true;
+    }
+    
+    void RS485Driver::closePort()
+    {
+        if (is_open_)
+        {
+            // 恢复原始配置
+            tcsetattr(serial_port_, TCSANOW, &original_termios_);
+            close(serial_port_);
+            is_open_ = false;
+            serial_port_ = -1;
+            std::cout << "串口已关闭" << std::endl;
+        }
+    }
+    
+    bool RS485Driver::sendData(const std::vector<uint8_t> &data)
+    {
+        if (!is_open_)
+        {
+            std::cerr << "串口未打开，无法发送数据" << std::endl;
+            return false;
+        }
+        
+        ssize_t bytes_written = write(serial_port_, data.data(), data.size());
+        if (bytes_written != static_cast<ssize_t>(data.size()))
+        {
+            std::cerr << "发送数据失败，预期发送 " << data.size() 
+                      << " 字节，实际发送 " << bytes_written << " 字节" << std::endl;
+            return false;
+        }
+        
+        return true;
+    }
+    
+    bool RS485Driver::receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms)
+    {
+        if (!is_open_)
+        {
+            std::cerr << "串口未打开，无法接收数据" << std::endl;
+            return false;
+        }
+        
+        data.clear();
+        uint8_t buffer[256];
+        ssize_t bytes_read;
+        auto start_time = std::chrono::steady_clock::now();
+        
+        while (data.size() < max_length)
+        {
+            // 检查超时
+            auto current_time = std::chrono::steady_clock::now();
+            auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+                current_time - start_time).count();
+            
+            if (elapsed_ms >= timeout_ms)
+            {
+                break;  // 超时
+            }
+            
+            // 尝试读取数据
+            bytes_read = read(serial_port_, buffer, std::min(sizeof(buffer), max_length - data.size()));
+            
+            if (bytes_read > 0)
+            {
+                data.insert(data.end(), buffer, buffer + bytes_read);
+                start_time = std::chrono::steady_clock::now();  // 重置超时计时器
+            }
+            else if (bytes_read < 0)
+            {
+                std::cerr << "接收数据错误" << std::endl;
+                return false;
+            }
+            else
+            {
+                // 没有数据，短暂休眠
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            }
+        }
+        
+        return !data.empty();
+    }
+
+    uint8_t RS485Driver::calculateChecksum(const std::vector<uint8_t> &data)
+    {
+        uint8_t checksum = 0;
+        for (uint8_t byte : data)
+        {
+            checksum ^= byte;
+        }
+        return checksum;
+    }
+    
+    bool RS485Driver::controlMotor(uint8_t motor_id, int32_t speed, bool stop)
+    {
+        if (!isOpen())
+        {
+            std::cerr << "RS485未初始化，无法控制电机" << std::endl;
+            return false;
+        }
+        
+        if (motor_id < 1 || motor_id > max_motors_)
+        {
+            std::cerr << "无效的电机ID: " << static_cast<int>(motor_id) 
+                      << ", 必须在1到" << static_cast<int>(max_motors_) << "之间" << std::endl;
+            return false;
+        }
+        
+        // 限制速度范围
+        if (speed > 1000) speed = 1000;
+        if (speed < -1000) speed = -1000;
+        
+        // 构建命令帧
+        std::vector<uint8_t> command;
+        command.push_back(START_BYTE);
+        
+        if (stop)
+        {
+            command.push_back(STOP_CMD);
+        }
+        else
+        {
+            command.push_back(CONTROL_CMD);
+        }
+        
+        command.push_back(motor_id);
+        
+        // 将速度转换为4字节并添加到命令
+        uint8_t speed_bytes[4];
+        std::memcpy(speed_bytes, &speed, sizeof(speed));
+        for (int i = 0; i < 4; ++i)
+        {
+            command.push_back(speed_bytes[i]);
+        }
+        
+        // 计算并添加校验和
+        uint8_t checksum = calculateChecksum(command);
+        command.push_back(checksum);
+        
+        // 添加结束字节
+        command.push_back(STOP_BYTE);
+        
+        // 发送命令
+        bool success = sendData(command);
+        
+        if (success)
+        {
+            if (stop)
+            {
+                std::cout << "已发送停止命令到电机 " << static_cast<int>(motor_id) << std::endl;
+            }
+            else
+            {
+                std::cout << "已发送命令到电机 " << static_cast<int>(motor_id) 
+                          << ", 速度: " << speed << std::endl;
+            }
+        }
+        else
+        {
+            std::cerr << "发送命令到电机 " << static_cast<int>(motor_id) << " 失败" << std::endl;
+        }
+        
+        return success;
+    }
+    
+    bool RS485Driver::controlAllMotors(const std::vector<int32_t> &speeds)
+    {
+        if (speeds.size() != max_motors_)
+        {
+            std::cerr << "控制所有电机需要提供 " << static_cast<int>(max_motors_) 
+                      << " 个速度值，实际提供了 " << speeds.size() << " 个" << std::endl;
+            return false;
+        }
+        
+        if (!isOpen())
+        {
+            std::cerr << "RS485未初始化，无法控制电机" << std::endl;
+            return false;
+        }
+        
+        // 构建命令帧
+        
+        bool success = false;
+
+        static uint8_t i = 1;
+
+        // 添加所有电机的速度
+
+        std::vector<uint8_t> command;
+
+        if (i < 5)
+        {
+            control_index_ = 1;
+            // 限制速度范围
+            int32_t clamped_speed = speeds[control_index_ - 1];
+            if (clamped_speed > 1000) clamped_speed = 1000;
+            if (clamped_speed < -1000) clamped_speed = -1000;
+
+            uint8_t high_byte = (clamped_speed >> 8) & 0xFF;  // 高8位
+            uint8_t low_byte  = clamped_speed & 0xFF;         // 低8位
+
+            command.push_back(2);
+            command.push_back(CONTROL_CMD);
+            command.push_back(high_byte);
+            command.push_back(low_byte);
+            command.push_back(ZERO_CMD);
+            command.push_back(ZERO_CMD);
+            command.push_back(DEFAULT_TIME);
+            command.push_back(ZERO_CMD);
+            command.push_back(ZERO_CMD);
+
+            unsigned char crc8 = CRC8_Table(command.data(), 9);
+
+            command.push_back(crc8);
+            
+            // 发送命令
+            success = sendData(command);
+            // std::cout << "已发送命令到电机 " << static_cast<int>(control_index_) << ", 速度: " << clamped_speed << std::endl;
+        }
+        else if ( i > 5)
+        {
+            control_index_ = 2;
+            // 限制速度范围
+            int32_t clamped_speed = speeds[control_index_ - 1];
+            if (clamped_speed > 1000) clamped_speed = 1000;
+            if (clamped_speed < -1000) clamped_speed = -1000;
+
+            uint8_t high_byte = (clamped_speed >> 8) & 0xFF;  // 高8位
+            uint8_t low_byte  = clamped_speed & 0xFF;         // 低8位
+
+            command.push_back(3);
+            command.push_back(CONTROL_CMD);
+            command.push_back(high_byte);
+            command.push_back(low_byte);
+            command.push_back(ZERO_CMD);
+            command.push_back(ZERO_CMD);
+            command.push_back(DEFAULT_TIME);
+            command.push_back(ZERO_CMD);
+            command.push_back(ZERO_CMD);
+
+            unsigned char crc8 = CRC8_Table(command.data(), 9);
+
+            command.push_back(crc8);
+            
+            // 发送命令
+            success = sendData(command);
+            // std::cout << "已发送命令到电机 " << static_cast<int>(control_index_) << ", 速度: " << clamped_speed << std::endl;
+        }
+
+        ++i;
+
+        // std::cout << "i = " << static_cast<int>(i) << std::endl;
+
+        if (i > 8)
+        {
+            i = 1;
+            return success;
+        }
+        
+        return false;
+    }
+    
+    bool RS485Driver::stopAllMotors()
+    {
+        std::vector<int32_t> speeds(max_motors_, 0);
+        return controlAllMotors(speeds);
+    }
+}

src/robot_control/src/trapezoidal_trajectory.cpp

@@ -0,0 +1,407 @@
+#include "trapezoidal_trajectory.hpp"
+#include <cmath>
+#include <stdexcept>
+#include <algorithm>
+#include <iostream>
+
+namespace robot_control
+{
+
+// 多轴构造函数
+TrapezoidalTrajectory::TrapezoidalTrajectory(const std::vector<double>& max_velocities, 
+                                             const std::vector<double>& max_accelerations)
+    : is_stopping_(false), is_single_joint_(false)
+{
+    if (max_velocities.empty() || max_accelerations.empty())
+        throw std::invalid_argument("Max velocities and accelerations cannot be empty");
+    if (max_velocities.size() != max_accelerations.size())
+        throw std::invalid_argument("Max velocities and accelerations must have same size");
+    
+    for (double v : max_velocities)
+        if (v <= 0) throw std::invalid_argument("Max velocity must be positive");
+    for (double a : max_accelerations)
+        if (a <= 0) throw std::invalid_argument("Max acceleration must be positive");
+    
+    max_velocity_ = max_velocities;
+    max_acceleration_ = max_accelerations;
+}
+
+// 单轴构造函数
+TrapezoidalTrajectory::TrapezoidalTrajectory(double max_velocity, double max_acceleration)
+    : is_stopping_(false), is_single_joint_(true)
+{
+    if (max_velocity <= 0)
+        throw std::invalid_argument("Max velocity must be positive");
+    if (max_acceleration <= 0)
+        throw std::invalid_argument("Max acceleration must be positive");
+    
+    max_velocity_ = {max_velocity};
+    max_acceleration_ = {max_acceleration};
+}
+
+// 多轴初始化
+void TrapezoidalTrajectory::init(const std::vector<double>& start_pos, 
+                                const std::vector<double>& target_pos)
+{
+    if (start_pos.size() != target_pos.size())
+        throw std::invalid_argument("Start and target positions must have same size");
+    if (start_pos.size() != max_velocity_.size())
+        throw std::invalid_argument("Position size does not match max velocity size");
+    
+    is_single_joint_ = false;
+    is_stopping_ = false;
+    start_pos_ = start_pos;
+    target_pos_ = target_pos;
+    size_t dof = start_pos.size();
+    
+    // 初始化向量大小
+    total_distance_.resize(dof);
+    current_pos_.resize(dof);
+    current_velocity_.resize(dof);
+    current_acceleration_.resize(dof);
+    cruise_velocity_.resize(dof);
+    acceleration_.resize(dof);
+    deceleration_.resize(dof);
+    
+    // 计算各轴总位移
+    for (size_t i = 0; i < dof; ++i)
+        total_distance_[i] = std::fabs(target_pos[i] - start_pos[i]);
+    
+    calculateTrajectoryParams();
+}
+
+// 单轴初始化
+void TrapezoidalTrajectory::init(double start_pos, double target_pos)
+{
+    is_single_joint_ = true;
+    is_stopping_ = false;
+    start_pos_ = {start_pos};
+    target_pos_ = {target_pos};
+    total_distance_ = {std::fabs(target_pos - start_pos)};
+    
+    current_pos_.resize(1);
+    current_velocity_.resize(1);
+    current_acceleration_.resize(1);
+    cruise_velocity_.resize(1);
+    acceleration_.resize(1);
+    deceleration_.resize(1);
+    
+    calculateTrajectoryParams();
+}
+
+// 核心：计算轨迹参数（统一时间+各轴巡航速度）
+void TrapezoidalTrajectory::calculateTrajectoryParams()
+{
+    size_t dof = start_pos_.size();
+    std::vector<double> axis_total_time(dof, 0.0);  // 各轴理论最小时间
+    std::vector<double> axis_accel_time(dof, 0.0);  // 各轴理论加速时间
+    std::vector<double> axis_decel_time(dof, 0.0);  // 各轴理论减速时间
+    std::vector<double> axis_cruise_time(dof, 0.0); // 各轴理论匀速时间
+    std::vector<double> axis_cruise_vel(dof, 0.0);  // 各轴理论巡航速度
+
+    // 步骤1：计算每个轴的理论最小运动参数（独立约束）
+    for (size_t i = 0; i < dof; ++i)
+    {
+        double dist = total_distance_[i];
+        if (dist < 1e-6)  // 位移为0的轴无需运动
+        {
+            axis_total_time[i] = 0.0;
+            axis_accel_time[i] = 0.0;
+            axis_decel_time[i] = 0.0;
+            axis_cruise_time[i] = 0.0;
+            axis_cruise_vel[i] = 0.0;
+            continue;
+        }
+
+        double a = max_acceleration_[i];
+        double v_max = max_velocity_[i];
+        double t_acc_max = v_max / a;  // 加速到最大速度的时间
+        double dist_acc_max = 0.5 * a * t_acc_max * t_acc_max;  // 加速阶段最大位移
+        double dist_decel_max = dist_acc_max;  // 减速阶段最大位移（对称）
+
+        if (dist <= dist_acc_max + dist_decel_max)
+        {
+            // 无法达到最大速度（无匀速阶段）
+            double t = std::sqrt(dist / a);  // 加速时间=减速时间
+            axis_accel_time[i] = t;
+            axis_decel_time[i] = t;
+            axis_cruise_time[i] = 0.0;
+            axis_cruise_vel[i] = a * t;  // 实际最大速度（小于v_max）
+            axis_total_time[i] = 2 * t;
+        }
+        else
+        {
+            // 可达到最大速度（有匀速阶段）
+            axis_accel_time[i] = t_acc_max;
+            axis_decel_time[i] = t_acc_max;  // 减速时间=加速时间（对称）
+            double dist_cruise = dist - dist_acc_max - dist_decel_max;
+            axis_cruise_time[i] = dist_cruise / v_max;
+            axis_cruise_vel[i] = v_max;
+            axis_total_time[i] = t_acc_max + axis_cruise_time[i] + t_acc_max;
+        }
+    }
+
+    for (size_t i = 0; i < axis_total_time.size(); i++)
+    {
+        if (total_time_ < axis_total_time[i])
+        {
+            total_time_ = axis_total_time[i];
+            acceleration_time_ = axis_accel_time[i];
+            deceleration_time_ = axis_decel_time[i];
+            cruise_time_ = axis_cruise_time[i];
+        }
+    }
+    
+    if (total_time_ < 1e-6)  // 所有轴位移为0
+    {
+        acceleration_time_ = 0.0;
+        deceleration_time_ = 0.0;
+        cruise_time_ = 0.0;
+        cruise_velocity_.assign(dof, 0.0);
+        return;
+    }
+
+    // 步骤3：计算各轴最终巡航速度（适配统一时间）
+    for (size_t i = 0; i < dof; ++i)
+    {
+        double dist = total_distance_[i];
+        if (dist < 1e-6)
+        {
+            cruise_velocity_[i] = 0.0;
+            continue;
+        }
+
+        //TODO:  最大加速度需要保持一致,否则这里的计算会有问题.
+        cruise_velocity_[i] = dist / (acceleration_time_ + cruise_time_);
+        acceleration_[i] = cruise_velocity_[i] / acceleration_time_;
+        deceleration_[i] = cruise_velocity_[i] / deceleration_time_;
+    }
+}
+
+// 多轴位置更新
+std::vector<double> TrapezoidalTrajectory::update(double time)
+{
+    if (is_single_joint_)
+        throw std::runtime_error("Call updateSingle for single joint mode");
+    if (is_stopping_)
+        throw std::runtime_error("In stop mode, use updateStop instead");
+
+    size_t dof = start_pos_.size();
+    double t = std::clamp(time, 0.0, total_time_);  // 统一时间
+
+    for (size_t i = 0; i < dof; ++i)
+    {
+        double dist = total_distance_[i];
+        if (dist < 1e-6)  // 无位移轴保持静止
+        {
+            current_pos_[i] = start_pos_[i];
+            current_velocity_[i] = 0.0;
+            current_acceleration_[i] = 0.0;
+            continue;
+        }
+
+        double dir = (target_pos_[i] - start_pos_[i]) > 0 ? 1.0 : -1.0;  // 方向
+        double a = acceleration_[i];
+        double v_cruise = cruise_velocity_[i];
+        double accel = 0.0;
+        double vel = 0.0;
+        double pos_offset = 0.0;
+
+        // 阶段1：加速（所有轴同步进入）
+        if (t <= acceleration_time_)
+        {
+            accel = dir * a;
+            vel = dir * a * t;
+            pos_offset = 0.5 * dir * a * t * t;
+        }
+        // 阶段2：匀速（所有轴同步进入）
+        else if (t <= acceleration_time_ + cruise_time_)
+        {
+            accel = 0.0;
+            vel = dir * v_cruise;
+            double t_cruise = t - acceleration_time_;
+            double pos_accel = 0.5 * dir * a * acceleration_time_ * acceleration_time_;
+            pos_offset = pos_accel + dir * v_cruise * t_cruise;
+        }
+        // 阶段3：减速（所有轴同步进入）
+        else
+        {
+            accel = -dir * a;
+            double t_decel = t - (acceleration_time_ + cruise_time_);
+            vel = dir * v_cruise + accel * t_decel;
+            double pos_accel = 0.5 * dir * a * acceleration_time_ * acceleration_time_;
+            double pos_cruise = dir * v_cruise * cruise_time_;
+            double pos_decel = dir * v_cruise * t_decel + 0.5 * accel * t_decel * t_decel;
+            pos_offset = pos_accel + pos_cruise + pos_decel;
+        }
+
+        // 更新当前状态
+        current_pos_[i] = start_pos_[i] + pos_offset;
+        current_velocity_[i] = vel;
+        current_acceleration_[i] = accel;
+    }
+
+    return current_pos_;
+}
+
+// 单轴位置更新
+double TrapezoidalTrajectory::updateSingle(double time)
+{
+    if (!is_single_joint_)
+        throw std::runtime_error("Call update for multi-joint mode");
+    if (is_stopping_)
+        throw std::runtime_error("In stop mode, use updateStop instead");
+
+    double t = std::clamp(time, 0.0, total_time_);
+    double dist = total_distance_[0];
+
+    if (dist < 1e-6)
+    {
+        current_pos_[0] = start_pos_[0];
+        current_velocity_[0] = 0.0;
+        current_acceleration_[0] = 0.0;
+        return current_pos_[0];
+    }
+
+    double dir = (target_pos_[0] - start_pos_[0]) > 0 ? 1.0 : -1.0;
+    double a = max_acceleration_[0];
+    double v_cruise = cruise_velocity_[0];
+    double accel = 0.0;
+    double vel = 0.0;
+    double pos_offset = 0.0;
+
+    if (t <= acceleration_time_)
+    {
+        accel = dir * a;
+        vel = dir * a * t;
+        pos_offset = 0.5 * dir * a * t * t;
+    }
+    else if (t <= acceleration_time_ + cruise_time_)
+    {
+        accel = 0.0;
+        vel = dir * v_cruise;
+        double t_cruise = t - acceleration_time_;
+        pos_offset = 0.5 * dir * a * acceleration_time_ * acceleration_time_ 
+                   + dir * v_cruise * t_cruise;
+    }
+    else
+    {
+        accel = -dir * a;
+        double t_decel = t - (acceleration_time_ + cruise_time_);
+        vel = dir * v_cruise + accel * t_decel;
+        double pos_accel = 0.5 * dir * a * acceleration_time_ * acceleration_time_;
+        double pos_cruise = dir * v_cruise * cruise_time_;
+        double pos_decel = dir * v_cruise * t_decel + 0.5 * accel * t_decel * t_decel;
+        pos_offset = pos_accel + pos_cruise + pos_decel;
+    }
+
+    current_pos_[0] = start_pos_[0] + pos_offset;
+    current_velocity_[0] = vel;
+    current_acceleration_[0] = accel;
+
+    return current_pos_[0];
+}
+
+// 设置各轴最大速度
+void TrapezoidalTrajectory::setMaxVelocities(const std::vector<double>& max_velocities)
+{
+    if (max_velocities.size() != max_velocity_.size())
+        throw std::invalid_argument("Velocity size mismatch");
+    for (double v : max_velocities)
+        if (v <= 0) throw std::invalid_argument("Max velocity must be positive");
+    max_velocity_ = max_velocities;
+}
+
+// 设置单轴最大速度
+void TrapezoidalTrajectory::setMaxVelocity(double max_velocity)
+{
+    if (max_velocity <= 0)
+        throw std::invalid_argument("Max velocity must be positive");
+    max_velocity_[0] = max_velocity;
+}
+
+// 设置各轴最大加速度
+void TrapezoidalTrajectory::setMaxAccelerations(const std::vector<double>& max_accelerations)
+{
+    if (max_accelerations.size() != max_acceleration_.size())
+        throw std::invalid_argument("Acceleration size mismatch");
+    for (double a : max_accelerations)
+        if (a <= 0) throw std::invalid_argument("Max acceleration must be positive");
+    max_acceleration_ = max_accelerations;
+}
+
+// 设置单轴最大加速度
+void TrapezoidalTrajectory::setMaxAcceleration(double max_acceleration)
+{
+    if (max_acceleration <= 0)
+        throw std::invalid_argument("Max acceleration must be positive");
+    max_acceleration_[0] = max_acceleration;
+}
+
+// 计算急停轨迹参数
+void TrapezoidalTrajectory::calculateStopTrajectory()
+{
+    size_t dof = current_pos_.size();
+    is_stopping_ = true;
+    stop_start_pos_ = current_pos_;
+    stop_start_vel_ = current_velocity_;
+    stop_decel_.resize(dof);
+    std::vector<double> stop_axis_time(dof, 0.0);  // 各轴急停时间
+
+    // 计算各轴所需急停时间和减速度
+    for (size_t i = 0; i < dof; ++i)
+    {
+        double vel = stop_start_vel_[i];
+        if (std::fabs(vel) < 1e-6)  // 静止轴无需减速
+        {
+            stop_decel_[i] = 0.0;
+            stop_axis_time[i] = 0.0;
+            continue;
+        }
+
+        // 减速度方向与速度相反，大小不超过最大加速度
+        stop_decel_[i] = -std::copysign(max_acceleration_[i], vel);
+        // 急停时间 = 速度 / 减速度大小
+        stop_axis_time[i] = std::fabs(vel) / max_acceleration_[i];
+    }
+
+    // 统一急停时间（取最长时间）
+    stop_total_time_ = *std::max_element(stop_axis_time.begin(), stop_axis_time.end());
+}
+
+// 更新急停轨迹
+std::vector<double> TrapezoidalTrajectory::updateStop(double dt)
+{
+    if (!is_stopping_)
+        throw std::runtime_error("Not in stop mode, call calculateStopTrajectory first");
+
+    size_t dof = stop_start_pos_.size();
+    double t = std::clamp(dt, 0.0, stop_total_time_);  // 统一急停时间
+
+    for (size_t i = 0; i < dof; ++i)
+    {
+        double vel0 = stop_start_vel_[i];
+        double decel = stop_decel_[i];
+
+        if (std::fabs(vel0) < 1e-6)  // 静止轴保持不动
+        {
+            current_pos_[i] = stop_start_pos_[i];
+            current_velocity_[i] = 0.0;
+            current_acceleration_[i] = 0.0;
+            continue;
+        }
+
+        // 计算当前位置和速度（匀减速运动）
+        current_pos_[i] = stop_start_pos_[i] + vel0 * t + 0.5 * decel * t * t;
+        current_velocity_[i] = vel0 + decel * t;
+
+        // 避免微小速度（数值稳定性）
+        if (std::fabs(current_velocity_[i]) < 1e-6)
+            current_velocity_[i] = 0.0;
+        current_acceleration_[i] = decel;
+    }
+
+    return current_pos_;
+}
+
+}  // namespace robot_control

src/robot_control/src/urdf_parser.cpp

@@ -0,0 +1,247 @@
+#include "urdf_parser.hpp"
+#include <fstream>
+#include <sstream>
+#include <algorithm>
+#include <stdexcept>
+#include <iostream>
+
+using namespace robot_control;
+
+std::shared_ptr<RobotModel> UrdfParser::parse_from_file(const std::string& urdf_file_path)
+{
+    // 读取URDF文件内容
+    std::ifstream file(urdf_file_path);
+    if (!file.is_open())
+    {
+        std::cerr << "Failed to open URDF file: " << urdf_file_path << std::endl;
+        return nullptr;
+    }
+
+    std::stringstream ss;
+    ss << file.rdbuf();
+    std::string urdf_xml = ss.str();
+    file.close();
+
+    return parse_from_string(urdf_xml);
+}
+
+std::shared_ptr<RobotModel> UrdfParser::parse_from_string(const std::string& urdf_xml)
+{
+    urdf::Model model;
+    if (!model.initString(urdf_xml))
+    {
+        std::cerr << "Failed to parse URDF XML string" << std::endl;
+        return nullptr;
+    }
+
+    return build_robot_model(model);
+}
+
+std::shared_ptr<RobotModel> UrdfParser::build_robot_model(const urdf::Model& model)
+{
+    auto robot_model = std::make_shared<RobotModel>();
+    
+    // 我们需要访问RobotModel的私有成员来构建模型
+    // 这里假设RobotModel类有一个友元类UrdfParser，或者提供了构建接口
+    // 为了演示，我们直接修改RobotModel的私有成员（实际项目中应使用适当的接口）
+    
+    // 遍历所有关节和连杆
+    for (const auto& joint_pair : model.joints_)
+    {
+        const auto& joint = joint_pair.second;
+        auto parsed_joint = parse_joint(joint);
+        if (!parsed_joint)
+            continue;
+
+        // 确定关节属于哪个部分
+        std::string part = identify_joint_part(joint->name);
+
+        // 手臂关节
+        if (part == "arm")
+        {
+            robot_model->arm_.joints.push_back(parsed_joint);
+        }
+        // 腿部关节
+        else if (part == "leg")
+        {
+            std::string leg_side = identify_leg_side(joint->name);
+            
+            // 查找或创建对应的腿
+            bool leg_found = false;
+            for (auto& leg : robot_model->legs_)
+            {
+                if (leg.side == leg_side)
+                {
+                    leg.joints.push_back(parsed_joint);
+                    leg_found = true;
+                    break;
+                }
+            }
+            
+            // 如果没有找到对应的腿，则创建新的
+            if (!leg_found)
+            {
+                Leg new_leg;
+                new_leg.side = leg_side;
+                new_leg.joints.push_back(parsed_joint);
+                robot_model->legs_.push_back(new_leg);
+            }
+        }
+    }
+
+    // 解析连杆
+    for (const auto& link_pair : model.links_)
+    {
+        const auto& link = link_pair.second;
+        auto parsed_link = parse_link(link);
+        if (!parsed_link)
+            continue;
+
+        // 确定连杆属于哪个部分
+        std::string part = identify_joint_part(link->name);
+
+        // 手臂连杆
+        if (part == "arm")
+        {
+            robot_model->arm_.links.push_back(parsed_link);
+        }
+        // 腿部连杆
+        else if (part == "leg")
+        {
+            std::string leg_side = identify_leg_side(link->name);
+            
+            // 查找对应的腿
+            for (auto& leg : robot_model->legs_)
+            {
+                if (leg.side == leg_side)
+                {
+                    leg.links.push_back(parsed_link);
+                    break;
+                }
+            }
+        }
+    }
+
+    // 确保腿部数量为4
+    if (robot_model->legs_.size() != 4)
+    {
+        std::cerr << "Warning: Expected 4 legs, but found " << robot_model->legs_.size() << std::endl;
+    }
+
+    return robot_model;
+}
+
+std::shared_ptr<Link> UrdfParser::parse_link(const urdf::LinkConstSharedPtr& link)
+{
+    if (!link)
+        return nullptr;
+
+    // 获取连杆长度（简化处理，假设是盒子形状）
+    double length = 0.1;  // 默认长度
+    if (link->collision && link->collision->geometry && 
+        link->collision->geometry->type == urdf::Geometry::BOX)
+    {
+        auto box = std::dynamic_pointer_cast<urdf::Box>(link->collision->geometry);
+        if (box)
+        {
+            // 假设X轴为长度方向
+            length = box->dim.x;
+        }
+    }
+
+    // 获取质量
+    double mass = 1.0;  // 默认质量
+    if (link->inertial)
+    {
+        mass = link->inertial->mass;
+    }
+
+    return std::make_shared<Link>(link->name, length, mass);
+}
+
+std::shared_ptr<Joint> UrdfParser::parse_joint(const urdf::JointConstSharedPtr& joint)
+{
+    if (!joint)
+        return nullptr;
+
+    // 只处理旋转关节
+    if (joint->type != urdf::Joint::REVOLUTE && joint->type != urdf::Joint::CONTINUOUS)
+        return nullptr;
+
+    // 获取关节角度限制
+    double min_angle = -M_PI;
+    double max_angle = M_PI;
+    
+    if (joint->type == urdf::Joint::REVOLUTE)
+    {
+        min_angle = joint->limits->lower;
+        max_angle = joint->limits->upper;
+    }
+
+    // 获取力限制
+    double effort_limit = 100.0;  // 默认力限制
+    if (joint->limits)
+    {
+        effort_limit = joint->limits->effort;
+    }
+
+    return std::make_shared<Joint>(joint->name, min_angle, max_angle, effort_limit);
+}
+
+std::string UrdfParser::identify_joint_part(const std::string& name)
+{
+    // 转换为小写以便不区分大小写匹配
+    std::string lower_name = name;
+    std::transform(lower_name.begin(), lower_name.end(), lower_name.begin(), ::tolower);
+
+    if (lower_name.find("arm") != std::string::npos || 
+        lower_name.find("shoulder") != std::string::npos ||
+        lower_name.find("elbow") != std::string::npos ||
+        lower_name.find("wrist") != std::string::npos)
+    {
+        return "arm";
+    }
+    else if (lower_name.find("leg") != std::string::npos || 
+             lower_name.find("hip") != std::string::npos ||
+             lower_name.find("knee") != std::string::npos ||
+             lower_name.find("thigh") != std::string::npos ||
+             lower_name.find("shin") != std::string::npos)
+    {
+        return "leg";
+    }
+
+    return "unknown";
+}
+
+std::string UrdfParser::identify_leg_side(const std::string& name)
+{
+    // 转换为小写以便不区分大小写匹配
+    std::string lower_name = name;
+    std::transform(lower_name.begin(), lower_name.end(), lower_name.begin(), ::tolower);
+
+    bool is_front = (lower_name.find("front") != std::string::npos || 
+                     lower_name.find("fore") != std::string::npos);
+    bool is_rear = (lower_name.find("rear") != std::string::npos || 
+                    lower_name.find("hind") != std::string::npos);
+    bool is_left = (lower_name.find("left") != std::string::npos);
+    bool is_right = (lower_name.find("right") != std::string::npos);
+
+    std::string position;
+    if (is_front)
+        position = "front_";
+    else if (is_rear)
+        position = "rear_";
+    else
+        position = "unknown_";
+
+    std::string side;
+    if (is_left)
+        side = "left";
+    else if (is_right)
+        side = "right";
+    else
+        side = "unknown";
+
+    return position + side;
+}
+

src/robot_control/src/waist_control.cpp

@@ -0,0 +1,102 @@
+#include "waist_control.hpp"
+#include <stdexcept>
+#include <vector>
+
+namespace robot_control
+{
+
+WaistControl::WaistControl(
+    size_t total_joints, 
+    const std::vector<double>& lengthParameters, 
+    const std::vector<double>& maxSpeed,
+    const std::vector<double>& maxAcc,
+    const std::vector<double>& minLimits, 
+    const std::vector<double>& maxLimits, 
+    const std::vector<double>& home_positions,
+    const std::vector<double>& zero_positions,
+    const std::vector<int>& joint_directions
+): ControlBase(
+    total_joints, 
+    lengthParameters, 
+    maxSpeed, maxAcc, 
+    minLimits, 
+    maxLimits, 
+    home_positions, 
+    zero_positions,
+    joint_directions)
+{
+    std::cout << "WaistControl Constructor" << std::endl;
+}
+
+WaistControl::~WaistControl()
+{
+    delete trapezoidalTrajectory_;
+}
+
+
+bool WaistControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
+{
+    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
+    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
+    std::vector<double> joint_target(total_joints_, 0.0);
+    
+    // 实际应用中应在此处添加逆运动学求解
+    // joint_target = inverse_kinematics(target_pos);
+
+    for (size_t i = 0; i < target_pos.size(); i++)
+    {
+        std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
+    }
+
+    // 使用梯形轨迹规划移动到目标关节位置
+    return MoveToTargetJoint(joint_positions, joint_target, dt);
+}
+
+bool WaistControl::MoveWaist(std::vector<double>& joint_positions, double dt)
+{    
+    if (!is_moving_)
+    {
+        if (!is_target_set_)
+        {
+            std::cout << "Target position not set!" << std::endl;
+            return true;
+        }
+    }
+
+    bool result = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
+
+    if (result)
+    {
+        is_target_set_ = false;
+    }
+
+    return result;
+}
+
+bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double moveyaw)
+{
+    std::vector<double> tempPosition;
+    tempPosition.resize(total_joints_, 0.0);
+
+    tempPosition[0] = joint_position_[0] - joint_zero_positions_[0] + movepitch;
+    tempPosition[1] = joint_position_[1] - joint_zero_positions_[1] + moveyaw;
+
+    if (!checkJointLimits(tempPosition))
+    {
+        std::cout << "Joint limits exceeded!" << std::endl;
+        return false;
+    }
+
+    for (size_t i = 0; i < joint_position_desired_.size(); i++)
+    {
+        joint_position_desired_[i] = tempPosition[i] + joint_zero_positions_[i];
+    }
+    
+    std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
+
+    is_target_set_ = true;
+
+    return true;
+}
+
+}  // namespace robot_control

src/robot_control/src/wheel_control.cpp

@@ -0,0 +1,108 @@
+#include "wheel_control.hpp"
+#include <vector>
+
+namespace robot_control
+{
+
+WheelControl::WheelControl(
+    size_t total_joints, 
+    const std::vector<double>& lengthParameters, 
+    const std::vector<double>& maxSpeed,
+    const std::vector<double>& maxAcc,
+    const std::vector<double>& minLimits, 
+    const std::vector<double>& maxLimits, 
+    const std::vector<double>& home_positions,
+    const std::vector<double>& zero_positions,
+    const std::vector<int>& joint_directions
+): ControlBase(
+    total_joints, 
+    lengthParameters, 
+    maxSpeed, maxAcc, 
+    minLimits, 
+    maxLimits, 
+    home_positions, 
+    zero_positions,
+    joint_directions)
+{
+    std::cout << "WheelControl Constructor" << std::endl;
+}
+
+WheelControl::~WheelControl()
+{
+    delete trapezoidalTrajectory_;
+}
+
+bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle)
+{
+    // 设置移动轮参数
+    double theta = moveWheelDistance / lengthParameters_[0] * 180.0 / M_PI;
+
+    for (size_t i = 0; i < joint_directions_.size(); i++)
+    {
+        if (joint_directions_[i] == 1)
+        {
+            joint_position_desired_[i] = joint_position_[i] + theta;
+        }
+        else
+        {
+            joint_position_desired_[i] = joint_position_[i] - theta;
+        }
+    }
+
+    //TODO: 设置移动轮角度
+    (void) moveWheelAngle;
+
+    std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] <<  " , " << joint_position_desired_[1] << std::endl;
+
+    is_target_set_ = true;
+
+    return true;
+}
+
+
+bool WheelControl::MoveToTargetPoint(std::vector<double>& joint_positions, const std::vector<Eigen::Vector3d>& target_pos, double dt)
+{
+    // 注意：这里需要运动学逆解将笛卡尔空间目标位置转换为关节空间
+    // 为简化示例，这里假设已完成逆解，直接使用虚拟关节位置
+    std::vector<double> joint_target(total_joints_, 0.0);
+    
+    // 实际应用中应在此处添加逆运动学求解
+    // joint_target = inverse_kinematics(target_pos);
+    
+    for (size_t i = 0; i < target_pos.size(); i++)
+    {
+        std::cout << "Target Position: " << target_pos[i].transpose() << std::endl;
+    }
+
+    // 使用梯形轨迹规划移动到目标关节位置
+    return MoveToTargetJoint(joint_positions, joint_target, dt);
+}
+
+bool WheelControl::MoveWheel(std::vector<double>& joint_positions)
+{    
+    if (!is_moving_)
+    {
+        if (!is_target_set_)
+        {
+            std::cout << "Target position not set!" << std::endl;
+            return true;
+        }
+    }
+
+    joint_positions = joint_position_desired_;
+
+    is_target_set_ = false;
+
+    return true;
+}
+
+
+bool WheelControl::GoHome(std::vector<double>& joint_positions, double dt)
+{
+    // do nothing
+    (void) joint_positions;
+    (void) dt;
+    return true;
+}
+
+}