draft version

.gitignore

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

.vscode/settings.json

@@ -6,7 +6,6 @@
         "cctype": "cpp",
         "clocale": "cpp",
         "cmath": "cpp",
-        "csignal": "cpp",
         "cstdarg": "cpp",
         "cstddef": "cpp",
         "cstdio": "cpp",
@@ -15,14 +14,10 @@
         "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",
@@ -47,14 +42,11 @@
         "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",
@@ -66,18 +58,24 @@
         "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",
-        "filesystem": "cpp"
+        "csignal": "cpp",
+        "any": "cpp",
+        "strstream": "cpp",
+        "bitset": "cpp",
+        "codecvt": "cpp",
+        "regex": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "shared_mutex": "cpp",
+        "cfenv": "cpp",
+        "variant": "cpp"
     }
 }

CMakeLists.txt

@@ -0,0 +1,128 @@
+cmake_minimum_required(VERSION 3.10)
+project(hivecore_robot_motion)
+
+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(ament_cmake_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+
+# 定义Action接口
+set(action_files
+  "action/MoveJ.action"
+  "action/MoveL.action"
+)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  ${action_files}
+  DEPENDENCIES action_msgs geometry_msgs
+)
+
+# 库和可执行文件
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/controllers/joint_trajectory_controller.cpp
+  src/controllers/cartesian_trajectory_controller.cpp
+  src/hardware/ethercat_hardware_interface.cpp
+  src/hardware/canopen_hardware_interface.cpp
+  src/hardware/robot_hardware_interface.cpp
+  src/nodes/motion_control_node.cpp
+  src/planning/moveit_planner.cpp
+)
+
+target_include_directories(
+  ${PROJECT_NAME}
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  rclcpp
+  rclcpp_action
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  tf2
+  tf2_ros
+  moveit_core
+  moveit_ros_planning
+  moveit_ros_planning_interface
+  controller_interface
+  hardware_interface
+  pluginlib
+  rclcpp_lifecycle
+  realtime_tools
+  ethercat_interface
+  canopen_core
+)
+
+# 链接Action接口
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
+target_link_libraries(${PROJECT_NAME} "${cpp_typesupport_target}")
+
+# 安装库和可执行文件
+install(
+  TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+# 安装启动文件和配置文件
+install(
+  DIRECTORY launch config urdf
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# 安装节点
+add_executable(motion_control_node src/nodes/motion_control_main.cpp)
+target_link_libraries(motion_control_node ${PROJECT_NAME})
+
+install(TARGETS
+  motion_control_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+
+ament_package()

README.md

@@ -1,2 +1,16 @@
 # hivecore_robot_motion
 
+## 简介
+
+本项目为HiveCore机器人运动控制模块，包含运动控制算法、运动控制接口、运动控制测试等。
+
+## 功能
+
+- 运动控制算法：包括位置控制、速度控制、加速度控制等。
+- 运动控制接口：包括运动控制命令、运动控制状态查询等。
+- 运动控制测试：包括运动控制命令发送、运动控制状态查询等。
+
+## 使用方法
+
+1. 安装依赖库：在项目根目录下执行`pip install -r requirements.txt`命令安装依赖库。
+

action/MoveJ.action

@@ -0,0 +1,24 @@
+# 目标：规划组名称
+string planning_group
+# 关节名称列表
+string[] joint_names
+# 目标关节位置（弧度）
+float64[] target_positions
+# 运动速度缩放因子 (0-1)
+float64 speed = 0.5
+# 运动加速度缩放因子 (0-1)
+float64 acceleration = 0.5
+
+---
+# 结果：是否成功
+bool success
+# 错误信息（如失败）
+string error_message
+# 最终关节位置
+float64[] final_joint_positions
+
+---
+# 反馈：当前关节位置
+float64[] current_joint_positions
+# 完成百分比
+float64 progress

action/MoveL.action

@@ -0,0 +1,26 @@
+# 目标：规划组名称
+string planning_group
+# 末端执行器帧名称
+string frame_name
+# 目标位姿
+geometry_msgs/Pose target_pose
+# 参考坐标系
+string reference_frame = "base_link"
+# 运动速度缩放因子 (0-1)
+float64 speed = 0.5
+# 运动加速度缩放因子 (0-1)
+float64 acceleration = 0.5
+
+---
+# 结果：是否成功
+bool success
+# 错误信息（如失败）
+string error_message
+# 最终位姿
+geometry_msgs/Pose final_pose
+
+---
+# 反馈：当前位姿
+geometry_msgs/Pose current_pose
+# 完成百分比
+float64 progress

config/kinematics.yaml

@@ -0,0 +1,35 @@
+left_arm_group:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3
+
+right_arm_group:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3
+
+left_frount_leg_group:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3
+
+left_behind_leg_group:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3
+
+right_frount_leg_group:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3
+
+right_behind_leg_group:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3

config/srdf/robot.srdf

@@ -0,0 +1,107 @@
+<?xml version="1.0" ?>
+<robot name="dual_arm_leg_robot">
+  <!-- 左右手臂和腿部的规划组定义 -->
+  <group name="left_arm_group">
+    <joint name="left_arm_joint1"/>
+    <joint name="left_arm_joint2"/>
+    <joint name="left_arm_joint3"/>
+    <joint name="left_arm_joint4"/>
+    <joint name="left_arm_joint5"/>
+    <joint name="left_arm_joint6"/>
+    <link name="left_arm_link1"/>
+    <link name="left_arm_link2"/>
+    <link name="left_arm_link3"/>
+    <link name="left_arm_link4"/>
+    <link name="left_arm_link5"/>
+    <link name="left_arm_end_effector"/>
+  </group>
+
+  <group name="right_arm_group">
+    <joint name="right_arm_joint1"/>
+    <joint name="right_arm_joint2"/>
+    <joint name="right_arm_joint3"/>
+    <joint name="right_arm_joint4"/>
+    <joint name="right_arm_joint5"/>
+    <joint name="right_arm_joint6"/>
+    <link name="right_arm_link1"/>
+    <link name="right_arm_link2"/>
+    <link name="right_arm_link3"/>
+    <link name="right_arm_link4"/>
+    <link name="right_arm_link5"/>
+    <link name="right_arm_end_effector"/>
+  </group>
+
+  <group name="left_front_leg_group">
+    <joint name="left_leg_joint1"/>
+    <joint name="left_leg_joint2"/>
+    <joint name="left_leg_joint3"/>
+    <link name="left_leg_link1"/>
+    <link name="left_leg_link2"/>
+    <link name="left_foot"/>
+  </group>
+
+  <group name="left_behind_leg_group">
+    <joint name="left_behind_leg_joint1"/>
+    <joint name="left_behind_leg_joint2"/>
+    <joint name="left_behind_leg_joint3"/>
+    <link name="left_behind_leg_link1"/>
+    <link name="left_behind_leg_link2"/>
+    <link name="left_behind_foot"/>
+  </group>
+
+  <group name="right_frount_leg_group">
+    <joint name="right_leg_joint1"/>
+    <joint name="right_leg_joint2"/>
+    <joint name="right_leg_joint3"/>
+    <link name="right_leg_link1"/>
+    <link name="right_leg_link2"/>
+    <link name="right_foot"/>
+  </group>
+
+  <!-- 末端执行器定义 -->
+  <end_effector name="left_end_effector" parent_link="left_arm_end_effector" group="left_arm_group">
+    <touch_links>
+      <link name="left_arm_end_effector"/>
+    </touch_links>
+  </end_effector>
+
+  <end_effector name="right_end_effector" parent_link="right_arm_end_effector" group="right_arm_group">
+    <touch_links>
+      <link name="right_arm_end_effector"/>
+    </touch_links>
+  </end_effector>
+
+  <!-- 默认姿态 -->
+  <group_state name="home" group="left_arm_group">
+    <joint name="left_arm_joint1" value="0"/>
+    <joint name="left_arm_joint2" value="0"/>
+    <joint name="left_arm_joint3" value="0"/>
+    <joint name="left_arm_joint4" value="0"/>
+    <joint name="left_arm_joint5" value="0"/>
+    <joint name="left_arm_joint6" value="0"/>
+  </group_state>
+
+  <group_state name="home" group="right_arm_group">
+    <joint name="right_arm_joint1" value="0"/>
+    <joint name="right_arm_joint2" value="0"/>
+    <joint name="right_arm_joint3" value="0"/>
+    <joint name="right_arm_joint4" value="0"/>
+    <joint name="right_arm_joint5" value="0"/>
+    <joint name="right_arm_joint6" value="0"/>
+  </group_state>
+
+  <group_state name="home" group="left_leg_group">
+    <joint name="left_leg_joint1" value="0"/>
+    <joint name="left_leg_joint2" value="0.5"/>
+    <joint name="left_leg_joint3" value="0"/>
+  </group_state>
+
+  <group_state name="home" group="right_leg_group">
+    <joint name="right_leg_joint1" value="0"/>
+    <joint name="right_leg_joint2" value="0.5"/>
+    <joint name="right_leg_joint3" value="0"/>
+  </group_state>
+
+  <!-- 虚拟关节（可选） -->
+  <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="base_link"/>
+</robot>

include/RobotControlManager.hpp

@@ -0,0 +1,83 @@
+#ifndef ROBOT_CONTROL_SYSTEM_H
+#define ROBOT_CONTROL_SYSTEM_H
+
+#include "robot_control/ros/RobotMotionAction.h"
+#include "robot_control/planning/RealtimeInterpolator.h"
+#include "robot_control/state_machine/RobotStateMachine.h"
+#include "robot_control/model/RobotModel.h"
+#include <rclcpp/rclcpp.hpp>
+
+namespace robot_control {
+
+class RobotControlManager {
+public:
+    using Ptr = std::shared_ptr<RobotControlManager>;
+    using ConstPtr = std::shared_ptr<const RobotControlManager>;
+
+    RobotControlManager(rclcpp::Node::SharedPtr node);
+    ~RobotControlManager() = default;
+    
+    // 初始化系统
+    bool initialize(const std::string& upperBodyUrdf, const std::string& lowerBodyUrdf);
+    
+    // 启动系统
+    void start();
+    
+    // 停止系统
+    void stop();
+    
+    // 更新系统状态
+    void update();
+    
+    // 获取当前状态
+    state_machine::RobotState getCurrentState() const;
+    
+    // 处理运动目标
+    void handleMotionGoal(const ros::MotionGoal& goal);
+    
+    // 处理运动取消
+    void handleMotionCancel();
+
+private:
+    rclcpp::Node::SharedPtr node_;
+    
+    // 模型
+    model::UpperBodyModel::Ptr upperBodyModel_;
+    model::LowerBodyModel::Ptr lowerBodyModel_;
+    
+    // 控制器
+    control::UpperBodyController::Ptr upperBodyController_;
+    control::LowerBodyController::Ptr lowerBodyController_;
+    
+    // ROS通信
+    ros::RobotMotionAction::Ptr motionAction_;
+    
+    // 实时插补器
+    planning::RealtimeInterpolator::Ptr upperBodyInterpolator_;
+    planning::RealtimeInterpolator::Ptr lowerBodyInterpolator_;
+    
+    // 状态机
+    state_machine::RobotStateMachine::Ptr stateMachine_;
+    
+    // 控制频率
+    double controlFrequency_;
+    
+    // 定时器
+    rclcpp::TimerBase::SharedPtr controlTimer_;
+    
+    // 状态机回调函数
+    void registerStateCallbacks();
+    
+    // 控制回调
+    void controlCallback();
+    
+    // 发布反馈
+    void publishFeedback();
+    
+    // 处理状态转换
+    void onStateTransition(state_machine::RobotState newState);
+};
+
+} // namespace robot_control
+
+#endif // ROBOT_CONTROL_SYSTEM_H

include/control/ArmController.hpp

@@ -0,0 +1,39 @@
+#ifndef ARM_CONTROLLER_H
+#define ARM_CONTROLLER_H
+
+#include "ControllerBase.hpp"
+
+namespace control {
+
+class UpperBodyController : public ControllerBase {
+public:
+    using Ptr = std::shared_ptr<UpperBodyController>;
+    using ConstPtr = std::shared_ptr<const UpperBodyController>;
+
+    UpperBodyController(model::UpperBodyModel::Ptr model);
+    ~UpperBodyController() override = default;
+
+    // 从基类继承的方法
+    ocs2::vector_t computeControl(const ocs2::scalar_t t, const ocs2::vector_t& x) override;
+    void reset() override;
+    bool isGoalReached(const ocs2::vector_t& x) const override;
+    
+    // 设置目标轨迹
+    void setTargetTrajectories(const ocs2::TargetTrajectories& targetTrajectories);
+    
+    // 设置单个手臂目标位置
+    void setArmTargetPosition(bool leftArm, const Eigen::Vector3d& targetPos);
+
+private:
+    model::UpperBodyModel::Ptr model_;
+    std::unique_ptr<ocs2::Ocs2Solver> solver_;
+    ocs2::TargetTrajectories targetTrajectories_;
+    
+    // 控制器参数
+    double positionTolerance_ = 0.01; // 位置容差 (m)
+    double velocityTolerance_ = 0.05; // 速度容差 (m/s)
+};
+
+} // namespace control
+
+#endif // UPPER_BODY_CONTROLLER_H

include/control/ControllerBase.hpp

@@ -0,0 +1,31 @@
+#ifndef CONTROLLER_BASE_H
+#define CONTROLLER_BASE_H
+
+#include <memory>
+#include <Eigen/Dense>
+
+namespace robot_control {
+namespace control {
+
+class ControllerBase {
+public:
+    using Ptr = std::shared_ptr<ControllerBase>;
+    using ConstPtr = std::shared_ptr<const ControllerBase>;
+
+    ControllerBase() = default;
+    ~ControllerBase() = default;
+
+    // 计算控制输入
+    Eigen::VectorXd computeControl(const  t, const  x);
+    
+    // 重置控制器
+    virtual void reset() = 0;
+    
+    // 检查是否到达目标
+    virtual bool isGoalReached(const ocs2::vector_t& x) const = 0;
+};
+
+} // namespace control
+} // namespace robot_control
+
+#endif // CONTROLLER_BASE_H

include/control/LegController.hpp

@@ -0,0 +1,48 @@
+#ifndef LOWER_BODY_CONTROLLER_H
+#define LOWER_BODY_CONTROLLER_H
+
+#include "robot_control/control/ControllerBase.h"
+#include "robot_control/model/LowerBodyModel.h"
+#include <ocs2_oc/oc_solver/Ocs2Solver.h>
+#include <ocs2_core/reference/TargetTrajectories.h>
+
+namespace robot_control {
+namespace control {
+
+class LowerBodyController : public ControllerBase {
+public:
+    using Ptr = std::shared_ptr<LowerBodyController>;
+    using ConstPtr = std::shared_ptr<const LowerBodyController>;
+
+    LowerBodyController(model::LowerBodyModel::Ptr model);
+    ~LowerBodyController() override = default;
+
+    // 从基类继承的方法
+    ocs2::vector_t computeControl(const ocs2::scalar_t t, const ocs2::vector_t& x) override;
+    void reset() override;
+    bool isGoalReached(const ocs2::vector_t& x) const override;
+    
+    // 设置目标轨迹
+    void setTargetTrajectories(const ocs2::TargetTrajectories& targetTrajectories);
+    
+    // 设置轮子速度
+    void setWheelVelocities(const Eigen::Vector4d& wheelVels);
+    
+    // 设置单腿目标位置
+    void setLegTargetPosition(int legIndex, const Eigen::Vector3d& targetPos);
+
+private:
+    model::LowerBodyModel::Ptr model_;
+    std::unique_ptr<ocs2::Ocs2Solver> solver_;
+    ocs2::TargetTrajectories targetTrajectories_;
+    Eigen::Vector4d wheelVelocities_;
+    
+    // 控制器参数
+    double positionTolerance_ = 0.01; // 位置容差 (m)
+    double velocityTolerance_ = 0.05; // 速度容差 (m/s)
+};
+
+} // namespace control
+} // namespace robot_control
+
+#endif // LOWER_BODY_CONTROLLER_H

include/model/RobotModel.hpp

@@ -0,0 +1,32 @@
+#ifndef ROBOT_MODEL_H
+#define ROBOT_MODEL_H
+
+#include <string>
+#include <memory>
+
+namespace robot_control {
+namespace model {
+
+class RobotModel {
+public:
+    using Ptr = std::shared_ptr<RobotModel>;
+    using ConstPtr = std::shared_ptr<const RobotModel>;
+
+    RobotModel(const std::string& urdfPath);
+    virtual ~RobotModel() = default;
+
+    const pinocchio::Model& getModel() const { return model_; }
+    pinocchio::Data& getData() { return data_; }
+    const pinocchio::Data& getData() const { return data_; }
+
+    virtual void update(const Eigen::VectorXd& q, const Eigen::VectorXd& v) const;
+
+protected:
+    pinocchio::Model model_;
+    mutable pinocchio::Data data_;
+};
+
+} // namespace model
+} // namespace robot_control
+
+#endif // ROBOT_MODEL_H

include/nodes/MotionControlNode.hpp

@@ -0,0 +1,113 @@
+#ifndef QUADRUPED_MANIPULATOR_CONTROL__MOTION_CONTROL_NODE_HPP_
+#define QUADRUPED_MANIPULATOR_CONTROL__MOTION_CONTROL_NODE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <controller_manager_msgs/srv/list_controllers.hpp>
+#include <controller_manager_msgs/srv/switch_controller.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <trajectory_msgs/msg/joint_trajectory_point.hpp>
+
+#include "quadruped_manipulator_control/planning/moveit_planner.hpp"
+#include "quadruped_manipulator_control/action/move_j.hpp"
+#include "quadruped_manipulator_control/action/move_l.hpp"
+#include "quadruped_manipulator_control/action/joint_movement.hpp"
+
+namespace quadruped_manipulator_control
+{
+
+class MotionControlNode : public rclcpp::Node
+{
+public:
+  using MoveJ = quadruped_manipulator_control::action::MoveJ;
+  using GoalHandleMoveJ = rclcpp_action::ServerGoalHandle<MoveJ>;
+  
+  using MoveL = quadruped_manipulator_control::action::MoveL;
+  using GoalHandleMoveL = rclcpp_action::ServerGoalHandle<MoveL>;
+  
+  using JointMovement = quadruped_manipulator_control::action::JointMovement;
+  using GoalHandleJointMovement = rclcpp_action::ServerGoalHandle<JointMovement>;
+
+  explicit MotionControlNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+  ~MotionControlNode() override = default;
+
+private:
+  // Action服务器
+  rclcpp_action::Server<MoveJ>::SharedPtr action_server_movej_;
+  rclcpp_action::Server<MoveL>::SharedPtr action_server_movel_;
+  rclcpp_action::Server<JointMovement>::SharedPtr action_server_joint_movement_;
+  
+  // 规划器 (仅保留MoveIt)
+  std::shared_ptr<MoveItPlanner> moveit_planner_;
+  
+  // 控制器客户端
+  rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr list_controllers_client_;
+  rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_client_;
+  rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_publisher_;
+  
+  // 参数
+  std::string controller_name_;
+  std::string urdf_path_;
+  double default_speed_;
+  double default_acceleration_;
+  
+  // Action回调
+  rclcpp_action::GoalResponse handle_goal_movej(
+    const rclcpp_action::GoalUUID& uuid,
+    std::shared_ptr<const MoveJ::Goal> goal);
+    
+  rclcpp_action::CancelResponse handle_cancel_movej(
+    const std::shared_ptr<GoalHandleMoveJ> goal_handle);
+    
+  void handle_accepted_movej(
+    const std::shared_ptr<GoalHandleMoveJ> goal_handle);
+    
+  rclcpp_action::GoalResponse handle_goal_movel(
+    const rclcpp_action::GoalUUID& uuid,
+    std::shared_ptr<const MoveL::Goal> goal);
+    
+  rclcpp_action::CancelResponse handle_cancel_movel(
+    const std::shared_ptr<GoalHandleMoveL> goal_handle);
+    
+  void handle_accepted_movel(
+    const std::shared_ptr<GoalHandleMoveL> goal_handle);
+    
+  rclcpp_action::GoalResponse handle_goal_joint_movement(
+    const rclcpp_action::GoalUUID& uuid,
+    std::shared_ptr<const JointMovement::Goal> goal);
+    
+  rclcpp_action::CancelResponse handle_cancel_joint_movement(
+    const std::shared_ptr<GoalHandleJointMovement> goal_handle);
+    
+  void handle_accepted_joint_movement(
+    const std::shared_ptr<GoalHandleJointMovement> goal_handle);
+  
+  // 执行函数
+  void execute_movej(const std::shared_ptr<GoalHandleMoveJ> goal_handle);
+  void execute_movel(const std::shared_ptr<GoalHandleMoveL> goal_handle);
+  void execute_joint_movement(const std::shared_ptr<GoalHandleJointMovement> goal_handle);
+  
+  // 辅助函数
+  bool switch_controller(const std::string& controller_name);
+  bool send_joint_trajectory(
+    const std::vector<std::string>& joint_names,
+    const std::vector<trajectory_msgs::msg::JointTrajectoryPoint>& points);
+  double calculate_trajectory_time(
+    const std::vector<double>& start_positions,
+    const std::vector<double>& end_positions,
+    double max_velocity,
+    double max_acceleration);
+    
+  // 从MoveIt获取关节限位
+  bool get_joint_limits(
+    const std::string& joint_name,
+    double& min_position,
+    double& max_position);
+};
+
+}  // namespace quadruped_manipulator_control
+
+#endif  // QUADRUPED_MANIPULATOR_CONTROL__MOTION_CONTROL_NODE_HPP_

include/nodes/RobotMotionAction.hpp

@@ -0,0 +1,131 @@
+#ifndef ROBOT_MOTION_ACTION_H
+#define ROBOT_MOTION_ACTION_H
+
+#include <action_msgs/msg/goal_status.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <Eigen/Dense>
+#include <string>
+
+// 定义Action消息类型
+namespace robot_control {
+namespace ros {
+
+// 运动目标
+struct MotionGoal {
+    // 运动类型：移动、操作、姿态调整等
+    enum class Type {
+        MOVE_BASE,        // 移动基座
+        ARM_MOTION,       // 手臂运动
+        LEG_MOTION,       // 腿部运动
+        BODY_POSE,        // 身体姿态
+        WHOLE_BODY_MOTION // 全身运动
+    };
+
+    Type type;
+    
+    // 基础移动目标 (用于MOVE_BASE类型)
+    struct BaseTarget {
+        double x;     // x位置 (m)
+        double y;     // y位置 (m)
+        double yaw;   // 偏航角 (rad)
+        double speed; // 移动速度 (m/s)
+    };
+    
+    // 手臂运动目标 (用于ARM_MOTION类型)
+    struct ArmTarget {
+        bool leftArm;          // 是否是左臂
+        Eigen::Vector3d position; // 位置目标 (m)
+        Eigen::Vector3d orientation; // 姿态目标 (rad)
+        bool relative;         // 是否是相对运动
+    };
+    
+    // 腿部运动目标 (用于LEG_MOTION类型)
+    struct LegTarget {
+        int legIndex;          // 腿索引 (0-3)
+        Eigen::Vector3d position; // 位置目标 (m)
+        bool relative;         // 是否是相对运动
+    };
+    
+    // 身体姿态目标 (用于BODY_POSE类型)
+    struct BodyPoseTarget {
+        double roll;  // 横滚角 (rad)
+        double pitch; // 俯仰角 (rad)
+        double yaw;   // 偏航角 (rad)
+        double height; // 高度 (m)
+    };
+
+    // 根据运动类型选择对应的目标数据
+    union {
+        BaseTarget base;
+        ArmTarget arm;
+        LegTarget leg;
+        BodyPoseTarget bodyPose;
+    } target;
+    
+    // 运动参数
+    double duration; // 期望运动时间 (s)，0表示使用默认
+    bool blocking;   // 是否阻塞等待完成
+};
+
+// 运动反馈
+struct MotionFeedback {
+    double progress; // 进度 (0.0-1.0)
+    Eigen::VectorXd currentState; // 当前状态
+    std::string status; // 状态描述
+};
+
+// 运动结果
+struct MotionResult {
+    bool success;    // 是否成功
+    std::string message; // 结果消息
+    double actualDuration; // 实际运动时间 (s)
+};
+
+// Action接口类
+class RobotMotionAction {
+public:
+    using Ptr = std::shared_ptr<RobotMotionAction>;
+    
+    using GoalHandle = rclcpp_action::ServerGoalHandle<robot_control_msgs::action::RobotMotion>;
+    
+    RobotMotionAction(rclcpp::Node::SharedPtr node);
+    
+    // 发送目标
+    void sendGoal(const MotionGoal& goal);
+    
+    // 取消目标
+    void cancelGoal();
+    
+    // 注册目标回调
+    using GoalCallback = std::function<void(const MotionGoal&)>;
+    void setGoalCallback(GoalCallback callback);
+    
+    // 注册取消回调
+    using CancelCallback = std::function<void()>;
+    void setCancelCallback(CancelCallback callback);
+    
+    // 发布反馈
+    void publishFeedback(const MotionFeedback& feedback);
+    
+    // 发布结果
+    void publishResult(GoalHandle::SharedPtr goalHandle, const MotionResult& result);
+
+private:
+    rclcpp::Node::SharedPtr node_;
+    rclcpp_action::Server<robot_control_msgs::action::RobotMotion>::SharedPtr actionServer_;
+    
+    GoalCallback goalCallback_;
+    CancelCallback cancelCallback_;
+    
+    // Action回调函数
+    rclcpp_action::GoalResponse handleGoal(const rclcpp_action::GoalUUID& uuid, 
+                                          std::shared_ptr<const robot_control_msgs::action::RobotMotion::Goal> goal);
+    rclcpp_action::CancelResponse handleCancel(const std::shared_ptr<GoalHandle> goalHandle);
+    void handleAccepted(const std::shared_ptr<GoalHandle> goalHandle);
+};
+
+} // namespace ros
+} // namespace robot_control
+
+#endif // ROBOT_MOTION_ACTION_H

include/planner/MoveitPlanner.hpp

@@ -0,0 +1,113 @@
+#include "quadruped_manipulator_control/planning/moveit_planner.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace quadruped_manipulator_control
+{
+
+MoveItPlanner::MoveItPlanner(
+  const rclcpp::Node::SharedPtr& node,
+  const std::string& planning_group)
+  : node_(node), planning_group_(planning_group)
+{
+  // 初始化机器人模型加载器
+  robot_model_loader_ = std::make_shared<robot_model_loader::RobotModelLoader>(node_, "robot_description");
+  
+  // 初始化运动规划组
+  move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, planning_group_);
+  
+  // 设置默认规划参数
+  set_planning_parameters();
+  
+  RCLCPP_INFO(node_->get_logger(), "MoveItPlanner initialized for planning group: %s", planning_group_.c_str());
+}
+
+void MoveItPlanner::set_planning_group(const std::string& planning_group)
+{
+  planning_group_ = planning_group;
+  move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, planning_group_);
+  set_planning_parameters();
+  RCLCPP_INFO(node_->get_logger(), "Switched to planning group: %s", planning_group_.c_str());
+}
+
+bool MoveItPlanner::plan_joint_goal(
+  const std::vector<double>& joint_positions,
+  moveit::planning_interface::MoveGroupInterface::Plan& plan)
+{
+  if (joint_positions.empty()) {
+    RCLCPP_ERROR(node_->get_logger(), "Joint positions vector is empty");
+    return false;
+  }
+
+  // 设置关节目标
+  move_group_->setJointValueTarget(joint_positions);
+  
+  // 规划运动
+  moveit::planning_interface::MoveItErrorCode result = move_group_->plan(plan);
+  
+  if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
+    RCLCPP_INFO(node_->get_logger(), "Successfully planned joint space motion");
+    return true;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to plan joint space motion. Error code: %d", result.val);
+    return false;
+  }
+}
+
+bool MoveItPlanner::plan_pose_goal(
+  const geometry_msgs::msg::Pose& target_pose,
+  moveit::planning_interface::MoveGroupInterface::Plan& plan)
+{
+  // 设置位姿目标
+  move_group_->setPoseTarget(target_pose);
+  
+  // 规划运动
+  moveit::planning_interface::MoveItErrorCode result = move_group_->plan(plan);
+  
+  if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
+    RCLCPP_INFO(node_->get_logger(), "Successfully planned Cartesian space motion");
+    return true;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to plan Cartesian space motion. Error code: %d", result.val);
+    return false;
+  }
+}
+
+bool MoveItPlanner::execute_plan(const moveit::planning_interface::MoveGroupInterface::Plan& plan)
+{
+  moveit::planning_interface::MoveItErrorCode result = move_group_->execute(plan);
+  
+  if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
+    RCLCPP_INFO(node_->get_logger(), "Successfully executed planned motion");
+    return true;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to execute planned motion. Error code: %d", result.val);
+    return false;
+  }
+}
+
+std::vector<double> MoveItPlanner::get_current_joint_positions()
+{
+  return move_group_->getCurrentJointValues();
+}
+
+geometry_msgs::msg::Pose MoveItPlanner::get_current_end_effector_pose()
+{
+  return move_group_->getCurrentPose().pose;
+}
+
+void MoveItPlanner::set_planning_parameters(
+  double planning_time,
+  double goal_tolerance,
+  double max_velocity_scaling,
+  double max_acceleration_scaling)
+{
+  move_group_->setPlanningTime(planning_time);
+  move_group_->setGoalTolerance(goal_tolerance);
+  move_group_->setMaxVelocityScalingFactor(max_velocity_scaling);
+  move_group_->setMaxAccelerationScalingFactor(max_acceleration_scaling);
+  
+  RCLCPP_INFO(node_->get_logger(), "Set planning parameters: time=%.2fs, tolerance=%.4fm, velocity=%.2f, acceleration=%.2f",
+              planning_time, goal_tolerance, max_velocity_scaling, max_acceleration_scaling);
+}
+
+}  // namespace quadruped_manipulator_control

include/planner/RealtimeInterpolator.hpp

@@ -0,0 +1,85 @@
+#ifndef REALTIME_INTERPOLATOR_H
+#define REALTIME_INTERPOLATOR_H
+
+#include <Eigen/Dense>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <atomic>
+#include <functional>
+
+namespace robot_control {
+namespace planning {
+
+class RealtimeInterpolator {
+public:
+    using Ptr = std::shared_ptr<RealtimeInterpolator>;
+    using ConstPtr = std::shared_ptr<const RealtimeInterpolator>;
+    
+    // 插值类型
+    enum class InterpolationType {
+        LINEAR,    // 线性插值
+        CUBIC,     // 三次多项式插值
+        QUINTIC    // 五次多项式插值
+    };
+    
+    RealtimeInterpolator(double controlFrequency = 1000.0);
+    ~RealtimeInterpolator();
+    
+    // 设置目标轨迹
+    void setTargetTrajectories(const ocs2::TargetTrajectories& trajectories, 
+                              InterpolationType type = InterpolationType::CUBIC);
+    
+    // 获取当前参考点
+    bool getCurrentReference(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    // 停止插值线程
+    void stop();
+    
+    // 注册控制输出回调
+    using ControlCallback = std::function<void(double, const ocs2::vector_t&, const ocs2::vector_t&)>;
+    void setControlCallback(ControlCallback callback);
+    
+    // 检查是否正在执行轨迹
+    bool isActive() const { return isActive_; }
+    
+    // 检查是否到达目标
+    bool isGoalReached() const { return isGoalReached_; }
+
+private:
+    // 插值线程主函数
+    void interpolationThread();
+    
+    // 根据类型进行插值计算
+    void interpolate(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    // 线性插值
+    void linearInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    // 三次多项式插值
+    void cubicInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    // 五次多项式插值
+    void quinticInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref);
+    
+    ocs2::TargetTrajectories trajectories_;
+    InterpolationType interpolationType_;
+    
+    double controlFrequency_;
+    double dt_;
+    
+    std::thread interpolationThread_;
+    std::mutex mutex_;
+    std::condition_variable cv_;
+    
+    std::atomic<bool> running_;
+    std::atomic<bool> isActive_;
+    std::atomic<bool> isGoalReached_;
+    
+    ControlCallback controlCallback_;
+};
+
+} // namespace planning
+} // namespace robot_control
+
+#endif // REALTIME_INTERPOLATOR_H

include/state_machine/RobotStateMachine.hpp

@@ -0,0 +1,92 @@
+#ifndef ROBOT_STATE_MACHINE_H
+#define ROBOT_STATE_MACHINE_H
+
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <functional>
+
+namespace robot_control {
+namespace state_machine {
+
+// 机器人状态定义
+enum class RobotState {
+    IDLE,           // 空闲
+    MOVING_BASE,    // 基座移动
+    ARM_MOTION,     // 手臂运动
+    LEG_MOTION,     // 腿部运动
+    BODY_ADJUST,    // 身体调整
+    TRANSITION,     // 状态过渡
+    ERROR           // 错误状态
+};
+
+// 事件定义
+enum class Event {
+    START_MOVE_BASE,
+    START_ARM_MOTION,
+    START_LEG_MOTION,
+    START_BODY_ADJUST,
+    MOTION_COMPLETED,
+    MOTION_FAILED,
+    STOP_MOTION,
+    ERROR_OCCURRED,
+    RECOVER_FROM_ERROR
+};
+
+// 状态转换函数
+using TransitionFunction = std::function<RobotState(const RobotState&, const Event&)>;
+
+// 状态进入/退出回调
+using StateCallback = std::function<void()>;
+
+class RobotStateMachine {
+public:
+    using Ptr = std::shared_ptr<RobotStateMachine>;
+    using ConstPtr = std::shared_ptr<const RobotStateMachine>;
+
+    RobotStateMachine();
+    
+    // 获取当前状态
+    RobotState getCurrentState() const { return currentState_; }
+    
+    // 处理事件
+    void handleEvent(Event event);
+    
+    // 注册状态转换规则
+    void registerTransition(RobotState fromState, Event event, RobotState toState);
+    
+    // 注册状态进入回调
+    void registerEnterCallback(RobotState state, StateCallback callback);
+    
+    // 注册状态退出回调
+    void registerExitCallback(RobotState state, StateCallback callback);
+    
+    // 获取状态的字符串表示
+    std::string stateToString(RobotState state) const;
+    
+    // 获取事件的字符串表示
+    std::string eventToString(Event event) const;
+
+private:
+    RobotState currentState_;
+    
+    // 状态转换表: fromState -> event -> toState
+    std::unordered_map<RobotState, std::unordered_map<Event, RobotState>> transitions_;
+    
+    // 状态进入回调
+    std::unordered_map<RobotState, StateCallback> enterCallbacks_;
+    
+    // 状态退出回调
+    std::unordered_map<RobotState, StateCallback> exitCallbacks_;
+    
+    // 检查是否允许转换
+    bool canTransition(RobotState fromState, Event event) const;
+    
+    // 执行状态转换
+    void transitionTo(RobotState newState);
+};
+
+} // namespace state_machine
+} // namespace robot_control
+
+#endif // ROBOT_STATE_MACHINE_H

launch/robot_motion.launch.py

@@ -0,0 +1,83 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+import os
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    # 获取包路径
+    package_dir = get_package_share_directory('dual_arm_leg_robot')
+    
+    # 文件路径
+    urdf_path = os.path.join(package_dir, 'urdf', 'dual_arm_leg_robot.urdf')
+    srdf_path = os.path.join(package_dir, 'config', 'srdf', 'dual_arm_leg_robot.srdf')
+    kinematics_path = os.path.join(package_dir, 'config', 'kinematics.yaml')
+    rviz_config_path = os.path.join(package_dir, 'config', 'moveit.rviz')
+    
+    # 声明参数
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    
+    # 机器人状态发布器
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{
+            'robot_description': open(urdf_path).read(),
+            'use_sim_time': use_sim_time
+        }]
+    )
+    
+    # 关节状态发布器
+    joint_state_publisher = Node(
+        package='joint_state_publisher_gui',
+        executable='joint_state_publisher_gui',
+        name='joint_state_publisher_gui',
+        output='screen'
+    )
+    
+    # MoveIt规划节点
+    move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        name='move_group',
+        output='screen',
+        parameters=[{
+            'robot_description': open(urdf_path).read(),
+            'robot_description_semantic': open(srdf_path).read(),
+            'robot_description_kinematics': kinematics_path,
+            'use_sim_time': use_sim_time
+        }]
+    )
+    
+    # 运动学演示节点
+    kinematics_demo = Node(
+        package='dual_arm_leg_robot',
+        executable='kinematics_demo',
+        name='kinematics_demo',
+        output='screen'
+    )
+    
+    # RViz可视化
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', rviz_config_path]
+    )
+    
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='是否使用仿真时间'
+        ),
+        robot_state_publisher,
+        joint_state_publisher,
+        move_group_node,
+        kinematics_demo,
+        rviz_node
+    ])

package.xml

@@ -0,0 +1,33 @@
+<?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>hivecore_robot_motion</name>
+  <version>1.0.0</version>
+  <description>hivecore_robot_motion package for HiveCore robot</description>
+  <maintainer email="ray@hivecore.cn">ray</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  
+  <!-- ROS2核心依赖 -->
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  
+  <!-- Action依赖 -->
+  <depend>action_msgs</depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  
+  <!-- MoveIt!依赖 -->
+  <depend>moveit_core</depend>
+  <depend>moveit_ros_planning</depend>
+  <depend>moveit_ros_planning_interface</depend>
+
+</package>

src/RobotControlSystem.cpp

@@ -0,0 +1,222 @@
+#include "robot_control/RobotControlSystem.h"
+#include "robot_control/state_machine/RobotStateMachine.h"
+#include <chrono>
+#include <thread>
+
+namespace robot_control {
+
+RobotControlSystem::RobotControlSystem(rclcpp::Node::SharedPtr node) 
+    : node_(node),
+      controlFrequency_(1000.0) {
+    // 从参数服务器获取控制频率
+    node_->get_parameter_or("control_frequency", controlFrequency_, 1000.0);
+    
+    // 初始化组件
+    motionAction_ = std::make_shared<ros::RobotMotionAction>(node_);
+    stateMachine_ = std::make_shared<state_machine::RobotStateMachine>();
+    
+    // 创建插值器
+    upperBodyInterpolator_ = std::make_shared<planning::RealtimeInterpolator>(controlFrequency_);
+    lowerBodyInterpolator_ = std::make_shared<planning::RealtimeInterpolator>(controlFrequency_);
+    
+    // 设置Action回调
+    motionAction_->setGoalCallback(
+        std::bind(&RobotControlSystem::handleMotionGoal, this, std::placeholders::_1)
+    );
+    
+    motionAction_->setCancelCallback(
+        std::bind(&RobotControlSystem::handleMotionCancel, this)
+    );
+    
+    // 注册状态回调
+    registerStateCallbacks();
+}
+
+bool RobotControlSystem::initialize(const std::string& upperBodyUrdf, const std::string& lowerBodyUrdf) {
+    try {
+        // 加载机器人模型
+        upperBodyModel_ = std::make_shared<model::UpperBodyModel>(upperBodyUrdf);
+        lowerBodyModel_ = std::make_shared<model::LowerBodyModel>(lowerBodyUrdf);
+        
+        // 初始化控制器
+        upperBodyController_ = std::make_shared<control::UpperBodyController>(upperBodyModel_);
+        lowerBodyController_ = std::make_shared<control::LowerBodyController>(lowerBodyModel_);
+        
+        // 初始化状态向量
+        upperBodyState_.setZero(model::UpperBodyModel::NUM_JOINTS * 2); // 位置 + 速度
+        lowerBodyState_.setZero(model::LowerBodyModel::NUM_LEG_JOINTS * 2 + 
+                               model::LowerBodyModel::NUM_WHEELS); // 腿部位置+速度 + 轮子速度
+        
+        upperBodyCommand_.setZero(model::UpperBodyModel::NUM_JOINTS);
+        lowerBodyCommand_.setZero(model::LowerBodyModel::NUM_LEG_JOINTS + 
+                                 model::LowerBodyModel::NUM_WHEELS);
+        
+        RCLCPP_INFO(node_->get_logger(), "Robot control system initialized successfully");
+        return true;
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(node_->get_logger(), "Failed to initialize robot control system: %s", e.what());
+        return false;
+    }
+}
+
+void RobotControlSystem::start() {
+    // 设置控制定时器
+    auto period = std::chrono::duration<double>(1.0 / controlFrequency_);
+    controlTimer_ = node_->create_wall_timer(
+        period,
+        std::bind(&RobotControlSystem::controlCallback, this)
+    );
+    
+    RCLCPP_INFO(node_->get_logger(), "Robot control system started");
+}
+
+void RobotControlSystem::stop() {
+    controlTimer_->cancel();
+    upperBodyInterpolator_->stop();
+    lowerBodyInterpolator_->stop();
+    
+    // 停止所有运动
+    upperBodyController_->reset();
+    lowerBodyController_->reset();
+    
+    RCLCPP_INFO(node_->get_logger(), "Robot control system stopped");
+}
+
+void RobotControlSystem::update() {
+    // 在实际实现中，这里会从硬件接口读取当前状态
+    // 这里仅作为示例
+}
+
+state_machine::RobotState RobotControlSystem::getCurrentState() const {
+    return stateMachine_->getCurrentState();
+}
+
+void RobotControlSystem::handleMotionGoal(const ros::MotionGoal& goal) {
+    RCLCPP_INFO(node_->get_logger(), "Received motion goal: %s", 
+               stateMachine_->eventToString(static_cast<state_machine::Event>(goal.type)).c_str());
+    
+    // 根据目标类型转换为相应事件
+    state_machine::Event event;
+    switch (goal.type) {
+        case ros::MotionGoal::Type::MOVE_BASE:
+            event = state_machine::Event::START_MOVE_BASE;
+            // 设置基座移动目标
+            // ...
+            break;
+        case ros::MotionGoal::Type::ARM_MOTION:
+            event = state_machine::Event::START_ARM_MOTION;
+            // 设置手臂运动目标
+            upperBodyController_->setArmTargetPosition(
+                goal.target.arm.leftArm, 
+                goal.target.arm.position
+            );
+            break;
+        case ros::MotionGoal::Type::LEG_MOTION:
+            event = state_machine::Event::START_LEG_MOTION;
+            // 设置腿部运动目标
+            lowerBodyController_->setLegTargetPosition(
+                goal.target.leg.legIndex,
+                goal.target.leg.position
+            );
+            break;
+        case ros::MotionGoal::Type::BODY_POSE:
+            event = state_machine::Event::START_BODY_ADJUST;
+            // 设置身体姿态目标
+            // ...
+            break;
+        default:
+            RCLCPP_WARN(node_->get_logger(), "Unknown motion goal type");
+            return;
+    }
+    
+    // 处理事件
+    stateMachine_->handleEvent(event);
+}
+
+void RobotControlSystem::handleMotionCancel() {
+    RCLCPP_INFO(node_->get_logger(), "Received motion cancel request");
+    stateMachine_->handleEvent(state_machine::Event::STOP_MOTION);
+}
+
+void RobotControlSystem::registerStateCallbacks() {
+    // 注册状态进入回调
+    stateMachine_->registerEnterCallback(state_machine::RobotState::IDLE, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Entering IDLE state");
+        upperBodyController_->reset();
+        lowerBodyController_->reset();
+    });
+    
+    stateMachine_->registerEnterCallback(state_machine::RobotState::MOVING_BASE, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Entering MOVING_BASE state");
+        // 启动基座移动控制
+    });
+    
+    stateMachine_->registerEnterCallback(state_machine::RobotState::ARM_MOTION, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Entering ARM_MOTION state");
+        // 启动手臂运动控制
+    });
+    
+    stateMachine_->registerEnterCallback(state_machine::RobotState::LEG_MOTION, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Entering LEG_MOTION state");
+        // 启动腿部运动控制
+    });
+    
+    stateMachine_->registerEnterCallback(state_machine::RobotState::ERROR, [this]() {
+        RCLCPP_ERROR(node_->get_logger(), "Entering ERROR state");
+        // 错误处理
+    });
+    
+    // 注册状态退出回调
+    stateMachine_->registerExitCallback(state_machine::RobotState::MOVING_BASE, [this]() {
+        RCLCPP_INFO(node_->get_logger(), "Exiting MOVING_BASE state");
+    });
+    
+    // 其他状态回调...
+}
+
+void RobotControlSystem::controlCallback() {
+    // 更新系统状态
+    update();
+    
+    // 获取当前时间
+    const double t = node_->now().seconds();
+    
+    // 计算控制命令
+    upperBodyCommand_ = upperBodyController_->computeControl(t, upperBodyState_);
+    lowerBodyCommand_ = lowerBodyController_->computeControl(t, lowerBodyState_);
+    
+    // 检查运动是否完成
+    if (upperBodyController_->isGoalReached(upperBodyState_) &&
+        lowerBodyController_->isGoalReached(lowerBodyState_)) {
+        stateMachine_->handleEvent(state_machine::Event::MOTION_COMPLETED);
+    }
+    
+    // 发布反馈
+    publishFeedback();
+}
+
+void RobotControlSystem::publishFeedback() {
+    ros::MotionFeedback feedback;
+    
+    // 计算进度 (示例实现)
+    feedback.progress = 0.5; // 在实际实现中应根据实际情况计算
+    
+    // 设置当前状态
+    feedback.currentState = Eigen::VectorXd::Zero(
+        upperBodyState_.size() + lowerBodyState_.size()
+    );
+    feedback.currentState << upperBodyState_, lowerBodyState_;
+    
+    // 设置状态描述
+    feedback.status = stateMachine_->stateToString(getCurrentState());
+    
+    // 发布反馈
+    motionAction_->publishFeedback(feedback);
+}
+
+void RobotControlSystem::onStateTransition(state_machine::RobotState newState) {
+    RCLCPP_INFO(node_->get_logger(), "State transition to: %s", 
+               stateMachine_->stateToString(newState).c_str());
+}
+
+} // namespace robot_control

src/control/LegController.cpp

@@ -0,0 +1,183 @@
+#include "robot_control/lower_body_controller.h"
+
+namespace robot_control {
+
+LowerBodyController::LowerBodyController(LowerBodyModel* model)
+    : model_(model), wheel_mode_(WheelMode::OMNI), gait_type_(GaitType::STAND) {
+    if (!model_) {
+        RCLCPP_ERROR(rclcpp::get_logger("LowerBodyController"), "无效的下肢模型指针");
+        throw std::invalid_argument("无效的下肢模型指针");
+    }
+    
+    // 初始化足端目标位置（站立姿态）
+    for (int i = 0; i < 4; ++i) {
+        foot_target_positions_[i] = model_->getFootPosition(i);
+    }
+    
+    // 初始化OCS2控制器
+    initOCS2Controller();
+}
+
+void LowerBodyController::initOCS2Controller() {
+    // 计算总自由度
+    const size_t leg_dof = model_->getLegJointIndices().size() * 3; // 4条腿，每条3个自由度
+    const size_t wheel_dof = model_->getWheelJointIndices().size();
+    const size_t state_dim = leg_dof + wheel_dof;
+    const size_t input_dim = state_dim;
+    
+    // 创建OCS2问题
+    problem_.reset(new ocs2::OptimalControlProblem);
+    
+    // 设置成本函数
+    problem_->costPtr->add("tracking_cost", std::make_unique<ocs2::QuadraticTrackingCost>(
+        state_dim, input_dim, 
+        Eigen::VectorXd::Zero(state_dim),  // 目标状态
+        Eigen::VectorXd::Zero(input_dim),   // 目标输入
+        Eigen::MatrixXd::Identity(state_dim),  // 状态权重
+        Eigen::MatrixXd::Identity(input_dim)   // 输入权重
+    ));
+    
+    // 添加重心约束
+    problem_->equalityConstraintPtr->add("com_constraint", 
+        std::make_unique<ocs2::CoMConstraint>(state_dim, input_dim));
+    
+    // 初始化求解器
+    solver_.reset(new ocs2::SLQ);
+    solver_->setProblem(*problem_);
+    
+    // 设置求解器参数
+    ocs2::SLQ_Settings settings;
+    settings.maxIterations = 100;
+    settings.convergenceTolerance = 1e-3;
+    solver_->setSettings(settings);
+    
+    RCLCPP_INFO(rclcpp::get_logger("LowerBodyController"), "OCS2控制器初始化完成，状态维度: %zu, 输入维度: %zu",
+                state_dim, input_dim);
+}
+
+void LowerBodyController::setGaitType(GaitType type) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    gait_type_ = type;
+    
+    // 根据步态类型重置控制器
+    if (gait_type_ == GaitType::WHEELED) {
+        // 轮式模式下，设置腿部为支撑姿态
+        setLegsToWheelMode();
+    }
+}
+
+void LowerBodyController::setLegsToWheelMode() {
+    // 设置腿部到适合轮式移动的姿态
+    for (int i = 0; i < 4; ++i) {
+        Eigen::Vector3d wheel_mode_pos = model_->getFootPosition(i);
+        wheel_mode_pos.z() -= 0.05;  // 稍微抬高一点，确保轮子着地
+        foot_target_positions_[i] = wheel_mode_pos;
+        
+        // 计算IK并更新目标关节角度
+        Eigen::VectorXd target_joints;
+        model_->legIK(i, wheel_mode_pos, target_joints);
+    }
+}
+
+void LowerBodyController::setWheelVelocity(const Eigen::Vector3d& vel_cmd) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    
+    // 根据轮子模式计算各轮速度
+    if (wheel_mode_ == WheelMode::OMNI) {
+        // 全向轮运动学
+        double vx = vel_cmd(0);
+        double vy = vel_cmd(1);
+        double wz = vel_cmd(2);
+        
+        // 假设轮子布局为矩形
+        const double L = 0.3;  // 前后轮距离的一半
+        const double W = 0.2;  // 左右轮距离的一半
+        
+        // 计算四个轮子的目标速度
+        wheel_target_velocities_[0] = (vx - vy - wz * (L + W)) / model_->getWheelRadius();  // 前左
+        wheel_target_velocities_[1] = (vx + vy + wz * (L + W)) / model_->getWheelRadius();  // 前右
+        wheel_target_velocities_[2] = (vx + vy - wz * (L + W)) / model_->getWheelRadius();  // 后左
+        wheel_target_velocities_[3] = (vx - vy + wz * (L + W)) / model_->getWheelRadius();  // 后右
+    } else if (wheel_mode_ == WheelMode::DIFFERENTIAL) {
+        // 差速驱动
+        double v = vel_cmd(0);
+        double wz = vel_cmd(2);
+        
+        wheel_target_velocities_[0] = (v - wz * model_->getTrackWidth() / 2) / model_->getWheelRadius();  // 左
+        wheel_target_velocities_[1] = (v + wz * model_->getTrackWidth() / 2) / model_->getWheelRadius();  // 右
+        wheel_target_velocities_[2] = wheel_target_velocities_[0];  // 后左
+        wheel_target_velocities_[3] = wheel_target_velocities_[1];  // 后右
+    }
+}
+
+void LowerBodyController::setFootTargets(const std::vector<Eigen::Vector3d>& positions) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (positions.size() != 4) {
+        RCLCPP_ERROR(rclcpp::get_logger("LowerBodyController"), "足端目标数量必须为4");
+        return;
+    }
+    
+    for (int i = 0; i < 4; ++i) {
+        foot_target_positions_[i] = positions[i];
+        
+        // 计算IK并更新目标关节角度
+        Eigen::VectorXd target_joints;
+        model_->legIK(i, positions[i], target_joints);
+    }
+}
+
+void LowerBodyController::computeControl(const Eigen::VectorXd& leg_joints, 
+                                        const Eigen::VectorXd& leg_velocities,
+                                        const Eigen::VectorXd& wheel_joints,
+                                        const Eigen::VectorXd& wheel_velocities,
+                                        Eigen::VectorXd& leg_commands,
+                                        Eigen::VectorXd& wheel_commands) {
+    if (!isActive()) {
+        leg_commands.setZero(leg_joints.size());
+        wheel_commands.setZero(wheel_joints.size());
+        return;
+    }
+    
+    std::lock_guard<std::mutex> lock(mutex_);
+    
+    // 根据步态类型计算不同的控制命令
+    if (gait_type_ == GaitType::WHEELED) {
+        // 轮式模式：主要控制轮子，腿部保持固定姿态
+        computeWheelControl(wheel_joints, wheel_velocities, wheel_commands);
+        computeLegControl(leg_joints, leg_velocities, leg_commands);
+    } else {
+        // 步行模式：主要控制腿部，轮子不动
+        wheel_commands.setZero(wheel_joints.size());
+        computeLegControl(leg_joints, leg_velocities, leg_commands);
+    }
+}
+
+void LowerBodyController::computeLegControl(const Eigen::VectorXd& joints, 
+                                           const Eigen::VectorXd& velocities,
+                                           Eigen::VectorXd& commands) {
+    // 准备状态向量 [关节位置; 关节速度]
+    const size_t nq = joints.size();
+    Eigen::VectorXd state(2 * nq);
+    state << joints, velocities;
+    
+    // 求解最优控制
+    ocs2::scalar_t t = 0.0;  // 当前时间
+    ocs2::vector_t optimal_input;
+    solver_->computeInput(t, state, optimal_input);
+    
+    // 将最优输入作为关节指令
+    commands = optimal_input;
+}
+
+void LowerBodyController::computeWheelControl(const Eigen::VectorXd& joints, 
+                                             const Eigen::VectorXd& velocities,
+                                             Eigen::VectorXd& commands) {
+    // 简单的PD控制
+    commands.resize(velocities.size());
+    for (size_t i = 0; i < velocities.size() && i < wheel_target_velocities_.size(); ++i) {
+        commands(i) = kp_wheels_(i) * (wheel_target_velocities_[i] - velocities(i));
+    }
+}
+
+} // namespace robot_control
+    

src/kinematicsDemo.cpp

@@ -0,0 +1,153 @@
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+int main(int argc, char* argv[])
+{
+  // 初始化ROS节点
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>("dual_arm_leg_demo");
+  
+  // 设置执行器
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(node);
+  std::thread([&executor]() { executor.spin(); }).detach();
+
+  // 创建四个独立的MoveGroup接口，分别控制四个肢体
+  using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface;
+  
+  MoveGroupInterface left_arm_group(node, "left_arm_group");
+  MoveGroupInterface right_arm_group(node, "right_arm_group");
+  MoveGroupInterface left_leg_group(node, "left_leg_group");
+  MoveGroupInterface right_leg_group(node, "right_leg_group");
+
+  // 设置参考坐标系
+  left_arm_group.setPoseReferenceFrame("base_link");
+  right_arm_group.setPoseReferenceFrame("base_link");
+  left_leg_group.setPoseReferenceFrame("base_link");
+  right_leg_group.setPoseReferenceFrame("base_link");
+
+  // 设置目标位置的参考坐标系
+  left_arm_group.setGoalPoseTopic("target_pose");
+  
+  // 允许规划失败后重试
+  left_arm_group.allowReplanning(true);
+  right_arm_group.allowReplanning(true);
+  left_leg_group.allowReplanning(true);
+  right_leg_group.allowReplanning(true);
+
+  // 设置规划时间和目标公差
+  left_arm_group.setPlanningTime(10.0);
+  right_arm_group.setPlanningTime(10.0);
+  left_leg_group.setPlanningTime(10.0);
+  right_leg_group.setPlanningTime(10.0);
+
+  left_arm_group.setGoalPositionTolerance(0.01);
+  right_arm_group.setGoalPositionTolerance(0.01);
+  left_arm_group.setGoalOrientationTolerance(0.01);
+  right_arm_group.setGoalOrientationTolerance(0.01);
+
+  RCLCPP_INFO(node->get_logger(), "机器人初始化完成，开始演示...");
+
+  // 1. 移动到初始位置
+  RCLCPP_INFO(node->get_logger(), "移动到初始位置...");
+  left_arm_group.setNamedTarget("home");
+  right_arm_group.setNamedTarget("home");
+  left_leg_group.setNamedTarget("home");
+  right_leg_group.setNamedTarget("home");
+
+  left_arm_group.move();
+  right_arm_group.move();
+  left_leg_group.move();
+  right_leg_group.move();
+  
+  rclcpp::sleep_for(std::chrono::seconds(2));
+
+  // 2. 控制左臂移动到目标位置
+  RCLCPP_INFO(node->get_logger(), "控制左臂移动到目标位置...");
+  geometry_msgs::msg::Pose left_arm_target;
+  
+  // 左臂目标位置（在基座坐标系中）
+  left_arm_target.orientation.w = 1.0;  // 无旋转
+  left_arm_target.position.x = 0.4;
+  left_arm_target.position.y = 0.3;
+  left_arm_target.position.z = 0.3;
+  
+  left_arm_group.setPoseTarget(left_arm_target);
+  left_arm_group.move();
+  
+  // 显示左臂正运动学结果
+  auto current_left_pose = left_arm_group.getCurrentPose();
+  RCLCPP_INFO_STREAM(node->get_logger(), "左臂当前位置: x=" << current_left_pose.pose.position.x 
+                    << ", y=" << current_left_pose.pose.position.y 
+                    << ", z=" << current_left_pose.pose.position.z);
+  rclcpp::sleep_for(std::chrono::seconds(2));
+
+  // 3. 控制右臂移动到目标位置
+  RCLCPP_INFO(node->get_logger(), "控制右臂移动到目标位置...");
+  geometry_msgs::msg::Pose right_arm_target;
+  
+  // 右臂目标位置（在基座坐标系中）
+  right_arm_target.orientation.w = 1.0;  // 无旋转
+  right_arm_target.position.x = 0.4;
+  right_arm_target.position.y = -0.3;
+  right_arm_target.position.z = 0.3;
+  
+  right_arm_group.setPoseTarget(right_arm_target);
+  right_arm_group.move();
+  
+  // 显示右臂正运动学结果
+  auto current_right_pose = right_arm_group.getCurrentPose();
+  RCLCPP_INFO_STREAM(node->get_logger(), "右臂当前位置: x=" << current_right_pose.pose.position.x 
+                    << ", y=" << current_right_pose.pose.position.y 
+                    << ", z=" << current_right_pose.pose.position.z);
+  rclcpp::sleep_for(std::chrono::seconds(2));
+
+  // 4. 控制左腿移动
+  RCLCPP_INFO(node->get_logger(), "控制左腿移动...");
+  std::vector<double> left_leg_joint_targets = {0.2, 0.8, 0.1};
+  left_leg_group.setJointValueTarget(left_leg_joint_targets);
+  left_leg_group.move();
+  
+  // 显示左腿关节角度
+  auto left_leg_joints = left_leg_group.getCurrentJointValues();
+  RCLCPP_INFO_STREAM(node->get_logger(), "左腿关节角度: " 
+                    << left_leg_joints[0] << ", " 
+                    << left_leg_joints[1] << ", " 
+                    << left_leg_joints[2]);
+  rclcpp::sleep_for(std::chrono::seconds(2));
+
+  // 5. 控制右腿移动
+  RCLCPP_INFO(node->get_logger(), "控制右腿移动...");
+  std::vector<double> right_leg_joint_targets = {-0.2, 0.8, -0.1};
+  right_leg_group.setJointValueTarget(right_leg_joint_targets);
+  right_leg_group.move();
+  
+  // 显示右腿关节角度
+  auto right_leg_joints = right_leg_group.getCurrentJointValues();
+  RCLCPP_INFO_STREAM(node->get_logger(), "右腿关节角度: " 
+                    << right_leg_joints[0] << ", " 
+                    << right_leg_joints[1] << ", " 
+                    << right_leg_joints[2]);
+  rclcpp::sleep_for(std::chrono::seconds(2));
+
+  // 6. 回到初始位置
+  RCLCPP_INFO(node->get_logger(), "回到初始位置...");
+  left_arm_group.setNamedTarget("home");
+  right_arm_group.setNamedTarget("home");
+  left_leg_group.setNamedTarget("home");
+  right_leg_group.setNamedTarget("home");
+
+  left_arm_group.move();
+  right_arm_group.move();
+  left_leg_group.move();
+  right_leg_group.move();
+
+  RCLCPP_INFO(node->get_logger(), "演示完成！");
+
+  // 关闭节点
+  rclcpp::shutdown();
+  return 0;
+}

src/main.cpp

@@ -0,0 +1,40 @@
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include "robot_control/RobotControlSystem.h"
+
+int main(int argc, char* argv[]) {
+    // 初始化ROS
+    rclcpp::init(argc, argv);
+    
+    // 创建节点
+    auto node = rclcpp::Node::make_shared("robot_control_node");
+    
+    // 声明URDF路径参数
+    node->declare_parameter<std::string>("upper_body_urdf", "urdf/upper_body.urdf");
+    node->declare_parameter<std::string>("lower_body_urdf", "urdf/lower_body.urdf");
+    
+    // 获取URDF路径
+    std::string upperBodyUrdf, lowerBodyUrdf;
+    node->get_parameter("upper_body_urdf", upperBodyUrdf);
+    node->get_parameter("lower_body_urdf", lowerBodyUrdf);
+    
+    // 创建并初始化控制系统
+    auto controlSystem = std::make_shared<robot_control::RobotControlSystem>(node);
+    if (!controlSystem->initialize(upperBodyUrdf, lowerBodyUrdf)) {
+        RCLCPP_ERROR(node->get_logger(), "Failed to initialize control system");
+        return 1;
+    }
+    
+    // 启动控制系统
+    controlSystem->start();
+    
+    // 运行ROS循环
+    rclcpp::spin(node);
+    
+    // 停止系统
+    controlSystem->stop();
+    
+    // 关闭ROS
+    rclcpp::shutdown();
+    return 0;
+}

src/model/RobotModel.cpp

@@ -0,0 +1,52 @@
+#include "robot_control/robot_model.h"
+
+namespace robot_control {
+
+RobotModel::RobotModel(const std::string& urdf_path) {
+    // 加载URDF模型
+    pinocchio::urdf::buildModel(urdf_path, model_);
+    data_ = std::make_unique<pinocchio::Data>(model_);
+    
+    // 初始化上下身模型
+    upper_body_ = std::make_unique<UpperBodyModel>(model_, data_.get());
+    lower_body_ = std::make_unique<LowerBodyModel>(model_, data_.get());
+    
+    RCLCPP_INFO(rclcpp::get_logger("RobotModel"), "机器人模型加载成功，关节数量: %d", model_.nq);
+}
+
+void RobotModel::update(const Eigen::VectorXd& q, const Eigen::VectorXd& dq) {
+    // 更新机器人状态
+    pinocchio::forwardKinematics(model_, *data_, q, dq);
+    pinocchio::updateFramePlacements(model_, *data_);
+    
+    // 更新上下身模型
+    upper_body_->update(q, dq);
+    lower_body_->update(q, dq);
+}
+
+const Eigen::VectorXd& RobotModel::getJointPositions() const {
+    return joint_positions_;
+}
+
+const Eigen::VectorXd& RobotModel::getJointVelocities() const {
+    return joint_velocities_;
+}
+
+UpperBodyModel* RobotModel::getUpperBody() {
+    return upper_body_.get();
+}
+
+LowerBodyModel* RobotModel::getLowerBody() {
+    return lower_body_.get();
+}
+
+const pinocchio::Model& RobotModel::getPinocchioModel() const {
+    return model_;
+}
+
+pinocchio::Data* RobotModel::getPinocchioData() {
+    return data_.get();
+}
+
+} // namespace robot_control
+    

src/nodes/MotionControlNode.cpp

@@ -0,0 +1,564 @@
+#include "quadruped_manipulator_control/nodes/motion_control_node.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include <chrono>
+#include <cmath>
+#include <moveit/robot_model/joint_model.h>
+#include <moveit/robot_model/robot_model.h>
+
+namespace quadruped_manipulator_control
+{
+
+MotionControlNode::MotionControlNode(const rclcpp::NodeOptions& options)
+  : Node("motion_control_node", options)
+{
+  // 声明参数
+  declare_parameter<std::string>("controller_name", "joint_trajectory_controller");
+  declare_parameter<std::string>("urdf_path", "");
+  declare_parameter<double>("default_speed", 0.5);
+  declare_parameter<double>("default_acceleration", 0.5);
+  
+  // 获取参数
+  get_parameter("controller_name", controller_name_);
+  get_parameter("urdf_path", urdf_path_);
+  get_parameter("default_speed", default_speed_);
+  get_parameter("default_acceleration", default_acceleration_);
+  
+  // 初始化MoveIt规划器
+  moveit_planner_ = std::make_shared<MoveItPlanner>(shared_from_this(), "whole_body");
+  
+  // 初始化客户端和发布器
+  list_controllers_client_ = create_client<controller_manager_msgs::srv::ListControllers>("/controller_manager/list_controllers");
+  switch_controller_client_ = create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
+  trajectory_publisher_ = create_publisher<trajectory_msgs::msg::JointTrajectory>(
+    "/" + controller_name_ + "/joint_trajectory", 10);
+  
+  // 等待控制器服务
+  while (!list_controllers_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+    RCLCPP_INFO(get_logger(), "Waiting for controller_manager services...");
+  }
+  
+  // 初始化Action服务器
+  action_server_movej_ = rclcpp_action::create_server<MoveJ>(
+    this,
+    "/motion_control/movej",
+    std::bind(&MotionControlNode::handle_goal_movej, this, std::placeholders::_1, std::placeholders::_2),
+    std::bind(&MotionControlNode::handle_cancel_movej, this, std::placeholders::_1),
+    std::bind(&MotionControlNode::handle_accepted_movej, this, std::placeholders::_1)
+  );
+  
+  action_server_movel_ = rclcpp_action::create_server<MoveL>(
+    this,
+    "/motion_control/movel",
+    std::bind(&MotionControlNode::handle_goal_movel, this, std::placeholders::_1, std::placeholders::_2),
+    std::bind(&MotionControlNode::handle_cancel_movel, this, std::placeholders::_1),
+    std::bind(&MotionControlNode::handle_accepted_movel, this, std::placeholders::_1)
+  );
+  
+  action_server_joint_movement_ = rclcpp_action::create_server<JointMovement>(
+    this,
+    "/motion_control/joint_movement",
+    std::bind(&MotionControlNode::handle_goal_joint_movement, this, std::placeholders::_1, std::placeholders::_2),
+    std::bind(&MotionControlNode::handle_cancel_joint_movement, this, std::placeholders::_1),
+    std::bind(&MotionControlNode::handle_accepted_joint_movement, this, std::placeholders::_1)
+  );
+  
+  RCLCPP_INFO(get_logger(), "MotionControlNode initialized");
+}
+
+// MoveJ Action处理
+rclcpp_action::GoalResponse MotionControlNode::handle_goal_movej(
+  const rclcpp_action::GoalUUID& uuid,
+  std::shared_ptr<const MoveJ::Goal> goal)
+{
+  RCLCPP_INFO(get_logger(), "Received MoveJ goal for group: %s", goal->planning_group.c_str());
+  
+  // 验证目标
+  if (goal->joint_names.empty() || goal->target_positions.empty()) {
+    RCLCPP_ERROR(get_logger(), "MoveJ goal has empty joint names or target positions");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  
+  if (goal->joint_names.size() != goal->target_positions.size()) {
+    RCLCPP_ERROR(get_logger(), "MoveJ joint names and target positions size mismatch");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  
+  (void)uuid;
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse MotionControlNode::handle_cancel_movej(
+  const std::shared_ptr<GoalHandleMoveJ> goal_handle)
+{
+  RCLCPP_INFO(get_logger(), "Received cancel request for MoveJ");
+  (void)goal_handle;
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void MotionControlNode::handle_accepted_movej(
+  const std::shared_ptr<GoalHandleMoveJ> goal_handle)
+{
+  // 使用单独的线程执行目标
+  std::thread{std::bind(&MotionControlNode::execute_movej, this, std::placeholders::_1), goal_handle}.detach();
+}
+
+// MoveL Action处理
+rclcpp_action::GoalResponse MotionControlNode::handle_goal_movel(
+  const rclcpp_action::GoalUUID& uuid,
+  std::shared_ptr<const MoveL::Goal> goal)
+{
+  RCLCPP_INFO(get_logger(), "Received MoveL goal for frame: %s", goal->frame_name.c_str());
+  
+  // 验证目标
+  if (goal->planning_group.empty() || goal->frame_name.empty()) {
+    RCLCPP_ERROR(get_logger(), "MoveL goal has empty planning group or frame name");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  
+  (void)uuid;
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse MotionControlNode::handle_cancel_movel(
+  const std::shared_ptr<GoalHandleMoveL> goal_handle)
+{
+  RCLCPP_INFO(get_logger(), "Received cancel request for MoveL");
+  (void)goal_handle;
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void MotionControlNode::handle_accepted_movel(
+  const std::shared_ptr<GoalHandleMoveL> goal_handle)
+{
+  // 使用单独的线程执行目标
+  std::thread{std::bind(&MotionControlNode::execute_movel, this, std::placeholders::_1), goal_handle}.detach();
+}
+
+// JointMovement Action处理
+rclcpp_action::GoalResponse MotionControlNode::handle_goal_joint_movement(
+  const rclcpp_action::GoalUUID& uuid,
+  std::shared_ptr<const JointMovement::Goal> goal)
+{
+  RCLCPP_INFO(get_logger(), "Received joint movement goal for joint: %s", goal->joint_name.c_str());
+  
+  // 验证目标
+  if (goal->joint_name.empty()) {
+    RCLCPP_ERROR(get_logger(), "Joint movement goal has empty joint name");
+    return rclcpp_action::GoalResponse::REJECT;
+  }
+  
+  if (goal->speed <= 0 || goal->acceleration <= 0) {
+    RCLCPP_WARN(get_logger(), "Invalid speed or acceleration, using defaults");
+  }
+  
+  (void)uuid;
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+}
+
+rclcpp_action::CancelResponse MotionControlNode::handle_cancel_joint_movement(
+  const std::shared_ptr<GoalHandleJointMovement> goal_handle)
+{
+  RCLCPP_INFO(get_logger(), "Received cancel request for joint movement");
+  (void)goal_handle;
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+
+void MotionControlNode::handle_accepted_joint_movement(
+  const std::shared_ptr<GoalHandleJointMovement> goal_handle)
+{
+  // 使用单独的线程执行目标
+  std::thread{std::bind(&MotionControlNode::execute_joint_movement, this, std::placeholders::_1), goal_handle}.detach();
+}
+
+// 执行函数
+void MotionControlNode::execute_movej(const std::shared_ptr<GoalHandleMoveJ> goal_handle)
+{
+  const auto goal = goal_handle->get_goal();
+  auto feedback = std::make_shared<MoveJ::Feedback>();
+  auto result = std::make_shared<MoveJ::Result>();
+  
+  RCLCPP_INFO(get_logger(), "Executing MoveJ for group: %s", goal->planning_group.c_str());
+  
+  // 切换规划组
+  moveit_planner_->set_planning_group(goal->planning_group);
+  
+  // 设置规划参数
+  double speed = goal->speed > 0 ? goal->speed : default_speed_;
+  double acceleration = goal->acceleration > 0 ? goal->acceleration : default_acceleration_;
+  moveit_planner_->set_planning_parameters(5.0, 0.01, speed, acceleration);
+  
+  // 获取当前关节位置
+  std::vector<double> current_joints = moveit_planner_->get_current_joint_positions();
+  feedback->current_joint_positions = current_joints;
+  feedback->progress = 0.0;
+  goal_handle->publish_feedback(feedback);
+  
+  // 规划运动
+  moveit::planning_interface::MoveGroupInterface::Plan plan;
+  bool planning_success = moveit_planner_->plan_joint_goal(goal->target_positions, plan);
+  
+  if (!planning_success) {
+    RCLCPP_ERROR(get_logger(), "MoveJ planning failed");
+    result->success = false;
+    result->error_message = "Planning failed";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  // 确保控制器已激活
+  if (!switch_controller(controller_name_)) {
+    RCLCPP_ERROR(get_logger(), "Failed to activate controller");
+    result->success = false;
+    result->error_message = "Failed to activate controller";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  // 执行规划
+  bool execution_success = moveit_planner_->execute_plan(plan);
+  
+  if (!execution_success) {
+    RCLCPP_ERROR(get_logger(), "MoveJ execution failed");
+    result->success = false;
+    result->error_message = "Execution failed";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  // 获取最终关节位置
+  result->final_joint_positions = moveit_planner_->get_current_joint_positions();
+  result->success = true;
+  goal_handle->succeed(result);
+  RCLCPP_INFO(get_logger(), "MoveJ completed successfully");
+}
+
+void MotionControlNode::execute_movel(const std::shared_ptr<GoalHandleMoveL> goal_handle)
+{
+  const auto goal = goal_handle->get_goal();
+  auto feedback = std::make_shared<MoveL::Feedback>();
+  auto result = std::make_shared<MoveL::Result>();
+  
+  RCLCPP_INFO(get_logger(), "Executing MoveL for frame: %s", goal->frame_name.c_str());
+  
+  // 切换规划组
+  moveit_planner_->set_planning_group(goal->planning_group);
+  
+  // 设置规划参数
+  double speed = goal->speed > 0 ? goal->speed : default_speed_;
+  double acceleration = goal->acceleration > 0 ? goal->acceleration : default_acceleration_;
+  moveit_planner_->set_planning_parameters(5.0, 0.01, speed, acceleration);
+  
+  // 获取当前位姿
+  geometry_msgs::msg::Pose current_pose = moveit_planner_->get_current_end_effector_pose();
+  feedback->current_pose = current_pose;
+  feedback->progress = 0.0;
+  goal_handle->publish_feedback(feedback);
+  
+  // 规划运动
+  moveit::planning_interface::MoveGroupInterface::Plan plan;
+  bool planning_success = moveit_planner_->plan_pose_goal(goal->target_pose, plan);
+  
+  if (!planning_success) {
+    RCLCPP_ERROR(get_logger(), "MoveL planning failed");
+    result->success = false;
+    result->error_message = "Planning failed";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  // 确保控制器已激活
+  if (!switch_controller(controller_name_)) {
+    RCLCPP_ERROR(get_logger(), "Failed to activate controller");
+    result->success = false;
+    result->error_message = "Failed to activate controller";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  // 执行规划
+  bool execution_success = moveit_planner_->execute_plan(plan);
+  
+  if (!execution_success) {
+    RCLCPP_ERROR(get_logger(), "MoveL execution failed");
+    result->success = false;
+    result->error_message = "Execution failed";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  // 获取最终位姿
+  result->final_pose = moveit_planner_->get_current_end_effector_pose();
+  result->success = true;
+  goal_handle->succeed(result);
+  RCLCPP_INFO(get_logger(), "MoveL completed successfully");
+}
+
+void MotionControlNode::execute_joint_movement(const std::shared_ptr<GoalHandleJointMovement> goal_handle)
+{
+  const auto goal = goal_handle->get_goal();
+  auto feedback = std::make_shared<JointMovement::Feedback>();
+  auto result = std::make_shared<JointMovement::Result>();
+  
+  RCLCPP_INFO(get_logger(), "Executing movement for joint: %s to position: %.4f",
+              goal->joint_name.c_str(), goal->target_position);
+  
+  // 获取当前关节位置（使用MoveIt获取）
+  moveit_planner_->set_planning_group("whole_body");
+  std::vector<double> current_joints = moveit_planner_->get_current_joint_positions();
+  std::vector<std::string> joint_names = moveit_planner_->get_joint_names();
+  
+  // 找到目标关节索引
+  auto it = std::find(joint_names.begin(), joint_names.end(), goal->joint_name);
+  if (it == joint_names.end()) {
+    RCLCPP_ERROR(get_logger(), "Joint %s not found", goal->joint_name.c_str());
+    result->success = false;
+    result->error_message = "Joint not found";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  size_t joint_index = std::distance(joint_names.begin(), it);
+  double start_position = current_joints[joint_index];
+  double target_position = goal->target_position;
+  
+  // 检查关节限位 (使用MoveIt获取)
+  double min_pos, max_pos;
+  if (get_joint_limits(goal->joint_name, min_pos, max_pos)) {
+    if (target_position < min_pos || target_position > max_pos) {
+      RCLCPP_ERROR(get_logger(), "Target position out of joint limits [%.4f, %.4f]", min_pos, max_pos);
+      result->success = false;
+      result->error_message = "Target position out of joint limits";
+      goal_handle->abort(result);
+      return;
+    }
+  } else {
+    RCLCPP_WARN(get_logger(), "Could not retrieve joint limits for %s", goal->joint_name.c_str());
+  }
+  
+  // 设置速度和加速度
+  double speed = goal->speed > 0 ? goal->speed : default_speed_;
+  double acceleration = goal->acceleration > 0 ? goal->acceleration : default_acceleration_;
+  
+  // 计算运动时间
+  double distance = std::fabs(target_position - start_position);
+  double time = 0.0;
+  
+  if (distance > 1e-6) {  // 如果需要移动
+    // 计算达到最大速度所需的时间和距离
+    double time_to_max_speed = speed / acceleration;
+    double distance_to_max_speed = 0.5 * acceleration * time_to_max_speed * time_to_max_speed;
+    
+    if (distance <= 2 * distance_to_max_speed) {
+      // 三角形速度曲线（加速后立即减速）
+      time = 2 * std::sqrt(distance / acceleration);
+    } else {
+      // 梯形速度曲线（加速、匀速、减速）
+      time = time_to_max_speed + (distance - 2 * distance_to_max_speed) / speed + time_to_max_speed;
+    }
+  }
+  
+  // 创建轨迹点
+  std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
+  
+  // 起始点（当前位置）
+  trajectory_msgs::msg::JointTrajectoryPoint start_point;
+  start_point.positions.resize(joint_names.size(), 0.0);
+  start_point.positions[joint_index] = start_position;
+  start_point.time_from_start = rclcpp::Duration::from_seconds(0.0);
+  points.push_back(start_point);
+  
+  // 目标点
+  trajectory_msgs::msg::JointTrajectoryPoint target_point;
+  target_point.positions.resize(joint_names.size(), 0.0);
+  target_point.positions[joint_index] = target_position;
+  target_point.velocities.resize(joint_names.size(), 0.0);
+  target_point.accelerations.resize(joint_names.size(), 0.0);
+  target_point.time_from_start = rclcpp::Duration::from_seconds(time);
+  points.push_back(target_point);
+  
+  // 确保控制器已激活
+  if (!switch_controller(controller_name_)) {
+    RCLCPP_ERROR(get_logger(), "Failed to activate controller");
+    result->success = false;
+    result->error_message = "Failed to activate controller";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  // 发送轨迹
+  bool send_success = send_joint_trajectory(joint_names, points);
+  
+  if (!send_success) {
+    RCLCPP_ERROR(get_logger(), "Failed to send joint trajectory");
+    result->success = false;
+    result->error_message = "Failed to send trajectory";
+    goal_handle->abort(result);
+    return;
+  }
+  
+  // 等待运动完成并发布反馈
+  rclcpp::Rate rate(10);  // 10Hz反馈
+  double elapsed = 0.0;
+  double current_position = start_position;
+  
+  while (elapsed < time && rclcpp::ok() && !goal_handle->is_canceling()) {
+    // 简单的位置插值用于反馈（实际应从硬件读取）
+    double fraction = std::min(elapsed / time, 1.0);
+    current_position = start_position + fraction * (target_position - start_position);
+    
+    feedback->current_position = current_position;
+    feedback->progress = fraction;
+    goal_handle->publish_feedback(feedback);
+    
+    elapsed += 0.1;  // 100ms
+    rate.sleep();
+  }
+  
+  // 检查是否被取消
+  if (goal_handle->is_canceling()) {
+    result->success = false;
+    result->error_message = "Movement canceled";
+    goal_handle->canceled(result);
+    return;
+  }
+  
+  // 获取最终位置（实际应从硬件读取）
+  result->final_position = target_position;
+  result->success = true;
+  goal_handle->succeed(result);
+  RCLCPP_INFO(get_logger(), "Joint movement completed successfully");
+}
+
+// 辅助函数
+bool MotionControlNode::switch_controller(const std::string& controller_name)
+{
+  // 首先检查控制器是否已激活
+  auto list_request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>();
+  auto list_result = list_controllers_client_->async_send_request(list_request);
+  
+  if (rclcpp::spin_until_future_complete(shared_from_this(), list_result) !=
+      rclcpp::FutureReturnCode::SUCCESS) {
+    RCLCPP_ERROR(get_logger(), "Failed to call list_controllers service");
+    return false;
+  }
+  
+  bool is_active = false;
+  for (const auto& controller : list_result.get()->controller) {
+    if (controller.name == controller_name && controller.state == "active") {
+      is_active = true;
+      break;
+    }
+  }
+  
+  if (is_active) {
+    RCLCPP_INFO(get_logger(), "Controller %s is already active", controller_name.c_str());
+    return true;
+  }
+  
+  // 如果未激活，则切换控制器
+  RCLCPP_INFO(get_logger(), "Activating controller %s", controller_name.c_str());
+  
+  auto switch_request = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+  switch_request->start_controllers = {controller_name};
+  switch_request->stop_controllers = {};
+  switch_request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+  switch_request->start_asap = false;
+  
+  auto switch_result = switch_controller_client_->async_send_request(switch_request);
+  
+  if (rclcpp::spin_until_future_complete(shared_from_this(), switch_result) !=
+      rclcpp::FutureReturnCode::SUCCESS) {
+    RCLCPP_ERROR(get_logger(), "Failed to call switch_controller service");
+    return false;
+  }
+  
+  if (!switch_result.get()->ok) {
+    RCLCPP_ERROR(get_logger(), "Failed to activate controller %s", controller_name.c_str());
+    return false;
+  }
+  
+  RCLCPP_INFO(get_logger(), "Successfully activated controller %s", controller_name.c_str());
+  return true;
+}
+
+bool MotionControlNode::send_joint_trajectory(
+  const std::vector<std::string>& joint_names,
+  const std::vector<trajectory_msgs::msg::JointTrajectoryPoint>& points)
+{
+  trajectory_msgs::msg::JointTrajectory trajectory;
+  trajectory.header.stamp = now();
+  trajectory.joint_names = joint_names;
+  trajectory.points = points;
+  
+  trajectory_publisher_->publish(trajectory);
+  return true;
+}
+
+double MotionControlNode::calculate_trajectory_time(
+  const std::vector<double>& start_positions,
+  const std::vector<double>& end_positions,
+  double max_velocity,
+  double max_acceleration)
+{
+  if (start_positions.size() != end_positions.size()) {
+    RCLCPP_ERROR(get_logger(), "Start and end positions size mismatch");
+    return 1.0;  // 默认时间
+  }
+  
+  double max_distance = 0.0;
+  for (size_t i = 0; i < start_positions.size(); ++i) {
+    max_distance = std::max(max_distance, std::fabs(end_positions[i] - start_positions[i]));
+  }
+  
+  if (max_distance < 1e-6) {
+    return 0.0;  // 无需移动
+  }
+  
+  // 计算达到最大速度所需的时间和距离
+  double time_to_max_speed = max_velocity / max_acceleration;
+  double distance_to_max_speed = 0.5 * max_acceleration * time_to_max_speed * time_to_max_speed;
+  
+  if (max_distance <= 2 * distance_to_max_speed) {
+    // 三角形速度曲线
+    return 2 * std::sqrt(max_distance / max_acceleration);
+  } else {
+    // 梯形速度曲线
+    return time_to_max_speed + (max_distance - 2 * distance_to_max_speed) / max_velocity + time_to_max_speed;
+  }
+}
+
+// 从MoveIt获取关节限位
+bool MotionControlNode::get_joint_limits(
+  const std::string& joint_name,
+  double& min_position,
+  double& max_position)
+{
+  const moveit::core::RobotModelConstPtr& robot_model = moveit_planner_->get_robot_model();
+  if (!robot_model) {
+    RCLCPP_ERROR(get_logger(), "Failed to get robot model");
+    return false;
+  }
+  
+  const moveit::core::JointModel* joint_model = robot_model->getJointModel(joint_name);
+  if (!joint_model) {
+    RCLCPP_ERROR(get_logger(), "Failed to get joint model for %s", joint_name.c_str());
+    return false;
+  }
+  
+  if (joint_model->getType() == moveit::core::JointModel::REVOLUTE ||
+      joint_model->getType() == moveit::core::JointModel::PRISMATIC) {
+    const moveit::core::JointModel::Bounds& bounds = joint_model->getVariableBounds();
+    if (!bounds.empty()) {
+      min_position = bounds[0].min_position_;
+      max_position = bounds[0].max_position_;
+      return true;
+    }
+  }
+  
+  // 对于连续关节或其他类型，不设置限位
+  min_position = -std::numeric_limits<double>::infinity();
+  max_position = std::numeric_limits<double>::infinity();
+  return false;
+}
+
+}  // namespace quadruped_manipulator_control

src/planner/MoveitPlanner.cpp

@@ -0,0 +1,124 @@
+#include "quadruped_manipulator_control/planning/moveit_planner.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace quadruped_manipulator_control
+{
+
+MoveItPlanner::MoveItPlanner(
+  const rclcpp::Node::SharedPtr& node,
+  const std::string& planning_group)
+  : node_(node), planning_group_(planning_group)
+{
+  // 初始化机器人模型加载器
+  robot_model_loader_ = std::make_shared<robot_model_loader::RobotModelLoader>(node_, "robot_description");
+  
+  // 获取机器人模型
+  robot_model_ = robot_model_loader_->getModel();
+  if (!robot_model_) {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to load robot model");
+  }
+  
+  // 初始化运动规划组
+  move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, planning_group_);
+  
+  // 设置默认规划参数
+  set_planning_parameters();
+  
+  RCLCPP_INFO(node_->get_logger(), "MoveItPlanner initialized for planning group: %s", planning_group_.c_str());
+}
+
+void MoveItPlanner::set_planning_group(const std::string& planning_group)
+{
+  planning_group_ = planning_group;
+  move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, planning_group_);
+  set_planning_parameters();
+  RCLCPP_INFO(node_->get_logger(), "Switched to planning group: %s", planning_group_.c_str());
+}
+
+bool MoveItPlanner::plan_joint_goal(
+  const std::vector<double>& joint_positions,
+  moveit::planning_interface::MoveGroupInterface::Plan& plan)
+{
+  if (joint_positions.empty()) {
+    RCLCPP_ERROR(node_->get_logger(), "Joint positions vector is empty");
+    return false;
+  }
+
+  // 设置关节目标
+  move_group_->setJointValueTarget(joint_positions);
+  
+  // 规划运动
+  moveit::planning_interface::MoveItErrorCode result = move_group_->plan(plan);
+  
+  if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
+    RCLCPP_INFO(node_->get_logger(), "Successfully planned joint space motion");
+    return true;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to plan joint space motion. Error code: %d", result.val);
+    return false;
+  }
+}
+
+bool MoveItPlanner::plan_pose_goal(
+  const geometry_msgs::msg::Pose& target_pose,
+  moveit::planning_interface::MoveGroupInterface::Plan& plan)
+{
+  // 设置位姿目标
+  move_group_->setPoseTarget(target_pose);
+  
+  // 规划运动
+  moveit::planning_interface::MoveItErrorCode result = move_group_->plan(plan);
+  
+  if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
+    RCLCPP_INFO(node_->get_logger(), "Successfully planned Cartesian space motion");
+    return true;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to plan Cartesian space motion. Error code: %d", result.val);
+    return false;
+  }
+}
+
+bool MoveItPlanner::execute_plan(const moveit::planning_interface::MoveGroupInterface::Plan& plan)
+{
+  moveit::planning_interface::MoveItErrorCode result = move_group_->execute(plan);
+  
+  if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
+    RCLCPP_INFO(node_->get_logger(), "Successfully executed planned motion");
+    return true;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to execute planned motion. Error code: %d", result.val);
+    return false;
+  }
+}
+
+std::vector<double> MoveItPlanner::get_current_joint_positions()
+{
+  return move_group_->getCurrentJointValues();
+}
+
+std::vector<std::string> MoveItPlanner::get_joint_names()
+{
+  return move_group_->getJointNames();
+}
+
+geometry_msgs::msg::Pose MoveItPlanner::get_current_end_effector_pose()
+{
+  return move_group_->getCurrentPose().pose;
+}
+
+void MoveItPlanner::set_planning_parameters(
+  double planning_time,
+  double goal_tolerance,
+  double max_velocity_scaling,
+  double max_acceleration_scaling)
+{
+  move_group_->setPlanningTime(planning_time);
+  move_group_->setGoalTolerance(goal_tolerance);
+  move_group_->setMaxVelocityScalingFactor(max_velocity_scaling);
+  move_group_->setMaxAccelerationScalingFactor(max_acceleration_scaling);
+  
+  RCLCPP_INFO(node_->get_logger(), "Set planning parameters: time=%.2fs, tolerance=%.4fm, velocity=%.2f, acceleration=%.2f",
+              planning_time, goal_tolerance, max_velocity_scaling, max_acceleration_scaling);
+}
+
+}  // namespace quadruped_manipulator_control

src/planner/RealtimeINterpolation.cpp

@@ -0,0 +1,310 @@
+#include "robot_control/realtime_interpolator.h"
+#include <ocs2_core/misc/LinearInterpolation.h>
+#include <ocs2_core/misc/Algebra.h>
+#include <rclcpp/rclcpp.hpp>
+#include <chrono>
+#include <algorithm>
+
+namespace robot_control {
+namespace planning {
+
+RealtimeInterpolator::RealtimeInterpolator(double controlFrequency) 
+    : controlFrequency_(controlFrequency),
+      dt_(1.0 / controlFrequency),
+      running_(true),
+      isActive_(false),
+      isGoalReached_(true),
+      interpolationType_(InterpolationType::CUBIC) {
+    // 启动插值线程
+    interpolationThread_ = std::thread(&RealtimeInterpolator::interpolationThread, this);
+}
+
+RealtimeInterpolator::~RealtimeInterpolator() {
+    stop();
+    if (interpolationThread_.joinable()) {
+        interpolationThread_.join();
+    }
+}
+
+void RealtimeInterpolator::setTargetTrajectories(const ocs2::TargetTrajectories& trajectories, 
+                                                InterpolationType type) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    
+    // 检查轨迹是否有效
+    if (trajectories.timeTrajectory.empty() || trajectories.stateTrajectory.empty()) {
+        RCLCPP_WARN(rclcpp::get_logger("RealtimeInterpolator"), "设置了空轨迹");
+        return;
+    }
+    
+    // 确保时间轨迹单调递增
+    bool timeValid = true;
+    for (size_t i = 1; i < trajectories.timeTrajectory.size(); ++i) {
+        if (trajectories.timeTrajectory[i] <= trajectories.timeTrajectory[i-1]) {
+            timeValid = false;
+            break;
+        }
+    }
+    
+    if (!timeValid) {
+        RCLCPP_ERROR(rclcpp::get_logger("RealtimeInterpolator"), "时间轨迹必须单调递增");
+        return;
+    }
+    
+    trajectories_ = trajectories;
+    interpolationType_ = type;
+    isActive_ = true;
+    isGoalReached_ = false;
+    
+    // 通知插值线程有新轨迹
+    cv_.notify_one();
+}
+
+bool RealtimeInterpolator::getCurrentReference(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    
+    if (!isActive_ || trajectories_.timeTrajectory.empty()) {
+        return false;
+    }
+    
+    // 检查是否超出轨迹时间范围
+    const double t_start = trajectories_.timeTrajectory.front();
+    const double t_end = trajectories_.timeTrajectory.back();
+    
+    if (t < t_start) {
+        x_ref = trajectories_.stateTrajectory.front();
+        u_ref = trajectories_.inputTrajectory.empty() ? ocs2::vector_t::Zero(0) : trajectories_.inputTrajectory.front();
+        return true;
+    } else if (t > t_end) {
+        x_ref = trajectories_.stateTrajectory.back();
+        u_ref = trajectories_.inputTrajectory.empty() ? ocs2::vector_t::Zero(0) : trajectories_.inputTrajectory.back();
+        isGoalReached_ = true;
+        return true;
+    }
+    
+    // 根据当前插值类型计算参考点
+    interpolate(t, x_ref, u_ref);
+    return true;
+}
+
+void RealtimeInterpolator::stop() {
+    running_ = false;
+    isActive_ = false;
+    cv_.notify_one();
+}
+
+void RealtimeInterpolator::setControlCallback(ControlCallback callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    controlCallback_ = callback;
+}
+
+void RealtimeInterpolator::interpolationThread() {
+    rclcpp::Time startTime = rclcpp::Clock().now();
+    
+    while (running_) {
+        // 等待新轨迹或定期唤醒
+        std::unique_lock<std::mutex> lock(mutex_);
+        cv_.wait_for(lock, std::chrono::duration<double>(dt_));
+        
+        if (!running_) {
+            break;
+        }
+        
+        if (isActive_ && !trajectories_.timeTrajectory.empty()) {
+            // 计算当前时间（相对于轨迹开始时间）
+            const double currentTime = (rclcpp::Clock().now() - startTime).seconds();
+            
+            // 获取当前参考点
+            ocs2::vector_t x_ref, u_ref;
+            bool valid = getCurrentReference(currentTime, x_ref, u_ref);
+            
+            // 检查是否到达目标
+            if (isGoalReached_) {
+                isActive_ = false;
+            }
+            
+            // 如果有回调函数且参考点有效，则调用回调
+            if (valid && controlCallback_) {
+                controlCallback_(currentTime, x_ref, u_ref);
+            }
+        }
+    }
+}
+
+void RealtimeInterpolator::interpolate(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    switch (interpolationType_) {
+        case InterpolationType::LINEAR:
+            linearInterpolation(t, x_ref, u_ref);
+            break;
+        case InterpolationType::CUBIC:
+            cubicInterpolation(t, x_ref, u_ref);
+            break;
+        case InterpolationType::QUINTIC:
+            quinticInterpolation(t, x_ref, u_ref);
+            break;
+        default:
+            linearInterpolation(t, x_ref, u_ref);
+    }
+}
+
+void RealtimeInterpolator::linearInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    // 使用OCS2的线性插值工具
+    const auto& time = trajectories_.timeTrajectory;
+    const auto& state = trajectories_.stateTrajectory;
+    const auto& input = trajectories_.inputTrajectory;
+    
+    size_t index;
+    double alpha;
+    ocs2::LinearInterpolation::interpolationIndices(time, t, index, alpha);
+    
+    x_ref = (1 - alpha) * state[index] + alpha * state[index + 1];
+    
+    if (!input.empty()) {
+        u_ref = (1 - alpha) * input[index] + alpha * input[index + 1];
+    } else {
+        u_ref = ocs2::vector_t::Zero(0);
+    }
+}
+
+void RealtimeInterpolator::cubicInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    const auto& time = trajectories_.timeTrajectory;
+    const auto& state = trajectories_.stateTrajectory;
+    const auto& input = trajectories_.inputTrajectory;
+    
+    if (time.size() < 2) {
+        linearInterpolation(t, x_ref, u_ref);
+        return;
+    }
+    
+    size_t index;
+    double alpha;
+    ocs2::LinearInterpolation::interpolationIndices(time, t, index, alpha);
+    
+    const double t0 = time[index];
+    const double t1 = time[index + 1];
+    const double dt = t1 - t0;
+    const double s = (t - t0) / dt;  // 标准化时间 [0, 1]
+    
+    // 三次多项式系数: a*s³ + b*s² + c*s + d
+    const ocs2::vector_t x0 = state[index];
+    const ocs2::vector_t x1 = state[index + 1];
+    
+    // 计算导数（速度）
+    ocs2::vector_t dx0, dx1;
+    
+    if (index > 0) {
+        dx0 = (x1 - state[index - 1]) / (t1 - time[index - 1]);
+    } else {
+        dx0 = (x1 - x0) / dt;  // 起点使用前向差分
+    }
+    
+    if (index + 1 < time.size() - 1) {
+        dx1 = (state[index + 2] - x0) / (time[index + 2] - t0);
+    } else {
+        dx1 = (x1 - x0) / dt;  // 终点使用后向差分
+    }
+    
+    // 三次多项式插值
+    const double s2 = s * s;
+    const double s3 = s2 * s;
+    
+    x_ref = (2*s3 - 3*s2 + 1) * x0 + 
+            (s3 - 2*s2 + s) * dt * dx0 + 
+            (-2*s3 + 3*s2) * x1 + 
+            (s3 - s2) * dt * dx1;
+    
+    // 输入插值使用线性插值
+    if (!input.empty()) {
+        u_ref = (1 - alpha) * input[index] + alpha * input[index + 1];
+    } else {
+        u_ref = ocs2::vector_t::Zero(0);
+    }
+}
+
+void RealtimeInterpolator::quinticInterpolation(double t, ocs2::vector_t& x_ref, ocs2::vector_t& u_ref) {
+    const auto& time = trajectories_.timeTrajectory;
+    const auto& state = trajectories_.stateTrajectory;
+    const auto& input = trajectories_.inputTrajectory;
+    
+    if (time.size() < 2) {
+        linearInterpolation(t, x_ref, u_ref);
+        return;
+    }
+    
+    size_t index;
+    double alpha;
+    ocs2::LinearInterpolation::interpolationIndices(time, t, index, alpha);
+    
+    const double t0 = time[index];
+    const double t1 = time[index + 1];
+    const double dt = t1 - t0;
+    const double s = (t - t0) / dt;  // 标准化时间 [0, 1]
+    
+    // 五次多项式需要位置、速度和加速度信息
+    const ocs2::vector_t x0 = state[index];
+    const ocs2::vector_t x1 = state[index + 1];
+    
+    // 计算速度 (一阶导数)
+    ocs2::vector_t dx0, dx1;
+    // 计算加速度 (二阶导数)
+    ocs2::vector_t ddx0, ddx1;
+    
+    // 计算速度
+    if (index > 0) {
+        dx0 = (x1 - state[index - 1]) / (t1 - time[index - 1]);
+    } else {
+        dx0 = (x1 - x0) / dt;
+    }
+    
+    if (index + 1 < time.size() - 1) {
+        dx1 = (state[index + 2] - x0) / (time[index + 2] - t0);
+    } else {
+        dx1 = (x1 - x0) / dt;
+    }
+    
+    // 计算加速度
+    if (index > 1) {
+        ddx0 = (dx0 - (x0 - state[index - 2]) / (t0 - time[index - 2])) / 
+               ((t0 - time[index - 2]) / 2.0);
+    } else {
+        ddx0 = ocs2::vector_t::Zero(x0.size());
+    }
+    
+    if (index + 2 < time.size() - 1) {
+        ddx1 = ((state[index + 2] - x1) / (time[index + 2] - t1) - dx1) / 
+               ((time[index + 2] - t1) / 2.0);
+    } else {
+        ddx1 = ocs2::vector_t::Zero(x1.size());
+    }
+    
+    // 五次多项式系数计算
+    const double s2 = s * s;
+    const double s3 = s2 * s;
+    const double s4 = s3 * s;
+    const double s5 = s4 * s;
+    
+    // 五次多项式基函数
+    const double a0 = 1.0 - 10.0*s3 + 15.0*s4 - 6.0*s5;
+    const double a1 = s - 6.0*s3 + 8.0*s4 - 3.0*s5;
+    const double a2 = 0.5*s2 - 1.5*s3 + 1.5*s4 - 0.5*s5;
+    const double b0 = 10.0*s3 - 15.0*s4 + 6.0*s5;
+    const double b1 = -4.0*s3 + 7.0*s4 - 3.0*s5;
+    const double b2 = 0.5*s3 - 1.0*s4 + 0.5*s5;
+    
+    // 计算插值位置
+    x_ref = a0 * x0 + 
+            a1 * dt * dx0 + 
+            a2 * dt*dt * ddx0 + 
+            b0 * x1 + 
+            b1 * dt * dx1 + 
+            b2 * dt*dt * ddx1;
+    
+    // 输入插值使用线性插值
+    if (!input.empty()) {
+        u_ref = (1 - alpha) * input[index] + alpha * input[index + 1];
+    } else {
+        u_ref = ocs2::vector_t::Zero(0);
+    }
+}
+
+} // namespace planning
+} // namespace robot_control

src/robot_control/CMakeLists.txt

@@ -1,69 +0,0 @@
-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

@@ -1,202 +0,0 @@
-
-                                 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

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

src/robot_control/action/MoveLeg.action

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

src/robot_control/action/MoveWaist.action

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

src/robot_control/action/MoveWheel.action

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

src/robot_control/include/arm_control.hpp

@@ -1,32 +0,0 @@
-#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

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

src/robot_control/include/control_base.hpp

@@ -1,89 +0,0 @@
-#pragma once
-
-#include <Eigen/Dense>
-#include "rclcpp/rclcpp.hpp"
-#include "trapezoidal_trajectory.hpp"
-#include "motion_parameters.hpp"
-
-#define POSITION_TOLERANCE 1.0
-#define TIME_OUT_COUNT 10000
-
-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_commands_;                // 当前关节指令
-            std::vector<double> joint_velocity_;                // 当前关节速度
-            std::vector<double> joint_velocity_commands_;       // 当前关节速度指令
-            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_;                                // 是否已设置目标点
-            bool is_cmd_send_finished_; 
-            bool is_joint_position_initialized_;              // 是否已初始化关节位置
-
-            int time_out_count_;                                     // 超时时间
-        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();
-
-            virtual bool IsReached(const std::vector<double>& target_joint); // 判断是否到达目标点
-
-            virtual void SetHomePositions(const std::vector<double>& home_positions);
-
-            bool checkJointLimits(const std::vector<double>& target_joint);
-    };
-
-}

src/robot_control/include/deceleration_planner.hpp

@@ -1,105 +0,0 @@
-#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/extended_kalman_filter.hpp

@@ -1,43 +0,0 @@
-#pragma once
-
-#include <iostream>
-#include <vector>
-#include <cmath>
-#include <Eigen/Dense>
-#include <fstream>  // 用于保存轨迹数据
-
-namespace robot_control {
-class EKF 
-{
-private:
-    double dt;
-    
-    // 卡尔曼滤波参数
-    Eigen::Matrix4d Q;        // 过程噪声协方差矩阵
-    Eigen::MatrixXd R;        // 测量噪声协方差矩阵
-    
-    // 状态向量 [x, y, θ, ω]
-    Eigen::Vector4d X;
-    // 协方差矩阵
-    Eigen::Matrix4d P;
-    
-    // 角度归一化到[-π, π]
-    double normalizeAngle(double angle) {
-        while (angle > M_PI) angle -= 2.0 * M_PI;
-        while (angle < -M_PI) angle += 2.0 * M_PI;
-        return angle;
-    }
-
-public:
-    EKF(double init_x = 0, double init_y = 0, double init_theta = 0, double init_omega = 0);
-    
-    // 预测步骤
-    void predict(double v, double omega_wheel);
-    
-    // 更新步骤
-    void update(double gyro_omega);
-    
-    // 获取当前状态
-    Eigen::Vector4d getState() { return X; }
-};
-}

src/robot_control/include/leg_control.hpp

@@ -1,39 +0,0 @@
-#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

@@ -1,310 +0,0 @@
-#pragma once
-
-#include <vector>
-#include <iostream>
-#include <map>
-#include <string>
-
-
-#define CYCLE_TIME 8    // 插补周期（毫秒）
-
-#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.55;
-
-                wheel_length_ = {
-                    wheel_radius_,
-                    wheel_separation_
-                };
-
-                // 腿长参数 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_ = {
-                    35,
-                    100,
-                    50,
-                    50,
-                    50,
-                    50
-                }; 
-
-                max_joint_acceleration_ = {
-                    80,
-                    200,
-                    100,
-                    100,
-                    100,
-                    100
-                };
-
-                // 初始化关节索引
-                wheel_joint_indices_ = {0, 1}; 
-
-                waist_joint_indices_ = {0}; 
-
-                leg_joint_indices_ = {2, 3, 4, 5};
-
-                total_joints_ = 1; 
-
-                // 初始化关节方向
-                wheel_joint_directions_ = {1, -1};  
-
-                waist_joint_directions_ = {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, 40.0,     -40.0, LimitType::POSITION),
-                    JointLimit(1, 190.0,    -190.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
-                    // 52.66
-                };
-
-                leg_zero_positions_ = {
-                    -18.36, 
-                    // 129.71, 
-                    // 45.02,
-                    25.54, 
-                    -21.54, 
-                    // 167.96, 
-                    // 16.26, 
-                    -127.24
-                };
-
-                waist_home_positions_ = {
-                    181.0
-                    // 52.66
-                };
-
-
-                // 后腿零点为站直时候的状态, 然后 + 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

@@ -1,127 +0,0 @@
-#pragma once
-
-#include <cmath>
-#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 "sensor_msgs/msg/imu.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"
-#include "interfaces/msg/imu_msg.hpp"
-
-using MotorPos = interfaces::msg::MotorPos;
-using ImuMsg = sensor_msgs::msg::Imu;
-
-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);
-            void SetJogWheel(bool value);
-            bool GetJogWheel();
-
-            void WheelReset(){isWheelHomed_ = false;};
-            void ImuReset(){isGyroInited_ = false;}
-
-            // 检查参数是否合理
-            bool SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle);
-            bool SetMoveLegParameters(double moveLegDistance);
-            bool SetMoveWaistParameters(double movePitchAngle, double moveYawAngle);
-            
-            // 机器人关节指令
-            std_msgs::msg::Float64MultiArray GetJointCommands();
-            std_msgs::msg::Float64MultiArray GetJointFeedback();
-            std_msgs::msg::Float64MultiArray GetWheelCommands();
-            std_msgs::msg::Float64MultiArray GetWheelFeedback();
-            double GetWheelRatio();
-
-            // 机器人状态
-            void CheckJointLimits(std_msgs::msg::Float64MultiArray& jointCommands);
-            void UpdateJointStates(const sensor_msgs::msg::JointState::SharedPtr msg);
-            void UpdateWheelStates(const MotorPos::SharedPtr msg);
-            void UpdateImuMsg(const ImuMsg::SharedPtr msg);
-
-            MotionParameters GetMotionParameters();
-
-            std::vector<double> GetImuDifference();
-
-            bool IsMoving(MovementPart part);
-
-            bool RobotInitFinished();
-            
-        private:
-            void init();
-
-            MotionParameters motionParams_;
-
-            bool isWaistHomed_;
-            bool isLegHomed_;
-            bool isWheelHomed_;
-
-            bool is_wheel_jog_;
-
-            // 控制器
-            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_;
-
-            // 陀螺仪状态
-            ImuMsg imuMsg_;
-            bool isGyroInited_;
-            std::vector<double> gyroValues_; // 陀螺仪数据（弧度/秒）
-            std::vector<double> gyroInitValues_; // 陀螺仪初始位置
-            std::vector<double> gyroVelocities_; // 陀螺仪速度（弧度/秒）
-            std::vector<double> gyroAccelerations_; // 陀螺仪加速度（弧度/秒^2）
-            
-            // 临时变量
-            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();
-
-            void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
-            void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw);
-    };
-
-} // namespace robot_control_manager

src/robot_control/include/robot_control_node.hpp

@@ -1,115 +0,0 @@
-#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"
-#include "interfaces/srv/motor_param.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;
-
-using MotorParam = interfaces::srv::MotorParam;
-
-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_;
-            rclcpp::Subscription<ImuMsg>::SharedPtr imuMsgSub_;
-            rclcpp::Client<MotorParam>::SharedPtr motorParamClient_;
-
-            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_;
-
-            double lastSpeed_;
-
-            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);
-            void ImuMsgCallback(const ImuMsg::SharedPtr msg);
-    };
-
-} // namespace robot_control
-

src/robot_control/include/robot_kinematics.hpp

@@ -1,90 +0,0 @@
-#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

@@ -1,111 +0,0 @@
-#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

@@ -1,172 +0,0 @@
-#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

@@ -1,76 +0,0 @@
-#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

@@ -1,34 +0,0 @@
-#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;
-
-            // void SetHomePositions(const std::vector<double>& home_positions) 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

@@ -1,41 +0,0 @@
-#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;
-
-            double moveRatio_= 1.0;
-
-            double lastMoveDistance = 0.0;
-
-            double GetWheelRatioInternal(){ return moveRatio_ ;};
-
-            bool SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle);
-
-            bool SetMoveWheelParametersInternalJog(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

@@ -1,23 +0,0 @@
-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

@@ -1,40 +0,0 @@
-<?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

@@ -1,112 +0,0 @@
-#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

@@ -1,206 +0,0 @@
-#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);
-    joint_commands_.resize(total_joints_, 0.0);
-    joint_velocity_commands_.resize(total_joints_, 0.0);
-    
-    // 初始化梯形轨迹规划器
-    trapezoidalTrajectory_ = new TrapezoidalTrajectory(maxSpeed, maxAcc);
-
-    is_moving_ = false;
-    is_stopping_ = false;
-    is_target_set_ = false;
-    is_cmd_send_finished_ = false;
-    is_joint_position_initialized_ = false;
-
-    stopDurationTime_ = 0.0;
-    movingDurationTime_ = 0.0;
-
-    time_out_count_ = 0;
-}
-
-ControlBase::~ControlBase()
-{
-    delete trapezoidalTrajectory_;
-}
-
-void ControlBase::SetHomePositions(const std::vector<double>& home_positions)
-{
-    joint_home_positions_ = home_positions;
-    joint_position_desired_ = home_positions;
-}
-
-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_)
-    {
-        if (IsReached(target_joint))
-        {
-            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;
-        joint_commands_ = target_joint;
-        is_moving_ = false;
-        movingDurationTime_ = 0.0;
-
-        return true;
-    }
-
-    joint_commands_ = desired_pos;
-    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;
-
-    if( !is_joint_position_initialized_)
-    {
-        joint_commands_ = joint_position_;
-        is_joint_position_initialized_ = true;
-        std::cout << "joint commands initialized" << std::endl;
-    }
-
-    // 3. 更新关节速度
-    joint_velocity_ = joint_velocities;
-
-    // 4. 更新关节力矩
-    joint_torque_ = joint_torques;
-}
-
-bool ControlBase::IsMoving()
-{
-    return is_moving_;
-}
-
-bool ControlBase::IsReached(const std::vector<double>& target_joint)
-{
-    bool result = true;
-
-    for (size_t i = 0; i < target_joint.size(); i++)
-    {
-        if (std::abs(joint_position_[i] - target_joint[i]) > POSITION_TOLERANCE)
-        {
-            result = false;
-            break;
-        }
-    }
-
-    return result;
-}
-
-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 + 1, target_joint[i], minLimits_[i], maxLimits_[i]);
-            return false;
-        }
-    }
-
-    return true;
-}
-
-}  // namespace robot_control

src/robot_control/src/extended_kalman_filter.cpp

@@ -1,73 +0,0 @@
-#include "extended_kalman_filter.hpp"
-
-namespace robot_control {
-EKF::EKF(double init_x, double init_y, double init_theta, double init_omega) 
-{
-    this->dt = 0.001;
-    
-    // 初始化状态向量
-    X << init_x, init_y, init_theta, init_omega;
-    
-    // 初始化协方差矩阵
-    P = Eigen::Matrix4d::Zero();
-    P.diagonal() << 0.1, 0.1, 0.05, 0.01;
-}
-    
-    // 预测步骤
-void EKF::predict(double v, double omega_wheel) {
-    // double x = X(0);
-    // double y = X(1);
-    double theta = X(2);
-    // double omega = X(3);
-    
-    // 状态转移矩阵F
-    Eigen::Matrix4d F = Eigen::Matrix4d::Identity();
-    F(0, 2) = -v * sin(theta) * dt;
-    F(1, 2) = v * cos(theta) * dt;
-    F(2, 3) = dt;
-    
-    // 输入矩阵B
-    Eigen::Matrix<double, 4, 2> B;
-    B << cos(theta) * dt, 0,
-            sin(theta) * dt, 0,
-            0, dt,
-            0, 1;
-            
-    // 控制输入
-    Eigen::Vector2d u;
-    u << v, omega_wheel;
-    
-    // 预测状态
-    X = F * X + B * u;
-    // 预测协方差
-    P = F * P * F.transpose() + Q;
-    
-    // 归一化航向角
-    X(2) = normalizeAngle(X(2));
-}
-    
-// 更新步骤
-void EKF::update(double gyro_omega) {
-    // 观测矩阵H
-    Eigen::Matrix<double, 1, 4> H;
-    H << 0, 0, 0, 1;
-    
-    // 测量残差
-    Eigen::VectorXd z(1);
-    z << gyro_omega;
-    Eigen::VectorXd y = z - H * X;
-    
-    // 计算卡尔曼增益
-    Eigen::MatrixXd S = H * P * H.transpose() + R;
-    Eigen::MatrixXd K = P * H.transpose() * S.inverse();
-    
-    // 更新状态
-    X = X + K * y;
-    // 更新协方差
-    Eigen::Matrix4d I = Eigen::Matrix4d::Identity();
-    P = (I - K * H) * P;
-    
-    // 归一化航向角
-    X(2) = normalizeAngle(X(2));
-}
-}

src/robot_control/src/leg_control.cpp

@@ -1,142 +0,0 @@
-#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

@@ -1,26 +0,0 @@
-#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

@@ -1,65 +0,0 @@
-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

@@ -1,574 +0,0 @@
-#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);
-    }
-
-    gyroValues_.resize(4, 0.0);
-    gyroVelocities_.resize(3, 0.0);
-    gyroAccelerations_.resize(3, 0.0);
-
-    is_wheel_jog_ = false;
-
-    // Initialize the wheel commands
-    for (size_t i = 0; i < motionParams_.wheel_joint_indices_.size(); i++)
-    {
-        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;
-    isWheelHomed_ = false;
-
-    isJointInitValueSet_ = false;
-    isGyroInited_ = false;
-}
-
-RobotControlManager::~RobotControlManager()
-{
-    delete waistController_;
-    delete legController_;
-    delete wheelController_;
-}
-
-void RobotControlManager::SetJogWheel(bool value)
-{
-    is_wheel_jog_ = value;
-}
-
-bool RobotControlManager::GetJogWheel()
-{
-    return is_wheel_jog_;
-}
-
-bool RobotControlManager::SetMoveWheelParameters(double moveWheelDistance, double moveWheelAngle)
-{
-    if (is_wheel_jog_)
-    {
-        return wheelController_->SetMoveWheelParametersInternalJog(moveWheelDistance, moveWheelAngle);
-    }
-
-    return wheelController_->SetMoveWheelParametersInternal(moveWheelDistance, moveWheelAngle);
-}
-
-bool RobotControlManager::SetMoveLegParameters(double moveLegDistance)
-{
-    return legController_->SetMoveLegParametersInternal(moveLegDistance);
-}
-
-bool RobotControlManager::SetMoveWaistParameters(double movePitchAngle, double moveYawAngle)
-{
-    return waistController_->SetMoveWaistParametersInternal(movePitchAngle, moveYawAngle);
-}
-
-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;
-}
-
-double RobotControlManager::GetWheelRatio()
-{
-    return wheelController_->GetWheelRatioInternal();
-}
-
-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
-}
-
-
-
-std::vector<double> RobotControlManager::GetImuDifference()
-{
-    std::vector<double> result;
-    result.resize(gyroValues_.size(), 0.0);
-
-    for (size_t i = 0; i < gyroValues_.size(); i++)
-    {
-        result[i] = gyroValues_[i] - gyroInitValues_[i];
-    }
-
-    //TODO: optimization
-    // gyroInitValues_[2] = gyroInitValues_[2] - 0.03;
-
-    std::cout << "get gyro init value : " << gyroInitValues_[2]  << std::endl;
-    std::cout << "get gyro value : " << gyroValues_[2]  << std::endl;
-    
-    return result;
-}
-
-void RobotControlManager::UpdateImuMsg(const ImuMsg::SharedPtr msg)
-{
-    imuMsg_ = *msg;
-    ImuMsg tempMsg = imuMsg_;
-
-    if (tempMsg.orientation.w == 0 && tempMsg.orientation.x == 0 && tempMsg.orientation.y == 0 && tempMsg.orientation.z == 0)
-    {
-        return;
-    }
-
-    if (tempMsg.linear_acceleration.x == 0 && tempMsg.linear_acceleration.y == 0 && tempMsg.linear_acceleration.z == 0)
-    {
-        return;
-    }
-
-    QuaternionToRPYDeg(
-        tempMsg.orientation.w, 
-        tempMsg.orientation.x, 
-        tempMsg.orientation.y, 
-        tempMsg.orientation.z, 
-        gyroValues_[0], 
-        gyroValues_[1], 
-        gyroValues_[2]
-    );
-
-    if (!isGyroInited_)
-    {
-        gyroInitValues_ = gyroValues_;
-        isGyroInited_ = true;
-
-        std::cout << "IMU values : " <<  gyroInitValues_[0] << "  " << gyroInitValues_[1] << "  " << gyroInitValues_[2] << std::endl;
-    }
-    
-
-    gyroVelocities_[0] = tempMsg.angular_velocity.x;
-    gyroVelocities_[1] = tempMsg.angular_velocity.y;
-    gyroVelocities_[2] = tempMsg.angular_velocity.z;
-
-    gyroAccelerations_[0] = tempMsg.linear_acceleration.x;
-    gyroAccelerations_[1] = tempMsg.linear_acceleration.y;
-    gyroAccelerations_[2] = tempMsg.linear_acceleration.z;
-
-    // std::cout << "gyroValues_ : " << gyroValues_[0] << " " << gyroValues_[1] << " " << gyroValues_[2] << std::endl;
-
-}
-
-void RobotControlManager::QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
-{
-    double norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz);
-    if (norm < 1e-12) {  // 避免除以零
-        roll = 0;
-        pitch = 0;
-        yaw = 0;
-        return;
-    }
-
-    qw /= norm;
-    qx /= norm;
-    qy /= norm;
-    qz /= norm;
-    
-    // 计算Roll角（X轴）
-    roll = atan2(2 * (qw*qx + qy*qz), 1 - 2 * (qx*qx + qy*qy));
-    
-    // 计算Pitch角（Y轴），限制输入范围避免数值误差（asin输入需在[-1,1]）
-    double sin_pitch = 2 * (qw*qy - qz*qx);
-    sin_pitch = std::max(std::min(sin_pitch, 1.0), -1.0); 
-    pitch = asin(sin_pitch);
-    
-    // 计算Yaw角（Z轴）
-    yaw = atan2(2 * (qw*qz + qx*qy), 1 - 2 * (qy*qy + qz*qz));
-}
-
-void RobotControlManager::QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
-{
-    QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw);
-
-    roll = roll * 180.0 / M_PI;
-    pitch = pitch * 180.0 / M_PI;
-    yaw = yaw * 180.0 / M_PI;
-}
-
-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;
-    }
-
-    if (!isWheelHomed_)
-    {
-        wheelController_ -> SetHomePositions(wheelPositions_);
-        isWheelHomed_ = true;
-    }
-    
-
-    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 true; // legController_->IsMoving();
-        case MovementPart::WAIST:
-            return waistController_->IsMoving();
-        case MovementPart::ALL:
-            return wheelController_->IsMoving()  && waistController_->IsMoving() ; //&& legController_->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 waistStopped && wheelStopped; //legStopped  && 
-}
-
-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);
-    }
-
-    isLegHomed_ = true;
-    
-    // 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

@@ -1,774 +0,0 @@
-#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;
-    lastSpeed_ = 51;
- 
-    // 初始化数据文件（设置路径，确保目录存在）
-    #if 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);  // 移动到文件末尾
-    }
-    #endif
-
-    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);
-    imuMsgSub_ = this->create_subscription<ImuMsg>("/imu_msg", 10,std::bind(&RobotControlNode::ImuMsgCallback, this, std::placeholders::_1));
-    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));  // 绑定到新的控制函数);
-    motorParamClient_ = this->create_client<MotorParam>("/motor_param");
-
-    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)
-{
-    (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)
-{
-    (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;
-
-    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)
-{
-    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)
-{
-    (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;
-
-    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)
-{
-    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)
-{
-    (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;
-    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)
-{
-    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 (goal->move_distance > 2.1 || goal->move_distance < -1.0 ||  goal->move_angle < -10 ||  goal->move_angle > 10 )
-    {
-        RCLCPP_ERROR(this->get_logger(), "exceed limit");
-        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)
-{
-    (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>();
-
-    double wheelAngle = 0;
-
-    if (abs(goal->move_angle) > 0)
-    {
-        robotControlManager_.SetJogWheel(true);
-        wheelAngle = goal->move_angle;
-    }
-
-    if (abs(goal->move_angle) == 0 && abs(goal->move_distance) == 0)
-    {
-        robotControlManager_.SetJogWheel(false);
-    }
-    
-    if (!robotControlManager_.GetJogWheel())
-    {
-        double tempValue = robotControlManager_.GetImuDifference()[2];
-        wheelAngle = abs(tempValue) > 40.0 ? 0.0 : tempValue;
-    }
-    
-
-    // double wheelAngle = 1.5;
-
-    robotControlManager_.SetMoveWheelParameters(goal->move_distance, wheelAngle);
-
-    double ratio = robotControlManager_.GetWheelRatio();
-
-    auto request = std::make_shared<MotorParam::Request>();
-
-    if((goal->move_distance > 0.1 ) && !robotControlManager_.GetJogWheel())  // || goal->move_distance < -0.5
-    {
-        // if (lastSpeed_ < 51 &&  goal->move_distance < -0.5)
-        // {
-        //      ratio = 1.02;
-        // }
-        request->motor_id = 1;
-        request->max_speed = static_cast<uint16_t>(round((ratio) * 51));
-
-        // if (goal->move_distance < -0.5)
-        // {
-        //     request->add_acc = 4;
-        // }
-        
-        request->add_acc = 8;
-        request->dec_acc = 8;
-        
-        motorParamClient_->async_send_request(request);
-
-        std::cout << "set motor 1 max speed : " << request->max_speed << std::endl;
-
-        rclcpp::sleep_for(std::chrono::milliseconds(50)); 
-    }
-    
-    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])};
-
-    // std::cout << "wheel_commands.data[0] " << wheel_commands.data[0] << std::endl;
-    // std::cout << "wheel_commands.data[1] " << wheel_commands.data[1] << std::endl;
-
-    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 (abs(joint_feedback.data[0] - wheel_commands.data[0]) < 20.0 && abs(joint_feedback.data[1] - wheel_commands.data[1]) < 20.0)
-        {
-            move_wheel_executing_ = false;
-        }
-
-        feedback->current_pos = joint_feedback.data[0];
-
-        //TODO: get the angle from lidar.
-        //feedback->current_angle = robotControlManager_.GetImuDifference()[2];
-
-        goal_handle->publish_feedback(feedback);
-
-        loop_rate.sleep();
-    }
-
-    if ((goal->move_distance > 0.0) && !robotControlManager_.GetJogWheel())   //|| goal->move_distance < -0.5
-    {
-        request->motor_id = 1;
-        request->max_speed = 51;
-        request->add_acc = 8;
-        request->dec_acc = 8;
-        
-        motorParamClient_->async_send_request(request);
-    }
-
-    // 检查目标是否仍在活跃状态
-    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))
-        {
-            robotControlManager_.SetJogWheel(false);
-            move_home_executing_ = false;
-            robotControlManager_.WheelReset();
-            robotControlManager_.ImuReset();
-        }
-    }
-
-    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();
-    }
-
-    #if 0
-    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();  // 强制刷新
-    }
-
-    #endif
-
-    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::ImuMsgCallback(const ImuMsg::SharedPtr cmd_msg)
-{
-    if (!cmd_msg) {  // 检查消息是否有效
-        std::cout << "get null imu msg!" << std::endl;
-        return;
-    }
-
-    robotControlManager_.UpdateImuMsg(cmd_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

@@ -1,213 +0,0 @@
-#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

@@ -1,184 +0,0 @@
-#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

@@ -1,419 +0,0 @@
-#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

@@ -1,413 +0,0 @@
-#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);  // 各轴理论巡航速度
-    total_time_ = 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];
-
-            // std::cout << "total time :  " << total_time_ << std::endl;
-            // std::cout << "acceleration_time_ :  " << acceleration_time_ << std::endl;
-            // std::cout << "deceleration_time_ :  " << deceleration_time_ << std::endl;
-            // std::cout << "cruise_time_ :  " << cruise_time_ << std::endl;
-        }
-    }
-    
-    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

@@ -1,247 +0,0 @@
-#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

@@ -1,133 +0,0 @@
-#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;
-        }
-    }
-    
-    if (!is_cmd_send_finished_)
-    {
-        is_cmd_send_finished_ = MoveToTargetJoint(joint_positions, joint_position_desired_, dt);
-    }
-    else
-    {
-        if (IsReached(joint_position_desired_) || (time_out_count_ > TIME_OUT_COUNT))
-        {
-            std::cout << "Waist reached target position!" << std::endl;
-            is_target_set_ = false;
-            is_cmd_send_finished_ = false;
-            time_out_count_ = 0;
-            return true;
-        }
-        else
-        {
-            time_out_count_ ++;
-            // std::cout << "Waist not reached target position!" << std::endl;
-            return false;
-        }
-    }
-
-    return false;
-}
-
-// void WaistControl::SetHomePositions(const std::vector<double>& home_positions)
-// {
-//     if (home_positions.size() != total_joints_)
-//     {
-//         throw std::invalid_argument("Home positions size does not match total joints!");
-//     }
-
-//     // if (home_positions[1] < 0)
-//     // {
-//     //     joint_home_positions_[1] = joint_home_positions_[1] - 360.0;
-//     // }
-    
-//     std::cout << "Home positions set to: " << joint_home_positions_[0] << ", " << joint_home_positions_[1] << std::endl;
-// }
-
-bool WaistControl::SetMoveWaistParametersInternal(double movepitch, double moveyaw)
-{
-    std::vector<double> tempPosition;
-    tempPosition.resize(total_joints_, 0.0);
-
-    tempPosition[0] = joint_commands_[0] - joint_home_positions_[0] + movepitch;
-    // tempPosition[1] = joint_commands_[1] - joint_home_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_home_positions_[i];
-    }
-    
-    // std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << ", " << joint_position_desired_[1] << std::endl;
-    std::cout << "Waist Target Joint Position: " << joint_position_desired_[0] << std::endl;
-
-    is_target_set_ = true;
-
-    return true;
-}
-
-}  // namespace robot_control

src/robot_control/src/wheel_control.cpp

@@ -1,274 +0,0 @@
-#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_;
-}
-
-int tempAdjustCount = 0;
-
-bool WheelControl::SetMoveWheelParametersInternal(double moveWheelDistance, double moveWheelAngle)
-{
-    double original1 = joint_position_desired_[0];
-    double original2 = joint_position_desired_[1];
-
-    std::vector<double> tempHomePositions;
-    std::vector<double> tempDesiredPositions;
-
-    tempHomePositions.resize(total_joints_, 0.0);
-    tempDesiredPositions.resize(total_joints_, 0.0);
-
-    for (size_t i = 0; i < total_joints_; i++)
-    {
-        tempHomePositions[i] = joint_home_positions_[i];
-        tempDesiredPositions[i] = joint_position_desired_[i];
-    }
-    
-
-    // 设置移动轮参数
-    double theta = moveWheelDistance / lengthParameters_[0] * 180.0 / M_PI;
-
-    double beta = moveWheelAngle * 0.8 / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;
-
-    if(moveWheelDistance > lastMoveDistance && (moveWheelDistance - lastMoveDistance > 1.5) && beta != 0)
-    {
-        if (beta > 0)
-        {
-            tempHomePositions[0] = joint_home_positions_[0] - beta - 8;
-        }
-        else
-        {
-            tempHomePositions[0] = joint_home_positions_[0] - beta + 8;
-        }
-    }
-
-    if(moveWheelDistance < lastMoveDistance && (moveWheelDistance - lastMoveDistance < -1.5) && beta != 0)
-    {
-        tempAdjustCount ++; 
-        if (beta > 0)
-        {
-            tempHomePositions[0] = joint_home_positions_[0] - beta * 2.0;
-            
-        }
-        else
-        {
-            tempHomePositions[0] = joint_home_positions_[0] - beta;
-        }
-
-         if (tempAdjustCount == 1)
-        {
-            tempHomePositions[0] - 40;
-            tempAdjustCount = 0;
-        }
-
-        // if (tempAdjustCount == 2)
-        // {
-        //     tempAdjustCount = 0;
-        // }
-        
-    }
-    
-    for (size_t i = 0; i < joint_directions_.size(); i++)
-    {
-        if (joint_directions_[i] == 1)
-        {
-            tempDesiredPositions[i] = tempHomePositions[i] + theta;
-        }
-        else
-        {
-            tempDesiredPositions[i] = tempHomePositions[i] - theta; // * 0.98 * 1.02;
-        }
-    }
-
-    double a1 = abs(tempDesiredPositions[0] - original1);
-    double a2 = abs(tempDesiredPositions[1] - original2);
-
-    if (a2 > 0.0 && a1 > 0.0)
-    {
-        moveRatio_ = ((a1 / a2) - 1.0) * 0.9;
-    }
-    else
-    {
-        moveRatio_ = 0.0;
-    }
-
-    moveRatio_ = moveRatio_ + 1.0;
-
-    if (moveRatio_ > 0.99 && moveRatio_ < 1.009)
-    {
-        for (size_t i = 0; i < joint_directions_.size(); i++)
-        {
-            if (joint_directions_[i] == 1)
-            {
-                joint_position_desired_[i] = joint_home_positions_[i] + theta;
-            }
-            else
-            {
-                joint_position_desired_[i] = joint_home_positions_[i] - theta; // * 0.98 * 1.02;
-            }
-        }
-    }
-    else
-    {
-        std::cout << "change distance " << std::endl;
-        for (size_t i = 0; i < joint_directions_.size(); i++)
-        {
-           joint_position_desired_[i] = tempDesiredPositions[i];
-           joint_home_positions_[i] = tempHomePositions[i];
-        }
-    }
-
-    std::cout << "motor 1 distance : " << joint_position_desired_[0] - original1 << std::endl;
-    std::cout << "motor 2 distance : " << joint_position_desired_[1] - original2 << std::endl;
-
-    std::cout << "Set move ratio : " << moveRatio_ << std::endl;
-
-    // std::cout << "Move Wheel joint_position_desired_: " << joint_position_desired_[0] <<  " , " << joint_position_desired_[1] << std::endl;
-
-    lastMoveDistance = moveWheelDistance;
-
-    is_target_set_ = true;
-
-    return true;
-}
-
-bool WheelControl::SetMoveWheelParametersInternalJog(double moveWheelDistance, double moveWheelAngle)
-{
-    double original1 = joint_position_desired_[0];
-    double original2 = joint_position_desired_[1];
-
-    std::vector<double> tempHomePositions;
-    std::vector<double> tempDesiredPositions;
-
-    tempHomePositions.resize(total_joints_, 0.0);
-    tempDesiredPositions.resize(total_joints_, 0.0);
-
-    for (size_t i = 0; i < total_joints_; i++)
-    {
-        tempHomePositions[i] = joint_home_positions_[i];
-        tempDesiredPositions[i] = joint_position_desired_[i];
-    }
-    
-
-    // 设置移动轮参数
-    double theta = moveWheelDistance / lengthParameters_[0] * 180.0 / M_PI;
-
-    double beta = moveWheelAngle / 360.0 * M_PI * 2 * lengthParameters_[1] / lengthParameters_[0] * 180.0 / M_PI;
-
-    for (size_t i = 0; i < joint_directions_.size(); i++)
-    {
-        tempHomePositions[0] = joint_home_positions_[0] +  beta;
-        tempHomePositions[1] = joint_home_positions_[1] +  beta;
-    }
-
-    
-    for (size_t i = 0; i < joint_directions_.size(); i++)
-    {
-        if (joint_directions_[i] == 1)
-        {
-            tempHomePositions[i] += theta;
-            tempDesiredPositions[i] = tempHomePositions[i] ;
-        }
-        else
-        {
-             tempHomePositions[i] -= theta;
-            tempDesiredPositions[i] = tempHomePositions[i]; // * 0.98 * 1.02;
-        }
-    }
-
-    moveRatio_ = 1;
-
-    for (size_t i = 0; i < joint_directions_.size(); i++)
-    {
-        joint_position_desired_[i] = tempDesiredPositions[i];
-        joint_home_positions_[i] = tempHomePositions[i];
-    }
-
-
-    std::cout << "motor 1 distance : " << joint_position_desired_[0] - original1 << std::endl;
-    std::cout << "motor 2 distance : " << joint_position_desired_[1] - original2 << std::endl;
-
-    std::cout << "Set move ratio : " << moveRatio_ << std::endl;
-
-    // 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;
-}
-
-}

src/state_machine/RobotStateMachine.cpp

@@ -0,0 +1,113 @@
+#include "robot_control/state_machine.h"
+
+namespace robot_control {
+
+StateMachine::StateMachine() : current_state_(RobotState::INITIALIZING) {
+    RCLCPP_INFO(rclcpp::get_logger("StateMachine"), "状态机初始化，初始状态: %s", stateToString(current_state_).c_str());
+}
+
+void StateMachine::handleEvent(RobotEvent event) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    RobotState new_state = current_state_;
+    
+    // 根据当前状态和事件确定新状态
+    switch (current_state_) {
+        case RobotState::INITIALIZING:
+            if (event == RobotEvent::INIT_COMPLETE) {
+                new_state = RobotState::STANDBY;
+            }
+            break;
+            
+        case RobotState::STANDBY:
+            if (event == RobotEvent::START_MOTION) {
+                new_state = RobotState::MOVING;
+            } else if (event == RobotEvent::ERROR_OCCURRED) {
+                new_state = RobotState::ERROR;
+            }
+            break;
+            
+        case RobotState::MOVING:
+            if (event == RobotEvent::MOTION_COMPLETE) {
+                new_state = RobotState::STANDBY;
+            } else if (event == RobotEvent::STOP_MOTION) {
+                new_state = RobotState::STANDBY;
+            } else if (event == RobotEvent::ERROR_OCCURRED) {
+                new_state = RobotState::ERROR;
+            } else if (event == RobotEvent::EMERGENCY_STOP) {
+                new_state = RobotState::ESTOP;
+            }
+            break;
+            
+        case RobotState::ERROR:
+            if (event == RobotEvent::ERROR_RESOLVED) {
+                new_state = RobotState::STANDBY;
+            } else if (event == RobotEvent::EMERGENCY_STOP) {
+                new_state = RobotState::ESTOP;
+            }
+            break;
+            
+        case RobotState::ESTOP:
+            if (event == RobotEvent::ESTOP_RELEASED) {
+                new_state = RobotState::INITIALIZING;
+            }
+            break;
+    }
+    
+    // 如果状态发生变化，调用状态回调
+    if (new_state != current_state_) {
+        RCLCPP_INFO(rclcpp::get_logger("StateMachine"), "状态转换: %s -> %s", 
+                   stateToString(current_state_).c_str(), 
+                   stateToString(new_state).c_str());
+        
+        // 调用退出当前状态的回调
+        if (exit_state_callbacks_[current_state_]) {
+            exit_state_callbacks_[current_state_]();
+        }
+        
+        current_state_ = new_state;
+        
+        // 调用进入新状态的回调
+        if (enter_state_callbacks_[current_state_]) {
+            enter_state_callbacks_[current_state_]();
+        }
+        
+        // 调用通用状态变化回调
+        if (state_change_callback_) {
+            state_change_callback_(current_state_);
+        }
+    }
+}
+
+RobotState StateMachine::getCurrentState() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return current_state_;
+}
+
+std::string StateMachine::stateToString(RobotState state) {
+    switch (state) {
+        case RobotState::INITIALIZING: return "INITIALIZING";
+        case RobotState::STANDBY: return "STANDBY";
+        case RobotState::MOVING: return "MOVING";
+        case RobotState::ERROR: return "ERROR";
+        case RobotState::ESTOP: return "ESTOP";
+        default: return "UNKNOWN";
+    }
+}
+
+void StateMachine::setEnterStateCallback(RobotState state, std::function<void()> callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    enter_state_callbacks_[state] = callback;
+}
+
+void StateMachine::setExitStateCallback(RobotState state, std::function<void()> callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    exit_state_callbacks_[state] = callback;
+}
+
+void StateMachine::setStateChangeCallback(std::function<void(RobotState)> callback) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    state_change_callback_ = callback;
+}
+
+} // namespace robot_control
+    

urdf/robot.urdf

@@ -0,0 +1,487 @@
+<?xml version="1.0"?>
+<robot name="dual_arm_leg_robot">
+  <!-- 基础底座 -->
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.3 0.3 0.1"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.3 0.3 0.1"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 左手 (6自由度) -->
+  <!-- 肩关节1 - 旋转 -->
+  <joint name="left_arm_joint1" type="revolute">
+    <parent link="base_link"/>
+    <child link="left_arm_link1"/>
+    <origin xyz="0.1 0.15 0.05" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="left_arm_link1">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="blue">
+        <color rgba="0 0 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 肩关节2 -->
+  <joint name="left_arm_joint2" type="revolute">
+    <parent link="left_arm_link1"/>
+    <child link="left_arm_link2"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="left_arm_link2">
+    <visual>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.02"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 肘关节 -->
+  <joint name="left_arm_joint3" type="revolute">
+    <parent link="left_arm_link2"/>
+    <child link="left_arm_link3"/>
+    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="left_arm_link3">
+    <visual>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.02"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 腕关节1 -->
+  <joint name="left_arm_joint4" type="revolute">
+    <parent link="left_arm_link3"/>
+    <child link="left_arm_link4"/>
+    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="left_arm_link4">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 腕关节2 -->
+  <joint name="left_arm_joint5" type="revolute">
+    <parent link="left_arm_link4"/>
+    <child link="left_arm_link5"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="left_arm_link5">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 腕关节3 -->
+  <joint name="left_arm_joint6" type="revolute">
+    <parent link="left_arm_link5"/>
+    <child link="left_arm_end_effector"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <limit lower="-3.14" upper="3.14" effort="100" velocity="1.0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="left_arm_end_effector">
+    <visual>
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+      <material name="cyan">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 右手 (6自由度) -->
+  <!-- 肩关节1 - 旋转 -->
+  <joint name="right_arm_joint1" type="revolute">
+    <parent link="base_link"/>
+    <child link="right_arm_link1"/>
+    <origin xyz="0.1 -0.15 0.05" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="right_arm_link1">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="red">
+        <color rgba="1 0 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 肩关节2 -->
+  <joint name="right_arm_joint2" type="revolute">
+    <parent link="right_arm_link1"/>
+    <child link="right_arm_link2"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="right_arm_link2">
+    <visual>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.02"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 肘关节 -->
+  <joint name="right_arm_joint3" type="revolute">
+    <parent link="right_arm_link2"/>
+    <child link="right_arm_link3"/>
+    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="right_arm_link3">
+    <visual>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.02"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 腕关节1 -->
+  <joint name="right_arm_joint4" type="revolute">
+    <parent link="right_arm_link3"/>
+    <child link="right_arm_link4"/>
+    <origin xyz="0 0 0.3" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="right_arm_link4">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 腕关节2 -->
+  <joint name="right_arm_joint5" type="revolute">
+    <parent link="right_arm_link4"/>
+    <child link="right_arm_link5"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <limit lower="-1.57" upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="right_arm_link5">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.1" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.2" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 腕关节3 -->
+  <joint name="right_arm_joint6" type="revolute">
+    <parent link="right_arm_link5"/>
+    <child link="right_arm_end_effector"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0"/>
+    <limit lower="-3.14" upper="3.14" effort="100" velocity="1.0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="right_arm_end_effector">
+    <visual>
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+      <material name="magenta">
+        <color rgba="1 0 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.05" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.03"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 左腿 (3自由度) -->
+  <!-- 髋关节1 -->
+  <joint name="left_leg_joint1" type="revolute">
+    <parent link="base_link"/>
+    <child link="left_leg_link1"/>
+    <origin xyz="-0.1 0.1 0" rpy="0 0 0"/>
+    <limit lower="-0.785" upper="0.785" effort="100" velocity="1.0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="left_leg_link1">
+    <visual>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.025"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 膝关节 -->
+  <joint name="left_leg_joint2" type="revolute">
+    <parent link="left_leg_link1"/>
+    <child link="left_leg_link2"/>
+    <origin xyz="0 0 -0.3" rpy="0 0 0"/>
+    <limit lower=0 upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="left_leg_link2">
+    <visual>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.025"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 踝关节 -->
+  <joint name="left_leg_joint3" type="revolute">
+    <parent link="left_leg_link2"/>
+    <child link="left_foot"/>
+    <origin xyz="0 0 -0.3" rpy="0 0 0"/>
+    <limit lower="-0.52" upper="0.52" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="left_foot">
+    <visual>
+      <origin xyz="0 0 -0.05" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.1 0.08 0.03"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.05" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.1 0.08 0.03"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 右腿 (3自由度) -->
+  <!-- 髋关节1 -->
+  <joint name="right_leg_joint1" type="revolute">
+    <parent link="base_link"/>
+    <child link="right_leg_link1"/>
+    <origin xyz="-0.1 -0.1 0" rpy="0 0 0"/>
+    <limit lower="-0.785" upper="0.785" effort="100" velocity="1.0"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="right_leg_link1">
+    <visual>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.025"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="1 1 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 膝关节 -->
+  <joint name="right_leg_joint2" type="revolute">
+    <parent link="right_leg_link1"/>
+    <child link="right_leg_link2"/>
+    <origin xyz="0 0 -0.3" rpy="0 0 0"/>
+    <limit lower=0 upper="1.57" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="right_leg_link2">
+    <visual>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.025"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.15" rpy="0 0 0"/>
+      <geometry>
+        <cylinder length="0.3" radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 踝关节 -->
+  <joint name="right_leg_joint3" type="revolute">
+    <parent link="right_leg_link2"/>
+    <child link="right_foot"/>
+    <origin xyz="0 0 -0.3" rpy="0 0 0"/>
+    <limit lower="-0.52" upper="0.52" effort="100" velocity="1.0"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+
+  <link name="right_foot">
+    <visual>
+      <origin xyz="0 0 -0.05" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.1 0.08 0.03"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 -0.05" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.1 0.08 0.03"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>