Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
122 changes: 122 additions & 0 deletions src/mechanical_arm/full_gripper_demo.py
Original file line number Diff line number Diff line change
@@ -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()
2 changes: 1 addition & 1 deletion src/mechanical_arm/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -435,4 +435,4 @@ def main():


if __name__ == "__main__":
main()
main()#结束过程
64 changes: 64 additions & 0 deletions src/mechanical_arm/model_test.py
Original file line number Diff line number Diff line change
@@ -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()
82 changes: 82 additions & 0 deletions src/mechanical_arm/motion_test.py
Original file line number Diff line number Diff line change
@@ -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()
Empty file.
98 changes: 98 additions & 0 deletions src/mechanical_arm/three_arm_test_three.py
Original file line number Diff line number Diff line change
@@ -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()
Loading
Loading