First Commit

xSIM_MUJOCO/LICENSE

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

xSIM_MUJOCO/README.md

@@ -0,0 +1,109 @@
+# MuJoCo DEX 机器人仿真项目
+
+本项目是基于 MuJoCo 物理引擎的 DEX 机器人仿真环境，用于开发和测试机器人控制算法。
+
+## 项目结构
+
+```
+.
+├── README.md                # 项目说明文档
+├── resources                # 机器人模型文件
+│   ├── evt2
+└── scripts                      # Python 脚本目录
+    ├── convert_xml.py           # URDF 到 XML 转换工具
+    ├── elastic_band.py          # 弹性带控制器
+    └── simulator_view_asyn.py   # 异步仿真器（支持 ROS2）
+```
+
+## 主要功能
+
+### 1. 机器人模型
+- 包含完整的 DEX 机器人模型，具有多个自由度
+- 支持腿部、腰部和手臂关节控制
+- 集成 IMU 传感器和关节传感器
+
+### 2. 控制系统
+- 支持位置控制和力矩控制模式
+- 实现 PVD（位置-速度-力矩）混合控制器
+- 提供传感器数据读取接口
+
+### 3. 仿真功能
+- 实时物理仿真
+- 3D 可视化显示
+- 数据记录和绘图功能
+- ROS2 集成支持（通过 simulator_view_asyn.py）
+
+## 安装依赖
+
+```bash
+# 建议使用虚拟环境
+pip install mujoco numpy matplotlib pynput
+```
+bodyctrl_msg 位于xmigcs/lib/下
+```bash
+sudo dpkg -i ros-jazzy*.deb
+```
+## 使用方法
+
+### 异步仿真（支持 ROS2）
+
+```bash
+source /opt/ros/jazzy/setup.bash
+export ROS_DOMAIN_ID=your_domain_id
+python scripts/simulator_view_asyn.py -m evt2(机器人名称)
+```
+
+## 主要脚本说明
+
+### simulator_view_asyn.py
+异步版本的机器人仿真器，支持：
+- 多线程运行
+- ROS2 集成
+- 键盘控制
+- 实时数据发布
+
+### elastic_band.py
+弹性带控制器
+
+## 机器人关节结构
+
+DEX 机器人包含以下关节组：
+
+1. **腿部关节**：
+   - hip_pitch_l_joint, hip_roll_l_joint, hip_yaw_l_joint
+   - knee_pitch_l_joint, ankle_pitch_l_joint, ankle_roll_l_joint
+   - 右腿对应关节
+
+2. **腰部关节**：
+   - waist_yaw_joint, waist_roll_joint, waist_pitch_joint
+
+3. **手臂关节**：
+   - shoulder_pitch_l_joint, shoulder_roll_l_joint, shoulder_yaw_l_joint
+   - elbow_pitch_l_joint, elbow_yaw_l_joint, wrist_pitch_l_joint, wrist_roll_l_joint
+   - 右臂对应关节
+
+## 控制接口
+
+通过修改 `data.ctrl[]` 数组来控制关节力矩，通过读取 `data.sensordata[]` 获取传感器数据。
+
+## 键盘控制
+
+在仿真运行时：
+- end：切换力控、位控
+- numlock: 暂停、继续
+- up: 升起机器人
+- down: 降下机器人
+- ESC: 释放机器人，可以自由运动
+
+
+## 数据记录
+
+仿真器会自动记录以下数据用于后续分析和可视化：
+- 关节位置、速度、力矩
+- IMU 方向、加速度、位置数据
+
+## 注意事项
+
+1. 确保 MuJoCo 模型文件路径正确
+2. 如需 ROS2 支持，请先启动 ROS2 环境
+3. 仿真速度受系统性能影响，可通过调整 timestep 参数优化

xSIM_MUJOCO/resources/evt2/urdf/evt2.xml

@@ -0,0 +1,499 @@
+<mujoco model="evt2">
+  <compiler angle="radian" meshdir="../meshes_new/"/>
+  <option integrator="implicitfast" timestep='0.001' iterations='100' solver='CG' gravity='0 0 -9.81'>
+  <flag contact="enable" energy="enable"/>
+  </option>
+  <!-- 禁用位置传感器 -->
+  <option actuatorgroupdisable="0 1 3"/>
+
+
+
+
+  <default>
+    <default class="large_actuator">
+      <position kp="2000" kv="100" forcerange="-105 105"/>
+    </default>
+    <default class="small_actuator">
+      <position kp="500" kv="50" forcerange="-52 52"/>
+    </default>
+  </default>
+
+  <default>
+    <default class="arm_motor">
+      <joint damping="1" armature="0.1" frictionloss="0.1"/>
+    </default>
+    <default class="wrist_motor">
+      <joint damping="1" armature="0.0236" frictionloss="0.1"/>
+    </default>
+
+    <default class="waist_motor">
+      <joint damping="1" armature="0.17" frictionloss="0.1"/>
+    </default>
+
+    <default class="hip_pitch_motor">
+      <joint damping="1" armature="0.24" frictionloss="0.1"/>
+    </default>
+  
+    <default class="hip_roll_motor">
+      <joint damping="1" armature="0.24" frictionloss="0.1"/>
+    </default>
+  
+    <default class="hip_yaw_motor">
+      <joint damping="1" armature="0.18" frictionloss="0.1"/>
+    </default>
+  
+    <default class="knee_motor">
+      <joint damping="1" armature="0.37" frictionloss="0.1"/>
+    </default>
+  
+    <default class="ankle_motor">
+      <joint damping="1" armature="0.032" frictionloss="0.1"/>
+    </default>
+
+  </default>
+
+  <actuator>
+    <!-- 位置控制器（pos_前缀） -->
+    <!-- 左腿关节驱动器 -->
+    <position class="large_actuator" name="pos_hip_pitch_l_joint" joint="hip_pitch_l_joint" ctrlrange="-3.14159 3.14159" group="0"/>
+    <position class="large_actuator" name="pos_hip_roll_l_joint" joint="hip_roll_l_joint" ctrlrange="-0.5236 2.618" group="0"/>
+    <position class="large_actuator" name="pos_hip_yaw_l_joint" joint="hip_yaw_l_joint" ctrlrange="-1.5708 4.5379" group="0"/>
+    <position class="large_actuator" name="pos_knee_pitch_l_joint" joint="knee_pitch_l_joint" ctrlrange="-0.0873 2.4435" group="0"/>
+    <position class="large_actuator" name="pos_ankle_pitch_l_joint" joint="ankle_pitch_l_joint" ctrlrange="-1.2217 0.5236" group="0"/>
+    <position class="small_actuator" name="pos_ankle_roll_l_joint" joint="ankle_roll_l_joint" ctrlrange="-0.5236 0.5236" group="0"/>
+    
+    <!-- 右腿关节驱动器 -->
+    <position class="large_actuator" name="pos_hip_pitch_r_joint" joint="hip_pitch_r_joint" ctrlrange="-3.14159 3.14159" group="0"/>
+    <position class="large_actuator" name="pos_hip_roll_r_joint" joint="hip_roll_r_joint" ctrlrange="-2.618 0.5236" group="0"/>
+    <position class="large_actuator" name="pos_hip_yaw_r_joint" joint="hip_yaw_r_joint" ctrlrange="-4.5379 1.5708" group="0"/>
+    <position class="large_actuator" name="pos_knee_pitch_r_joint" joint="knee_pitch_r_joint" ctrlrange="-0.0873 2.4435" group="0"/>
+    <position class="large_actuator" name="pos_ankle_pitch_r_joint" joint="ankle_pitch_r_joint" ctrlrange="-1.2217 0.5236" group="0"/>
+    <position class="small_actuator" name="pos_ankle_roll_r_joint" joint="ankle_roll_r_joint" ctrlrange="-0.5236 0.5236" group="0"/>
+    
+    <!-- 腰部关节驱动器 -->
+    <position class="large_actuator" name="pos_waist_yaw_joint" joint="waist_yaw_joint" ctrlrange="-2.9671 3.1416" group="1"/>
+    <position class="large_actuator" name="pos_waist_roll_joint" joint="waist_roll_joint" ctrlrange="-0.5236 0.5236" group="1"/>
+    <position class="large_actuator" name="pos_waist_pitch_joint" joint="waist_pitch_joint" ctrlrange="-0.5236 1.0472" group="1"/>
+
+    <!-- 上肢关节驱动器 -->
+    <position class="large_actuator" name="pos_shoulder_pitch_l_joint" joint="shoulder_pitch_l_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_shoulder_roll_l_joint" joint="shoulder_roll_l_joint" ctrlrange="-0.2618 3.4034" group="3"/>
+    <position class="large_actuator" name="pos_shoulder_yaw_l_joint" joint="shoulder_yaw_l_joint" ctrlrange="-2.9671 2.9671" group="3" />
+    <position class="large_actuator" name="pos_elbow_pitch_l_joint" joint="elbow_pitch_l_joint" ctrlrange="-2.618 0.2618" group="3"/>
+    <position class="large_actuator" name="pos_elbow_yaw_l_joint" joint="elbow_yaw_l_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_wrist_pitch_l_joint" joint="wrist_pitch_l_joint" ctrlrange="-1.309 1.65806" group="3"/>
+    <position class="small_actuator" name="pos_wrist_roll_l_joint" joint="wrist_roll_l_joint" ctrlrange="-1.0472 0.7854" group="3"/>
+    
+    <position class="large_actuator" name="pos_shoulder_pitch_r_joint" joint="shoulder_pitch_r_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_shoulder_roll_r_joint" joint="shoulder_roll_r_joint" ctrlrange="-3.4034 0.2618" group="3"/>
+    <position class="large_actuator" name="pos_shoulder_yaw_r_joint" joint="shoulder_yaw_r_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_elbow_pitch_r_joint" joint="elbow_pitch_r_joint" ctrlrange="-2.618 0.2618" group="3"/>
+    <position class="large_actuator" name="pos_elbow_yaw_r_joint" joint="elbow_yaw_r_joint" ctrlrange="-2.9671 2.9671" group="3"/>
+    <position class="large_actuator" name="pos_wrist_pitch_r_joint" joint="wrist_pitch_r_joint" ctrlrange="-1.309 1.65806" group="3"/>
+    <position class="small_actuator" name="pos_wrist_roll_r_joint" joint="wrist_roll_r_joint" ctrlrange="-0.7854 1.0472" group="3"/>
+
+    <!-- 力控制器（motor_前缀） -->
+    <!-- 左腿关节驱动器 -->
+    <motor name="motor_hip_pitch_l_joint" joint="hip_pitch_l_joint" ctrlrange="-200 200" group="2"/>
+    <motor name="motor_hip_roll_l_joint" joint="hip_roll_l_joint" ctrlrange="-200 200" group="2"/>
+    <motor name="motor_hip_yaw_l_joint" joint="hip_yaw_l_joint" ctrlrange="-142 142" group="2"/>
+    <motor name="motor_knee_pitch_l_joint" joint="knee_pitch_l_joint" ctrlrange="-330 330" group="2"/>
+    <motor name="motor_ankle_pitch_l_joint" joint="ankle_pitch_l_joint" ctrlrange="-100 100" group="2"/>
+    <motor name="motor_ankle_roll_l_joint" joint="ankle_roll_l_joint" ctrlrange="-50 50" group="2"/>
+    
+    <!-- 右腿关节驱动器 -->
+    <motor name="motor_hip_pitch_r_joint" joint="hip_pitch_r_joint" ctrlrange="-200 200" group="2"/>
+    <motor name="motor_hip_roll_r_joint" joint="hip_roll_r_joint" ctrlrange="-200 200" group="2"/>
+    <motor name="motor_hip_yaw_r_joint" joint="hip_yaw_r_joint" ctrlrange="-142 142" group="2"/>
+    <motor name="motor_knee_pitch_r_joint" joint="knee_pitch_r_joint" ctrlrange="-330 330" group="2"/>
+    <motor name="motor_ankle_pitch_r_joint" joint="ankle_pitch_r_joint" ctrlrange="-100 100" group="2"/>
+    <motor name="motor_ankle_roll_r_joint" joint="ankle_roll_r_joint" ctrlrange="-50 50" group="2"/>
+
+    <!-- 腰部关节驱动器 -->
+    <motor name="motor_waist_yaw_joint" joint="waist_yaw_joint" ctrlrange="-91 91" group="2"/>
+    <motor name="motor_waist_roll_joint" joint="waist_roll_joint" ctrlrange="-91 91" group="2"/>
+    <motor name="motor_waist_pitch_joint" joint="waist_pitch_joint" ctrlrange="-91 91" group="2"/>
+
+    <!-- 上肢关节驱动器 -->
+    <motor name="motor_shoulder_pitch_l_joint" joint="shoulder_pitch_l_joint" ctrlrange="-90 90" group="2"/>
+    <motor name="motor_shoulder_roll_l_joint" joint="shoulder_roll_l_joint" ctrlrange="-60 60" group="2"/>
+    <motor name="motor_shoulder_yaw_l_joint" joint="shoulder_yaw_l_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_elbow_pitch_l_joint" joint="elbow_pitch_l_joint" ctrlrange="-60 60" group="2"/>
+    <motor name="motor_elbow_yaw_l_joint" joint="elbow_yaw_l_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_wrist_pitch_l_joint" joint="wrist_pitch_l_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_wrist_roll_l_joint" joint="wrist_roll_l_joint" ctrlrange="-36 36" group="2"/>
+    
+    <motor name="motor_shoulder_pitch_r_joint" joint="shoulder_pitch_r_joint" ctrlrange="-90 90" group="2"/>
+    <motor name="motor_shoulder_roll_r_joint" joint="shoulder_roll_r_joint" ctrlrange="-60 60" group="2"/>
+    <motor name="motor_shoulder_yaw_r_joint" joint="shoulder_yaw_r_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_elbow_pitch_r_joint" joint="elbow_pitch_r_joint" ctrlrange="-60 60" group="2"/>
+    <motor name="motor_elbow_yaw_r_joint" joint="elbow_yaw_r_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_wrist_pitch_r_joint" joint="wrist_pitch_r_joint" ctrlrange="-36 36" group="2"/>
+    <motor name="motor_wrist_roll_r_joint" joint="wrist_roll_r_joint" ctrlrange="-36 36" group="2"/>
+  </actuator>
+
+  <sensor>
+    <!-- 左腿关节传感器 -->
+    <jointpos name="hip_pitch_l_joint_pos" joint="hip_pitch_l_joint"/>
+    <jointpos name="hip_roll_l_joint_pos" joint="hip_roll_l_joint"/>
+    <jointpos name="hip_yaw_l_joint_pos" joint="hip_yaw_l_joint"/>
+    <jointpos name="knee_pitch_l_joint_pos" joint="knee_pitch_l_joint"/>
+    <jointpos name="ankle_pitch_l_joint_pos" joint="ankle_pitch_l_joint"/>
+    <jointpos name="ankle_roll_l_joint_pos" joint="ankle_roll_l_joint"/>
+    
+    <jointvel name="hip_pitch_l_joint_vel" joint="hip_pitch_l_joint"/>
+    <jointvel name="hip_roll_l_joint_vel" joint="hip_roll_l_joint"/>
+    <jointvel name="hip_yaw_l_joint_vel" joint="hip_yaw_l_joint"/>
+    <jointvel name="knee_pitch_l_joint_vel" joint="knee_pitch_l_joint"/>
+    <jointvel name="ankle_pitch_l_joint_vel" joint="ankle_pitch_l_joint"/>
+    <jointvel name="ankle_roll_l_joint_vel" joint="ankle_roll_l_joint"/>
+    
+    <jointactuatorfrc name="hip_pitch_l_joint_torque" joint="hip_pitch_l_joint"/>
+    <jointactuatorfrc name="hip_roll_l_joint_torque" joint="hip_roll_l_joint"/>
+    <jointactuatorfrc name="hip_yaw_l_joint_torque" joint="hip_yaw_l_joint"/>
+    <jointactuatorfrc name="knee_pitch_l_joint_torque" joint="knee_pitch_l_joint"/>
+    <jointactuatorfrc name="ankle_pitch_l_joint_torque" joint="ankle_pitch_l_joint"/>
+    <jointactuatorfrc name="ankle_roll_l_joint_torque" joint="ankle_roll_l_joint"/>
+    
+    <!-- 右腿关节传感器 -->
+    <jointpos name="hip_pitch_r_joint_pos" joint="hip_pitch_r_joint"/>
+    <jointpos name="hip_roll_r_joint_pos" joint="hip_roll_r_joint"/>
+    <jointpos name="hip_yaw_r_joint_pos" joint="hip_yaw_r_joint"/>
+    <jointpos name="knee_pitch_r_joint_pos" joint="knee_pitch_r_joint"/>
+    <jointpos name="ankle_pitch_r_joint_pos" joint="ankle_pitch_r_joint"/>
+    <jointpos name="ankle_roll_r_joint_pos" joint="ankle_roll_r_joint"/>
+    
+    <jointvel name="hip_pitch_r_joint_vel" joint="hip_pitch_r_joint"/>
+    <jointvel name="hip_roll_r_joint_vel" joint="hip_roll_r_joint"/>
+    <jointvel name="hip_yaw_r_joint_vel" joint="hip_yaw_r_joint"/>
+    <jointvel name="knee_pitch_r_joint_vel" joint="knee_pitch_r_joint"/>
+    <jointvel name="ankle_pitch_r_joint_vel" joint="ankle_pitch_r_joint"/>
+    <jointvel name="ankle_roll_r_joint_vel" joint="ankle_roll_r_joint"/>
+    
+    <jointactuatorfrc name="hip_pitch_r_joint_torque" joint="hip_pitch_r_joint"/>
+    <jointactuatorfrc name="hip_roll_r_joint_torque" joint="hip_roll_r_joint"/>
+    <jointactuatorfrc name="hip_yaw_r_joint_torque" joint="hip_yaw_r_joint"/>
+    <jointactuatorfrc name="knee_pitch_r_joint_torque" joint="knee_pitch_r_joint"/>
+    <jointactuatorfrc name="ankle_pitch_r_joint_torque" joint="ankle_pitch_r_joint"/>
+    <jointactuatorfrc name="ankle_roll_r_joint_torque" joint="ankle_roll_r_joint"/>
+    
+    <!-- 腰部关节传感器 -->
+    <jointpos name="waist_yaw_joint_pos" joint="waist_yaw_joint"/>
+    <jointpos name="waist_roll_joint_pos" joint="waist_roll_joint"/>
+    <jointpos name="waist_pitch_joint_pos" joint="waist_pitch_joint"/>
+    
+    <jointvel name="waist_yaw_joint_vel" joint="waist_yaw_joint"/>
+    <jointvel name="waist_roll_joint_vel" joint="waist_roll_joint"/>
+    <jointvel name="waist_pitch_joint_vel" joint="waist_pitch_joint"/>
+    
+    <jointactuatorfrc name="waist_yaw_joint_torque" joint="waist_yaw_joint"/>
+    <jointactuatorfrc name="waist_roll_joint_torque" joint="waist_roll_joint"/>
+    <jointactuatorfrc name="waist_pitch_joint_torque" joint="waist_pitch_joint"/>
+    
+    <!-- 左臂关节传感器 -->
+    <jointpos name="shoulder_pitch_l_joint_pos" joint="shoulder_pitch_l_joint"/>
+    <jointpos name="shoulder_roll_l_joint_pos" joint="shoulder_roll_l_joint"/>
+    <jointpos name="shoulder_yaw_l_joint_pos" joint="shoulder_yaw_l_joint"/>
+    <jointpos name="elbow_pitch_l_joint_pos" joint="elbow_pitch_l_joint"/>
+    <jointpos name="elbow_yaw_l_joint_pos" joint="elbow_yaw_l_joint"/>
+    <jointpos name="wrist_pitch_l_joint_pos" joint="wrist_pitch_l_joint"/>
+    <jointpos name="wrist_roll_l_joint_pos" joint="wrist_roll_l_joint"/>
+    
+    <jointvel name="shoulder_pitch_l_joint_vel" joint="shoulder_pitch_l_joint"/>
+    <jointvel name="shoulder_roll_l_joint_vel" joint="shoulder_roll_l_joint"/>
+    <jointvel name="shoulder_yaw_l_joint_vel" joint="shoulder_yaw_l_joint"/>
+    <jointvel name="elbow_pitch_l_joint_vel" joint="elbow_pitch_l_joint"/>
+    <jointvel name="elbow_yaw_l_joint_vel" joint="elbow_yaw_l_joint"/>
+    <jointvel name="wrist_pitch_l_joint_vel" joint="wrist_pitch_l_joint"/>
+    <jointvel name="wrist_roll_l_joint_vel" joint="wrist_roll_l_joint"/>
+    
+    <jointactuatorfrc name="shoulder_pitch_l_joint_torque" joint="shoulder_pitch_l_joint"/>
+    <jointactuatorfrc name="shoulder_roll_l_joint_torque" joint="shoulder_roll_l_joint"/>
+    <jointactuatorfrc name="shoulder_yaw_l_joint_torque" joint="shoulder_yaw_l_joint"/>
+    <jointactuatorfrc name="elbow_pitch_l_joint_torque" joint="elbow_pitch_l_joint"/>
+    <jointactuatorfrc name="elbow_yaw_l_joint_torque" joint="elbow_yaw_l_joint"/>
+    <jointactuatorfrc name="wrist_pitch_l_joint_torque" joint="wrist_pitch_l_joint"/>
+    <jointactuatorfrc name="wrist_roll_l_joint_torque" joint="wrist_roll_l_joint"/>
+    
+    <!-- 右臂关节传感器 -->
+    <jointpos name="shoulder_pitch_r_joint_pos" joint="shoulder_pitch_r_joint"/>
+    <jointpos name="shoulder_roll_r_joint_pos" joint="shoulder_roll_r_joint"/>
+    <jointpos name="shoulder_yaw_r_joint_pos" joint="shoulder_yaw_r_joint"/>
+    <jointpos name="elbow_pitch_r_joint_pos" joint="elbow_pitch_r_joint"/>
+    <jointpos name="elbow_yaw_r_joint_pos" joint="elbow_yaw_r_joint"/>
+    <jointpos name="wrist_pitch_r_joint_pos" joint="wrist_pitch_r_joint"/>
+    <jointpos name="wrist_roll_r_joint_pos" joint="wrist_roll_r_joint"/>
+    
+    <jointvel name="shoulder_pitch_r_joint_vel" joint="shoulder_pitch_r_joint"/>
+    <jointvel name="shoulder_roll_r_joint_vel" joint="shoulder_roll_r_joint"/>
+    <jointvel name="shoulder_yaw_r_joint_vel" joint="shoulder_yaw_r_joint"/>
+    <jointvel name="elbow_pitch_r_joint_vel" joint="elbow_pitch_r_joint"/>
+    <jointvel name="elbow_yaw_r_joint_vel" joint="elbow_yaw_r_joint"/>
+    <jointvel name="wrist_pitch_r_joint_vel" joint="wrist_pitch_r_joint"/>
+    <jointvel name="wrist_roll_r_joint_vel" joint="wrist_roll_r_joint"/>
+    
+    <jointactuatorfrc name="shoulder_pitch_r_joint_torque" joint="shoulder_pitch_r_joint"/>
+    <jointactuatorfrc name="shoulder_roll_r_joint_torque" joint="shoulder_roll_r_joint"/>
+    <jointactuatorfrc name="shoulder_yaw_r_joint_torque" joint="shoulder_yaw_r_joint"/>
+    <jointactuatorfrc name="elbow_pitch_r_joint_torque" joint="elbow_pitch_r_joint"/>
+    <jointactuatorfrc name="elbow_yaw_r_joint_torque" joint="elbow_yaw_r_joint"/>
+    <jointactuatorfrc name="wrist_pitch_r_joint_torque" joint="wrist_pitch_r_joint"/>
+    <jointactuatorfrc name="wrist_roll_r_joint_torque" joint="wrist_roll_r_joint"/>
+
+    <!-- imu -->
+    <framequat name="orientation" objtype="site" objname="imu" />
+    <gyro name="angular-velocity" site="imu" />
+    <accelerometer name="linear-acceleration" site="imu" />
+
+    <framepos name="position" objtype="site" objname="imu" />
+    <framelinvel name="linear-velocity" objtype="site" objname="imu" />
+    <magnetometer  name='magnetometer' site='imu'/> 
+
+  </sensor>
+
+  <asset>
+    <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
+        width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
+    <texture name="texplane2" type="2d" builtin="checker" rgb1="1 0.3137 0.1843" rgb2="0.0 0.30196 0.38039"
+        width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
+    <material name="matplane" reflectance="0." texture="texplane" texrepeat="1 1" texuniform="true"/>
+    <material name="matplane2" reflectance="0.1" texture="texplane2" texrepeat="1 1" texuniform="true"/>
+    
+    <mesh name="pelvis" file="pelvis.STL"/>
+    <mesh name="hip_pitch_l_link" file="hip_pitch_l_link.STL"/>
+    <mesh name="hip_roll_l_link" file="hip_roll_l_link.STL"/>
+    <mesh name="hip_yaw_l_link" file="hip_yaw_l_link.STL"/>
+    <mesh name="knee_pitch_l_link" file="knee_pitch_l_link.STL"/>
+    <mesh name="ankle_pitch_l_link" file="ankle_pitch_l_link.STL"/>
+    <mesh name="ankle_roll_l_link" file="ankle_roll_l_link.STL"/>
+    <mesh name="hip_pitch_r_link" file="hip_pitch_r_link.STL"/>
+    <mesh name="hip_roll_r_link" file="hip_roll_r_link.STL"/>
+    <mesh name="hip_yaw_r_link" file="hip_yaw_r_link.STL"/>
+    <mesh name="knee_pitch_r_link" file="knee_pitch_r_link.STL"/>
+    <mesh name="ankle_pitch_r_link" file="ankle_pitch_r_link.STL"/>
+    <mesh name="ankle_roll_r_link" file="ankle_roll_r_link.STL"/>
+    <mesh name="waist_yaw_link" file="waist_yaw_link.STL"/>
+    <mesh name="waist_roll_link" file="waist_roll_link.STL"/>
+    <mesh name="waist_pitch_link" file="waist_pitch_link.STL"/>
+    <mesh name="head_yaw_link" file="head_yaw_link.STL"/>
+    <mesh name="head_pitch_link" file="head_pitch_link.STL"/>
+    <mesh name="camera_head_link" file="camera_head_link.STL"/>
+    <mesh name="radar_head_link" file="radar_head_link.STL"/>
+    <mesh name="imu_head_link" file="imu_head_link.STL"/>
+    <mesh name="shoulder_pitch_l_link" file="shoulder_pitch_l_link.STL"/>
+    <mesh name="shoulder_roll_l_link" file="shoulder_roll_l_link.STL"/>
+    <mesh name="shoulder_yaw_l_link" file="shoulder_yaw_l_link.STL"/>
+    <mesh name="elbow_pitch_l_link" file="elbow_pitch_l_link.STL"/>
+    <mesh name="elbow_yaw_l_link" file="elbow_yaw_l_link.STL"/>
+    <mesh name="wrist_pitch_l_link" file="wrist_pitch_l_link.STL"/>
+    <mesh name="wrist_roll_l_link" file="wrist_roll_l_link.STL"/>
+    <mesh name="left_tcp_link" file="left_tcp_link.STL"/>
+    <mesh name="shoulder_pitch_r_link" file="shoulder_pitch_r_link.STL"/>
+    <mesh name="shoulder_roll_r_link" file="shoulder_roll_r_link.STL"/>
+    <mesh name="shoulder_yaw_r_link" file="shoulder_yaw_r_link.STL"/>
+    <mesh name="elbow_pitch_r_link" file="elbow_pitch_r_link.STL"/>
+    <mesh name="elbow_yaw_r_link" file="elbow_yaw_r_link.STL"/>
+    <mesh name="wrist_pitch_r_link" file="wrist_pitch_r_link.STL"/>
+    <mesh name="wrist_roll_r_link" file="wrist_roll_r_link.STL"/>
+    <mesh name="right_tcp_link" file="right_tcp_link.STL"/>
+    <mesh name="camera_body_front_link" file="camera_body_front_link.STL"/>
+    <mesh name="imu_waist_link" file="imu_waist_link.STL"/>
+  </asset>
+
+  <worldbody>
+    <light directional="true" diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
+    <light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
+    <geom name="ground" type="plane" size="0 0 1" pos="0.001 0 0" quat="1 0 0 0" material="matplane" condim="1" conaffinity='15' friction="1.5 0.005 0.0001"/>
+    
+    <body name="pelvis" pos="0 0 1">
+      <inertial pos="0.000708938 -0.000476035 -0.0603041" quat="0.995372 0.0929922 0.0239168 -0.00377104" mass="10.5873" diaginertia="0.0891022 0.0725256 0.0697952"/>
+      <joint name="pelvis_to_world_joint" type="free" limited="false" actuatorfrclimited="false"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="pelvis"/>
+      <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="pelvis"/>
+      <geom pos="-0.081641 -0.00010952 -0.063057" quat="-2.59903e-06 -0.707565 0.706648 2.59566e-06" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="imu_waist_link"/>
+      <geom pos="-0.081641 -0.00010952 -0.063057" quat="-2.59903e-06 -0.707565 0.706648 2.59566e-06" type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="imu_waist_link"/>
+      <site name='imu' size='0.01' pos='0.0 0.0 0.0'/>
+      <body name="hip_pitch_l_link" pos="-0.00010305 0.076815 -0.11847" quat="0.996195 -0.0871543 -5.84587e-05 0.000668198">
+        <inertial pos="0.009589 0.069327 -0.024358" quat="0.868301 0.493294 -0.0452228 -0.0258879" mass="2.22138" diaginertia="0.00485603 0.0037213 0.00328367"/>
+        <joint name="hip_pitch_l_joint" class="hip_pitch_motor" pos="0 0 0" axis="0 1 0" range="-3.14159 2.87979" actuatorfrcrange="-235 235"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_pitch_l_link"/>
+        <body name="hip_roll_l_link" pos="9.9595e-05 0.073992 -0.026966" quat="0.996195 0.0871543 0 0">
+          <inertial pos="-0.000767 -4.3e-05 -0.03918" quat="0.478756 0.521129 0.520225 0.478104" mass="0.560034" diaginertia="0.001743 0.00147771 0.00109729"/>
+          <joint name="hip_roll_l_joint" class="hip_roll_motor" pos="0 0 0" axis="1 0 0" range="-0.418879 2.61799" actuatorfrcrange="-235 235"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_roll_l_link"/>
+          <!-- <geom size="0.07 0.05" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/> -->
+          <body name="hip_yaw_l_link" pos="0 0 -0.0785">
+            <inertial pos="0.001014 -0.022912 -0.125892" quat="0.997124 -0.028488 0.00651602 -0.0699284" mass="5.65816" diaginertia="0.0456692 0.0428675 0.0143123"/>
+            <joint name="hip_yaw_l_joint" class="hip_yaw_motor" pos="0 0 0" axis="0 0 1" range="-1.39626 4.53786" actuatorfrcrange="-150 150"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_yaw_l_link"/>
+            <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="hip_yaw_l_link"/>
+            <body name="knee_pitch_l_link" pos="-0.012927 9.9662e-05 -0.30599">
+              <inertial pos="0.02218 -0.000444 -0.151985" quat="0.634112 0.0235196 0.0364207 0.772025" mass="3.15858" diaginertia="0.0348599 0.0346942 0.00377596"/>
+              <joint name="knee_pitch_l_joint" class="knee_motor" pos="0 0 0" axis="0 1 0" range="-0.0872665 2.53073" actuatorfrcrange="-400 400"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="knee_pitch_l_link"/>
+              <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="knee_pitch_l_link"/>
+              <body name="ankle_pitch_l_link" pos="0 0 -0.4">
+                <inertial pos="0 0 0" quat="0 0.707107 0 0.707107" mass="0.142658" diaginertia="4.9e-05 3.2e-05 2.3e-05"/>
+                <joint name="ankle_pitch_l_joint" class="ankle_motor" pos="0 0 0" axis="0 1 0" range="-1.22173 0.523599" actuatorfrcrange="-55 55"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="ankle_pitch_l_link"/>
+                <body name="ankle_roll_l_link">
+                  <inertial pos="0.024536 -1.7e-05 -0.033593" quat="0.000973792 0.727447 0.00122134 0.686162" mass="0.888892" diaginertia="0.00514091 0.004839 0.000764092"/>
+                  <joint name="ankle_roll_l_joint" class="ankle_motor" pos="0 0 0" axis="1 0 0" range="-0.523599 0.523599" actuatorfrcrange="-55 55"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="ankle_roll_l_link"/>
+                  <geom name="foot_left_front_outer" size="0.012 0.03" pos="0.085 -0.03 -0.046" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_front_inner" size="0.012 0.03" pos="0.085 0.03 -0.046" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_mid_outer" size="0.01 0.1" pos="0.045 -0.02 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_mid_inner" size="0.01 0.1" pos="0.045 0.02 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_strip_outer" size="0.012 0.11" pos="0.045 -0.01 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_strip_center" size="0.012 0.12" pos="0.045 0 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_left_strip_inner" size="0.012 0.11" pos="0.045 0.01 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="hip_pitch_r_link" pos="0.00010305 -0.076815 -0.11847" quat="0.996195 0.0871543 5.84587e-05 0.000668198">
+        <inertial pos="0.009589 -0.069327 -0.024358" quat="0.493294 0.868301 0.0258879 0.0452228" mass="2.22138" diaginertia="0.00485603 0.0037213 0.00328367"/>
+        <joint name="hip_pitch_r_joint" class="hip_pitch_motor" pos="0 0 0" axis="0 1 0" range="-3.14159 2.87979" actuatorfrcrange="-235 235"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_pitch_r_link"/>
+        <body name="hip_roll_r_link" pos="9.9595e-05 -0.073992 -0.026966" quat="0.996195 -0.0871543 0 0">
+          <inertial pos="-0.000767 4.3e-05 -0.03918" quat="0.478104 0.520225 0.521129 0.478756" mass="0.560034" diaginertia="0.001743 0.00147771 0.00109729"/>
+          <joint name="hip_roll_r_joint" class="hip_roll_motor" pos="0 0 0" axis="1 0 0" range="-2.61799 0.418879" actuatorfrcrange="-235 235"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_roll_r_link"/>
+          <!-- <geom size="0.07 0.05" quat="0.707107 0 0.707107 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/> -->
+          <body name="hip_yaw_r_link" pos="0 0 -0.0785">
+            <inertial pos="0.001014 0.022912 -0.125892" quat="0.997124 0.028488 0.00651602 0.0699284" mass="5.65816" diaginertia="0.0456692 0.0428675 0.0143123"/>
+            <joint name="hip_yaw_r_joint" class="hip_yaw_motor" pos="0 0 0" axis="0 0 1" range="-4.53786 1.39626" actuatorfrcrange="-150 150"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="hip_yaw_r_link"/>
+            <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="hip_yaw_r_link"/>
+            <body name="knee_pitch_r_link" pos="-0.012927 -9.9662e-05 -0.30599">
+              <inertial pos="0.02218 0.000444 -0.151985" quat="0.772025 0.0364207 0.0235196 0.634112" mass="3.15858" diaginertia="0.0348599 0.0346942 0.00377596"/>
+              <joint name="knee_pitch_r_joint" class="knee_motor" pos="0 0 0" axis="0 1 0" range="-0.0872665 2.53073" actuatorfrcrange="-400 400"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="knee_pitch_r_link"/>
+              <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="knee_pitch_r_link"/>
+              <body name="ankle_pitch_r_link" pos="0 0 -0.4">
+                <inertial pos="0 0 0" quat="0 0.707107 0 0.707107" mass="0.142658" diaginertia="4.9e-05 3.2e-05 2.3e-05"/>
+                <joint name="ankle_pitch_r_joint" class="ankle_motor" pos="0 0 0" axis="0 1 0" range="-1.22173 0.523599" actuatorfrcrange="-55 55"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="ankle_pitch_r_link"/>
+                <body name="ankle_roll_r_link">
+                  <inertial pos="0.024536 1.7e-05 -0.033593" quat="-0.000973792 0.727447 -0.00122134 0.686162" mass="0.888892" diaginertia="0.00514091 0.004839 0.000764092"/>
+                  <joint name="ankle_roll_r_joint" class="ankle_motor" pos="0 0 0" axis="1 0 0" range="-0.523599 0.523599" actuatorfrcrange="-55 55"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="ankle_roll_r_link"/>
+                  <geom name="foot_right_front_outer" size="0.012 0.03" pos="0.085 -0.03 -0.046" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_front_inner" size="0.012 0.03" pos="0.085 0.03 -0.046" quat="0.707105 0 0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_mid_outer" size="0.01 0.1" pos="0.045 -0.02 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_mid_inner" size="0.01 0.1" pos="0.045 0.02 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_strip_outer" size="0.012 0.11" pos="0.045 -0.01 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_strip_center" size="0.012 0.12" pos="0.045 0 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                  <geom name="foot_right_strip_inner" size="0.012 0.11" pos="0.045 0.01 -0.046" quat="0.707105 0 -0.707108 0" type="cylinder" rgba="0.752941 0.752941 0.752941 1"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+      <body name="waist_yaw_link" quat="1 0 0 0.00067075">
+        <inertial pos="-0.001947 -0.000934 0.044217" quat="0.642825 -0.123511 -0.167177 0.737273" mass="1.68837" diaginertia="0.00247429 0.00229232 0.0018544"/>
+        <joint name="waist_yaw_joint" class="waist_motor" pos="0 0 0" axis="0 0 1" range="-2.74017 3.22886" actuatorfrcrange="-91 91"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="waist_yaw_link"/>
+        <body name="waist_roll_link" pos="0 0 0.0838">
+          <inertial pos="-0.006103 -0.006401 0.0721" quat="0.638305 0.66286 -0.234695 0.313212" mass="3.59519" diaginertia="0.0149204 0.0144797 0.00701787"/>
+          <joint name="waist_roll_joint" class="waist_motor" pos="0 0 0" axis="1 0 0" range="-0.436332 0.436332" actuatorfrcrange="-150 150"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="waist_roll_link"/>
+          <body name="waist_pitch_link" pos="0 0 0.078424">
+            <inertial pos="0.00047912 0.00141024 0.212505" quat="0.999945 -0.00130091 0.0101935 -0.00226087" mass="11.8972" diaginertia="0.25491 0.225892 0.0979488"/>
+            <joint name="waist_pitch_joint" class="waist_motor" pos="0 0 0" axis="0 1 0" range="-0.523599 0.959931" actuatorfrcrange="-150 150"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="waist_pitch_link"/>
+            <!-- <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="waist_pitch_link"/> -->
+            <geom pos="0 0 0.43627" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="head_yaw_link"/>
+            <geom pos="0.0689 -7.7578e-05 0.43567" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="head_pitch_link"/>
+            <geom pos="0.08445 0.0111314 0.435546" quat="0.501222 -0.498774 0.498775 -0.501224" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="camera_head_link"/>
+            <geom pos="0 0 0.38177" quat="-3.67321e-06 -1 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="radar_head_link"/>
+            <geom pos="-0.057774 0 0.42677" quat="-2.59734e-06 -0.707105 0.707108 2.59735e-06" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="imu_head_link"/>
+            <geom pos="0.092709 -0.02375 0.039976" quat="0.21264 0.674376 0.674378 0.212641" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="camera_body_front_link"/>
+            <geom pos="0.092709 -0.02375 0.039976" quat="0.21264 0.674376 0.674378 0.212641" type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="camera_body_front_link"/>
+            <body name="shoulder_pitch_l_link" pos="0 0.14552 0.18664" quat="0.999048 0.0436192 0 0">
+              <inertial pos="0.006861 0.060647 -2.7e-05" quat="-0.102177 0.698772 0.102786 0.700509" mass="0.914808" diaginertia="0.001164 0.000926669 0.000748328"/>
+              <joint name="shoulder_pitch_l_joint" class="arm_motor" pos="0 0 0" axis="0 1 0" range="-2.96706 2.96706" actuatorfrcrange="-90 90"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_pitch_l_link"/>
+              <body name="shoulder_roll_l_link" pos="0 0.0678 0">
+                <inertial pos="-0.000672 0 -0.043313" quat="0.707078 0.00642532 0.00642532 0.707078" mass="0.319153" diaginertia="0.000936 0.000764073 0.000543927"/>
+                <joint name="shoulder_roll_l_joint" class="arm_motor" pos="0 0 0" axis="1 0 0" range="-0.261799 3.40339" actuatorfrcrange="-90 90"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_roll_l_link"/>
+                <body name="shoulder_yaw_l_link" pos="0 0 -0.1004">
+                  <inertial pos="0.009141 0.003692 -0.094058" quat="0.996924 0.0324636 -0.0701011 -0.0132387" mass="1.50222" diaginertia="0.00793691 0.00755988 0.00117522"/>
+                  <joint name="shoulder_yaw_l_joint" class="arm_motor" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_yaw_l_link"/>
+                  <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_yaw_l_link"/>
+                  <body name="elbow_pitch_l_link" pos="0.02 0 -0.1596">
+                    <inertial pos="-0.017311 0.000579 -0.066888" quat="0.996392 -0.023316 0.0810929 -0.00914165" mass="0.647211" diaginertia="0.00137312 0.00123605 0.000683824"/>
+                    <joint name="elbow_pitch_l_joint" class="arm_motor" pos="0 0 0" axis="0 1 0" range="-2.61799 0.261799" actuatorfrcrange="-50 50"/>
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="elbow_pitch_l_link"/>
+                    <body name="elbow_yaw_l_link" pos="-0.02 0 -0.107">
+                      <inertial pos="0.001113 0.005481 -0.05106" quat="0.998888 -0.0470326 0.00308357 0.000463667" mass="0.229689" diaginertia="0.000368006 0.000290755 0.000205239"/>
+                      <joint name="elbow_yaw_l_joint" class="arm_motor" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-25 25"/>
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="elbow_yaw_l_link"/>
+                      <body name="wrist_pitch_l_link" pos="0 0 -0.091">
+                        <inertial pos="0.000815 0.001368 -0.031332" quat="0.703924 -0.0255071 0.00529546 0.709798" mass="0.931823" diaginertia="0.00155774 0.00154075 0.000635512"/>
+                        <joint name="wrist_pitch_l_joint" class="wrist_motor" pos="0 0 0" axis="0 1 0" range="-1.39626 1.39626" actuatorfrcrange="-25 25"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_pitch_l_link"/>
+                        <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_pitch_l_link"/>
+                        <body name="wrist_roll_l_link" pos="0 0 -0.062">
+                          <inertial pos="0.00513346 -0.00203116 -0.0838367" quat="0.780783 0.043744 0.102158 0.61484" mass="0.577436" diaginertia="0.0012863 0.00114426 0.000499739"/>
+                          <joint name="wrist_roll_l_joint" class="wrist_motor" pos="0 0 0" axis="1 0 0" range="-1.39626 1.39626" actuatorfrcrange="-25 25"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_roll_l_link"/>
+                          <geom pos="0 0 -0.045" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="left_tcp_link"/>
+                          <geom pos="0 0 -0.045" quat="1 0 0 0" type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="left_tcp_link"/>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+            <body name="shoulder_pitch_r_link" pos="0 -0.14552 0.18664" quat="0.999048 -0.0436192 0 0">
+              <inertial pos="0.006861 -0.060647 -2.7e-05" quat="0.102177 0.698772 -0.102786 0.700509" mass="0.914808" diaginertia="0.001164 0.000926669 0.000748328"/>
+              <joint name="shoulder_pitch_r_joint" class="arm_motor" pos="0 0 0" axis="0 1 0" range="-2.96706 2.96706" actuatorfrcrange="-90 90"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_pitch_r_link"/>
+              <body name="shoulder_roll_r_link" pos="0 -0.0678 0">
+                <inertial pos="-0.000672 0 -0.043313" quat="0.707078 0.00642532 0.00642532 0.707078" mass="0.319153" diaginertia="0.000936 0.000764073 0.000543927"/>
+                <joint name="shoulder_roll_r_joint" class="arm_motor" pos="0 0 0" axis="1 0 0" range="-3.40339 0.261799" actuatorfrcrange="-90 90"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_roll_r_link"/>
+                <body name="shoulder_yaw_r_link" pos="0 0 -0.1004">
+                  <inertial pos="0.009141 -0.003692 -0.094058" quat="0.996924 -0.0324636 -0.0701011 0.0132387" mass="1.50222" diaginertia="0.00793691 0.00755988 0.00117522"/>
+                  <joint name="shoulder_yaw_r_joint" class="arm_motor" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_yaw_r_link"/>
+                  <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="shoulder_yaw_r_link"/>
+                  <body name="elbow_pitch_r_link" pos="0.02 0 -0.1596">
+                    <inertial pos="-0.017311 -0.000579 -0.066888" quat="0.996392 0.023316 0.0810929 0.00914165" mass="0.647211" diaginertia="0.00137312 0.00123605 0.000683824"/>
+                    <joint name="elbow_pitch_r_joint" class="arm_motor" pos="0 0 0" axis="0 1 0" range="-2.61799 0.261799" actuatorfrcrange="-50 50"/>
+                    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="elbow_pitch_r_link"/>
+                    <body name="elbow_yaw_r_link" pos="-0.02 0 -0.107">
+                      <inertial pos="0.001113 -0.005481 -0.05106" quat="0.998888 0.0470326 0.00308357 -0.000463667" mass="0.229689" diaginertia="0.000368006 0.000290755 0.000205239"/>
+                      <joint name="elbow_yaw_r_joint" class="arm_motor" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-25 25"/>
+                      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="elbow_yaw_r_link"/>
+                      <body name="wrist_pitch_r_link" pos="0 0 -0.091">
+                        <inertial pos="0.000815 -0.001368 -0.031332" quat="0.709798 0.00529546 -0.0255071 0.703924" mass="0.931823" diaginertia="0.00155774 0.00154075 0.000635512"/>
+                        <joint name="wrist_pitch_r_joint" class="wrist_motor" pos="0 0 0" axis="0 1 0" range="-1.39626 1.39626" actuatorfrcrange="-25 25"/>
+                        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_pitch_r_link"/>
+                        <geom type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_pitch_r_link"/>
+                        <body name="wrist_roll_r_link" pos="0 0 -0.062">
+                          <inertial pos="0.00513346 0.00203116 -0.0838367" quat="0.61484 0.102158 0.043744 0.780783" mass="0.577436" diaginertia="0.0012863 0.00114426 0.000499739"/>
+                          <joint name="wrist_roll_r_joint" class="wrist_motor" pos="0 0 0" axis="1 0 0" range="-1.39626 1.39626" actuatorfrcrange="-25 25"/>
+                          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="wrist_roll_r_link"/>
+                          <geom pos="0 0 -0.045" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.752941 0.752941 0.752941 1" mesh="right_tcp_link"/>
+                          <geom pos="0 0 -0.045" quat="1 0 0 0" type="mesh" rgba="0.752941 0.752941 0.752941 1" mesh="right_tcp_link"/>
+                        </body>
+                      </body>
+                    </body>
+                  </body>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>

xSIM_MUJOCO/scripts/convert_xml.py

@@ -0,0 +1,9 @@
+import mujoco
+
+# model = mujoco.MjModel.from_xml_path("resources/dex_v3/urdf/dex_v3_mujoco.urdf")
+
+# mujoco.mj_saveLastXML("resources/dex_v3/urdf/dex_v3_mujoco_v2.xml",model)
+
+model = mujoco.MjModel.from_xml_path("resources/evt2/urdf/evt2_mujoco.urdf")
+
+mujoco.mj_saveLastXML("resources/evt2/urdf/evt2.xml",model)

xSIM_MUJOCO/scripts/elastic_band.py

@@ -0,0 +1,47 @@
+import mujoco
+import numpy as np
+from typing import Literal
+
+
+class ElasticBand:
+
+    def __init__(self, property: Literal['spring', 'elastic'] = 'elastic', enable=True):
+        self.stiffness = 300
+        self.damping = 100
+        self.point = np.array([0, 0, 3])
+        self.length = 0
+        self.enable = enable
+        self.property = property
+
+    def Advance(self, x, dx):
+        """
+        Args:
+          δx: desired position - current position
+          dx: current velocity
+        """
+        δx = self.point - x
+        distance = np.linalg.norm(δx)
+        direction = δx / distance
+        v = np.dot(dx, direction)
+        if self.property =="spring":
+            f = (self.stiffness *
+                (distance - self.length) - self.damping * v) * direction
+        else:
+            # 只有当距离大于自然长度时才产生拉力
+            if distance > self.length:
+                f = (self.stiffness * (distance - self.length) - self.damping * v) * direction
+            else:
+                # 距离小于等于自然长度时不产生力
+                f = np.zeros_like(direction)
+        # f[0]=0
+        # f[1]=0
+        return f
+
+    def MujuocoKeyCallback(self, key):
+        glfw = mujoco.glfw.glfw
+        if key == glfw.KEY_UP:
+            self.length -= 0.1
+        if key == glfw.KEY_DOWN:
+            self.length += 0.1
+        if key == glfw.KEY_ESCAPE:
+            self.enable = not self.enable

xSIM_MUJOCO/scripts/simulator_view_asyn.py

@@ -0,0 +1,965 @@
+"""
+MuJoCo机器人控制脚本
+实现位置和力矩混合控制，读取传感器数据
+"""
+import mujoco
+import mujoco.viewer
+import time
+import numpy as np
+import matplotlib.pyplot as plt
+from collections import deque
+import threading
+import rclpy
+from rclpy.node import Node
+# import message_filters
+from bodyctrl_msgs.msg import MotorCtrl, MotorStatus,MotorStatusMsg, CmdMotorCtrl, Imu, Euler
+from queue import Queue, Full
+from pynput import keyboard
+from scipy.spatial.transform import Rotation
+from std_msgs.msg import String
+import os
+from elastic_band import ElasticBand
+import argparse
+
+
+def quaternion_to_euler_scipy(w, x, y, z, degrees=False):
+    """
+    使用SciPy进行四元数转欧拉角
+    """
+    # 创建旋转对象 (注意顺序: [x, y, z, w])
+    rotation = Rotation.from_quat([x, y, z, w])
+
+    # 转换为欧拉角 (顺序: 'xyz' 对应 roll, pitch, yaw)
+    euler_angles = rotation.as_euler('xyz', degrees=degrees)
+
+    return euler_angles[0], euler_angles[1], euler_angles[2]
+
+
+class RobotSimulator:
+
+    def __init__(self, model_path, node: Node | None = None, debug=False, robot_config='full'):
+        self.debug = debug
+        # 加载模型
+        self.model = mujoco.MjModel.from_xml_path(model_path)
+        self.data = mujoco.MjData(self.model)
+        self.default_timestep = self.model.opt.timestep
+
+        # 初始化控制模式 (0=(位置控制(仅上半身)和力控都存在), 1=全身仅力矩控制)
+        self.control_mode = 1
+        self.execution_mode = "torque_pvd"
+        self.direct_position_states = {"XSIMRUN"}
+        self.torque_disable_mask = int(self.model.opt.disableactuator)
+        self.direct_position_disable_mask = 4
+
+        # 记录数据用于绘图
+        record_length = 10000
+        self.time_history = deque(maxlen=record_length)
+        self.joint_pos_history = deque(maxlen=record_length)
+        self.joint_vel_history = deque(maxlen=record_length)
+        self.joint_torque_history = deque(maxlen=record_length)
+        self.imu_orient_history = deque(maxlen=record_length)
+        self.imu_accel_history = deque(maxlen=record_length)
+        self.imu_pos_history = deque(maxlen=record_length)
+
+        # 根据机器人配置设置关节映射
+        if robot_config == '21':
+            # 21自由度配置（没有手腕关节）
+            self.motor_id_to_joint = {
+                1: 'head_roll_joint',
+                2: 'head_pitch_joint',
+                3: 'head_yaw_joint',
+                11: 'shoulder_pitch_l_joint',
+                12: 'shoulder_roll_l_joint',
+                13: 'shoulder_yaw_l_joint',
+                14: 'elbow_pitch_l_joint',
+
+                21: 'shoulder_pitch_r_joint',
+                22: 'shoulder_roll_r_joint',
+                23: 'shoulder_yaw_r_joint',
+                24: 'elbow_pitch_r_joint',
+
+                33: 'waist_yaw_joint',
+                51: 'hip_pitch_l_joint',
+                52: 'hip_roll_l_joint',
+                53: 'hip_yaw_l_joint',
+                54: 'knee_pitch_l_joint',
+                55: 'ankle_pitch_l_joint',
+                56: 'ankle_roll_l_joint',
+                61: 'hip_pitch_r_joint',
+                62: 'hip_roll_r_joint',
+                63: 'hip_yaw_r_joint',
+                64: 'knee_pitch_r_joint',
+                65: 'ankle_pitch_r_joint',
+                66: 'ankle_roll_r_joint'
+            }
+            
+            #双臂关节名称
+            self.arm_joint_names = [
+                'shoulder_pitch_l_joint', 'shoulder_roll_l_joint',
+                'shoulder_yaw_l_joint', 'elbow_pitch_l_joint', 
+                'shoulder_pitch_r_joint', 'shoulder_roll_r_joint',
+                'shoulder_yaw_r_joint', 'elbow_pitch_r_joint', 
+            ]
+            #腰部关节名
+            self.waist_joint_names = [
+                'waist_yaw_joint', 
+            ]
+        else:
+            # 全配置（包含手腕关节）
+            self.motor_id_to_joint = {
+                1: 'head_roll_joint',
+                2: 'head_pitch_joint',
+                3: 'head_yaw_joint',
+                11: 'shoulder_pitch_l_joint',
+                12: 'shoulder_roll_l_joint',
+                13: 'shoulder_yaw_l_joint',
+                14: 'elbow_pitch_l_joint',
+                15: 'elbow_yaw_l_joint',
+                16: 'wrist_pitch_l_joint',
+                17: 'wrist_roll_l_joint',
+                21: 'shoulder_pitch_r_joint',
+                22: 'shoulder_roll_r_joint',
+                23: 'shoulder_yaw_r_joint',
+                24: 'elbow_pitch_r_joint',
+                25: 'elbow_yaw_r_joint',
+                26: 'wrist_pitch_r_joint',
+                27: 'wrist_roll_r_joint',
+                # 31: 'waist_yaw_joint',
+                # 32: 'waist_roll_joint',
+                # 33: 'waist_pitch_joint',
+                33: 'waist_yaw_joint',
+                32: 'waist_roll_joint',
+                31: 'waist_pitch_joint',
+                51: 'hip_pitch_l_joint',
+                52: 'hip_roll_l_joint',
+                53: 'hip_yaw_l_joint',
+                54: 'knee_pitch_l_joint',
+                55: 'ankle_pitch_l_joint',
+                56: 'ankle_roll_l_joint',
+                61: 'hip_pitch_r_joint',
+                62: 'hip_roll_r_joint',
+                63: 'hip_yaw_r_joint',
+                64: 'knee_pitch_r_joint',
+                65: 'ankle_pitch_r_joint',
+                66: 'ankle_roll_r_joint'
+            }
+            
+            #双臂关节名称
+            self.arm_joint_names = [
+                'shoulder_pitch_l_joint', 'shoulder_roll_l_joint',
+                'shoulder_yaw_l_joint', 'elbow_pitch_l_joint', 'elbow_yaw_l_joint',
+                'wrist_pitch_l_joint', 'wrist_roll_l_joint',
+                'shoulder_pitch_r_joint', 'shoulder_roll_r_joint',
+                'shoulder_yaw_r_joint', 'elbow_pitch_r_joint', 'elbow_yaw_r_joint',
+                'wrist_pitch_r_joint', 'wrist_roll_r_joint'
+            ]
+            #腰部关节名
+            self.waist_joint_names = [
+                'waist_yaw_joint', 'waist_roll_joint', 'waist_pitch_joint'
+            ]
+        
+        # 关节到电机ID的映射
+        self.joint_to_motor_id = {
+            joint: id
+            for id, joint in self.motor_id_to_joint.items()
+        }
+
+        #腿部关节名称
+        self.leg_joint_names = [
+            'hip_roll_l_joint', 'hip_pitch_l_joint', 'hip_yaw_l_joint',
+            'knee_pitch_l_joint', 'ankle_pitch_l_joint', 'ankle_roll_l_joint',
+            'hip_roll_r_joint', 'hip_pitch_r_joint', 'hip_yaw_r_joint',
+            'knee_pitch_r_joint', 'ankle_pitch_r_joint', 'ankle_roll_r_joint'
+        ]
+        # 获取传感器/执行器索引
+        self.get_all_robot_info()
+
+        self.sim_view_lock = threading.Lock()
+        
+        # 升降带
+        self.elastic_band = ElasticBand(enable=True)
+        # 根据配置设置弹性带连接的链接
+        if robot_config == '21':
+            self.band_attached_link = self.model.body("waist_yaw_link").id
+        else:
+            self.band_attached_link = self.model.body("waist_pitch_link").id
+
+        # 启动可视化器
+        self.paused = False
+        # self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
+        self.viewer = mujoco.viewer.launch_passive(
+        self.model, self.data, key_callback=self.elastic_band.MujuocoKeyCallback
+        )
+        self.view_dt = 0.03
+        # 线程控制变量
+        self.joint_cmd_lock = threading.Lock()
+        self.stop_event = threading.Event()
+
+        #创建ROS相关话题
+        self.node = node
+        if self.node:
+            self.node.create_subscription(CmdMotorCtrl, 'arm/cmd_ctrl',
+                                          self.joint_cmd_callback, 10)
+            self.node.create_subscription(CmdMotorCtrl, 'leg/cmd_ctrl',
+                                          self.joint_cmd_callback, 10)
+            self.node.create_subscription(CmdMotorCtrl, 'waist/cmd_ctrl',
+                                          self.joint_cmd_callback, 10)
+            self.node.create_subscription(String, '/control/fsm_state',
+                                          self.fsm_state_callback, 10)
+            #创建发布者
+            self.arm_status_pub = self.node.create_publisher(
+                MotorStatusMsg, 'arm/status', 20)
+            self.leg_status_pub = self.node.create_publisher(
+                MotorStatusMsg, 'leg/status', 20)
+            self.waist_status_pub = self.node.create_publisher(
+                MotorStatusMsg, 'waist/status', 10)
+            self.imu_status_pub = self.node.create_publisher(
+                Imu, 'imu/status', 10)
+        # 传感器返回的机器人状态
+        self.joint_positions = {}
+        self.joint_velocities = {}
+        self.joint_torques = {}
+        self.imu_orientation = None
+        self.imu_position = None
+        self.imu_angular_velocity = None
+        self.imu_linear_velocity = None
+        self.imu_linear_acceleration = None
+        self.imu_magnetometer = None
+
+        # 关节命令
+        self.raw_joint_commands = Queue()
+        self.joint_commands = {}
+        self.last_raw_joint_commands = {}
+
+        # 发布线程相关
+        self.pub_thread = None
+        self.pub_thread_lock = threading.Lock()
+        self.pub_thread_running = False
+        self.last_sensor_data = None
+        self.sensor_data_lock = threading.Lock()
+
+        # 初始化控制器
+        self.init_controller()
+    def init_controller(self):
+        self._set_execution_mode("torque_pvd")
+
+    def _set_execution_mode(self, mode):
+        if mode == self.execution_mode:
+            return
+        if mode == "direct_position":
+            self.model.opt.disableactuator = self.direct_position_disable_mask
+            self.model.opt.timestep = 0.005
+            self.execution_mode = "direct_position"
+            print("切换到直接位置控制模式：启用 pos_* 执行器，禁用 motor_* 执行器")
+            return
+
+        self.model.opt.disableactuator = self.torque_disable_mask
+        self.model.opt.timestep = self.default_timestep
+        self.execution_mode = "torque_pvd"
+        print("切换到PVD力矩控制模式：启用 motor_* 执行器，禁用 pos_* 执行器")
+
+    def switch_to_torque_control(self):
+        """切换到力矩控制：禁用位置控制器"""
+        self.control_mode = 1
+        self._set_execution_mode("torque_pvd")
+    
+    def switch_to_position_torque_control(self):
+        """切换到位置力控混合控制模式：启用位置控制器"""
+        self.control_mode = 0
+        self._set_execution_mode("direct_position")
+
+    def fsm_state_callback(self, msg: String):
+        state_name = msg.data.strip()
+        if state_name in self.direct_position_states:
+            self._set_execution_mode("direct_position")
+        else:
+            self._set_execution_mode("torque_pvd")
+
+    def start_keyboard_listener(self):
+        """使用 pynput 监听键盘"""
+
+        def on_press(key):
+            try:
+                if key == keyboard.Key.num_lock:
+                    self.paused = not self.paused
+                    print(f"仿真已{'暂停' if self.paused else '继续'}")
+                if key == keyboard.Key.end:
+                    if self.control_mode == 1:
+                        self.switch_to_position_torque_control()
+                    else:
+                        self.switch_to_torque_control()
+            except Exception as e:
+                print(f"键盘监听错误: {e}")
+
+        def listen():
+            with keyboard.Listener(on_press=on_press) as listener:
+                while not self.stop_event.is_set():
+                    time.sleep(0.1)
+                listener.stop()
+
+        thread = threading.Thread(target=listen, daemon=True)
+        thread.start()
+
+    def start_publishing(self):
+        """启动发布线程"""
+        if not self.node:
+            return
+        with self.pub_thread_lock:
+            if not self.pub_thread_running:
+                self.pub_thread_running = True
+                self.pub_thread = threading.Thread(target=self._publish_thread,
+                                                   daemon=True)
+                self.pub_thread.start()
+
+    def stop_publishing(self):
+        """停止发布线程"""
+        with self.pub_thread_lock:
+            if self.pub_thread_running:
+                self.pub_thread_running = False
+                if self.pub_thread:
+                    self.pub_thread.join(timeout=1.0)
+
+    def _publish_thread(self):
+        """发布状态的独立线程"""
+        pub_freq = 400  # 100Hz发布频率
+        pub_interval = 1.0 / pub_freq
+        sensor_data = None
+        while self.pub_thread_running and not self.stop_event.is_set():
+            start_time = time.perf_counter()
+
+            # 获取最新的传感器数据
+            with self.sensor_data_lock:
+                if self.last_sensor_data is not None:
+                    sensor_data = self.last_sensor_data.copy()
+            # 为每个关节组发布状态
+            self._publish_joint_group(self.arm_joint_names,
+                                      self.arm_status_pub, sensor_data)
+            self._publish_joint_group(self.waist_joint_names,
+                                      self.waist_status_pub, sensor_data)
+            self._publish_joint_group(self.leg_joint_names,
+                                      self.leg_status_pub, sensor_data)
+            # 发布Imu
+            self._publish_imu_status(self.imu_status_pub, sensor_data)
+            # 控制发布频率
+            elapsed = time.perf_counter() - start_time
+            sleep_time = pub_interval - elapsed
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+    def _publish_imu_status(self, publisher, sensor_data):
+        if publisher and sensor_data:
+            orientaion = sensor_data['imu_orientation']
+            # position = sensor_data['imu_position']
+            imu_angular_velocity = sensor_data['imu_angular_velocity']
+            imu_linear_acceleration = sensor_data['imu_linear_acceleration']
+            imu_msg: Imu = Imu()
+            imu_msg.header.stamp = self.node.get_clock().now().to_msg()
+            imu_msg.orientation.x = orientaion[1]
+            imu_msg.orientation.y = orientaion[2]
+            imu_msg.orientation.z = orientaion[3]
+            imu_msg.orientation.w = orientaion[0]
+            # 四元数转换为欧拉角
+            r, p, y = quaternion_to_euler_scipy(*list(orientaion))
+            imu_msg.euler = Euler()
+            imu_msg.euler.roll = r
+            imu_msg.euler.pitch = p
+            imu_msg.euler.yaw = y
+            imu_msg.angular_velocity.x = imu_angular_velocity[0]
+            imu_msg.angular_velocity.y = imu_angular_velocity[1]
+            imu_msg.angular_velocity.z = imu_angular_velocity[2]
+            imu_msg.linear_acceleration.x = imu_linear_acceleration[0]
+            imu_msg.linear_acceleration.y = imu_linear_acceleration[1]
+            imu_msg.linear_acceleration.z = imu_linear_acceleration[2]
+
+            try:
+                publisher.publish(imu_msg)
+            except Exception as e:
+                print(f"发布 imu 状态时出错: {e}")
+
+    def _publish_joint_group(self, joint_names, publisher, sensor_data):
+        """发布指定关节组的状态"""
+        if publisher and sensor_data:
+            joint_positions = sensor_data['joint_positions']
+            joint_velocities = sensor_data['joint_velocities']
+            joint_torques = sensor_data['joint_torques']
+            #构造关节状态消息
+            motor_status_msg = MotorStatusMsg()
+            motor_status_msg.header.stamp = self.node.get_clock().now().to_msg()
+            for joint_name in joint_names:
+                if joint_name in joint_positions and joint_name in joint_velocities:
+                    motor_status = self.construct_motor_status(
+                        joint_name, joint_positions[joint_name],
+                        joint_velocities[joint_name], joint_torques[joint_name])
+                    motor_status_msg.status.append(motor_status)
+            try:
+                publisher.publish(motor_status_msg)
+            except Exception as e:
+                print(f"发布状态{motor_status_msg}时出错: {e}")
+
+    def get_all_robot_info(self):
+        # 获取关节索引
+        self.joint_names = self.arm_joint_names + self.waist_joint_names + self.leg_joint_names
+        # 获取执行器索引
+        self.pos_actuator_names = [f"pos_{name}" for name in self.joint_names]
+        self.motor_actuator_names = [
+            f"motor_{name}" for name in self.joint_names
+        ]
+
+        self.pos_actuator_indices_map = {}
+        self.motor_actuator_indices_map = {}
+
+        for name in self.pos_actuator_names:
+            idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR,
+                                    name)
+            self.pos_actuator_indices_map[name] = idx
+
+        for name in self.motor_actuator_names:
+            idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR,
+                                    name)
+            self.motor_actuator_indices_map[name] = idx
+
+        # 获取执行器增益
+        self.original_gains = self.model.actuator_gainprm.copy()
+        self.original_biases = self.model.actuator_biasprm.copy()
+
+        # 获取传感器索引
+        self.joint_pos_sensor_names = [
+            f"{name}_pos" for name in self.joint_names
+        ]
+        self.joint_vel_sensor_names = [
+            f"{name}_vel" for name in self.joint_names
+        ]
+        self.joint_torque_sensor_names = [
+            f"{name}_torque" for name in self.joint_names
+        ]
+
+        self.joint_pos_sensor_indices_map = {}
+        self.joint_vel_sensor_indices_map = {}
+        self.joint_torque_sensor_indices_map = {}
+
+        for name in self.joint_pos_sensor_names:
+            idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR,
+                                    name)
+            self.joint_pos_sensor_indices_map[name] = idx
+
+        for name in self.joint_vel_sensor_names:
+            idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR,
+                                    name)
+            self.joint_vel_sensor_indices_map[name] = idx
+
+        for name in self.joint_torque_sensor_names:
+            idx = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SENSOR,
+                                    name)
+            self.joint_torque_sensor_indices_map[name] = idx
+
+        # IMU传感器索引
+        self.imu_orient_idx = mujoco.mj_name2id(self.model,
+                                                mujoco.mjtObj.mjOBJ_SENSOR,
+                                                'orientation')
+        self.imu_orient_address = self.model.sensor_adr[self.imu_orient_idx]
+        self.imu_pos_idx = mujoco.mj_name2id(self.model,
+                                             mujoco.mjtObj.mjOBJ_SENSOR,
+                                             'position')
+        self.imu_pos_address = self.model.sensor_adr[self.imu_pos_idx]
+        self.imu_gyro_idx = mujoco.mj_name2id(self.model,
+                                              mujoco.mjtObj.mjOBJ_SENSOR,
+                                              'angular-velocity')
+        self.imu_gyro_address = self.model.sensor_adr[self.imu_gyro_idx]
+        self.imu_vel_idx = mujoco.mj_name2id(self.model,
+                                             mujoco.mjtObj.mjOBJ_SENSOR,
+                                             'linear-velocity')
+        self.imu_vel_address = self.model.sensor_adr[self.imu_vel_idx]
+        self.imu_accel_idx = mujoco.mj_name2id(self.model,
+                                               mujoco.mjtObj.mjOBJ_SENSOR,
+                                               'linear-acceleration')
+        self.imu_accel_address = self.model.sensor_adr[self.imu_accel_idx]
+        self.imu_mag_idx = mujoco.mj_name2id(self.model,
+                                             mujoco.mjtObj.mjOBJ_SENSOR,
+                                             'magnetometer')
+        self.imu_mag_address = self.model.sensor_adr[self.imu_mag_idx]
+
+    def pvd_controller(self, joint_name, pos_d, spd_d, tor_d, kp, kd):
+        """
+        PVD控制器实现函数，用于计算关节的输出扭矩
+        
+        参数:
+        joint_name: 关节名称，用于获取关节状态
+        pos_d: 期望位置
+        spd_d: 期望速度
+        tor_d: 期望扭矩
+        kp: 位置比例增益系数
+        kd: 速度比例增益系数
+        
+        返回值:
+        output_torque: 计算得到的输出扭矩值
+        """
+
+        status = self.get_status(joint_name)
+        pos_a = status['position']
+        spd_a = status['velocity']
+        output_torque = kp * (pos_d - pos_a) + kd * (spd_d - spd_a) + tor_d
+        return output_torque
+
+    def joint_cmd_callback(self, msg: CmdMotorCtrl):
+        cmd: MotorCtrl = None
+        # 每一组命令全部接受完再释放锁
+        with self.joint_cmd_lock:
+            current_time = self.node.get_clock().now().to_msg()
+            msg_time = msg.header.stamp
+            time_diff = (current_time.sec - msg_time.sec) * 1000000000 + (current_time.nanosec - msg_time.nanosec)
+            time_diff_ms = time_diff / 1000000.0
+            if time_diff_ms > 100:
+                print(f"[WARNING] Time difference: {time_diff_ms} ms, abandon this command")
+                return
+                
+            for cmd in msg.cmds:
+                try:
+                    self.raw_joint_commands.put_nowait(cmd)
+                except Full:
+                    print(f"raw joint command 队列已满")
+                    self.raw_joint_commands.get_nowait()
+                    self.raw_joint_commands.put_nowait(cmd)
+
+    def calc_motor_cmd(self, joint_name, pos, spd, tor, kp, kd):
+        # joint_name = self.motor_id_to_joint[name_id]
+        # 计算下发力矩
+        target_torque = self.pvd_controller(joint_name, pos, spd, tor, kp, kd)
+        self.joint_commands[joint_name] = target_torque
+        if self.debug:
+            print(f"{joint_name} torque : {target_torque}\n")
+
+    def set_motor_cmd(self):
+        with self.joint_cmd_lock:
+            while not self.raw_joint_commands.empty():
+                cmd: MotorCtrl = self.raw_joint_commands.get_nowait()
+                name = cmd.name
+                pos = cmd.pos
+                spd = cmd.spd
+                tor = cmd.tor
+                kp = cmd.kp
+                kd = cmd.kd
+                joint_name = self.motor_id_to_joint[name]
+                self.last_raw_joint_commands[joint_name] = { "pos": pos, "spd": spd, "tor": tor, "kp": kp, "kd": kd}
+
+        if self.execution_mode == "direct_position":
+            for motor_index in self.motor_actuator_indices_map.values():
+                if motor_index != -1:
+                    self.data.ctrl[motor_index] = 0.0
+
+            for joint_name in self.joint_names:
+                if joint_name in self.last_raw_joint_commands:
+                    pos = self.last_raw_joint_commands[joint_name].get("pos")
+                    pos_name = f"pos_{joint_name}"
+                    pos_index = self.pos_actuator_indices_map.get(pos_name, -1)
+                    if pos_index != -1:
+                        self.data.ctrl[pos_index] = pos
+            return
+
+        for joint_name in self.joint_names:
+            if joint_name in self.last_raw_joint_commands:
+                pos = self.last_raw_joint_commands[joint_name].get("pos")
+                spd = self.last_raw_joint_commands[joint_name].get("spd")
+                tor = self.last_raw_joint_commands[joint_name].get("tor")
+                kp = self.last_raw_joint_commands[joint_name].get("kp")
+                kd = self.last_raw_joint_commands[joint_name].get("kd")
+                self.calc_motor_cmd(joint_name, pos, spd, tor, kp, kd)
+                motor_name = f"motor_{joint_name}"
+                motor_index = self.motor_actuator_indices_map[motor_name]
+                if motor_index != -1:
+                    # 确保电机存在且已收到命令
+                    self.data.ctrl[motor_index] = self.joint_commands[joint_name]
+    def viewer_thread(self):
+        try:
+            while self.viewer.is_running() and not self.stop_event.is_set():
+                with self.sim_view_lock:
+                    self.viewer.sync()
+                time.sleep(self.view_dt)
+        except Exception as e:
+            print(f"Viewer thread error: {e}")
+        finally:
+            print("Viewer thread stopped")
+
+    def read_sensors(self):
+        """读取所有传感器数据"""
+        # 读取关节传感器数据
+
+        for name in self.joint_names:
+            pos_sensor_name = f"{name}_pos"
+            vel_sensor_name = f"{name}_vel"
+            torque_sensor_name = f"{name}_torque"
+            pos_idx = self.joint_pos_sensor_indices_map[pos_sensor_name]
+            vel_idx = self.joint_vel_sensor_indices_map[vel_sensor_name]
+            torque_idx = self.joint_torque_sensor_indices_map[
+                torque_sensor_name]
+
+            self.joint_positions[name] = self.data.sensordata[pos_idx]
+            self.joint_velocities[name] = self.data.sensordata[vel_idx]
+            self.joint_torques[name] = self.data.sensordata[torque_idx]
+
+        # 读取IMU数据
+        self.imu_orientation = self.data.sensordata[self.imu_orient_address:self.
+                                                    imu_orient_address + 4]
+        self.imu_position = self.data.sensordata[self.
+                                                 imu_pos_address:self.imu_pos_address +
+                                                 3]
+        self.imu_angular_velocity = self.data.sensordata[self.imu_gyro_address:self
+                                                         .imu_gyro_address + 3]
+        self.imu_linear_velocity = self.data.sensordata[self.imu_vel_address:self.
+                                                        imu_vel_address + 3]
+        self.imu_linear_acceleration = self.data.sensordata[
+            self.imu_accel_address:self.imu_accel_address + 3]
+        self.imu_magnetometer = self.data.sensordata[self.imu_mag_address:self.
+                                                     imu_mag_address + 3]
+        # 更新传感器数据供发布线程使用
+        plot_data = {
+            'joint_positions': np.array(list(self.joint_positions.values())),
+            'joint_velocities': np.array(list(self.joint_velocities.values())),
+            'joint_torques': np.array(list(self.joint_torques.values())),
+            'imu_orientation': self.imu_orientation,
+            'imu_position': self.imu_position,
+            'imu_angular_velocity': self.imu_angular_velocity,
+            'imu_linear_velocity': self.imu_linear_velocity,
+            'imu_linear_acceleration': self.imu_linear_acceleration,
+            'imu_magnetometer': self.imu_magnetometer
+        }
+        with self.sensor_data_lock:
+            self.last_sensor_data = {
+                'joint_positions': self.joint_positions.copy(),
+                'joint_velocities': self.joint_velocities.copy(),
+                'joint_torques': self.joint_torques.copy(),
+                'imu_orientation': self.imu_orientation,
+                'imu_position': self.imu_position,
+                'imu_angular_velocity': self.imu_angular_velocity,
+                'imu_linear_velocity': self.imu_linear_velocity,
+                'imu_linear_acceleration': self.imu_linear_acceleration,
+                'imu_magnetometer': self.imu_magnetometer
+            }
+
+        return plot_data
+
+    def construct_motor_status(self, joint_name, pos, speed, torque=None):
+        """
+        构建电机状态消息
+        
+        Args:
+            joint_name (str): 关节名称
+            pos (float): 位置值
+            speed (float): 速度值
+            
+        Returns:
+            MotorStatus: 电机状态消息
+        """
+        motor_status = MotorStatus()
+        motor_status.name = self.joint_to_motor_id.get(joint_name, int())
+        motor_status.pos = pos
+        motor_status.speed = speed
+        motor_status.current = torque
+
+        return motor_status
+
+    def get_status(self, joint_name):
+        # Todo: 处理还未收到传感器状态的情况
+        with self.sensor_data_lock:
+            joint_positions = self.last_sensor_data['joint_positions'].copy()
+            joint_velocities = self.last_sensor_data['joint_velocities'].copy()
+            joint_torques = self.last_sensor_data['joint_torques'].copy()
+        pos = joint_positions[joint_name]
+        vel = joint_velocities[joint_name]
+        torque = joint_torques[joint_name]
+        return {'position': pos, 'velocity': vel, 'torque': torque}
+
+    def simulate_thread(self):
+        """运行控制循环"""
+        # 控制参数
+        time_step = 0
+        control_frequency = 100  # 100Hz控制频率
+        sensor_frequency = 100  # 100Hz传感器读取频率
+        control_interval = 1 / control_frequency
+
+        target_time = 0
+        # last_control_time = 0
+        try:
+            while self.viewer.is_running() and not self.stop_event.is_set():
+                step_start = time.perf_counter()
+
+                target_time += self.model.opt.timestep
+                with self.sim_view_lock:
+                    # 读取传感器数据
+                    if int(1.0 / self.model.opt.timestep * target_time) % int(
+                            1.0 / self.model.opt.timestep /
+                            sensor_frequency) == 0:
+                        sensor_data = self.read_sensors()
+                        # 存储数据用于绘图
+                        self.time_history.append(target_time)
+                        self.joint_pos_history.append(
+                            sensor_data['joint_positions'].copy())
+                        self.joint_vel_history.append(
+                            sensor_data['joint_velocities'].copy())
+                        self.joint_torque_history.append(
+                            sensor_data['joint_torques'].copy())
+                        self.imu_orient_history.append(
+                            sensor_data['imu_orientation'].copy())
+                        self.imu_accel_history.append(
+                            sensor_data['imu_linear_acceleration'].copy())
+                        self.imu_pos_history.append(
+                            sensor_data['imu_position'].copy())
+                        # 打印部分传感器数据
+                        if time_step % 100 == 0 and self.debug:
+                            print(f"Time: {target_time:.3f}s")
+                            print(
+                                f"Control Mode: {'Position' if self.control_mode == 0 else 'Torque'}"
+                            )
+                            print(
+                                f"Joint 0 Pos: {sensor_data['joint_positions'][0]:.3f}, "
+                                f"Vel: {sensor_data['joint_velocities'][0]:.3f}, "
+                                f"Torque: {sensor_data['joint_torques'][0]:.3f}"
+                            )
+                            print(
+                                f"IMU Orientation: [{sensor_data['imu_orientation'][0]:.3f}, "
+                                f"{sensor_data['imu_orientation'][1]:.3f}, "
+                                f"{sensor_data['imu_orientation'][2]:.3f}, "
+                                f"{sensor_data['imu_orientation'][3]:.3f}]")
+                            print(
+                                f"IMU Acceleration: [{sensor_data['imu_linear_acceleration'][0]:.3f}, "
+                                f"{sensor_data['imu_linear_acceleration'][1]:.3f}, "
+                                f"{sensor_data['imu_linear_acceleration'][2]:.3f}]"
+                            )
+                            print("-" * 50)
+                    # 添加虚拟力
+                    self.add_virtual_force()
+                    # 控制命令写入
+                    self.set_motor_cmd()
+                    # 前进仿真一步
+                    if not self.paused:
+                        mujoco.mj_step(self.model, self.data)
+                    else:
+                        mujoco.mj_forward(self.model, self.data)
+                time_step += 1
+
+                # 控制仿真速度
+                time_until_next_step = self.model.opt.timestep - (time.perf_counter() -
+                                                                  step_start)
+                print(f'sim timestep: {time_until_next_step}')
+                if time_until_next_step > 0:
+                    time.sleep(time_until_next_step)
+
+        except Exception as e:
+            print(f"Simulation thread error: {e}")
+        finally:
+            print("Simulation thread stopped")
+
+    def add_virtual_force(self):
+        if self.elastic_band.enable:
+            self.data.xfrc_applied[self.band_attached_link, :3] = self.elastic_band.Advance(
+            self.data.qpos[0:3], self.data.qvel[0:3]
+        )
+    def plot_sensor_data(self):
+        """绘制传感器数据"""
+        if len(self.time_history) == 0:
+            return
+
+        fig, axes = plt.subplots(2, 2, figsize=(15, 10))
+        fig.suptitle('Sensor Data')
+
+        time_array = np.array(list(self.time_history))
+
+        # 绘制关节位置
+        if len(self.joint_pos_history) > 0:
+            pos_array = np.array(list(self.joint_pos_history))
+            axes[0, 0].plot(time_array, pos_array[:, 0], label='Joint 0')
+            axes[0, 0].plot(time_array, pos_array[:, 1], label='Joint 1')
+            axes[0, 0].set_title('Joint Positions')
+            axes[0, 0].set_xlabel('Time (s)')
+            axes[0, 0].set_ylabel('Position (rad)')
+            axes[0, 0].legend()
+            axes[0, 0].grid(True)
+
+        # 绘制关节速度
+        if len(self.joint_vel_history) > 0:
+            vel_array = np.array(list(self.joint_vel_history))
+            axes[0, 1].plot(time_array, vel_array[:, 0], label='Joint 0')
+            axes[0, 1].plot(time_array, vel_array[:, 1], label='Joint 1')
+            axes[0, 1].set_title('Joint Velocities')
+            axes[0, 1].set_xlabel('Time (s)')
+            axes[0, 1].set_ylabel('Velocity (rad/s)')
+            axes[0, 1].legend()
+            axes[0, 1].grid(True)
+
+        # 绘制IMU方向
+        if len(self.imu_orient_history) > 0:
+            orient_array = np.array(list(self.imu_orient_history))
+            axes[1, 0].plot(time_array, orient_array[:, 0], label='w')
+            axes[1, 0].plot(time_array, orient_array[:, 1], label='x')
+            axes[1, 0].plot(time_array, orient_array[:, 2], label='y')
+            axes[1, 0].plot(time_array, orient_array[:, 3], label='z')
+            axes[1, 0].set_title('IMU Orientation (Quaternion)')
+            axes[1, 0].set_xlabel('Time (s)')
+            axes[1, 0].set_ylabel('Quaternion')
+            axes[1, 0].legend()
+            axes[1, 0].grid(True)
+
+        # # 绘制IMU加速度
+        # if len(self.imu_accel_history) > 0:
+        #     accel_array = np.array(list(self.imu_accel_history))
+        #     axes[1, 1].plot(time_array, accel_array[:, 0], label='X')
+        #     axes[1, 1].plot(time_array, accel_array[:, 1], label='Y')
+        #     axes[1, 1].plot(time_array, accel_array[:, 2], label='Z')
+        #     axes[1, 1].set_title('IMU Linear Acceleration')
+        #     axes[1, 1].set_xlabel('Time (s)')
+        #     axes[1, 1].set_ylabel('Acceleration (m/s²)')
+        #     axes[1, 1].legend()
+        #     axes[1, 1].grid(True)
+
+        # 绘制IMU位置
+        if len(self.imu_pos_history) > 0:
+            pos_array = np.array(list(self.imu_pos_history))
+            axes[1, 1].plot(time_array, pos_array[:, 0], label='X')
+            axes[1, 1].plot(time_array, pos_array[:, 1], label='Y')
+            axes[1, 1].plot(time_array, pos_array[:, 2], label='Z')
+            axes[1, 1].set_title('IMU Linear Position')
+            axes[1, 1].set_xlabel('Time (s)')
+            axes[1, 1].set_ylabel('Position (m)')
+            axes[1, 1].legend()
+            axes[1, 1].grid(True)
+
+        plt.tight_layout()
+        plt.show()
+
+    def start(self):
+        from threading import Thread
+        viewer_thread = Thread(target=self.viewer_thread)
+        sim_thread = Thread(target=self.simulate_thread)
+
+        # 设置为守护线程，确保主线程结束时它们也会结束
+        viewer_thread.daemon = True
+        sim_thread.daemon = True
+
+        try:
+            # 设置初始姿态
+            # self.set_keyframe("zero")  # 或者 "zero_height" 或 "zero_standup"， 'zero'
+            # print("机器人已设置为zero姿态")
+
+            self.start_keyboard_listener()
+            viewer_thread.start()
+            sim_thread.start()
+            # 启动发布线程
+            self.start_publishing()
+            # 主线程等待，直到收到 KeyboardInterrupt
+            while viewer_thread.is_alive() and sim_thread.is_alive():
+                time.sleep(0.1)
+
+        except KeyboardInterrupt:
+            print("\nReceived KeyboardInterrupt, stopping simulation...")
+        finally:
+            # 设置停止事件
+            self.stop_event.set()
+
+            # 等待线程结束
+            viewer_thread.join(timeout=1.0)
+            sim_thread.join(timeout=1.0)
+            # 停止发布线程
+            self.stop_publishing()
+            # 关闭 viewer
+            try:
+                self.viewer.close()
+            except:
+                pass
+
+            # 绘制图表
+            if self.debug:
+                print("Plotting sensor data...")
+                self.plot_sensor_data()
+            print("Simulation finished")
+
+    def set_keyframe(self, key_name):
+        """
+        将机器人设置为指定的关键帧姿态
+        
+        参数:
+            key_name: 关键帧名称 (例如 "zero", "zero_height", "zero_standup")
+        """
+        # 查找关键帧的索引
+        key_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_KEY,
+                                   key_name)
+
+        if key_id == -1:
+            print(f"关键帧 '{key_name}' 未找到")
+            return False
+        # # # 将关键帧的qpos值复制到当前状态
+        self.data.qpos[:] = self.model.key_qpos[key_id]
+
+        # 将关键帧的ctrl值复制到控制向量
+        if self.model.key_ctrl is not None:
+            self.data.ctrl[:] = self.model.key_ctrl[key_id]
+
+        mujoco.mj_forward(self.model, self.data)
+
+        # 更新系统状态
+        # mujoco.mj_resetDataKeyframe(self.model, self.data, key_id)
+        return True
+
+    def get_available_keyframes(self):
+        """
+        获取所有可用的关键帧名称
+        """
+        keyframes = []
+        for i in range(self.model.nkey):
+            # 获取关键帧名称
+            key_name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_KEY,
+                                         i)
+            if key_name:
+                keyframes.append(key_name)
+        return keyframes
+
+def _spin_wrapper(node):
+    """ROS2 spin wrapper for graceful shutdown."""
+    try:
+        while rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=0.001)
+    except Exception as e:
+        print(f"Error in ROS2 spin thread: {e}")
+    finally:
+        print("ROS2 spin thread terminated.")
+
+
+def main():
+    # 设置模型注册表
+    model_registry = {
+        'evt2': '../resources/evt2/urdf/evt2.xml'
+    }
+
+    # 解析命令行参数
+    parser = argparse.ArgumentParser(description='MuJoCo机器人仿真器')
+    parser.add_argument('--model', '-m', type=str, default='evt2',
+                        choices=list(model_registry.keys()),
+                        help='要加载的机器人模型')
+    parser.add_argument('--config', '-c', type=str, default='full',
+                        choices=['full', '21'],
+                        help='机器人配置 (full=完整配置, 21=21自由度配置)')
+
+    args = parser.parse_args()
+
+    # 设置模型路径
+    script_dir = os.path.dirname(os.path.abspath(__file__))
+    model_path = os.path.join(script_dir, model_registry[args.model])
+    model_path = os.path.abspath(model_path)
+    
+    print(f"正在加载模型: {model_path}")
+    print(f"机器人配置: {args.config}")
+
+    # 创建控制器
+    rclpy.init()
+    node = rclpy.create_node('mujoco_simulator_dex')
+    spin_thread = threading.Thread(target=_spin_wrapper,
+                                   args=(node, ),
+                                   daemon=True,
+                                   name="ROS2_Spin_Thread")
+    spin_thread.start()
+    simulator = RobotSimulator(model_path, node, debug=False, robot_config=args.config)
+
+    # 运行仿真
+    simulator.start()
+
+
+if __name__ == "__main__":
+    main()