draft version

2025-10-16 09:06:38 +08:00
parent b33663e1a3
commit 8b2c2eb5c1
34 changed files with 2054 additions and 135 deletions
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,18 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "/opt/ros/humble/include/**",
+                "/usr/include/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-x64"
+        }
+    ],
+    "version": 4
+}
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -0,0 +1,81 @@
+{
+    "files.associations": {
+        "*.launch": "python",
+        "*.world": "xml",
+        "*.xacro": "xml",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "bit": "cpp",
+        "chrono": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "semaphore": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "csignal": "cpp",
+        "any": "cpp",
+        "strstream": "cpp",
+        "bitset": "cpp",
+        "codecvt": "cpp",
+        "regex": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "shared_mutex": "cpp",
+        "cfenv": "cpp",
+        "variant": "cpp"
+    }
+}
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,102 +1,128 @@
 cmake_minimum_required(VERSION 3.10)
-project(robot_control)
+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(Boost REQUIRED COMPONENTS python3)
+# 查找依赖
 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(pinocchio REQUIRED)
-find_package(Eigen3 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"
+)

-# 生成Action接口
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "action/RobotMotion.action"
-  DEPENDENCIES geometry_msgs
+  ${action_files}
+  DEPENDENCIES action_msgs geometry_msgs
 )

-# 包含目录
-include_directories(
-  include
-  ${EIGEN3_INCLUDE_DIRS}
-  ${pinocchio_INCLUDE_DIRS}
-  ${Python3_INCLUDE_DIRS}  # 添加 Python 头文件目录（消除 Boost 警告）
+# 库和可执行文件
+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
 )

-# 链接库
-link_directories(
-  ${pinocchio_LIBRARY_DIRS}
-  ${Boost_LIBRARIES}        # 链接 Boost 库（消除警告）
-  ${Python3_LIBRARIES}      # 链接 Python 库（消除警告）
+target_include_directories(
+  ${PROJECT_NAME}
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
 )

-# 生成库
-add_library(robot_control_lib
-  src/robot_control/model/RobotModel.cpp
-  src/robot_control/model/UpperBodyModel.cpp
-  src/robot_control/model/LowerBodyModel.cpp
-  src/robot_control/control/ControllerBase.cpp
-  src/robot_control/control/UpperBodyController.cpp
-  src/robot_control/control/LowerBodyController.cpp
-  src/robot_control/ros/RobotMotionAction.cpp
-  src/robot_control/planning/RealtimeInterpolator.cpp
-  src/robot_control/state_machine/RobotStateMachine.cpp
-  src/robot_control/RobotControlSystem.cpp
-)
-
-# 链接依赖
-ament_target_dependencies(robot_control_lib
+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
 )

-target_link_libraries(robot_control_lib
-  pinocchio::pinocchio
+# 链接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
 )

-# 生成可执行文件
-add_executable(robot_control_node
-  src/main.cpp
+install(
+  DIRECTORY include/
+  DESTINATION include
 )

-# 链接可执行文件
-target_link_libraries(robot_control_node
-  robot_control_lib
+# 安装启动文件和配置文件
+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
-  robot_control_lib
-  robot_control_node
+  motion_control_node
  DESTINATION lib/${PROJECT_NAME}
 )

-# 安装头文件
-install(DIRECTORY include/
-  DESTINATION include/
+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
 )
-
-# 安装URDF等资源文件
-install(DIRECTORY urdf/
-  DESTINATION share/${PROJECT_NAME}/urdf/
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
 )
-
-# 注册依赖
-ament_export_include_directories(include)
-ament_export_libraries(robot_control_lib)
-ament_export_dependencies(rclcpp)
-ament_export_dependencies(rclcpp_action)
-ament_export_dependencies(pinocchio)
-
-rosidl_get_typesupport_target(cpp_typesupport_target
-  ${PROJECT_NAME} rosidl_typesupport_cpp)
-
-target_link_libraries(robot_control_lib "${cpp_typesupport_target}")

 ament_package()
--- a/action/MoveJ.action
+++ b/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
--- a/action/MoveL.action
+++ b/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
--- a/action/RobotMotion.action
+++ b/action/RobotMotion.action
@@ -1,21 +0,0 @@
-# 机器人运动目标
-float64 x                # x位置 (米)，用于移动基座
-float64 y                # y位置 (米)，用于移动基座
-float64 yaw              # 偏航角 (弧度)，用于移动基座
-int32 arm_index          # 手臂索引 (0:左臂, 1:右臂, -1:不使用)
-float64[] arm_position   # 手臂位置目标 (x, y, z)，米
-int32 leg_index          # 腿部索引 (0-3, -1:不使用)
-float64[] leg_position   # 腿部位置目标 (x, y, z)，米
-float64[] body_pose      # 身体姿态 (roll, pitch, yaw, height)，弧度/米
-float64 duration         # 期望运动时间 (秒)，0表示使用默认
-bool blocking            # 是否阻塞等待完成
---
-# 运动结果
-bool success             # 是否成功
-string message           # 结果消息
-float64 actual_duration  # 实际运动时间 (秒)
---
-# 运动反馈
-float64 progress         # 进度 (0.0-1.0)
-float64[] current_state  # 当前状态
-string status            # 状态描述
--- a/config/kinematics.yaml
+++ b/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
--- a/config/srdf/robot.srdf
+++ b/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>
--- a/include/robot_control/RobotControlSystem.h
+++ b/include/robot_control/RobotControlSystem.h
@@ -1,25 +1,21 @@
 #ifndef ROBOT_CONTROL_SYSTEM_H
 #define ROBOT_CONTROL_SYSTEM_H

-#include "robot_control/model/UpperBodyModel.h"
-#include "robot_control/model/LowerBodyModel.h"
-#include "robot_control/control/UpperBodyController.h"
-#include "robot_control/control/LowerBodyController.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>
-#include <ocs2_core/Types.h>

 namespace robot_control {

-class RobotControlSystem {
+class RobotControlManager {
 public:
-    using Ptr = std::shared_ptr<RobotControlSystem>;
-    using ConstPtr = std::shared_ptr<const RobotControlSystem>;
+    using Ptr = std::shared_ptr<RobotControlManager>;
+    using ConstPtr = std::shared_ptr<const RobotControlManager>;

-    RobotControlSystem(rclcpp::Node::SharedPtr node);
-    ~RobotControlSystem() = default;
+    RobotControlManager(rclcpp::Node::SharedPtr node);
+    ~RobotControlManager() = default;
    
    // 初始化系统
    bool initialize(const std::string& upperBodyUrdf, const std::string& lowerBodyUrdf);
@@ -63,12 +59,6 @@ private:
    // 状态机
    state_machine::RobotStateMachine::Ptr stateMachine_;
    
-    // 系统状态
-    ocs2::vector_t upperBodyState_;
-    ocs2::vector_t lowerBodyState_;
-    ocs2::vector_t upperBodyCommand_;
-    ocs2::vector_t lowerBodyCommand_;
-    
    // 控制频率
    double controlFrequency_;
    
--- a/include/robot_control/control/ArmController.h
+++ b/include/robot_control/control/ArmController.h
@@ -1,9 +1,8 @@
-#ifndef UPPER_BODY_CONTROLLER_H
-#define UPPER_BODY_CONTROLLER_H
+#ifndef ARM_CONTROLLER_H
+#define ARM_CONTROLLER_H

-#include "robot_control/control/ControllerBase.h"
+#include "ControllerBase.hpp"

-namespace robot_control {
 namespace control {

 class UpperBodyController : public ControllerBase {
@@ -36,6 +35,5 @@ private:
 };

 } // namespace control
-} // namespace robot_control

 #endif // UPPER_BODY_CONTROLLER_H
--- a/include/robot_control/control/BodyController.h
+++ b/include/robot_control/control/BodyController.h
--- a/include/robot_control/control/ControllerBase.h
+++ b/include/robot_control/control/ControllerBase.h
@@ -13,10 +13,10 @@ public:
    using ConstPtr = std::shared_ptr<const ControllerBase>;

    ControllerBase() = default;
-    ~ControllerBase() override = default;
+    ~ControllerBase() = default;

    // 计算控制输入
-    virtual EIGEN::vector_t computeControl(const ocs2::scalar_t t, const ocs2::vector_t& x) = 0;
+    Eigen::VectorXd computeControl(const  t, const  x);
    
    // 重置控制器
    virtual void reset() = 0;
--- a/include/robot_control/control/LegController.h
+++ b/include/robot_control/control/LegController.h
--- a/include/robot_control/control/WheelController.h
+++ b/include/robot_control/control/WheelController.h
--- a/include/robot_control/model/RobotModel.h
+++ b/include/robot_control/model/RobotModel.h
@@ -1,9 +1,6 @@
 #ifndef ROBOT_MODEL_H
 #define ROBOT_MODEL_H

-#include <pinocchio/multibody/model.hpp>
-#include <pinocchio/multibody/data.hpp>
-#include <pinocchio/parsers/urdf.hpp>
 #include <string>
 #include <memory>

--- a/include/nodes/MotionControlNode.hpp
+++ b/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_
--- a/include/robot_control/ros/RobotMotionAction.h
+++ b/include/robot_control/ros/RobotMotionAction.h
--- a/include/planner/MoveitPlanner.hpp
+++ b/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
--- a/include/robot_control/planning/RealtimeInterpolation.h
+++ b/include/robot_control/planning/RealtimeInterpolation.h
@@ -1,7 +1,6 @@
 #ifndef REALTIME_INTERPOLATOR_H
 #define REALTIME_INTERPOLATOR_H

-#include <ocs2_core/reference/TargetTrajectories.h>
 #include <Eigen/Dense>
 #include <thread>
 #include <mutex>
--- a/include/robot_control/state_machine/RobotStateMachine.h
+++ b/include/robot_control/state_machine/RobotStateMachine.h
@@ -1,7 +1,6 @@
 #ifndef ROBOT_STATE_MACHINE_H
 #define ROBOT_STATE_MACHINE_H

-#include <ocs2_core/Types.h>
 #include <memory>
 #include <string>
 #include <unordered_map>
--- a/launch/robot_motion.launch.py
+++ b/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
+    ])
--- a/package.xml
+++ b/package.xml
@@ -1,27 +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>robot_control</name>
-  <version>0.0.0</version>
-  <description>轮足双臂机器人运动控制系统</description>
-  <maintainer email="ray@hivecore.cn">Ray</maintainer>
+  <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>
-  <build_depend>rclcpp</build_depend>
-  <build_depend>rclcpp_action</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>pinocchio</build_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>rclcpp</exec_depend>
-  <exec_depend>rclcpp_action</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>pinocchio</exec_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>

-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
 </package>
--- a/setup.sh
+++ b/setup.sh
@@ -1,3 +0,0 @@
-sudo apt-get install ros-humble-pinocchio
-sudo apt-get install libboost-python-dev
-sudo apt-get install libboost-python3-dev
--- a/src/robot_control/RobotControlSystem.cpp
+++ b/src/robot_control/RobotControlSystem.cpp
--- a/src/robot_control/control/ArmController.cpp
+++ b/src/robot_control/control/ArmController.cpp
--- a/src/robot_control/control/LegController.cpp
+++ b/src/robot_control/control/LegController.cpp
--- a/src/kinematicsDemo.cpp
+++ b/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;
+}
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,6 +1,6 @@
-#include "robot_control/RobotControlSystem.h"
 #include <rclcpp/rclcpp.hpp>
 #include <string>
+#include "robot_control/RobotControlSystem.h"

 int main(int argc, char* argv[]) {
    // 初始化ROS
--- a/src/robot_control/model/RobotModel.cpp
+++ b/src/robot_control/model/RobotModel.cpp
--- a/src/nodes/MotionControlNode.cpp
+++ b/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
--- a/src/planner/MoveitPlanner.cpp
+++ b/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
--- a/src/robot_control/planning/RealtimeINterpolation.cpp
+++ b/src/robot_control/planning/RealtimeINterpolation.cpp
--- a/src/robot_control/state_machine/RobotStateMachine.cpp
+++ b/src/robot_control/state_machine/RobotStateMachine.cpp
--- a/urdf/robot.urdf
+++ b/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>