diff --git a/src/humanoid_3d_animation/humanoid.xml b/src/humanoid_3d_animation/ros2/humanoid.xml
similarity index 100%
rename from src/humanoid_3d_animation/humanoid.xml
rename to src/humanoid_3d_animation/ros2/humanoid.xml
diff --git a/src/humanoid_3d_animation/ros2/launch/mujoco_ros2.launch.py b/src/humanoid_3d_animation/ros2/launch/mujoco_ros2.launch.py
new file mode 100644
index 0000000000..c73d6ae174
--- /dev/null
+++ b/src/humanoid_3d_animation/ros2/launch/mujoco_ros2.launch.py
@@ -0,0 +1,21 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='mujoco_ros2', 
+            executable='mujoco_ros2_sim',  
+            name='mujoco_ros2_sim',
+            output='screen',
+            parameters=[],  
+        ),
+
+        Node(
+            package='mujoco_ros2', 
+            executable='robot_controller', 
+            name='robot_controller',
+            output='screen',
+            parameters=[], 
+        )
+    ])
\ No newline at end of file
diff --git a/src/humanoid_3d_animation/ros2/mujoco_ros2_cpp/mujoco_ros2_sim.py b/src/humanoid_3d_animation/ros2/mujoco_ros2_cpp/mujoco_ros2_sim.py
new file mode 100644
index 0000000000..e76ea52f86
--- /dev/null
+++ b/src/humanoid_3d_animation/ros2/mujoco_ros2_cpp/mujoco_ros2_sim.py
@@ -0,0 +1,75 @@
+import rclpy
+from rclpy.node import Node
+import numpy as np
+import mujoco
+import mujoco.viewer
+from my_interfaces.srv import RobotCtrl
+from my_interfaces.msg import RobotState
+import time
+current_action='stop'
+class MujocoRos2Sim(Node):
+    def __init__(self,name):
+        super().__init__(name)
+        self.get_logger().info("节点已启动：%s!" % name)
+        # 加载数据,模型,轨迹数据
+        self.model=mujoco.MjModel.from_xml_path("RobotH.xml")
+        self.data=mujoco.MjData(self.model)
+        self.data_walk=np.load("walk.npz")
+        self.data_squat=np.load("squat.npz")
+        # 创建viewer
+        self.viewer=mujoco.viewer.launch_passive(self.model, self.data)
+
+        #创建服务用来控制model运动
+        self.service=self.create_service(RobotCtrl, 'robot_control', self.control_callback)
+        #创建话题用来发布当前状态
+        self.pub=self.create_publisher(RobotState, 'robot_state', 10)
+
+    def control_callback(self, request, response):
+
+        self.get_logger().info(f"Received command: {request.command}")
+        if request.command == "squat":
+            current_action="squat"
+        elif request.command == "walk":
+            current_action="walk"
+        elif request.command == "stop":
+            current_action="stop"
+        response.result=True
+        return response
+    
+    def publish_status(self):
+        # 发布当前状态
+        msg=RobotState()
+        msg.action=current_action
+        self.pub.publish(msg)
+
+    def run_sim(self):
+        while True:
+            # 控制动作
+            if current_action=="squat":
+                self.data.qpos[0]=self.data_squat["qpos"][0]
+                self.data.qvel[0]=self.data_squat["qvel"][0]
+            elif current_action=="walk":
+                self.data.qpos[0]=self.data_walk["qpos"][0]
+                self.data.qvel[0]=self.data_walk["qvel"][0]
+            elif current_action=="stop":
+                self.data.qpos[0]=0
+                self.data.qvel[0]=0
+            # 模拟一步
+            mujoco.mj_step(self.model, self.data)
+            # 实时渲染
+            self.viewer.sync()
+            # 发布状态
+            self.publish_status()
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    sim=MujocoRos2Sim("mujoco_ros2_sim")
+    sim.run_sim()
+    rclpy.spin(sim)
+    sim.destroy_node()
+    rclpy.shutdown()
+
+if __name__=='__main__':
+    main()
\ No newline at end of file
diff --git a/src/humanoid_3d_animation/ros2/mujoco_ros2_cpp/robot_controller.cpp b/src/humanoid_3d_animation/ros2/mujoco_ros2_cpp/robot_controller.cpp
new file mode 100644
index 0000000000..5080713bcf
--- /dev/null
+++ b/src/humanoid_3d_animation/ros2/mujoco_ros2_cpp/robot_controller.cpp
@@ -0,0 +1,121 @@
+#include "rclcpp/rclcpp.hpp"
+#include "my_interfaces/srv/robot_ctrl.hpp"
+#include <termios.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <string>
+
+// 全局变量
+std::string current_action_key = "stop";
+bool action_switch_flag = false;
+
+// 非阻塞键盘读取
+char read_keypress_non_blocking() {
+    struct termios oldt, newt;
+    int ch;
+    int oldf;
+
+    // 获取终端设置
+    tcgetattr(STDIN_FILENO, &oldt);
+    newt = oldt;
+    // 禁用 canonical 模式和回显
+    newt.c_lflag &= ~(ICANON | ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
+    // 设置非阻塞模式
+    oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
+    fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
+
+    ch = getchar();
+
+    // 恢复终端设置
+    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
+    fcntl(STDIN_FILENO, F_SETFL, oldf);
+
+    return (ch != EOF) ? ch : 0;
+}
+
+class RobotController : public rclcpp::Node {
+public:
+    RobotController() : Node("robot_controller") {
+        // 创建服务客户端
+        client_ = this->create_client<my_interfaces::srv::RobotCtrl>("robot_control");
+        
+        // 等待服务可用
+        while (!client_->wait_for_service(std::chrono::seconds(1))) {
+            if (!rclcpp::ok()) {
+                RCLCPP_ERROR(this->get_logger(), "等待服务时被中断");
+                return;
+            }
+            RCLCPP_INFO(this->get_logger(), "等待服务 'robot_control' 启动...");
+        }
+        
+        RCLCPP_INFO(this->get_logger(), "操作提示：w=行走 | s=下蹲 | q=退出 | 其他键=停止");
+    }
+
+    void send_request(const std::string &action) {
+        auto request = std::make_shared<my_interfaces::srv::RobotCtrl::Request>();
+        request->command = action;
+
+        // 发送异步请求
+        auto result_future = client_->async_send_request(
+            request,
+            std::bind(&RobotController::result_callback, this, std::placeholders::_1)
+        );
+        RCLCPP_INFO(this->get_logger(), "已发送指令：%s", action.c_str());
+    }
+
+private:
+    rclcpp::Client<my_interfaces::srv::RobotCtrl>::SharedPtr client_;
+
+    void result_callback(rclcpp::Client<my_interfaces::srv::RobotCtrl>::SharedFuture future) {
+        auto response = future.get();
+        RCLCPP_INFO(this->get_logger(), "指令执行成功：%s", response->message.c_str());
+    }
+};
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<RobotController>();
+
+    // 初始发送停止指令
+    node->send_request(current_action_key);
+
+    try {
+        while (rclcpp::ok()) {
+            // 读取键盘输入
+            char key = read_keypress_non_blocking();
+            if (key) {
+                if (key == 'w' && current_action_key != "walk") {
+                    current_action_key = "walk";
+                    action_switch_flag = true;
+                } else if (key == 's' && current_action_key != "squat") {
+                    current_action_key = "squat";
+                    action_switch_flag = true;
+                } else if (key == 'q') {  // 退出
+                    RCLCPP_INFO(node->get_logger(), "收到退出指令，程序终止");
+                    break;
+                } else if (key != 'w' && key != 's') {  // 其他键停止
+                    if (current_action_key != "stop") {
+                        current_action_key = "stop";
+                        action_switch_flag = true;
+                    }
+                }
+            }
+
+            // 发送指令
+            if (action_switch_flag) {
+                node->send_request(current_action_key);
+                action_switch_flag = false;
+            }
+
+            // 处理ROS事件
+            rclcpp::spin_some(node);
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
+    } catch (const std::exception &e) {
+        RCLCPP_ERROR(node->get_logger(), "异常：%s", e.what());
+    }
+
+    rclcpp::shutdown();
+    return 0;
+}
\ No newline at end of file
diff --git a/src/humanoid_3d_animation/ros2/mujoco_ros2_py/mujoco_ros2_sim.cpp b/src/humanoid_3d_animation/ros2/mujoco_ros2_py/mujoco_ros2_sim.cpp
new file mode 100644
index 0000000000..8965ba04e4
--- /dev/null
+++ b/src/humanoid_3d_animation/ros2/mujoco_ros2_py/mujoco_ros2_sim.cpp
@@ -0,0 +1,139 @@
+#include "rclcpp/rclcpp.hpp"
+#include "my_interfaces/srv/robot_ctrl.hpp"
+#include "my_interfaces/msg/robot_state.hpp"
+#include <mujoco/mujoco.h>
+#include <fstream>
+#include <string>
+#include <thread>
+
+// 全局变量
+std::string current_action = "stop";
+
+class MujocoRos2Sim : public rclcpp::Node {
+public:
+    MujocoRos2Sim() : Node("mujoco_ros2_sim") {
+        RCLCPP_INFO(this->get_logger(), "节点已启动：mujoco_ros2_sim!");
+
+        // 加载模型
+        char error[1000];
+        model_ = mj_loadXML("RobotH.xml", nullptr, error, 1000);
+        if (!model_) {
+            RCLCPP_ERROR(this->get_logger(), "模型加载失败: %s", error);
+            exit(1);
+        }
+        data_ = mj_makeData(model_);
+
+        // 加载轨迹数据（这里简化处理，实际需要根据npz格式解析）
+        load_trajectory_data();
+
+        // 创建viewer
+        viewer_ = mjViewerCreate();
+        mjv_defaultCamera(&cam_);
+        mjv_defaultOption(&opt_);
+        mjr_defaultContext(&con_);
+        mjv_makeScene(model_, &scene_, 1000);
+        mjr_makeContext(model_, &con_, mjFONTSCALE_150);
+        mjViewerSetup(viewer_, &scene_, &cam_, &opt_, &con_);
+
+        // 创建服务
+        service_ = this->create_service<my_interfaces::srv::RobotCtrl>(
+            "robot_control",
+            std::bind(&MujocoRos2Sim::control_callback, this, std::placeholders::_1, std::placeholders::_2)
+        );
+
+        // 创建发布者
+        publisher_ = this->create_publisher<my_interfaces::msg::RobotState>("robot_state", 10);
+
+        // 启动模拟线程
+        sim_thread_ = std::thread(&MujocoRos2Sim::run_sim, this);
+    }
+
+    ~MujocoRos2Sim() {
+        if (sim_thread_.joinable()) {
+            sim_thread_.join();
+        }
+        mjViewerClose(viewer_);
+        mjr_freeContext(&con_);
+        mjv_freeScene(&scene_);
+        mj_deleteData(data_);
+        mj_deleteModel(model_);
+    }
+
+private:
+    mjModel* model_ = nullptr;
+    mjData* data_ = nullptr;
+    mjViewer* viewer_ = nullptr;
+    mjvScene scene_;
+    mjvCamera cam_;
+    mjvOption opt_;
+    mjrContext con_;
+    rclcpp::Service<my_interfaces::srv::RobotCtrl>::SharedPtr service_;
+    rclcpp::Publisher<my_interfaces::msg::RobotState>::SharedPtr publisher_;
+    std::thread sim_thread_;
+
+    // 轨迹数据（示例）
+    std::vector<double> walk_qpos, walk_qvel;
+    std::vector<double> squat_qpos, squat_qvel;
+
+    void load_trajectory_data() {
+        // 实际应用中需要解析npz文件
+        // 这里仅做示例初始化
+        walk_qpos.push_back(0.0);
+        walk_qvel.push_back(0.1);
+        squat_qpos.push_back(-0.5);
+        squat_qvel.push_back(0.0);
+    }
+
+    void control_callback(
+        const std::shared_ptr<my_interfaces::srv::RobotCtrl::Request> request,
+        std::shared_ptr<my_interfaces::srv::RobotCtrl::Response> response
+    ) {
+        RCLCPP_INFO(this->get_logger(), "收到指令: %s", request->command.c_str());
+        current_action = request->command;
+        response->result = true;
+        response->message = "指令已接收";
+    }
+
+    void publish_status() {
+        auto msg = my_interfaces::msg::RobotState();
+        msg.action = current_action;
+        publisher_->publish(msg);
+    }
+
+    void run_sim() {
+        while (rclcpp::ok()) {
+            // 控制动作
+            if (current_action == "squat" && !squat_qpos.empty()) {
+                data_->qpos[0] = squat_qpos[0];
+                data_->qvel[0] = squat_qvel[0];
+            } else if (current_action == "walk" && !walk_qpos.empty()) {
+                data_->qpos[0] = walk_qpos[0];
+                data_->qvel[0] = walk_qvel[0];
+            } else if (current_action == "stop") {
+                data_->qpos[0] = 0;
+                data_->qvel[0] = 0;
+            }
+
+            // 模拟一步
+            mj_step(model_, data_);
+
+            // 渲染
+            mjv_updateScene(model_, data_, &opt_, nullptr, &cam_, mjCAT_ALL, &scene_);
+            mjViewerRender(viewer_, &scene_, &con_);
+
+            // 发布状态
+            publish_status();
+
+            // 小延迟
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
+    }
+};
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<MujocoRos2Sim>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}
\ No newline at end of file
diff --git a/src/humanoid_3d_animation/ros2/mujoco_ros2_py/robot_controller.py b/src/humanoid_3d_animation/ros2/mujoco_ros2_py/robot_controller.py
new file mode 100644
index 0000000000..43dcf9b4ef
--- /dev/null
+++ b/src/humanoid_3d_animation/ros2/mujoco_ros2_py/robot_controller.py
@@ -0,0 +1,95 @@
+import sys
+import select
+import rclpy
+from rclpy.node import Node
+from my_interfaces.srv import RobotCtrl
+
+# 全局变量（提前初始化，避免未绑定）
+current_action_key = 'stop'
+action_switch_flag = False
+
+
+class RobotController(Node):
+    def __init__(self, name):
+        super().__init__(name)
+        # 创建服务客户端，连接机器人控制服务
+        self.client = self.create_client(RobotCtrl, 'robot_control')
+        # 等待服务上线
+        while not self.client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('等待服务 "robot_control" 启动...')
+        self.get_logger().info("操作提示：w=行走 | s=下蹲 | q=退出 | 其他键=停止")
+
+    def result_callback(self, future):
+        """服务调用结果的回调函数"""
+
+        response = future.result()
+        self.get_logger().info(f'指令执行成功：{response.message}')
+
+
+    def send_request(self, action):
+        """发送控制指令到服务端"""
+
+        request = RobotCtrl.Request()
+        request.command = action  # 传递动作指令
+        self.client.call_async(request).add_done_callback(self.result_callback)
+        self.get_logger().info(f'已发送指令：{action}')
+
+
+
+def read_keypress_non_blocking():
+    """非阻塞读取终端按键"""
+    if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
+        key = sys.stdin.read(1)
+        return key.strip()
+    return None
+
+
+def main(args=None):
+    # 关键：声明使用全局变量（修复核心错误）
+    global current_action_key, action_switch_flag
+    
+    rclpy.init(args=args)
+    robot_controller = RobotController("robot_controller")
+
+    # 初始发送停止指令（此时current_action_key已绑定全局值'stop'）
+    robot_controller.send_request(current_action_key)
+
+    try:
+        # 主循环：非阻塞读取按键 + 处理ROS事件
+        while rclpy.ok():
+            # 1. 读取终端按键
+            key = read_keypress_non_blocking()
+            if key:
+                # 按键逻辑
+                if key == 'w' and current_action_key != "walk":
+                    current_action_key = "walk"
+                    action_switch_flag = True
+                elif key == 's' and current_action_key != "squat":
+                    current_action_key = "squat"
+                    action_switch_flag = True
+                elif key == 'q':  # q键退出
+                    robot_controller.get_logger().info("收到退出指令，程序终止")
+                    break
+                elif key not in ['w', 's']:  # 其他键停止
+                    if current_action_key != "stop":
+                        current_action_key = "stop"
+                        action_switch_flag = True
+
+            # 2. 检测动作切换并发送指令
+            if action_switch_flag:
+                robot_controller.send_request(current_action_key)
+                action_switch_flag = False  # 重置标记
+
+            # 3. 非阻塞处理ROS回调
+            rclpy.spin_once(robot_controller, timeout_sec=0.01)
+
+    except KeyboardInterrupt:
+        robot_controller.get_logger().info("用户强制退出")
+    finally:
+        robot_controller.destroy_node()
+        rclpy.shutdown()
+        print("程序已退出")
+
+
+if __name__ == '__main__':
+    main()
diff --git a/src/humanoid_3d_animation/ros2/package.xml b/src/humanoid_3d_animation/ros2/package.xml
new file mode 100644
index 0000000000..007dc76183
--- /dev/null
+++ b/src/humanoid_3d_animation/ros2/package.xml
@@ -0,0 +1,23 @@
+<?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>mujoco_ros2</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="heyh@todo.todo">heyh</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msg</depend>
+  <depend>my_interfaces</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>ament_xmllint</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/src/humanoid_3d_animation/ros2/setup.cfg b/src/humanoid_3d_animation/ros2/setup.cfg
new file mode 100644
index 0000000000..a0703142ca
--- /dev/null
+++ b/src/humanoid_3d_animation/ros2/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/mujoco_ros2
+[install]
+install_scripts=$base/lib/mujoco_ros2
diff --git a/src/humanoid_3d_animation/ros2/setup.py b/src/humanoid_3d_animation/ros2/setup.py
new file mode 100644
index 0000000000..b4c5817d8f
--- /dev/null
+++ b/src/humanoid_3d_animation/ros2/setup.py
@@ -0,0 +1,27 @@
+from setuptools import find_packages, setup
+
+package_name = 'mujoco_ros2'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='heyh',
+    maintainer_email='heyh@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'mujoco_ros2_sim=mujoco_ros2.mujoco_ros2_sim:main',
+            'robot_controller=mujoco_ros2.robot_controller:main',
+        ],
+    },
+)