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
93 changes: 93 additions & 0 deletions src/mechanical_arm/6dof_arm.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
<mujoco model="6-DOF Mechanical Arm">
<!-- 编译器配置:物理参数、仿真步长 -->
<compiler angle="radian" inertiafromgeom="true"/>
<option timestep="0.001" gravity="0 0 -9.81"/>

<!-- 视觉配置:背景、光照 -->
<visual>
<global azimuth="135" elevation="-30" perspective="0.01"/>
<light diffuse="0.8 0.8 0.8" ambient="0.2 0.2 0.2" dir="-0.5 -0.5 -1"/>
</visual>

<!-- 资产:材质、纹理(简化版) -->
<asset>
<material name="arm_mat" rgba="0.2 0.6 0.8 1" specular="0.2"/>
<material name="base_mat" rgba="0.1 0.1 0.1 1" specular="0.1"/>
<material name="ee_mat" rgba="0.8 0.2 0.2 1" specular="0.3"/>
</asset>

<!-- 世界体:基础坐标系 -->
<worldbody>
<!-- 地面:方便观察 -->
<geom name="floor" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.8 0.8 0.8 1"/>

<!-- 机械臂基座 -->
<body name="base" pos="0 0 0">
<geom name="base_geom" type="cylinder" size="0.15 0.1" pos="0 0 0" material="base_mat"/>
<joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0.1" range="-3.14 3.14" damping="0.1"/>

<!-- 连杆1(肩部) -->
<body name="link1" pos="0 0 0.1">
<geom name="link1_geom" type="cylinder" size="0.05 0.3" pos="0 0 0.3" axis="0 1 0" material="arm_mat"/>
<joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0.6" range="-1.57 1.57" damping="0.1"/>

<!-- 连杆2(肘部) -->
<body name="link2" pos="0 0 0.6">
<geom name="link2_geom" type="cylinder" size="0.04 0.3" pos="0 0 0.3" axis="0 1 0" material="arm_mat"/>
<joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0.6" range="-1.57 1.57" damping="0.1"/>

<!-- 连杆3(腕部1) -->
<body name="link3" pos="0 0 0.6">
<geom name="link3_geom" type="cylinder" size="0.03 0.2" pos="0 0 0.2" axis="0 1 0" material="arm_mat"/>
<joint name="joint4" type="hinge" axis="0 0 1" pos="0 0 0.4" range="-3.14 3.14" damping="0.1"/>

<!-- 连杆4(腕部2) -->
<body name="link4" pos="0 0 0.4">
<geom name="link4_geom" type="cylinder" size="0.03 0.15" pos="0 0 0.15" axis="0 1 0" material="arm_mat"/>
<joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0.3" range="-1.57 1.57" damping="0.1"/>

<!-- 连杆5(腕部3) -->
<body name="link5" pos="0 0 0.3">
<geom name="link5_geom" type="cylinder" size="0.02 0.1" pos="0 0 0.1" axis="0 1 0" material="arm_mat"/>
<joint name="joint6" type="hinge" axis="0 0 1" pos="0 0 0.2" range="-3.14 3.14" damping="0.1"/>

<!-- 末端执行器(夹爪简化版) -->
<body name="end_effector" pos="0 0 0.2">
<geom name="ee_geom" type="box" size="0.04 0.04 0.04" pos="0 0 0" material="ee_mat"/>
<!-- 末端执行器位点:方便抓取/坐标计算 -->
<site name="ee_site" pos="0 0 0.04" size="0.01" rgba="1 0 0 1"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>

<!-- 控制配置:关节力矩控制 -->
<actuator>
<motor name="motor1" joint="joint1" ctrlrange="-5 5" gear="100"/>
<motor name="motor2" joint="joint2" ctrlrange="-5 5" gear="100"/>
<motor name="motor3" joint="joint3" ctrlrange="-5 5" gear="100"/>
<motor name="motor4" joint="joint4" ctrlrange="-5 5" gear="100"/>
<motor name="motor5" joint="joint5" ctrlrange="-5 5" gear="100"/>
<motor name="motor6" joint="joint6" ctrlrange="-5 5" gear="100"/>
</actuator>

<!-- 传感器配置:关节角度、末端执行器力/位置 -->
<sensor>
<!-- 6个关节的角度传感器 -->
<jointpos name="joint1_pos" joint="joint1"/>
<jointpos name="joint2_pos" joint="joint2"/>
<jointpos name="joint3_pos" joint="joint3"/>
<jointpos name="joint4_pos" joint="joint4"/>
<jointpos name="joint5_pos" joint="joint5"/>
<jointpos name="joint6_pos" joint="joint6"/>

<!-- 末端执行器力传感器 -->
<force name="ee_force" site="ee_site"/>
<!-- 末端执行器位置传感器 -->
<framepos name="ee_pos" site="ee_site"/>
</sensor>
</mujoco>
1 change: 1 addition & 0 deletions src/mechanical_arm/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
机械臂
126 changes: 126 additions & 0 deletions src/mechanical_arm/main.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,126 @@
import matplotlib.pyplot as plt
import numpy as np


class ArmVisualizer:
def __init__(self, link_lengths=None):
"""
初始化机械臂可视化器
:param link_lengths: 各连杆长度列表,默认[5, 4, 3]
"""
# 默认连杆长度
self.link_lengths = link_lengths if link_lengths else [5, 4, 3]
self.num_joints = len(self.link_lengths) + 1 # 关节数 = 连杆数 + 1

# 初始化绘图
self.fig, self.ax = plt.subplots(figsize=(8, 8))
self.ax.set_xlim(-12, 12)
self.ax.set_ylim(-12, 12)
self.ax.set_aspect('equal')
self.ax.grid(True, alpha=0.3)
self.ax.set_xlabel('X (cm)')
self.ax.set_ylabel('Y (cm)')
self.ax.set_title('3自由度平面机械臂可视化')

def calculate_joint_positions(self, angles):
"""
根据关节角度计算各关节坐标
:param angles: 各关节角度列表(弧度),长度等于连杆数
:return: 关节坐标列表 [(x0,y0), (x1,y1), ..., (xn,yn)]
"""
if len(angles) != len(self.link_lengths):
raise ValueError(f"角度数量应等于连杆数量({len(self.link_lengths)})")

# 初始关节(基座)在原点
joints = [(0.0, 0.0)]
current_x, current_y = 0.0, 0.0
current_angle = 0.0

# 计算每个关节的位置
for i, (angle, length) in enumerate(zip(angles, self.link_lengths)):
current_angle += angle
current_x += length * np.cos(current_angle)
current_y += length * np.sin(current_angle)
joints.append((current_x, current_y))

return joints

def draw_arm(self, angles, save_path=None):
"""
绘制机械臂并可选保存图片
:param angles: 关节角度列表(弧度)
:param save_path: 保存路径,如'robot_arm.png'
"""
# 清空之前的绘图
self.ax.clear()
self.ax.set_xlim(-12, 12)
self.ax.set_ylim(-12, 12)
self.ax.set_aspect('equal')
self.ax.grid(True, alpha=0.3)
self.ax.set_xlabel('X (cm)')
self.ax.set_ylabel('Y (cm)')
self.ax.set_title('3自由度平面机械臂可视化')

# 计算关节位置
joints = self.calculate_joint_positions(angles)

# 提取x和y坐标
x_coords = [joint[0] for joint in joints]
y_coords = [joint[1] for joint in joints]

# 绘制连杆
self.ax.plot(x_coords, y_coords, 'b-', linewidth=4, label='连杆')

# 绘制关节(基座用红色,其他关节用蓝色)
self.ax.plot(x_coords[0], y_coords[0], 'ro', markersize=10, label='基座')
self.ax.plot(x_coords[1:], y_coords[1:], 'bo', markersize=8, label='关节')

# 绘制末端执行器
self.ax.plot(x_coords[-1], y_coords[-1], 'go', markersize=12, label='末端执行器')

# 添加角度标注
for i, (x, y) in enumerate(joints[:-1]):
angle_deg = np.degrees(sum(angles[:i + 1]))
self.ax.text(x + 0.2, y + 0.2, f'θ{i + 1}={angle_deg:.0f}°',
fontsize=9, bbox=dict(facecolor='white', alpha=0.7))

# 添加连杆长度标注
for i in range(len(self.link_lengths)):
mid_x = (x_coords[i] + x_coords[i + 1]) / 2
mid_y = (y_coords[i] + y_coords[i + 1]) / 2
self.ax.text(mid_x, mid_y, f'L{i + 1}={self.link_lengths[i]}',
fontsize=8, color='darkred')

# 添加末端坐标标注
end_x, end_y = joints[-1]
self.ax.text(end_x + 0.3, end_y - 0.3, f'末端坐标:\n({end_x:.2f}, {end_y:.2f})',
fontsize=9, bbox=dict(facecolor='lightgreen', alpha=0.7))

self.ax.legend(loc='upper right')

# 保存图片(如果指定路径)
if save_path:
plt.tight_layout()
plt.savefig(save_path, dpi=300, bbox_inches='tight')
print(f"机械臂图片已保存至: {save_path}")

plt.show()


# 示例使用
if __name__ == "__main__":
# 创建机械臂可视化器(可自定义连杆长度)
arm_visualizer = ArmVisualizer(link_lengths=[6, 4, 3])

# 设置关节角度(弧度),可以修改这些值查看不同姿态
# 示例1:初始姿态(伸展)
# angles = [0, 0, 0]

# 示例2:弯曲姿态
angles = [np.pi / 4, np.pi / 6, -np.pi / 3]

# 示例3:更复杂的姿态
# angles = [np.pi/3, -np.pi/4, np.pi/2]

# 绘制并保存机械臂图片
arm_visualizer.draw_arm(angles, save_path='robot_arm_visualization.png')
Loading