draft version

.gitignore

@@ -0,0 +1,4 @@
+build/**
+install/**
+log/**
+

.vscode/c_cpp_properties.json

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

.vscode/settings.json

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

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/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>