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/9] =?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/9] =?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/9] =?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/9] =?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/9] =?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/9] =?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/9] =?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 From 08ac6bb27c491ae651710e40262bf4c433e7efcb Mon Sep 17 00:00:00 2001 From: lijinghan12 <15894570343@163.com> Date: Tue, 23 Dec 2025 00:04:55 +0800 Subject: [PATCH 8/9] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BA=86=E6=9C=BA?= =?UTF-8?q?=E6=A2=B0=E8=87=82=E7=9A=84=E6=95=B4=E4=BD=93=E7=BB=93=E6=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mechanical_arm/new_arm.xml | 153 +++++++++++++++++++++++++++++++++ 1 file changed, 153 insertions(+) create mode 100644 src/mechanical_arm/new_arm.xml diff --git a/src/mechanical_arm/new_arm.xml b/src/mechanical_arm/new_arm.xml new file mode 100644 index 0000000000..252324aafd --- /dev/null +++ b/src/mechanical_arm/new_arm.xml @@ -0,0 +1,153 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 27ecdc0ae2d9a4c185192713aed77ff0df3951bf Mon Sep 17 00:00:00 2001 From: lijinghan12 <15894570343@163.com> Date: Tue, 23 Dec 2025 18:46:10 +0800 Subject: [PATCH 9/9] =?UTF-8?q?=E9=80=89=E6=8B=A9=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=E7=9A=84=E8=BF=90=E5=8A=A8=E6=A8=A1=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mechanical_arm/new_arm_test.py | 240 +++++++++++++++++++++++++++++ 1 file changed, 240 insertions(+) create mode 100644 src/mechanical_arm/new_arm_test.py diff --git a/src/mechanical_arm/new_arm_test.py b/src/mechanical_arm/new_arm_test.py new file mode 100644 index 0000000000..7dd92a4b2b --- /dev/null +++ b/src/mechanical_arm/new_arm_test.py @@ -0,0 +1,240 @@ +import mujoco +import mujoco.viewer +import numpy as np +import time +import glfw + + +def load_and_visualize(): + # 加载模型 + model = mujoco.MjModel.from_xml_path('new_arm.xml') + data = mujoco.MjData(model) + + # 使用查看器 + with mujoco.viewer.launch_passive(model, data) as viewer: + # 设置相机视角 + viewer.cam.azimuth = 180 + viewer.cam.elevation = -20 + viewer.cam.distance = 3.0 + viewer.cam.lookat[:] = [0.0, 0.0, 0.5] + + print("机械臂模型加载成功!") + print(f"自由度数量: {model.nv}") + print(f"关节数量: {model.njnt}") + print(f"执行器数量: {model.nu}") + + # 设置控制模式 + print("\n控制模式:") + print("1: 静止") + print("2: 正弦运动") + print("3: 随机运动") + print("4: 夹爪开合") + print("按 ESC 退出") + + mode = 1 # 默认静止模式 + start_time = time.time() + + while viewer.is_running(): + step_start = time.time() + + # 根据模式设置控制信号 + current_time = time.time() - start_time + + if mode == 1: # 静止 + data.ctrl[:] = 0 + + elif mode == 2: # 正弦运动 + # 创建小幅度平滑的正弦运动,速度放慢10000倍 + for i in range(6): + amplitude = abs(0.1 + i * 0.05) # 小幅度运动,确保幅度为正数 + frequency = abs(0.5 + i * 0.1) / 10000.0 # 频率降低10000倍,确保为正数 + phase = abs(i * 0.5) # 确保相位为正数 + # 添加偏置确保关节不会过度向下运动 + if i == 1: # 肩关节,避免向下过度运动 + data.ctrl[i] = 0.1 + amplitude * np.sin(2 * np.pi * frequency * current_time + phase) + elif i == 2: # 肘关节,避免向下过度运动 + data.ctrl[i] = 0.1 + amplitude * np.sin(2 * np.pi * frequency * current_time + phase) + else: + data.ctrl[i] = amplitude * np.sin(2 * np.pi * frequency * current_time + phase) + + # 固定夹爪 + data.ctrl[6] = 0.02 + data.ctrl[7] = 0.02 + + elif mode == 3: # 随机运动 + if int(current_time * 2 / 10000.0) % 10 == 0: # 更新频率降低10000倍 + # 生成小幅度随机运动,但对关节2和3添加偏置避免向下过度运动 + random_values = np.abs(np.random.uniform(-0.2, 0.2, 6)) + # 确保肩关节和肘关节不会过度向下 + random_values[1] = 0.1 + random_values[1] * 0.5 # 肩关节偏置 + random_values[2] = 0.1 + random_values[2] * 0.5 # 肘关节偏置 + data.ctrl[:6] = random_values + + elif mode == 4: # 夹爪开合 + # 固定关节位置,确保不会低于底座 + data.ctrl[:6] = [0, 0.2, 0.1, 0, 0, 0] # 调整关节位置,确保不会低于底座 + # 夹爪开合,速度放慢10000倍 + grip_pos = abs(0.01) * np.sin(2 * np.pi * abs(0.5) * current_time / 10000.0) + data.ctrl[6] = grip_pos + data.ctrl[7] = grip_pos + + # 执行物理仿真步 + mujoco.mj_step(model, data) + + # 同步查看器 + viewer.sync() + + # 处理按键输入 + # 注意:在新版本MuJoCo中,访问窗口上下文的方式已改变 + # 这里暂时注释掉按键检测,避免报错 + # if glfw.get_key(viewer._context.window, glfw.KEY_1) == glfw.PRESS: + # mode = 1 + # print("切换到模式1: 静止") + # elif glfw.get_key(viewer._context.window, glfw.KEY_2) == glfw.PRESS: + # mode = 2 + # print("切换到模式2: 正弦运动") + # elif glfw.get_key(viewer._context.window, glfw.KEY_3) == glfw.PRESS: + # mode = 3 + # print("切换到模式3: 随机运动") + # elif glfw.get_key(viewer._context.window, glfw.KEY_4) == glfw.PRESS: + # mode = 4 + # print("切换到模式4: 夹爪开合") + + # 控制帧率 + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + + +def simulate_trajectory(): + """演示一个预设的轨迹""" + model = mujoco.MjModel.from_xml_path('new_arm.xml') + data = mujoco.MjData(model) + + with mujoco.viewer.launch_passive(model, data) as viewer: + viewer.cam.azimuth = 180 + viewer.cam.elevation = -20 + viewer.cam.distance = 3.0 + viewer.cam.lookat[:] = [0.0, 0.0, 0.5] + + print("执行预设轨迹...") + print("1. 移动到初始位置") + print("2. 执行拾取动作") + print("3. 返回初始位置") + + steps = 0 + trajectory_phase = 0 + start_time = time.time() + + while viewer.is_running(): + current_time = time.time() - start_time + + # 定义轨迹 + if trajectory_phase == 0: # 初始位置 + target_pos = [0, 0, 0, 0, 0, 0, 0.02, 0.02] + if steps > 100: + trajectory_phase = 1 + steps = 0 + + elif trajectory_phase == 1: # 拾取位置 - 确保不低于底座 + target_pos = [0.1, 0.15, 0.05, 0.05, 0, 0, 0.02, 0.02] + if steps > 150: + trajectory_phase = 2 + steps = 0 + + elif trajectory_phase == 2: # 夹取物体 - 确保不低于底座 + target_pos = [0.1, 0.15, 0.05, 0.05, 0, 0, -0.01, -0.01] + if steps > 50: + trajectory_phase = 3 + steps = 0 + + elif trajectory_phase == 3: # 抬起 + target_pos = [0.1, 0.15, 0.15, 0.05, 0, 0, -0.01, -0.01] + if steps > 100: + trajectory_phase = 4 + steps = 0 + + elif trajectory_phase == 4: # 返回 + target_pos = [0, 0, 0, 0, 0, 0, -0.01, -0.01] + if steps > 150: + trajectory_phase = 5 + steps = 0 + + elif trajectory_phase == 5: # 释放 + target_pos = [0, 0, 0, 0, 0, 0, 0.02, 0.02] + if steps > 50: + # 重新开始 + trajectory_phase = 0 + steps = 0 + + # 使用PD控制器达到目标位置 + kp = 10.0 # 比例增益 + for i in range(8): + error = target_pos[i] - data.qpos[i] + data.ctrl[i] = kp * error + + mujoco.mj_step(model, data) + viewer.sync() + + steps += 1 + time.sleep(0.01) + + +def get_robot_info(): + """获取机械臂信息""" + model = mujoco.MjModel.from_xml_path('new_arm.xml') + + print("=" * 50) + print("机械臂模型信息") + print("=" * 50) + print(f"模型名称: {model.names}") + print(f"自由度数量: {model.nv}") + print(f"关节数量: {model.njnt}") + print(f"执行器数量: {model.nu}") + print(f"几何体数量: {model.ngeom}") + print(f"体数量: {model.nbody}") + + print("\n关节列表:") + for i in range(model.njnt): + jnt_type = model.jnt_type[i] + print(f" 关节{i}: {model.joint(i).name}, 类型: {jnt_type}") + + print("\n执行器列表:") + for i in range(model.nu): + actuator_name = model.actuator(i).name + joint_id = model.actuator_trnid[i, 0] + joint_name = model.joint(joint_id).name + print(f" 执行器{i}: {actuator_name}, 控制的关节: {joint_name}") + + print("\n体列表:") + for i in range(model.nbody): + body_name = model.body(i).name + if body_name: + print(f" 体{i}: {body_name}") + + +if __name__ == "__main__": + print("MuJoCo 机械臂可视化") + print("=" * 50) + + # 显示模型信息 + get_robot_info() + + print("\n选择运行模式:") + print("1: 交互式可视化") + print("2: 预设轨迹演示") + + try: + choice = int(input("请输入选择 (1 或 2): ")) + + if choice == 1: + load_and_visualize() + elif choice == 2: + simulate_trajectory() + else: + print("无效选择,使用默认模式...") + load_and_visualize() + except KeyboardInterrupt: + print("\n程序被用户中断") + except Exception as e: + print(f"发生错误: {e}") \ No newline at end of file