华数杯A题一至四问够用，一键式运行【代码】

第一问：
import numpy as np
import matplotlib.pyplot as plt
from deap import base, creator, tools, algorithms

# 定义D-H参数
a = [0, 300, 1200, 300, 0, 0]
alpha = [0, -np.pi/2, 0, -np.pi/2, -np.pi/2, -np.pi/2]
d = [600, 0, 0, 1200, 0, 0]
theta_init = [0, -np.pi/2, 0, np.pi, -np.pi/2, 0]

# 目标点位置
target_position = np.array([1500, 1200, 200])

# 计算齐次变换矩阵
def dh_transform(a, alpha, d, theta):
    return np.array([
        [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)],
        [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)],
        [0, np.sin(alpha), np.cos(alpha), d],
        [0, 0, 0, 1]
    ])

# 计算末端位置
def forward_kinematics(joint_angles):
    T = np.eye(4)
    for i in range(6):
        T_i = dh_transform(a[i], alpha[i], d[i], joint_angles[i])
        T = np.dot(T, T_i)
    return T[:3, 3]

# 计算末端误差
def fitness_function(joint_angles):
    end_position = forward_kinematics(joint_angles)
    error = np.linalg.norm(end_position - target_position)
    return (error,)

# 遗传算法参数
creator.create("FitnessMin", base.Fitness, weights=(-1.0,))
creator.create("Individual", list, fitness=creator.FitnessMin)

toolbox = base.Toolbox()
toolbox.register("attr_float", np.random.uniform, -np.pi, np.pi)
toolbox.register("individual", tools.initRepeat, creator.Individual, toolbox.attr_float, n=6)
toolbox.register("population", tools.initRepeat, list, toolbox.individual)
toolbox.register("evaluate", fitness_function)
toolbox.register("mate", tools.cxBlend, alpha=0.5)  # 交叉概率参数alpha
toolbox.register("mutate", tools.mutGaussian, mu=0, sigma=0.1, indpb=0.2)  # 变异概率参数mu, sigma, indpb
toolbox.register("select", tools.selTournament, tournsize=3)  # 锦标赛选择参数tournsize

# 参数设置
population = toolbox.population(n=50)  # 种群大小
NGEN = 50  # 代数
CXPB, MUTPB = 0.5, 0.2  # 交叉概率和变异概率

# 遗传算法主循环
for gen in range(NGEN):
    # 评估当前种群中所有个体的适应度
    invalid_ind = [ind for ind in population if not ind.fitness.valid]
    fitnesses = map(toolbox.evaluate, invalid_ind)
    for ind, fit in zip(invalid_ind, fitnesses):
        ind.fitness.values = fit
    
    # 选择下一代个体
    offspring = toolbox.select(population, len(population))
    offspring = list(map(toolbox.clone, offspring))
    
    # 进行交叉和变异操作
    for child1, child2 in zip(offspring[::2], offspring[1::2]):
        if np.random.rand() < CXPB:
            toolbox.mate(child1, child2)
            del child1.fitness.values
            del child2.fitness.values

    for mutant in offspring:
        if np.random.rand() < MUTPB:
            toolbox.mutate(mutant)
            del mutant.fitness.values
    
    # 评估新生成个体的适应度
    invalid_ind = [ind for ind in offspring if not ind.fitness.valid]
    fitnesses = map(toolbox.evaluate, invalid_ind)
    for ind, fit in zip(invalid_ind, fitnesses):
        ind.fitness.values = fit

    population[:] = offspring

    # 评估每一代的最佳个体
    best_individual = tools.selBest(population, 1)[0]
    print(f"Generation {gen}: Best fitness: {best_individual.fitness.values[0]}")

# 找到最佳个体
best_individual = tools.selBest(population, 1)[0]

# 检查并打印最佳个体及其适应度
print("Best individual:", best_individual)
print("Best fitness:", best_individual.fitness.values if best_individual.fitness.values else "No fitness values")

# 可视化结果
end_position = forward_kinematics(best_individual)
plt.scatter(target_position[0], target_position[1], c='r', label='Target Position')
plt.scatter(end_position[0], end_position[1], c='b', label='End Position')
plt.xlabel('X Position (mm)')
plt.ylabel('Y Position (mm)')
plt.legend()
plt.title('Target vs End Position')
plt.grid(True)
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from deap import base, creator, tools, algorithms

# 定义D-H参数
a = [0, 300, 1200, 300, 0, 0]
alpha = [0, -np.pi/2, 0, -np.pi/2, -np.pi/2, -np.pi/2]
d = [600, 0, 0, 1200, 0, 0]
theta_init = [0, -np.pi/2, 0, np.pi, -np.pi/2, 0]

# 目标点位置
target_position = np.array([1500, 1200, 200])

# 计算齐次变换矩阵
def dh_transform(a, alpha, d, theta):
    return np.array([
        [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)],
        [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)],
        [0, np.sin(alpha), np.cos(alpha), d],
        [0, 0, 0, 1]
    ])

# 计算末端位置
def forward_kinematics(joint_angles):
    T = np.eye(4)
    for i in range(6):
        T_i = dh_transform(a[i], alpha[i], d[i], joint_angles[i])
        T = np.dot(T, T_i)
    return T[:3, 3]

# 计算末端误差
def fitness_function(joint_angles):
    end_position = forward_kinematics(joint_angles)
    error = np.linalg.norm(end_position - target_position)
    return (error,)

# 遗传算法参数
creator.create("FitnessMin", base.Fitness, weights=(-1.0,))
creator.create("Individual", list, fitness=creator.FitnessMin)

toolbox = base.Toolbox()
toolbox.register("attr_float", np.random.uniform, -np.pi, np.pi)
toolbox.register("individual", tools.initRepeat, creator.Individual, toolbox.attr_float, n=6)
toolbox.register("population", tools.initRepeat, list, toolbox.individual)
toolbox.register("evaluate", fitness_function)
toolbox.register("mate", tools.cxBlend, alpha=0.5)  # 交叉概率参数alpha
toolbox.register("mutate", tools.mutGaussian, mu=0, sigma=0.1, indpb=0.2)  # 变异概率参数mu, sigma, indpb
toolbox.register("select", tools.selTournament, tournsize=3)  # 锦标赛选择参数tourn