From 13b6d413fec00bb09d704ce494e8b2e381d72382 Mon Sep 17 00:00:00 2001
From: lijinghan12 <15894570343@163.com>
Date: Sat, 20 Dec 2025 13:05:02 +0800
Subject: [PATCH 1/7] =?UTF-8?q?=E7=94=A8=E6=8C=89=E9=94=AE=E6=8E=A7?=
=?UTF-8?q?=E5=88=B6=E4=B8=89=E6=8C=87=E6=9C=BA=E6=A2=B0=E8=87=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/mechanical_arm/main.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
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
From fdf3af658294bab3aff2f383efbbe5fb8a452b8c Mon Sep 17 00:00:00 2001
From: lijinghan12 <15894570343@163.com>
Date: Sat, 20 Dec 2025 20:43:15 +0800
Subject: [PATCH 2/7] =?UTF-8?q?=E7=94=A8mujoco=E8=BF=9B=E8=A1=8C=E6=9C=BA?=
=?UTF-8?q?=E6=A2=B0=E8=87=82=E7=9A=84=E5=88=9D=E6=AD=A5=E4=BB=BF=E7=9C=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/mechanical_arm/model_test.py | 64 ++++++++++++++++++++++++++++++++
1 file changed, 64 insertions(+)
create mode 100644 src/mechanical_arm/model_test.py
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
From c5e0debddcf5563fdf93088e3b9d8904e21327ae Mon Sep 17 00:00:00 2001
From: lijinghan12 <15894570343@163.com>
Date: Sun, 21 Dec 2025 10:35:01 +0800
Subject: [PATCH 3/7] =?UTF-8?q?mujoco=E4=BB=BF=E7=9C=9F=E6=9C=BA=E6=A2=B0?=
=?UTF-8?q?=E8=87=82=E8=BF=9B=E8=A1=8C=E8=A7=84=E5=BE=8B=E8=BF=90=E5=8A=A8?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/mechanical_arm/motion_test.py | 82 +++++++++++++++++++++++++++++++
1 file changed, 82 insertions(+)
create mode 100644 src/mechanical_arm/motion_test.py
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
From 82a01c8240b44a687e941accea2ad2a825898c51 Mon Sep 17 00:00:00 2001
From: lijinghan12 <15894570343@163.com>
Date: Sun, 21 Dec 2025 12:30:56 +0800
Subject: [PATCH 4/7] =?UTF-8?q?=E4=BC=98=E5=8C=96mujoco=E4=BB=BF=E7=9C=9F?=
=?UTF-8?q?=E6=A8=A1=E5=9E=8B=EF=BC=8C=E6=B7=BB=E5=8A=A0=E6=89=8B=E6=8C=87?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/mechanical_arm/three_arm_test.py | 0
src/mechanical_arm/three_fingered_arm.xml | 205 ++++++++++++++++++++++
2 files changed, 205 insertions(+)
create mode 100644 src/mechanical_arm/three_arm_test.py
create mode 100644 src/mechanical_arm/three_fingered_arm.xml
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_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
From 34ec94236417e2b191cdc9fa05241934804f20a6 Mon Sep 17 00:00:00 2001
From: lijinghan12 <15894570343@163.com>
Date: Sun, 21 Dec 2025 15:50:47 +0800
Subject: [PATCH 5/7] =?UTF-8?q?=E6=B7=BB=E5=8A=A0mujoco=E6=9C=BA=E6=A2=B0?=
=?UTF-8?q?=E8=87=82=E4=BB=BF=E7=9C=9F=E8=83=8C=E6=99=AF?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/mechanical_arm/three_arm_test_two.py | 98 ++++++++++++++++++++++++
1 file changed, 98 insertions(+)
create mode 100644 src/mechanical_arm/three_arm_test_two.py
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
From 5938f05b0edd672c41dbf67f580f1fe644671b0e Mon Sep 17 00:00:00 2001
From: lijinghan12 <15894570343@163.com>
Date: Sun, 21 Dec 2025 22:55:37 +0800
Subject: [PATCH 6/7] =?UTF-8?q?=E4=BC=98=E5=8C=96=E6=9C=BA=E6=A2=B0?=
=?UTF-8?q?=E8=87=82=E6=89=8B=E6=8C=87=E7=BB=93=E6=9E=84?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/mechanical_arm/three_arm_test_three.py | 98 +++++++
src/mechanical_arm/three_fingered_arm_two.xml | 263 ++++++++++++++++++
2 files changed, 361 insertions(+)
create mode 100644 src/mechanical_arm/three_arm_test_three.py
create mode 100644 src/mechanical_arm/three_fingered_arm_two.xml
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_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
From 2b24fc848a651ffe767a8e6aa11e04f6bdfa1f20 Mon Sep 17 00:00:00 2001
From: lijinghan12 <15894570343@163.com>
Date: Mon, 22 Dec 2025 11:12:27 +0800
Subject: [PATCH 7/7] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E6=89=8B?=
=?UTF-8?q?=E6=8C=87=E7=9A=84=E4=B8=89=E7=A7=8D=E8=BF=90=E5=8A=A8=E6=A8=A1?=
=?UTF-8?q?=E5=BC=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/mechanical_arm/full_gripper_demo.py | 122 ++++++++++++++++++++++++
1 file changed, 122 insertions(+)
create mode 100644 src/mechanical_arm/full_gripper_demo.py
diff --git a/src/mechanical_arm/full_gripper_demo.py b/src/mechanical_arm/full_gripper_demo.py
new file mode 100644
index 0000000000..cf02ceb579
--- /dev/null
+++ b/src/mechanical_arm/full_gripper_demo.py
@@ -0,0 +1,122 @@
+#!/usr/bin/env python3
+"""
+完整版三指夹爪控制演示
+此脚本展示了多种控制三指夹爪的方法:
+1. 同步开合
+2. 逐个手指控制
+3. 波浪式动作
+"""
+
+import mujoco
+import mujoco.viewer
+import numpy as np
+import time
+
+
+def main():
+ # 加载模型
+ model_path = "three_fingered_arm.xml"
+ model = mujoco.MjModel.from_xml_path(model_path)
+ data = mujoco.MjData(model)
+
+ # 获取夹爪关节的ID
+ finger_joint1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "finger_joint1")
+ finger_joint2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "finger_joint2")
+ finger_joint3_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "finger_joint3")
+
+ # 弯曲关节
+ finger_bend1_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "finger1_bend")
+ finger_bend2_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "finger2_bend")
+ finger_bend3_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "finger3_bend")
+
+ # 获取夹爪关节在qpos数组中的位置
+ finger_joint1_qpos_addr = model.jnt_qposadr[finger_joint1_id]
+ finger_joint2_qpos_addr = model.jnt_qposadr[finger_joint2_id]
+ finger_joint3_qpos_addr = model.jnt_qposadr[finger_joint3_id]
+
+ # 弯曲关节位置
+ finger_bend1_qpos_addr = model.jnt_qposadr[finger_bend1_id]
+ finger_bend2_qpos_addr = model.jnt_qposadr[finger_bend2_id]
+ finger_bend3_qpos_addr = model.jnt_qposadr[finger_bend3_id]
+
+ # 创建可视化环境
+ with mujoco.viewer.launch_passive(model, data) as viewer:
+ print("开始三指夹爪完整控制演示...")
+ print("演示包括三种不同的控制模式")
+ print("按Ctrl+C退出程序")
+
+ try:
+ start_time = time.time()
+ while viewer.is_running():
+ current_time = time.time() - start_time
+
+ # 每隔6秒切换一种模式
+ mode = int(current_time / 6) % 3
+
+ if mode == 0:
+ # 模式1: 同步开合
+ if int(current_time) % 6 == 0: # 每轮开始时打印
+ print("模式1: 三指同步开合...")
+
+ # 正弦波控制开合 (周期3秒)
+ open_close_value = 0.06 * (1 + np.sin(2 * np.pi * current_time / 3))
+
+ # 设置三个手指的基础关节
+ data.qpos[finger_joint1_qpos_addr] = open_close_value
+ data.qpos[finger_joint2_qpos_addr] = open_close_value
+ data.qpos[finger_joint3_qpos_addr] = open_close_value
+
+ # 微微弯曲指尖
+ bend_value = 0.2 * np.sin(2 * np.pi * current_time / 3)
+ data.qpos[finger_bend1_qpos_addr] = bend_value
+ data.qpos[finger_bend2_qpos_addr] = bend_value
+ data.qpos[finger_bend3_qpos_addr] = bend_value
+
+ elif mode == 1:
+ # 模式2: 顺序动作
+ if int(current_time) % 6 == 0: # 每轮开始时打印
+ print("模式2: 手指顺序动作...")
+
+ # 每个手指独立控制,相位差120度
+ phase1 = 0.06 * (1 + np.sin(2 * np.pi * current_time / 3))
+ phase2 = 0.06 * (1 + np.sin(2 * np.pi * current_time / 3 + 2 * np.pi / 3))
+ phase3 = 0.06 * (1 + np.sin(2 * np.pi * current_time / 3 + 4 * np.pi / 3))
+
+ data.qpos[finger_joint1_qpos_addr] = phase1
+ data.qpos[finger_joint2_qpos_addr] = phase2
+ data.qpos[finger_joint3_qpos_addr] = phase3
+
+ elif mode == 2:
+ # 模式3: 波浪式动作
+ if int(current_time) % 6 == 0: # 每轮开始时打印
+ print("模式3: 波浪式动作...")
+
+ # 创建波浪效果
+ wave1 = 0.06 * (1 + np.sin(4 * np.pi * current_time / 3))
+ wave2 = 0.06 * (1 + np.sin(4 * np.pi * current_time / 3 + np.pi / 2))
+ wave3 = 0.06 * (1 + np.sin(4 * np.pi * current_time / 3 + np.pi))
+
+ data.qpos[finger_joint1_qpos_addr] = wave1
+ data.qpos[finger_joint2_qpos_addr] = wave2
+ data.qpos[finger_joint3_qpos_addr] = wave3
+
+ # 同时控制弯曲关节产生波浪效果
+ bend1 = 0.3 * np.sin(4 * np.pi * current_time / 3)
+ bend2 = 0.3 * np.sin(4 * np.pi * current_time / 3 + np.pi / 2)
+ bend3 = 0.3 * np.sin(4 * np.pi * current_time / 3 + np.pi)
+
+ data.qpos[finger_bend1_qpos_addr] = bend1
+ data.qpos[finger_bend2_qpos_addr] = bend2
+ data.qpos[finger_bend3_qpos_addr] = bend3
+
+ # 运行仿真步骤
+ mujoco.mj_step(model, data)
+ viewer.sync()
+ time.sleep(0.01)
+
+ except KeyboardInterrupt:
+ print("\n程序已退出")
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file