import tkinter as tk
from tkinter import ttk, messagebox, filedialog, colorchooser
import math
import time
import csv
import os
import random
from PIL import Image, ImageTk
import numpy as np
class RobotArm:
"""工业机器人模型"""
def __init__(self):
# 机器人参数
self.base_x = 400 # 基座X坐标
self.base_y = 550 # 基座Y坐标
# 关节长度
self.joint1_length = 120 # 第一关节长度
self.joint2_length = 100 # 第二关节长度
self.joint3_length = 80 # 第三关节长度
self.joint4_length = 60 # 第四关节长度
self.end_effector_length = 40 # 末端执行器长度
# 关节角度 (单位: 度)
self.joint1_angle = 45 # 第一关节角度 (0-180)
self.joint2_angle = -30 # 第二关节角度 (-90-90)
self.joint3_angle = 20 # 第三关节角度 (-90-90)
self.joint4_angle = -10 # 第四关节角度 (-45-45)
# 末端位置
self.x = 0
self.y = 0
# 绘图状态
self.pen_down = False
self.pen_color = "#0000FF" # 蓝色
self.pen_width = 3
self.pen_style = "solid"
self.drawing_path = [] # 绘图路径
# 障碍物
self.obstacles = []
# 更新位置
self.update_position()
def update_position(self):
"""根据关节角度更新末端位置"""
# 将角度转换为弧度
j1_rad = math.radians(self.joint1_angle)
j2_rad = math.radians(self.joint2_angle + self.joint1_angle)
j3_rad = math.radians(self.joint3_angle + self.joint2_angle + self.joint1_angle)
j4_rad = math.radians(self.joint4_angle + self.joint3_angle + self.joint2_angle + self.joint1_angle)
# 计算关节位置
self.joint1_x = self.base_x + self.joint1_length * math.cos(j1_rad)
self.joint1_y = self.base_y - self.joint1_length * math.sin(j1_rad)
self.joint2_x = self.joint1_x + self.joint2_length * math.cos(j2_rad)
self.joint2_y = self.joint1_y - self.joint2_length * math.sin(j2_rad)
self.joint3_x = self.joint2_x + self.joint3_length * math.cos(j3_rad)
self.joint3_y = self.joint2_y - self.joint3_length * math.sin(j3_rad)
self.joint4_x = self.joint3_x + self.joint4_length * math.cos(j4_rad)
self.joint4_y = self.joint3_y - self.joint4_length * math.sin(j4_rad)
# 计算末端位置
self.x = self.joint4_x + self.end_effector_length * math.cos(j4_rad)
self.y = self.joint4_y - self.end_effector_length * math.sin(j4_rad)
def set_angles(self, j1, j2, j3, j4):
"""设置关节角度"""
self.joint1_angle = max(0, min(180, j1))
self.joint2_angle = max(-90, min(90, j2))
self.joint3_angle = max(-90, min(90, j3))
self.joint4_angle = max(-45, min(45, j4))
self.update_position()
def move_to(self, x, y, avoid_obstacles=False):
"""移动机械臂末端到指定位置（逆运动学）"""
# 简化的逆运动学计算
dx = x - self.base_x
dy = self.base_y - y
# 计算到目标点的距离
distance = math.sqrt(dx**2 + dy**2)
# 检查是否在可到达范围内
max_reach = self.joint1_length + self.joint2_length + self.joint3_length + self.joint4_length
if distance > max_reach:
# 如果超出范围，移动到最大范围处
scale = max_reach / distance
x = self.base_x + dx * scale
y = self.base_y - dy * scale
distance = max_reach
dx = x - self.base_x
dy = self.base_y - y
# 计算第一关节角度
j1_angle = math.degrees(math.atan2(dy, dx))
# 计算其他关节角度 (简化计算)
# 在实际应用中，这里应该使用更复杂的逆运动学算法
# 这里使用简化方法，保持其他关节角度相对固定
j2_angle = -30
j3_angle = 20
j4_angle = -10
# 设置角度
self.set_angles(j1_angle, j2_angle, j3_angle, j4_angle)
def start_drawing(self):
"""开始绘图"""
self.pen_down = True
# 记录起始点
self.drawing_path.append({
"x": self.x,
"y": self.y,
"color": self.pen_color,
"width": self.pen_width,
"style": self.pen_style,
"points": [(self.x, self.y)]
})
def add_drawing_point(self, x, y):
"""添加绘图点"""
if self.pen_down and self.drawing_path:
self.drawing_path[-1]["points"].append((x, y))
def stop_drawing(self):
"""停止绘图"""
self.pen_down = False
def add_obstacle(self, x1, y1, x2, y2):
"""添加障碍物"""
self.obstacles.append((x1, y1, x2, y2))
def clear_obstacles(self):
"""清除所有障碍物"""
self.obstacles = []
def is_collision(self, x, y):
"""检测给定点是否与障碍物碰撞"""
for obstacle in self.obstacles:
x1, y1, x2, y2 = obstacle
if x1 <= x <= x2 and y1 <= y <= y2:
return True
return False
def avoid_obstacles(self, start_x, start_y, target_x, target_y):
"""避障路径规划"""
# 简单避障算法 - 绕行矩形障碍物
if not self.obstacles:
return [(target_x, target_y)]
# 检查路径是否穿过障碍物
for obstacle in self.obstacles:
x1, y1, x2, y2 = obstacle
if self.line_intersects_rect(start_x, start_y, target_x, target_y, x1, y1, x2, y2):
# 计算绕行点
# 尝试从上方绕行
waypoint_x = (start_x + target_x) / 2
waypoint_y = min(y1, y2) - 20
# 检查绕行点是否在障碍物内
if not self.is_collision(waypoint_x, waypoint_y):
return [(waypoint_x, waypoint_y), (target_x, target_y)]
# 尝试从下方绕行
waypoint_y = max(y1, y2) + 20
if not self.is_collision(waypoint_x, waypoint_y):
return [(waypoint_x, waypoint_y), (target_x, target_y)]
# 尝试从左方绕行
waypoint_x = min(x1, x2) - 20
waypoint_y = (start_y + target_y) / 2
if not self.is_collision(waypoint_x, waypoint_y):
return [(waypoint_x, waypoint_y), (target_x, target_y)]
# 尝试从右方绕行
waypoint_x = max(x1, x2) + 20
if not self.is_collision(waypoint_x, waypoint_y):
return [(waypoint_x, waypoint_y), (target_x, target_y)]
# 如果没有障碍物阻挡，直接返回目标点
return [(target_x, target_y)]
def line_intersects_rect(self, x1, y1, x2, y2, rx1, ry1, rx2, ry2):
"""检查线段是否与矩形相交"""
# 确保矩形坐标有序
rx_min, rx_max = min(rx1, rx2), max(rx1, rx2)
ry_min, ry_max = min(ry1, ry2), max(ry1, ry2)
# 检查线段是否在矩形外
if max(x1, x2) < rx_min or min(x1, x2) > rx_max or max(y1, y2) < ry_min or min(y1, y2) > ry_max:
return False
# 检查线段是否与矩形边相交
# 左边缘
if self.line_intersects_line(x1, y1, x2, y2, rx_min, ry_min, rx_min, ry_max):
return True
# 右边缘
if self.line_intersects_line(x1, y1, x2, y2, rx_max, ry_min, rx_max, ry_max):
return True
# 上边缘
if self.line_intersects_line(x1, y1, x2, y2, rx_min, ry_min, rx_max, ry_min):
return True
# 下边缘
if self.line_intersects_line(x1, y1, x2, y2, rx_min, ry_max, rx_max, ry_max):
return True
return False
def line_intersects_line(self, x1, y1, x2, y2, x3, y3, x4, y4):
"""检查两条线段是否相交"""
# 计算分母
denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1)
if denom == 0:
return False # 平行线
ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / denom
ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / denom
# 检查交点是否在线段上
if 0 <= ua <= 1 and 0 <= ub <= 1:
return True
return False
class SimulationFrame(ttk.Frame):
"""主仿真界面 - 工业机器人绘图功能"""
def __init__(self, parent):
super().__init__(parent)
self.robot = RobotArm()
self.modes = ["手动绘图", "自动路径", "远程控制"]
self.current_mode = tk.IntVar(value=0) # 默认手动模式
self.playback_trajectory = []
self.is_playing = False
self.recording = False
self.playback_index = 0
self.dragging = False
self.dragging_arm = False
self.drag_start_x = 0
self.drag_start_y = 0
self.manual_drawing = False
self.path_types = ["圆形", "方形", "三角形", "五角星", "螺旋线", "心形", "正弦波", "字母A", "字母B", "数字8", "星形", "花朵", "迷宫", "螺旋方", "自定义"]
self.selected_path = tk.StringVar(value="圆形")
self.pen_styles = ["solid", "dash", "dot", "dashdot"]
self.create_widgets()
self.animate()
# 添加一些初始障碍物
self.robot.add_obstacle(300, 300, 400, 400)
self.robot.add_obstacle(500, 200, 600, 300)
def create_widgets(self):
"""创建界面组件"""
# 创建主框架
main_frame = ttk.Frame(self)
main_frame.pack(fill="both", expand=True, padx=10, pady=10)
# 左侧面板 - 状态和画布
left_frame = ttk.Frame(main_frame)
left_frame.pack(side="left", fill="both", expand=True)
# 状态栏
status_frame = ttk.Frame(left_frame)
status_frame.pack(fill="x", pady=(0, 10))
self.status_var = tk.StringVar(value="系统就绪 - 手动绘图模式")
status_label = ttk.Label(status_frame, textvariable=self.status_var,
foreground="green", font=("Arial", 10, "bold"))
status_label.pack(side="left")
# 坐标显示
coord_frame = ttk.Frame(status_frame)
coord_frame.pack(side="right")
self.coord_var = tk.StringVar(value="末端坐标: (0.0, 0.0)")
ttk.Label(coord_frame, textvariable=self.coord_var,
font=("Consolas", 9)).pack(side="left", padx=(10, 5))
# 关节角度显示
self.angle_var = tk.StringVar(value="关节角度: J1 0°, J2 0°, J3 0°, J4 0°")
ttk.Label(coord_frame, textvariable=self.angle_var,
font=("Consolas", 9)).pack(side="left", padx=5)
# 画布
canvas_frame = ttk.Frame(left_frame)
canvas_frame.pack(fill="both", expand=True)
# 增大画布尺寸
self.canvas = tk.Canvas(canvas_frame, width=800, height=600, bg="white", highlightthickness=1, highlightbackground="#CCCCCC")
self.canvas.pack(fill="both", expand=True, pady=(0, 10))
# 绑定鼠标事件
self.canvas.bind("<Button-1>", self.canvas_click)
self.canvas.bind("<B1-Motion>", self.canvas_drag)
self.canvas.bind("<ButtonRelease-1>", self.canvas_release)
# 绘制工作区
self.draw_workspace()
# 操作日志
log_frame = ttk.LabelFrame(left_frame, text="操作日志")
log_frame.pack(fill="both", expand=True)
self.log_text = tk.Text(log_frame, height=5, state="disabled")
scrollbar = ttk.Scrollbar(log_frame, command=self.log_text.yview)
self.log_text.config(yscrollcommand=scrollbar.set)
self.log_text.pack(side="left", fill="both", expand=True, padx=5, pady=5)
scrollbar.pack(side="right", fill="y", padx=(0, 5), pady=5)
self.log("系统初始化完成")
# 右侧控制面板
control_frame = ttk.LabelFrame(main_frame, text="控制面板")
control_frame.pack(side="right", fill="y", padx=(10, 0))
# 使用Notebook组织控制面板
notebook = ttk.Notebook(control_frame)
notebook.pack(fill="both", expand=True, padx=5, pady=5)
# 模式控制标签页
mode_tab = ttk.Frame(notebook)
notebook.add(mode_tab, text="模式控制")
# 路径控制标签页
path_tab = ttk.Frame(notebook)
notebook.add(path_tab, text="路径控制")
# 笔头设置标签页
pen_tab = ttk.Frame(notebook)
notebook.add(pen_tab, text="笔头设置")
# 轨迹控制标签页
traj_tab = ttk.Frame(notebook)
notebook.add(traj_tab, text="轨迹控制")
# 模式选择
mode_frame = ttk.LabelFrame(mode_tab, text="操作模式")
mode_frame.pack(fill="x", padx=10, pady=5)
for i, mode in enumerate(self.modes):
rb = ttk.Radiobutton(mode_frame, text=mode, variable=self.current_mode, value=i,
command=self.mode_changed)
rb.pack(side="top", padx=10, pady=2, fill="x")
# 路径设置
path_frame = ttk.LabelFrame(path_tab, text="路径设置")
path_frame.pack(fill="x", padx=10, pady=5)
# 路径类型选择
ttk.Label(path_frame, text="路径类型:").pack(side="top", anchor="w", padx=(5, 0))
path_combo = ttk.Combobox(path_frame, textvariable=self.selected_path,
values=self.path_types, width=15)
path_combo.pack(fill="x", padx=5, pady=2)
path_combo.bind("<<ComboboxSelected>>", self.path_changed)
# 路径控制按钮
path_btn_frame = ttk.Frame(path_frame)
path_btn_frame.pack(fill="x", pady=5)
self.start_path_btn = ttk.Button(path_btn_frame, text="开始路径", command=self.start_path)
self.start_path_btn.pack(side="left", padx=2, fill="x", expand=True)
stop_path_btn = ttk.Button(path_btn_frame, text="停止路径", command=self.stop_path)
stop_path_btn.pack(side="left", padx=2, fill="x", expand=True)
# 路径参数
path_param_frame = ttk.Frame(path_frame)
path_param_frame.pack(fill="x", pady=2)
ttk.Label(path_param_frame, text="尺寸:").pack(side="left", padx=(5, 0))
self.path_size_var = tk.IntVar(value=150)
size_slider = ttk.Scale(path_param_frame, from_=50, to=300,
orient="horizontal", variable=self.path_size_var,
length=120)
size_slider.pack(side="left", padx=5, fill="x", expand=True)
size_value = ttk.Label(path_param_frame, textvariable=self.path_size_var, width=3)
size_value.pack(side="left", padx=(0, 5))
# 笔头设置
pen_frame = ttk.LabelFrame(pen_tab, text="笔头设置")
pen_frame.pack(fill="x", padx=10, pady=5)
# 笔头颜色
color_frame = ttk.Frame(pen_frame)
color_frame.pack(fill="x", pady=2)
ttk.Label(color_frame, text="颜色:").pack(side="left", padx=(5, 0))
self.color_btn = ttk.Button(color_frame, text="选择颜色", command=self.choose_color,
width=10)
self.color_btn.pack(side="left", padx=5)
self.color_preview = tk.Canvas(color_frame, width=30, height=20, bg=self.robot.pen_color)
self.color_preview.pack(side="left", padx=5)
# 笔头粗细
width_frame = ttk.Frame(pen_frame)
width_frame.pack(fill="x", pady=2)
ttk.Label(width_frame, text="粗细:").pack(side="left", padx=(5, 0))
self.width_var = tk.IntVar(value=self.robot.pen_width)
width_slider = ttk.Scale(width_frame, from_=1, to=10,
orient="horizontal", variable=self.width_var,
command=self.update_pen_width, length=120)
width_slider.pack(side="left", padx=5, fill="x", expand=True)
width_value = ttk.Label(width_frame, textvariable=self.width_var, width=3)
width_value.pack(side="left", padx=(0, 5))
# 笔头样式
style_frame = ttk.Frame(pen_frame)
style_frame.pack(fill="x", pady=2)
ttk.Label(style_frame, text="样式:").pack(side="left", padx=(5, 0))
self.style_var = tk.StringVar(value=self.robot.pen_style)
style_combo = ttk.Combobox(style_frame, textvariable=self.style_var,
values=self.pen_styles, width=10)
style_combo.pack(side="left", padx=5, fill="x", expand=True)
style_combo.bind("<<ComboboxSelected>>", self.update_pen_style)
# 控制按钮
btn_frame = ttk.LabelFrame(pen_tab, text="机械臂控制")
btn_frame.pack(fill="x", padx=10, pady=5)
button_row1 = ttk.Frame(btn_frame)
button_row1.pack(fill="x", pady=2)
self.start_btn = ttk.Button(button_row1, text="开始绘图", command=self.start_drawing)
self.start_btn.pack(side="left", padx=2, pady=2, fill="x", expand=True)
self.stop_btn = ttk.Button(button_row1, text="停止绘图", command=self.stop_drawing)
self.stop_btn.pack(side="left", padx=2, pady=2, fill="x", expand=True)
button_row2 = ttk.Frame(btn_frame)
button_row2.pack(fill="x", pady=2)
self.home_btn = ttk.Button(button_row2, text="归位", command=self.go_home)
self.home_btn.pack(side="left", padx=2, pady=2, fill="x", expand=True)
self.clear_btn = ttk.Button(button_row2, text="清除画布", command=self.clear_canvas)
self.clear_btn.pack(side="left", padx=2, pady=2, fill="x", expand=True)
# 速度控制
speed_frame = ttk.LabelFrame(pen_tab, text="速度控制")
speed_frame.pack(fill="x", padx=10, pady=5)
ttk.Label(speed_frame, text="速度:").pack(side="left", padx=(5, 0))
self.speed_var = tk.DoubleVar(value=1.0)
speed_slider = ttk.Scale(speed_frame, from_=0.1, to=2.0,
orient="horizontal", variable=self.speed_var,
command=self.update_speed, length=150)
speed_slider.pack(side="left", padx=5, fill="x", expand=True)
speed_value = ttk.Label(speed_frame, textvariable=self.speed_var, width=4)
speed_value.pack(side="left", padx=(0, 5))
# 轨迹控制
traj_frame = ttk.LabelFrame(traj_tab, text="轨迹控制")
traj_frame.pack(fill="x", padx=10, pady=5)
self.record_btn = ttk.Button(traj_frame, text="开始记录轨迹", command=self.toggle_record_trajectory)
self.record_btn.pack(fill="x", padx=5, pady=2)
self.playback_btn = ttk.Button(traj_frame, text="回放轨迹", command=self.playback_trajectory)
self.playback_btn.pack(fill="x", padx=5, pady=2)
self.save_traj_btn = ttk.Button(traj_frame, text="保存轨迹", command=self.save_trajectory)
self.save_traj_btn.pack(fill="x", padx=5, pady=2)
self.clear_traj_btn = ttk.Button(traj_frame, text="清除轨迹", command=self.clear_trajectory)
self.clear_traj_btn.pack(fill="x", padx=5, pady=2)
# 保存和加载绘图
save_frame = ttk.LabelFrame(traj_tab, text="绘图操作")
save_frame.pack(fill="x", padx=10, pady=5)
save_btn = ttk.Button(save_frame, text="保存绘图", command=self.save_drawing)
save_btn.pack(fill="x", padx=5, pady=2)
load_btn = ttk.Button(save_frame, text="加载绘图", command=self.load_drawing)
load_btn.pack(fill="x", padx=5, pady=2)
# 避障控制
obstacle_frame = ttk.LabelFrame(traj_tab, text="避障设置")
obstacle_frame.pack(fill="x", padx=10, pady=5)
add_obstacle_btn = ttk.Button(obstacle_frame, text="添加障碍物", command=self.add_obstacle)
add_obstacle_btn.pack(fill="x", padx=5, pady=2)
clear_obstacle_btn = ttk.Button(obstacle_frame, text="清除障碍物", command=self.clear_obstacles)
clear_obstacle_btn.pack(fill="x", padx=5, pady=2)
self.avoid_var = tk.BooleanVar(value=True)
avoid_check = ttk.Checkbutton(obstacle_frame, text="启用自动避障", variable=self.avoid_var)
avoid_check.pack(fill="x", padx=5, pady=2)
def mode_changed(self):
"""模式改变事件处理"""
mode_index = self.current_mode.get()
self.status_var.set(f"系统就绪 - {self.modes[mode_index]}模式")
self.log(f"切换到{self.modes[mode_index]}模式")
def path_changed(self, event):
"""路径类型改变事件处理"""
self.log(f"已选择路径类型: {self.selected_path.get()}")
def choose_color(self):
"""选择笔头颜色"""
color = colorchooser.askcolor(title="选择笔头颜色", initialcolor=self.robot.pen_color)
if color[1]:
self.robot.pen_color = color[1]
self.color_preview.config(bg=color[1])
self.log(f"笔头颜色设置为: {color[1]}")
def update_pen_width(self, value):
"""更新笔头粗细"""
self.robot.pen_width = int(float(value))
self.log(f"笔头粗细设置为: {self.robot.pen_width}像素")
def update_pen_style(self, event):
"""更新笔头样式"""
self.robot.pen_style = self.style_var.get()
self.log(f"笔头样式设置为: {self.robot.pen_style}")
def start_drawing(self):
"""开始绘图"""
self.robot.start_drawing()
self.log("开始绘图")
def stop_drawing(self):
"""停止绘图"""
self.robot.stop_drawing()
self.log("停止绘图")
def canvas_click(self, event):
"""画布点击事件处理"""
# 检查是否点击在机械臂机身末尾位置
if self.is_near_base(event.x, event.y):
self.dragging_arm = True
self.drag_start_x = event.x
self.drag_start_y = event.y
self.log("开始拖动机器人底座")
elif self.current_mode.get() == 0: # 手动绘图模式
# 移动机械臂到点击位置
self.robot.move_to(event.x, event.y)
self.draw_workspace()
# 开始绘图
self.manual_drawing = True
self.robot.start_drawing()
self.robot.add_drawing_point(event.x, event.y) # 添加第一个点
self.log(f"开始在位置 ({event.x}, {event.y}) 绘图")
def is_near_base(self, x, y):
"""检查是否在机器人底座附近"""
distance = math.sqrt((x - self.robot.base_x)**2 + (y - self.robot.base_y)**2)
return distance < 30
def canvas_drag(self, event):
"""画布拖拽事件处理"""
if self.dragging_arm:
# 计算移动距离
dx = event.x - self.drag_start_x
dy = event.y - self.drag_start_y
# 更新底座位置
self.robot.base_x += dx
self.robot.base_y += dy
# 重新计算所有关节位置
self.robot.update_position()
# 更新起始点
self.drag_start_x = event.x
self.drag_start_y = event.y
# 重绘
self.draw_workspace()
elif self.current_mode.get() == 0 and self.manual_drawing: # 手动绘图模式
# 移动机械臂到拖拽位置
self.robot.move_to(event.x, event.y)
self.robot.add_drawing_point(event.x, event.y)
self.draw_workspace()
def canvas_release(self, event):
"""画布释放事件处理"""
if self.dragging_arm:
self.dragging_arm = False
self.log("结束拖动机器人底座")
elif self.current_mode.get() == 0 and self.manual_drawing: # 手动绘图模式
# 停止绘图
self.manual_drawing = False
self.robot.stop_drawing()
self.log("结束绘图")
def start_path(self):
"""开始路径运动"""
path_type = self.selected_path.get()
size = self.path_size_var.get()
if path_type == "自定义" and not self.playback_trajectory:
self.log("没有自定义轨迹可执行")
return
self.log(f"开始{path_type}路径运动，尺寸: {size}")
self.generate_path(path_type, size)
def stop_path(self):
"""停止路径运动"""
self.robot.stop_drawing()
self.log("路径运动已停止")
def generate_path(self, path_type, size=150):
"""生成指定类型的路径"""
center_x = 400
center_y = 300
points = []
if path_type == "圆形":
for i in range(0, 360, 10):
angle = math.radians(i)
x = center_x + size * math.cos(angle)
y = center_y + size * math.sin(angle)
points.append((x, y))
elif path_type == "方形":
half_size = size // 2
# 上边
for x in range(center_x - half_size, center_x + half_size, 10):
points.append((x, center_y - half_size))
# 右边
for y in range(center_y - half_size, center_y + half_size, 10):
points.append((center_x + half_size, y))
# 下边
for x in range(center_x + half_size, center_x - half_size, -10):
points.append((x, center_y + half_size))
# 左边
for y in range(center_y + half_size, center_y - half_size, -10):
points.append((center_x - half_size, y))
# 回到起点
points.append((center_x - half_size, center_y - half_size))
elif path_type == "三角形":
height = size * math.sqrt(3) / 2
points.append((center_x, center_y - height//2)) # 顶点
points.append((center_x + size//2, center_y + height//2)) # 右下角
points.append((center_x - size//2, center_y + height//2)) # 左下角
points.append((center_x, center_y - height//2)) # 回到顶点
elif path_type == "五角星":
for i in range(5):
# 外角点
angle1 = math.radians(90 + i * 72)
x1 = center_x + size * math.cos(angle1)
y1 = center_y + size * math.sin(angle1)
points.append((x1, y1))
# 内角点
angle2 = math.radians(90 + (i + 0.5) * 72)
x2 = center_x + size * 0.4 * math.cos(angle2)
y2 = center_y + size * 0.4 * math.sin(angle2)
points.append((x2, y2))
points.append(points[0]) # 闭合五角星
elif path_type == "螺旋线":
for i in range(100):
angle = math.radians(i * 10)
radius = size * (1 - i/150)
x = center_x + radius * math.cos(angle)
y = center_y + radius * math.sin(angle)
points.append((x, y))
elif path_type == "心形":
for i in range(0, 360, 5):
angle = math.radians(i)
# 心形曲线方程
x = center_x + 16 * math.sin(angle)**3
y = center_y - (13 * math.cos(angle) - 5 * math.cos(2*angle) -
2 * math.cos(3*angle) - math.cos(4*angle))
points.append((x, y))
elif path_type == "正弦波":
for i in range(0, 360, 10):
x = center_x + i
y = center_y + size * 0.5 * math.sin(math.radians(i * 2))
points.append((x, y))
elif path_type == "字母A":
points.extend([
(center_x - size//2, center_y + size//2),
(center_x, center_y - size//2),
(center_x + size//2, center_y + size//2),
(center_x - size//4, center_y),
(center_x + size//4, center_y)
])
elif path_type == "字母B":
points.extend([
(center_x - size//2, center_y - size//2),
(center_x - size//2, center_y + size//2),
(center_x, center_y + size//2),
(center_x + size//4, center_y + size//4),
(center_x, center_y),
(center_x + size//4, center_y - size//4),
(center_x, center_y - size//2),
(center_x - size//2, center_y - size//2)
])
elif path_type == "数字8":
for i in range(0, 360, 10):
angle = math.radians(i)
x = center_x + size * 0.5 * math.sin(angle)
y = center_y + size * 0.3 * math.cos(angle) * (1 + 0.5 * math.sin(angle))
points.append((x, y))
elif path_type == "星形":
for i in range(8):
outer_angle = math.radians(45 * i)
inner_angle = math.radians(45 * i + 22.5)
points.append((
center_x + size * math.cos(outer_angle),
center_y + size * math.sin(outer_angle)
))
points.append((
center_x + size * 0.5 * math.cos(inner_angle),
center_y + size * 0.5 * math.sin(inner_angle)
))
points.append(points[0]) # 闭合形状
elif path_type == "花朵":
for i in range(0, 720, 5):
angle = math.radians(i)
r = size * (0.5 + 0.5 * math.sin(5 * angle))
x = center_x + r * math.cos(angle)
y = center_y + r * math.sin(angle)
points.append((x, y))
elif path_type == "迷宫":
# 简单的迷宫路径
points.extend([
(center_x - size//2, center_y - size//2),
(center_x - size//2, center_y + size//2),
(center_x - size//4, center_y + size//2),
(center_x - size//4, center_y + size//4),
(center_x + size//4, center_y + size//4),
(center_x + size//4, center_y - size//4),
(center_x - size//4, center_y - size//4),
(center_x - size//4, center_y - size//2),
(center_x + size//2, center_y - size//2),
(center_x + size//2, center_y + size//2),
(center_x + size//4, center_y + size//2),
(center_x + size//4, center_y + size//4)
])
elif path_type == "螺旋方":
for i in range(100):
t = i / 10
x = center_x + size * 0.5 * (math.cos(t) + math.cos(3*t)/3)
y = center_y + size * 0.5 * (math.sin(t) + math.sin(3*t)/3)
points.append((x, y))
elif path_type == "自定义":
points = self.playback_trajectory
# 开始路径运动
self.follow_path(points)
def follow_path(self, points):
"""沿着路径点运动"""
if not points:
return
# 开始绘图
self.robot.start_drawing()
# 移动到第一个点
self.robot.move_to(points[0][0], points[0][1])
self.robot.add_drawing_point(points[0][0], points[0][1])
# 设置动画
self.path_points = points
self.path_index = 1
self.follow_next_point()
def follow_next_point(self):
"""移动到下一个路径点"""
if self.path_index < len(self.path_points):
x, y = self.path_points[self.path_index]
# 检查是否需要避障
if self.avoid_var.get():
waypoints = self.robot.avoid_obstacles(self.robot.x, self.robot.y, x, y)
else:
waypoints = [(x, y)]
# 移动到每个路径点
self.follow_waypoints(waypoints, self.path_index)
else:
self.robot.stop_drawing()
self.log("路径运动完成")
def follow_waypoints(self, waypoints, path_index):
"""移动到一系列路径点"""
if not waypoints:
self.path_index += 1
self.follow_next_point()
return
# 获取下一个路径点
x, y = waypoints[0]
waypoints = waypoints[1:]
# 移动到该点
self.robot.move_to(x, y)
self.robot.add_drawing_point(x, y)
self.draw_workspace()
# 移动到下一个路径点
if waypoints:
self.after(50, lambda: self.follow_waypoints(waypoints, path_index))
else:
self.path_index += 1
self.after(50, self.follow_next_point)
def draw_workspace(self):
"""绘制工作区"""
self.canvas.delete("all")
# 绘制网格背景
self.draw_grid()
# 绘制障碍物
self.draw_obstacles()
# 绘制机器人
self.draw_robot()
# 绘制绘图路径
self.draw_drawing_path()
# 更新坐标显示
self.coord_var.set(f"末端坐标: ({self.robot.x:.1f}, {self.robot.y:.1f})")
angles = f"J1 {self.robot.joint1_angle:.1f}°, J2 {self.robot.joint2_angle:.1f}°, "
angles += f"J3 {self.robot.joint3_angle:.1f}°, J4 {self.robot.joint4_angle:.1f}°"
self.angle_var.set(f"关节角度: {angles}")
def draw_grid(self):
"""绘制网格背景"""
# 绘制网格
for x in range(0, 800, 50):
self.canvas.create_line(x, 0, x, 600, fill="#F0F0F0", dash=(1, 4))
for y in range(0, 600, 50):
self.canvas.create_line(0, y, 800, y, fill="#F0F0F0", dash=(1, 4))
# 绘制工作区边界
self.canvas.create_rectangle(50, 50, 750, 550, outline="#999", width=2, dash=(2, 2))
self.canvas.create_text(400, 30, text="工业机器人工作区", font=("Arial", 12, "bold"))
def draw_obstacles(self):
"""绘制障碍物"""
for obstacle in self.robot.obstacles:
x1, y1, x2, y2 = obstacle
self.canvas.create_rectangle(x1, y1, x2, y2, fill="#FF9999", outline="#FF0000", width=2)
self.canvas.create_text((x1+x2)/2, (y1+y2)/2, text="障碍物", fill="#990000")
def draw_robot(self):
"""绘制工业机器人"""
# 绘制底座
self.canvas.create_rectangle(
self.robot.base_x - 40, self.robot.base_y - 15,
self.robot.base_x + 40, self.robot.base_y + 15,
fill="#555", outline="#333", width=2
)
self.canvas.create_oval(
self.robot.base_x - 25, self.robot.base_y - 25,
self.robot.base_x + 25, self.robot.base_y + 25,
fill="#666", outline="#333", width=2
)
# 绘制关节1
self.canvas.create_line(
self.robot.base_x, self.robot.base_y,
self.robot.joint1_x, self.robot.joint1_y,
fill="#4A6FA5", width=14, capstyle=tk.ROUND
)
self.canvas.create_oval(
self.robot.joint1_x - 16, self.robot.joint1_y - 16,
self.robot.joint1_x + 16, self.robot.joint1_y + 16,
fill="#F0A202", outline="#333", width=2
)
# 绘制关节2
self.canvas.create_line(
self.robot.joint1_x, self.robot.joint1_y,
self.robot.joint2_x, self.robot.joint2_y,
fill="#4A6FA5", width=12, capstyle=tk.ROUND
)
self.canvas.create_oval(
self.robot.joint2_x - 14, self.robot.joint2_y - 14,
self.robot.joint2_x + 14, self.robot.joint2_y + 14,
fill="#F0A202", outline="#333", width=2
)
# 绘制关节3
self.canvas.create_line(
self.robot.joint2_x, self.robot.joint2_y,
self.robot.joint3_x, self.robot.joint3_y,
fill="#4A6FA5", width=10, capstyle=tk.ROUND
)
self.canvas.create_oval(
self.robot.joint3_x - 12, self.robot.joint3_y - 12,
self.robot.joint3_x + 12, self.robot.joint3_y + 12,
fill="#F0A202", outline="#333", width=2
)
# 绘制关节4
self.canvas.create_line(
self.robot.joint3_x, self.robot.joint3_y,
self.robot.joint4_x, self.robot.joint4_y,
fill="#4A6FA5", width=8, capstyle=tk.ROUND
)
self.canvas.create_oval(
self.robot.joint4_x - 10, self.robot.joint4_y - 10,
self.robot.joint4_x + 10, self.robot.joint4_y + 10,
fill="#F0A202", outline="#333", width=2
)
# 绘制末端执行器
self.canvas.create_line(
self.robot.joint4_x, self.robot.joint4_y,
self.robot.x, self.robot.y,
fill="#333", width=6, capstyle=tk.ROUND
)
# 绘制末端执行器（笔尖）
self.canvas.create_oval(
self.robot.x - 8, self.robot.y - 8,
self.robot.x + 8, self.robot.y + 8,
fill="#E71D36", outline="#333", width=2
)
# 如果正在绘图，绘制笔头
if self.robot.pen_down:
self.canvas.create_line(
self.robot.x, self.robot.y,
self.robot.x, self.robot.y + 25,
fill=self.robot.pen_color, width=self.robot.pen_width
)
self.canvas.create_oval(
self.robot.x - 6, self.robot.y + 25,
self.robot.x + 6, self.robot.y + 37,
fill=self.robot.pen_color, outline="#333", width=1
)
def draw_drawing_path(self):
"""绘制绘图路径"""
for segment in self.robot.drawing_path:
if len(segment["points"]) > 1:
# 根据样式设置dash参数
dash = None
if segment["style"] == "dash":
dash = (10, 5)
elif segment["style"] == "dot":
dash = (2, 4)
elif segment["style"] == "dashdot":
dash = (10, 5, 2, 5)
# 绘制线段
self.canvas.create_line(
segment["points"],
fill=segment["color"],
width=segment["width"],
dash=dash,
smooth=True
)
def animate(self):
"""动画循环"""
# 更新显示
self.draw_workspace()
self.after(50, self.animate)
def go_home(self):
"""归位"""
self.robot.set_angles(45, -30, 20, -10)
self.draw_workspace()
self.log("机器人已归位")
def clear_canvas(self):
"""清除画布"""
self.robot.drawing_path = []
self.draw_workspace()
self.log("画布已清除")
def update_speed(self, value):
"""更新速度"""
self.log(f"速度设置为: {float(value):.1f}")
def toggle_record_trajectory(self):
"""切换轨迹记录状态"""
self.recording = not self.recording
if self.recording:
self.playback_trajectory = []
self.record_btn.config(text="停止记录")
self.log("开始记录轨迹")
else:
self.record_btn.config(text="开始记录轨迹")
self.log(f"停止记录轨迹，共记录 {len(self.playback_trajectory)} 个点")
def playback_trajectory(self):
"""回放轨迹"""
if not self.playback_trajectory:
self.log("没有可回放的轨迹")
return
# 设置路径为自定义
self.selected_path.set("自定义")
self.start_path()
def save_trajectory(self):
"""保存轨迹到文件"""
if not self.playback_trajectory:
self.log("没有轨迹可保存")
return
try:
file_path = filedialog.asksaveasfilename(
defaultextension=".csv",
filetypes=[("CSV文件", "*.csv"), ("所有文件", "*.*")]
)
if file_path:
with open(file_path, "w", newline="") as f:
writer = csv.writer(f)
writer.writerow(["X", "Y"])
for x, y in self.playback_trajectory:
writer.writerow([x, y])
self.log(f"轨迹已保存到 {file_path}")
except Exception as e:
self.log(f"保存轨迹失败: {str(e)}")
def load_trajectory(self):
"""从文件加载轨迹"""
try:
file_path = filedialog.askopenfilename(
filetypes=[("CSV文件", "*.csv"), ("所有文件", "*.*")]
)
if file_path:
self.playback_trajectory = []
with open(file_path, "r") as f:
reader = csv.reader(f)
next(reader) # 跳过标题行
for row in reader:
if len(row) >= 2:
self.playback_trajectory.append((float(row[0]), float(row[1])))
self.log(f"从文件加载了 {len(self.playback_trajectory)} 个轨迹点")
except Exception as e:
self.log(f"加载轨迹失败: {str(e)}")
def clear_trajectory(self):
"""清除轨迹"""
self.playback_trajectory = []
self.log("轨迹已清除")
def save_drawing(self):
"""保存绘图为图片"""
try:
file_path = filedialog.asksaveasfilename(
defaultextension=".png",
filetypes=[("PNG图片", "*.png"), ("JPEG图片", "*.jpg"), ("所有文件", "*.*")]
)
if file_path:
# 创建画布的PostScript表示
self.canvas.postscript(file=file_path + ".eps", colormode="color")
# 转换为PNG
img = Image.open(file_path + ".eps")
img.save(file_path, "png")
os.remove(file_path + ".eps")
self.log(f"绘图已保存为 {file_path}")
except Exception as e:
self.log(f"保存绘图失败: {str(e)}")
def load_drawing(self):
"""加载绘图"""
try:
file_path = filedialog.askopenfilename(
filetypes=[("图片文件", "*.png;*.jpg;*.jpeg"), ("所有文件", "*.*")]
)
if file_path:
# 清除当前绘图
self.robot.drawing_path = []
# 加载图片
img = Image.open(file_path)
img = img.resize((800, 600))
self.photo_img = ImageTk.PhotoImage(img)
# 在画布上显示图片
self.canvas.create_image(0, 0, image=self.photo_img, anchor="nw")
self.log(f"已加载绘图: {os.path.basename(file_path)}")
except Exception as e:
self.log(f"加载绘图失败: {str(e)}")
def log(self, message):
"""记录日志"""
self.log_text.config(state="normal")
timestamp = time.strftime("%H:%M:%S")
self.log_text.insert(tk.END, f"[{timestamp}] {message}\n")
self.log_text.see(tk.END)
self.log_text.config(state="disabled")
def add_obstacle(self):
"""添加障碍物"""
# 随机位置添加障碍物
x = random.randint(100, 700)
y = random.randint(100, 500)
size = random.randint(50, 100)
self.robot.add_obstacle(x, y, x+size, y+size)
self.draw_workspace()
self.log(f"添加障碍物: ({x}, {y}) - ({x+size}, {y+size})")
def clear_obstacles(self):
"""清除所有障碍物"""
self.robot.clear_obstacles()
self.draw_workspace()
self.log("所有障碍物已清除")
if __name__ == "__main__":
root = tk.Tk()
root.title("工业机器人仿真系统 - 绘图功能")
root.geometry("1200x800")
# 设置主题
style = ttk.Style()
style.theme_use("clam")
style.configure("TButton", padding=6)
style.configure("TFrame", background="#F5F5F5")
# 设置字体
default_font = ("Microsoft YaHei", 9)
root.option_add("*Font", default_font)
app = SimulationFrame(root)
app.pack(fill="both", expand=True)
root.mainloop()开始路径无法画出正常的路径图也不进行避障，要 能手动设置障碍物，机械臂只要一个可以活动的关节,使用纯python
本文介绍了如何使用Python的math.atan2函数计算线段之间的角度，特别是在空间分析问题中。作者遇到的问题是，由于线段顶点顺序的不同，导致计算出的角度值也不同。他们寻求一种方法，能确保无论线段顺序如何，都能得到0到180度范围内的正值。
最低0.47元/天 解锁文章
扫一扫
762
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
