diff --git a/src/mechanical_arm/main.py b/src/mechanical_arm/main.py
index 8750bf5f82..6199a351e7 100644
--- a/src/mechanical_arm/main.py
+++ b/src/mechanical_arm/main.py
@@ -435,4 +435,4 @@ def main():
if __name__ == "__main__":
- main()
\ No newline at end of file
+ main()#结束过程
\ No newline at end of file
diff --git a/src/mechanical_arm/model_test.py b/src/mechanical_arm/model_test.py
new file mode 100644
index 0000000000..8f463605cc
--- /dev/null
+++ b/src/mechanical_arm/model_test.py
@@ -0,0 +1,64 @@
+import mujoco
+import time
+import numpy as np
+import os
+
+try:
+ # 尝试使用较新版本的mujoco.viewer
+ import mujoco.viewer
+
+ use_new_viewer = True
+except ImportError:
+ # 如果没有mujoco.viewer,则使用glfw_viewer(如果可用)
+ try:
+ from mujoco import glfw_viewer
+
+ use_new_viewer = False
+ except ImportError:
+ # 如果都不可用,给出提示
+ print("未找到可用的可视化模块,请确认安装了正确的MuJoCo版本")
+ exit(1)
+
+# 读取模型文件内容
+model_path = os.path.join(os.path.dirname(__file__), "机械臂.xml")
+with open(model_path, 'r', encoding='utf-8') as f:
+ model_xml = f.read()
+
+# 从字符串加载模型
+model = mujoco.MjModel.from_xml_string(model_xml)
+data = mujoco.MjData(model)
+
+# 设置到home位置
+data.qpos[:] = [0, 0.785398, 0, -1.5708, 0, 0, 0]
+mujoco.mj_forward(model, data)
+
+if use_new_viewer:
+ # 使用新版本的viewer
+ with mujoco.viewer.launch_passive(model, data) as viewer:
+ print("按ESC键退出")
+ while viewer.is_running():
+ step_start = time.time()
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+
+ # 控制步频
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(time_until_next_step)
+else:
+ # 使用glfw_viewer
+ viewer = glfw_viewer.GlfwViewer(model, data)
+ print("按ESC键退出")
+ while viewer.is_running():
+ step_start = time.time()
+
+ mujoco.mj_step(model, data)
+ viewer.render()
+
+ # 控制步频
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(time_until_next_step)
+
+ viewer.close()
\ No newline at end of file
diff --git a/src/mechanical_arm/motion_test.py b/src/mechanical_arm/motion_test.py
new file mode 100644
index 0000000000..554b2d1225
--- /dev/null
+++ b/src/mechanical_arm/motion_test.py
@@ -0,0 +1,82 @@
+import mujoco
+import time
+import numpy as np
+import os
+
+try:
+ # 尝试使用较新版本的mujoco.viewer
+ import mujoco.viewer
+
+ use_new_viewer = True
+except ImportError:
+ # 如果没有mujoco.viewer,则使用glfw_viewer(如果可用)
+ try:
+ from mujoco import glfw_viewer
+
+ use_new_viewer = False
+ except ImportError:
+ # 如果都不可用,给出提示
+ print("未找到可用的可视化模块,请确认安装了正确的MuJoCo版本")
+ exit(1)
+
+# 读取模型文件内容
+model_path = os.path.join(os.path.dirname(__file__), "arm.xml")
+with open(model_path, 'r', encoding='utf-8') as f:
+ model_xml = f.read()
+
+# 从字符串加载模型
+model = mujoco.MjModel.from_xml_string(model_xml)
+data = mujoco.MjData(model)
+
+# 设置到home位置
+data.qpos[:] = [0, 0.785398, 0, -1.5708, 0, 0, 0]
+mujoco.mj_forward(model, data)
+
+
+def move_arm_in_wave(data, t):
+ """生成波动运动轨迹"""
+ # 基础正弦波运动
+ for i in range(7):
+ data.ctrl[i] = np.sin(t + i * np.pi / 4) * 0.5
+
+
+if use_new_viewer:
+ # 使用新版本的viewer
+ with mujoco.viewer.launch_passive(model, data) as viewer:
+ print("按ESC键退出")
+ t = 0
+ while viewer.is_running():
+ step_start = time.time()
+
+ # 更新控制信号,使机械臂运动
+ move_arm_in_wave(data, t)
+ t += 0.01
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+
+ # 控制步频
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(time_until_next_step)
+else:
+ # 使用glfw_viewer
+ viewer = glfw_viewer.GlfwViewer(model, data)
+ print("按ESC键退出")
+ t = 0
+ while viewer.is_running():
+ step_start = time.time()
+
+ # 更新控制信号,使机械臂运动
+ move_arm_in_wave(data, t)
+ t += 0.01
+
+ mujoco.mj_step(model, data)
+ viewer.render()
+
+ # 控制步频
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(time_until_next_step)
+
+ viewer.close()
\ No newline at end of file
diff --git a/src/mechanical_arm/three_arm_test.py b/src/mechanical_arm/three_arm_test.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/src/mechanical_arm/three_arm_test_three.py b/src/mechanical_arm/three_arm_test_three.py
new file mode 100644
index 0000000000..e038739745
--- /dev/null
+++ b/src/mechanical_arm/three_arm_test_three.py
@@ -0,0 +1,98 @@
+import mujoco
+import time
+import numpy as np
+import os
+
+try:
+ # 尝试使用较新版本的mujoco.viewer
+ import mujoco.viewer
+
+ use_new_viewer = True
+except ImportError:
+ # 如果没有mujoco.viewer,则使用glfw_viewer(如果可用)
+ try:
+ from mujoco import glfw_viewer
+
+ use_new_viewer = False
+ except ImportError:
+ # 如果都不可用,给出提示
+ print("未找到可用的可视化模块,请确认安装了正确的MuJoCo版本")
+ exit(1)
+
+# 读取模型文件内容
+model_path = os.path.join(os.path.dirname(__file__), "three_fingered_arm.xml")
+with open(model_path, 'r', encoding='utf-8') as f:
+ model_xml = f.read()
+
+# 从字符串加载模型
+model = mujoco.MjModel.from_xml_string(model_xml)
+data = mujoco.MjData(model)
+
+# 设置到home位置
+data.qpos[:] = [0, 0.785398, 0, -1.5708, 0, 0, 0, 0, 0, 0]
+mujoco.mj_forward(model, data)
+
+if use_new_viewer:
+ # 使用新版本的viewer
+ with mujoco.viewer.launch_passive(model, data) as viewer:
+ print("按ESC键退出")
+ print("演示三指夹爪开合动作...")
+
+ # 记录初始时间用于动画控制
+ start_time = time.time()
+
+ while viewer.is_running():
+ step_start = time.time()
+
+ # 控制夹爪开合的简单动画
+ elapsed = time.time() - start_time
+ gripper_position = 0.09 * (1 + np.sin(elapsed)) # 增大运动幅度以适应更长的手指
+
+ # 确保位置在有效范围内
+ gripper_position = max(0, min(0.12, gripper_position))
+
+ # 设置夹爪关节位置
+ data.ctrl[7] = gripper_position # finger_joint1
+ data.ctrl[8] = gripper_position # finger_joint2
+ data.ctrl[9] = gripper_position # finger_joint3
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+
+ # 控制步频
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(time_until_next_step)
+else:
+ # 使用glfw_viewer
+ viewer = glfw_viewer.GlfwViewer(model, data)
+ print("按ESC键退出")
+ print("演示三指夹爪开合动作...")
+
+ # 记录初始时间用于动画控制
+ start_time = time.time()
+
+ while viewer.is_running():
+ step_start = time.time()
+
+ # 控制夹爪开合的简单动画
+ elapsed = time.time() - start_time
+ gripper_position = 0.02 * (1 + np.sin(elapsed))
+
+ # 确保位置在有效范围内
+ gripper_position = max(0, min(0.04, gripper_position))
+
+ # 设置夹爪关节位置
+ data.ctrl[7] = gripper_position # finger_joint1
+ data.ctrl[8] = gripper_position # finger_joint2
+ data.ctrl[9] = gripper_position # finger_joint3
+
+ mujoco.mj_step(model, data)
+ viewer.render()
+
+ # 控制步频
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(time_until_next_step)
+
+ viewer.close()
\ No newline at end of file
diff --git a/src/mechanical_arm/three_arm_test_two.py b/src/mechanical_arm/three_arm_test_two.py
new file mode 100644
index 0000000000..e038739745
--- /dev/null
+++ b/src/mechanical_arm/three_arm_test_two.py
@@ -0,0 +1,98 @@
+import mujoco
+import time
+import numpy as np
+import os
+
+try:
+ # 尝试使用较新版本的mujoco.viewer
+ import mujoco.viewer
+
+ use_new_viewer = True
+except ImportError:
+ # 如果没有mujoco.viewer,则使用glfw_viewer(如果可用)
+ try:
+ from mujoco import glfw_viewer
+
+ use_new_viewer = False
+ except ImportError:
+ # 如果都不可用,给出提示
+ print("未找到可用的可视化模块,请确认安装了正确的MuJoCo版本")
+ exit(1)
+
+# 读取模型文件内容
+model_path = os.path.join(os.path.dirname(__file__), "three_fingered_arm.xml")
+with open(model_path, 'r', encoding='utf-8') as f:
+ model_xml = f.read()
+
+# 从字符串加载模型
+model = mujoco.MjModel.from_xml_string(model_xml)
+data = mujoco.MjData(model)
+
+# 设置到home位置
+data.qpos[:] = [0, 0.785398, 0, -1.5708, 0, 0, 0, 0, 0, 0]
+mujoco.mj_forward(model, data)
+
+if use_new_viewer:
+ # 使用新版本的viewer
+ with mujoco.viewer.launch_passive(model, data) as viewer:
+ print("按ESC键退出")
+ print("演示三指夹爪开合动作...")
+
+ # 记录初始时间用于动画控制
+ start_time = time.time()
+
+ while viewer.is_running():
+ step_start = time.time()
+
+ # 控制夹爪开合的简单动画
+ elapsed = time.time() - start_time
+ gripper_position = 0.09 * (1 + np.sin(elapsed)) # 增大运动幅度以适应更长的手指
+
+ # 确保位置在有效范围内
+ gripper_position = max(0, min(0.12, gripper_position))
+
+ # 设置夹爪关节位置
+ data.ctrl[7] = gripper_position # finger_joint1
+ data.ctrl[8] = gripper_position # finger_joint2
+ data.ctrl[9] = gripper_position # finger_joint3
+
+ mujoco.mj_step(model, data)
+ viewer.sync()
+
+ # 控制步频
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(time_until_next_step)
+else:
+ # 使用glfw_viewer
+ viewer = glfw_viewer.GlfwViewer(model, data)
+ print("按ESC键退出")
+ print("演示三指夹爪开合动作...")
+
+ # 记录初始时间用于动画控制
+ start_time = time.time()
+
+ while viewer.is_running():
+ step_start = time.time()
+
+ # 控制夹爪开合的简单动画
+ elapsed = time.time() - start_time
+ gripper_position = 0.02 * (1 + np.sin(elapsed))
+
+ # 确保位置在有效范围内
+ gripper_position = max(0, min(0.04, gripper_position))
+
+ # 设置夹爪关节位置
+ data.ctrl[7] = gripper_position # finger_joint1
+ data.ctrl[8] = gripper_position # finger_joint2
+ data.ctrl[9] = gripper_position # finger_joint3
+
+ mujoco.mj_step(model, data)
+ viewer.render()
+
+ # 控制步频
+ time_until_next_step = model.opt.timestep - (time.time() - step_start)
+ if time_until_next_step > 0:
+ time.sleep(time_until_next_step)
+
+ viewer.close()
\ No newline at end of file
diff --git a/src/mechanical_arm/three_fingered_arm.xml b/src/mechanical_arm/three_fingered_arm.xml
new file mode 100644
index 0000000000..25790c6cb7
--- /dev/null
+++ b/src/mechanical_arm/three_fingered_arm.xml
@@ -0,0 +1,205 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/mechanical_arm/three_fingered_arm_two.xml b/src/mechanical_arm/three_fingered_arm_two.xml
new file mode 100644
index 0000000000..d7aa711b07
--- /dev/null
+++ b/src/mechanical_arm/three_fingered_arm_two.xml
@@ -0,0 +1,263 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file