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
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.
205 changes: 205 additions & 0 deletions src/mechanical_arm/three_fingered_arm.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,205 @@
<?xml version="1.0" encoding="utf-8"?>
<mujoco model="iiwa14_with_three_finger_gripper">
<compiler angle="radian" autolimits="true"/>

<option integrator="implicitfast"/>

<default>
<default class="iiwa">
<material specular="0.5" shininess="0.25"/>
<joint axis="0 0 1" damping="0.1"/>
<general gaintype="fixed" biastype="affine" gainprm="2000" biasprm="0 -2000 -200"/>
<default class="joint1">
<joint range="-2.96706 2.96706"/>
<general ctrlrange="-2.96706 2.96706"/>
<default class="joint2">
<joint range="-2.0944 2.0944"/>
<general ctrlrange="-2.0944 2.0944"/>
</default>
</default>
<default class="joint3">
<joint range="-3.05433 3.05433"/>
<general ctrlrange="-3.05433 3.05433"/>
</default>
<default class="visual">
<geom type="capsule" size="0.05" contype="0" conaffinity="0" group="2" material="gray"/>
</default>
<default class="collision">
<geom group="3"/>
</default>
<site size="0.001" rgba="1 0 0 1" group="4"/>

<!-- Gripper defaults -->
<default class="gripper">
<joint damping="0.5" frictionloss="0.1"/>
<default class="finger_joint">
<joint range="0 0.12" axis="0 1 0"/>
<general ctrlrange="0 0.12"/>
</default>
</default>
<default class="gripper_visual">
<geom type="box" size="0.02 0.02 0.18" contype="0" conaffinity="0" group="2" material="orange"/>
</default>
<default class="gripper_collision">
<geom group="3"/>
</default>
</default>
</default>

<asset>
<material class="iiwa" name="gray" rgba="0.4 0.4 0.4 1"/>
<material class="iiwa" name="light_gray" rgba="0.6 0.6 0.6 1"/>
<material class="iiwa" name="black" rgba="0 0 0 1"/>
<material class="iiwa" name="orange" rgba="1 0.423529 0.0392157 1"/>
</asset>

<worldbody>
<light name="top" pos="0 0 2" mode="trackcom"/>
<body name="base" childclass="iiwa">
<inertial mass="5" pos="-0.1 0 0.07" diaginertia="0.05 0.06 0.03"/>
<geom class="visual" type="cylinder" size="0.1 0.1"/>
<geom class="collision" size="0.12" pos="0 0 0.03"/>
<geom class="collision" size="0.08" pos="-0.08 0 0.103"/>
<geom class="collision" size="0.08" pos="-0.08 0 0.04"/>
<geom class="collision" size="0.1" pos="0 0 0.14"/>
<body name="link1" pos="0 0 0.1575">
<inertial mass="5.76" pos="0 -0.03 0.12" diaginertia="0.0333 0.033 0.0123"/>
<joint name="joint1" class="joint1"/>
<geom class="visual" type="capsule" size="0.05 0.1"/>
<geom class="collision" size="0.08" pos="0 0 -0.0005"/>
<geom class="collision" size="0.075" pos="0.01 -0.025 0.0425"/>
<geom class="collision" size="0.075" pos="-0.01 -0.025 0.0425"/>
<geom class="collision" size="0.07" pos="0.01 -0.045 0.1025"/>
<geom class="collision" size="0.07" pos="-0.01 -0.045 0.1025"/>
<body name="link2" pos="0 0 0.2025" quat="0 0 1 1">
<inertial mass="6.35" pos="0.0003 0.059 0.042" diaginertia="0.0305 0.0304 0.011" quat="0 0 1 1"/>
<joint name="joint2" class="joint2"/>
<geom class="visual" type="capsule" size="0.05 0.1" material="orange"/>
<geom class="collision" size="0.095" pos="0 0 -0.01"/>
<geom class="collision" size="0.09" pos="0 0 0.045"/>
<geom class="collision" size="0.07" pos="-0.01 0.04 0.054"/>
<geom class="collision" size="0.065" pos="-0.01 0.09 0.04"/>
<geom class="collision" size="0.065" pos="-0.01 0.13 0.02"/>
<geom class="collision" size="0.07" pos="0.01 0.04 0.054"/>
<geom class="collision" size="0.065" pos="0.01 0.09 0.04"/>
<geom class="collision" size="0.065" pos="0.01 0.13 0.02"/>
<geom class="collision" size="0.075" pos="0 0.18 0"/>
<body name="link3" pos="0 0.2045 0" quat="0 0 1 1">
<inertial mass="3.5" pos="0 0.03 0.13" diaginertia="0.025 0.0238 0.0076"/>
<joint name="joint3" class="joint1"/>
<geom class="visual" type="capsule" size="0.05 0.1"/>
<geom class="collision" size="0.075" pos="0 0 0.0355"/>
<geom class="collision" size="0.06" pos="0.01 0.023 0.0855"/>
<geom class="collision" size="0.055" pos="0.01 0.048 0.1255"/>
<geom class="collision" size="0.06" pos="0.01 0.056 0.1755"/>
<geom class="collision" size="0.06" pos="-0.01 0.023 0.0855"/>
<geom class="collision" size="0.055" pos="-0.01 0.048 0.1255"/>
<geom class="collision" size="0.06" pos="-0.01 0.056 0.1755"/>
<geom class="collision" size="0.075" pos="0 0.045 0.2155"/>
<geom class="collision" size="0.075" pos="0 0 0.2155"/>
<body name="link4" pos="0 0 0.2155" quat="1 1 0 0">
<inertial mass="3.5" pos="0 0.067 0.034" diaginertia="0.017 0.0164 0.006" quat="1 1 0 0"/>
<joint name="joint4" class="joint2"/>
<geom class="visual" type="capsule" size="0.05 0.1" material="orange"/>
<geom class="collision" size="0.078" pos="0 0.01 0.046"/>
<geom class="collision" size="0.06" pos="0.01 0.06 0.052"/>
<geom class="collision" size="0.065" pos="0.01 0.12 0.034"/>
<geom class="collision" size="0.06" pos="-0.01 0.06 0.052"/>
<geom class="collision" size="0.065" pos="-0.01 0.12 0.034"/>
<geom class="collision" size="0.075" pos="0 0.184 0"/>
<body name="link5" pos="0 0.1845 0" quat="0 0 1 1">
<inertial mass="3.5" pos="0.0001 0.021 0.076" diaginertia="0.01 0.0087 0.00449"/>
<joint name="joint5" class="joint1"/>
<geom class="visual" type="capsule" size="0.05 0.1"/>
<geom class="collision" size="0.075" pos="0 0 0.0335"/>
<geom class="collision" size="0.05" pos="-0.012 0.031 0.0755"/>
<geom class="collision" size="0.05" pos="0.012 0.031 0.0755"/>
<geom class="collision" size="0.04" pos="-0.012 0.06 0.1155"/>
<geom class="collision" size="0.04" pos="0.012 0.06 0.1155"/>
<geom class="collision" size="0.04" pos="-0.01 0.065 0.1655"/>
<geom class="collision" size="0.04" pos="0.01 0.065 0.1655"/>
<geom class="collision" size="0.035" pos="-0.012 0.065 0.1855"/>
<geom class="collision" size="0.035" pos="0.012 0.065 0.1855"/>
<body name="link6" pos="0 0 0.2155" quat="1 1 0 0">
<inertial mass="1.8" pos="0 0.0006 0.0004" diaginertia="0.0049 0.0047 0.0036" quat="1 1 0 0"/>
<joint name="joint6" class="joint2"/>
<geom class="visual" type="capsule" size="0.05 0.1" material="orange"/>
<geom class="collision" size="0.055" pos="0 0 -0.059"/>
<geom class="collision" size="0.065" pos="0 -0.03 0.011"/>
<geom class="collision" size="0.08"/>
<body name="link7" pos="0 0.081 0" quat="0 0 1 1">
<inertial mass="1.2" pos="0 0 0.02" diaginertia="0.001 0.001 0.001"/>
<joint name="joint7" class="joint3"/>
<geom class="visual" type="capsule" size="0.05 0.05"/>
<geom class="collision" size="0.06" pos="0 0 0.001"/>
<site pos="0 0 0.045" name="attachment_site"/>

<!-- 三指夹爪 -->
<body name="gripper_base" pos="0 0 0.045">
<inertial mass="0.1" pos="0 0 0" diaginertia="0.001 0.001 0.001"/>
<geom class="visual" type="box" size="0.02 0.02 0.02" material="black"/>
<geom class="collision" type="box" size="0.02 0.02 0.02"/>

<!-- 手指1 -->
<body name="finger1" pos="0.04 0 0.02" euler="0 0 0">
<inertial mass="0.01" pos="0 0 0.09" diaginertia="0.0001 0.0001 0.0001"/>
<joint name="finger_joint1" class="finger_joint"/>
<geom class="gripper_visual" pos="0 0 0.09" material="orange"/>
<geom class="gripper_collision" type="box" size="0.02 0.02 0.09"/>
</body>

<!-- 手指2 -->
<body name="finger2" pos="-0.02 0.0346 0.02" euler="0 0 2.0944"> <!-- 120度 -->
<inertial mass="0.01" pos="0 0 0.09" diaginertia="0.0001 0.0001 0.0001"/>
<joint name="finger_joint2" class="finger_joint"/>
<geom class="gripper_visual" pos="0 0 0.09" material="orange"/>
<geom class="gripper_collision" type="box" size="0.02 0.02 0.09"/>
</body>

<!-- 手指3 -->
<body name="finger3" pos="-0.02 -0.0346 0.02" euler="0 0 4.1888"> <!-- 240度 -->
<inertial mass="0.01" pos="0 0 0.09" diaginertia="0.0001 0.0001 0.0001"/>
<joint name="finger_joint3" class="finger_joint"/>
<geom class="gripper_visual" pos="0 0 0.09" material="orange"/>
<geom class="gripper_collision" type="box" size="0.02 0.02 0.09"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>

<contact>
<exclude body1="base" body2="link1"/>
<exclude body1="base" body2="link2"/>
<exclude body1="base" body2="link3"/>
<exclude body1="link1" body2="link3"/>
<exclude body1="link3" body2="link5"/>
<exclude body1="link4" body2="link7"/>
<exclude body1="link5" body2="link7"/>
</contact>

<actuator>
<general name="actuator1" joint="joint1" class="joint1"/>
<general name="actuator2" joint="joint2" class="joint2"/>
<general name="actuator3" joint="joint3" class="joint1"/>
<general name="actuator4" joint="joint4" class="joint2"/>
<general name="actuator5" joint="joint5" class="joint1"/>
<general name="actuator6" joint="joint6" class="joint2"/>
<general name="actuator7" joint="joint7" class="joint3"/>

<!-- 夹爪驱动 -->
<general name="gripper_actuator1" joint="finger_joint1" class="finger_joint"/>
<general name="gripper_actuator2" joint="finger_joint2" class="finger_joint"/>
<general name="gripper_actuator3" joint="finger_joint3" class="finger_joint"/>
</actuator>

<keyframe>
<key name="home" qpos="0 0.785398 0 -1.5708 0 0 0 0 0 0" ctrl="0 0.785398 0 -1.5708 0 0 0 0 0 0"/>
</keyframe>
</mujoco>
Loading