while ||Δp|| > ε and iter < max_iters do
      1. 计算当前末端位置 p_e
      2. 计算合力 F = F_att + F_rep
      3. 计算雅可比矩阵 J(q)
      4. 求阻尼伪逆 J⁺ = J^T(JJ^T + λ²I)^-1
      5. 更新关节角度：q ← q + αJ⁺F
      6. 可视化检测
   end while

clc; clear; close all;

% 机械臂参数
L1 = 1; L2 = 1; L3 = 0.5;
q = [0; 0; 0];                % 初始关节角度
q_goal = [pi/4; pi/6; -pi/6]; % 目标关节角度

% 计算目标末端位置（笛卡尔空间）
target_x = L1*cos(q_goal(1)) + L2*cos(q_goal(1)+q_goal(2)) + L3*cos(q_goal(1)+q_goal(2)+q_goal(3));
target_y = L1*sin(q_goal(1)) + L2*sin(q_goal(1)+q_goal(2)) + L3*sin(q_goal(1)+q_goal(2)+q_goal(3));
target = [target_x; target_y];

% 势场参数
k_att = 1; 
k_rep = 0.1; 
d0 = 0.5;

% 障碍物位置（笛卡尔坐标）
obstacle = [1.2, 1.2];

% 迭代参数
max_iters = 200;
alpha = 0.05;     % 步长因子
lambda = 0.05;   % 阻尼因子（关键修改）

% GIF 文件设置
gif_filename = 'robot_arm_motion.gif';
delay_time = 0.1;  % 每帧间隔时间

figure;
for iter = 1:max_iters
    % 计算各关节位置
    x0 = 0; y0 = 0;
    x1 = L1*cos(q(1));
    y1 = L1*sin(q(1));
    x2 = x1 + L2*cos(q(1)+q(2));
    y2 = y1 + L2*sin(q(1)+q(2));
    x_end = x2 + L3*cos(q(1)+q(2)+q(3));
    y_end = y2 + L3*sin(q(1)+q(2)+q(3));
    end_effector = [x_end; y_end];

    % 计算吸引力（目标势场梯度）
    F_att = -k_att * (end_effector - target);

    % 计算斥力（障碍物势场梯度）
    d_obs = norm(end_effector - obstacle');
    if d_obs < d0
        F_rep = k_rep * (1/d_obs - 1/d0) * (1/d_obs^3) * (end_effector - obstacle');
    else
        F_rep = [0; 0];
    end

    % 总合力
    F = F_att + F_rep;

    % 计算雅可比矩阵 J (2x3)（基于当前关节角度 q）
    J = [ -L1*sin(q(1)) - L2*sin(q(1)+q_goal(2)) - L3*sin(q(1)+q_goal(2)+q_goal(3)), ...
       -L2*sin(q(1)+q_goal(2)) - L3*sin(q(1)+q_goal(2)+q_goal(3)), ...
       -L3*sin(q(1)+q_goal(2)+q_goal(3));
        L1*cos(q(1)) + L2*cos(q(1)+q_goal(2)) + L3*cos(q(1)+q_goal(2)+q_goal(3)), ...
        L2*cos(q(1)+q_goal(2)) + L3*cos(q(1)+q_goal(2)+q_goal(3)), ...
        L3*cos(q(1)+q_goal(2)+q_goal(3))];

    % 伪逆计算（带SVD稳定性处理）✅
    [U,S,V] = svd(J*J' + lambda^2*eye(2));
    inv_term = V * diag(1./(diag(S)+eps)) * U'; % 添加eps避免除以零
    dq = alpha * J' * inv_term * F;
    % 更新关节角度
    q = q + dq;


    % 可视化机械臂和环境
    clf;
    hold on;
    plot([x0, x1, x2, x_end], [y0, y1, y2, y_end], 'ro-', 'LineWidth',2);
    plot(obstacle(1), obstacle(2), 'ks', 'MarkerSize', 10, 'MarkerFaceColor', 'k');
    plot(target(1), target(2), 'g*', 'MarkerSize', 10);
    axis equal;
    axis([-1 3 -1 3]);
    grid on;
    title(sprintf('Iteration: %d', iter));
    drawnow;

    % 保存 GIF
    frame = getframe(gcf);
    img = frame2im(frame);
    [A, map] = rgb2ind(img, 256);
    if iter == 1
        imwrite(A, map, gif_filename, 'gif', 'LoopCount', Inf, 'DelayTime', delay_time);
    else
        imwrite(A, map, gif_filename, 'gif', 'WriteMode', 'append', 'DelayTime', delay_time);
    end

    pause(0.1);

    % 判断是否已接近目标
    if norm(end_effector - target) < 0.01
        break;
    end
end

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from matplotlib.animation import PillowWriter
import matplotlib.patches as patches
import time

class ThreeLinkArm:
    def __init__(self, link_lengths=[1.2, 1.0, 0.8]):
        """初始化三连杆机械臂
        Args:
            link_lengths: 三个连杆的长度列表，默认为[1.2, 1.0, 0.8]
        """
        self.link_lengths = link_lengths
        self.num_links = len(link_lengths)  # 连杆数量
        self.joints = np.zeros((self.num_links + 1, 2))  # 存储所有关节的坐标，包括基座和末端
        self.angles = np.zeros(self.num_links)  # 存储各关节角度
        self.update_joints()

    def update_joints(self):
        """根据关节角度更新各关节位置"""
        self.joints[0] = [0, 0]  # 基座位置
        cumulative_angle = 0
        for i in range(self.num_links):
            cumulative_angle += self.angles[i]
            x = self.joints[i][0] + self.link_lengths[i] * np.cos(cumulative_angle)
            y = self.joints[i][1] + self.link_lengths[i] * np.sin(cumulative_angle)
            self.joints[i+1] = [x, y]

    def get_end_effector(self):
        """获取末端执行器位置"""
        return self.joints[-1]

    def set_angles(self, angles):
        """设置关节角度"""
        self.angles = np.array(angles)
        self.update_joints()

    def forward_kinematics(self, angles):
        """前向运动学：给定角度计算末端位置"""
        x = 0
        y = 0
        cumulative_angle = 0
        for i in range(self.num_links):
            cumulative_angle += angles[i]
            x += self.link_lengths[i] * np.cos(cumulative_angle)
            y += self.link_lengths[i] * np.sin(cumulative_angle)
        return np.array([x, y])

    def jacobian(self):
        """计算雅可比矩阵"""
        J = np.zeros((2, self.num_links))
        for i in range(self.num_links):
            x_partial = 0
            y_partial = 0
            for j in range(i, self.num_links):
                angle_sum = np.sum(self.angles[:j+1])
                x_partial -= self.link_lengths[j] * np.sin(angle_sum)
                y_partial += self.link_lengths[j] * np.cos(angle_sum)
            J[0, i] = x_partial
            J[1, i] = y_partial
        return J

def artificial_potential_field(arm, target, obstacles, k_att=1.0, k_rep=10.0, d0=0.5):
    """计算基于人工势场的合力
    Args:
        arm: 机械臂对象
        target: 目标位置坐标 [x, y]
        obstacles: 障碍物列表，每个元素为 [x, y, radius]
        k_att: 引力系数
        k_rep: 斥力系数
        d0: 障碍物影响阈值距离
    Returns:
        total_force: 合力向量 [Fx, Fy]
    """
    current_pos = arm.get_end_effector()

    # 计算引力：与目标点的距离成正比
    att_vec = k_att * (np.array(target) - current_pos)

    # 计算斥力：当距离障碍物小于阈值d0时产生
    rep_vec = np.zeros(2)
    for obs in obstacles:
        obs_pos = np.array(obs[:2])
        obs_radius = obs[2]
        # 计算到障碍物边缘的距离向量
        dist_vec = current_pos - obs_pos
        # `np.linalg.norm(dist_vec)` 计算这个向量的模长（欧几里得距离）
        # `- obs_radius` 减去障碍物的半径
        dist = np.linalg.norm(dist_vec) - obs_radius # 到障碍物边缘的实际距离
        dist = max(dist, 0.01)  # 防止距离为0导致除法错误

        if dist < d0:  # 只有在影响范围内才计算斥力
            rep_magnitude = k_rep * (1/dist - 1/d0) * (1/dist**2)
            rep_vec += rep_magnitude * (dist_vec/dist)

    total_force = att_vec + rep_vec
    return total_force


def plan_path(arm, start_angles, target_position, obstacles, max_steps=2000, dt=0.01):
    """使用人工势场法规划路径
    Args:
        arm: 机械臂对象
        start_angles: 初始关节角度
        target_position: 目标位置 [x, y]
        obstacles: 障碍物列表
        max_steps: 最大迭代步数
        dt: 时间步长
    Returns:
        path: 关节角度路径列表
        positions: 末端执行器位置列表
    """
    arm.set_angles(start_angles)

    path = [arm.angles.copy()]
    positions = [arm.get_end_effector().copy()]

    for step in range(max_steps):
        current_pos = arm.get_end_effector()
        # 检查是否到达目标（末端位置与目标距离小于阈值）
        if np.linalg.norm(np.array(target_position) - current_pos) < 0.05:
            print(f"目标达成！在第 {step} 步")
            break

        # 计算人工势场力
        force = artificial_potential_field(arm, target_position, obstacles)
        # 通过雅可比矩阵将末端力转换为关节角度变化
        J = arm.jacobian()
        try:
            J_inv = np.linalg.pinv(J) # 计算雅可比矩阵的伪逆（Moore-Penrose伪逆），用于将末端力转换为关节力矩
            angle_changes = J_inv @ force # 将末端力转换为关节角度变化量（Δθ = α * J⁺ * F，α为缩放因子）
            angle_changes = np.clip(angle_changes, -0.1, 0.1) # 对关节角度变化量进行限幅，防止单步变化过大（数值稳定性）
            arm.angles += dt * angle_changes # 根据时间步长 dt 更新关节角度（θ_{k+1} = θ_k + dt * Δθ）
            arm.update_joints() # 更新机械臂关节的几何位置（前向运动学）

            path.append(arm.angles.copy()) # 记录当前关节角度和末端位置，用于路径可视化
            positions.append(arm.get_end_effector().copy())
        except np.linalg.LinAlgError:
            print("雅可比矩阵求逆出错，可能处于奇异点") # 处理雅可比矩阵奇异的情况（如机械臂处于奇异构型）
            break

    return path, positions

def visualize_arm_motion(arm, path, positions, obstacles, target, save_animation=False):
    """可视化机械臂运动过程
    Args:
        arm: 机械臂对象
        path: 关节角度路径列表
        positions: 末端执行器位置列表
        obstacles: 障碍物列表
        target: 目标位置
        save_animation: 是否保存动画为GIF文件
    """
    # 创建画布和坐标轴
    fig, ax = plt.subplots(figsize=(10, 8))

    # 设置坐标轴范围和网格
    ax.set_xlim(-1, 4)
    ax.set_ylim(-1, 2)
    ax.set_aspect('equal')  # 保持横纵比例相等
    ax.grid(True)

    # 绘制目标点（绿色圆形）
    target_point = plt.Circle(target, 0.1, color='green', alpha=0.7)
    ax.add_patch(target_point)
    ax.text(target[0]+0.1, target[1]+0.1, "目标", fontsize=12)

    # 绘制障碍物（红色圆形）
    for obs in obstacles:
        obstacle = plt.Circle(obs[:2], obs[2], color='red', alpha=0.5)
        ax.add_patch(obstacle)

    # 绘制末端执行器的运动轨迹
    positions_arr = np.array(positions)
    ax.plot(positions_arr[:, 0], positions_arr[:, 1], 'b--', alpha=0.3, label='末端轨迹')

    # 创建机械臂连杆和关节的绘图对象
    arm_links = [plt.Line2D([], [], color='black', lw=3) for _ in range(arm.num_links)]
    arm_joints = [plt.Circle((0, 0), 0.05, color='blue', zorder=3) for _ in range(arm.num_links + 1)]

    for link in arm_links:
        ax.add_line(link)
    for joint in arm_joints:
        ax.add_patch(joint)

    end_effector = plt.Circle((0, 0), 0.08, color='purple', zorder=3)
    ax.add_patch(end_effector)

    # 添加文本信息
    time_text = ax.text(0.02, 0.95, '', transform=ax.transAxes)
    angle_text = ax.text(0.02, 0.90, '', transform=ax.transAxes)

    ax.legend(loc='upper right')
    ax.set_title('三连杆机械臂人工势场法路径规划')

    def init():
        for link in arm_links:
            link.set_data([], [])
        return arm_links + arm_joints + [end_effector, time_text, angle_text]

    def animate(i):
        arm.set_angles(path[i])
        # 更新连杆数据
        for j in range(arm.num_links):
            x_data = [arm.joints[j][0], arm.joints[j+1][0]]
            y_data = [arm.joints[j][1], arm.joints[j+1][1]]
            arm_links[j].set_data(x_data, y_data)
        # 更新关节位置
        for j in range(arm.num_links + 1):
            arm_joints[j].center = arm.joints[j]
        end_effector.center = arm.joints[-1]
        time_text.set_text(f'步骤: {i}/{len(path)-1}')
        angle_text.set_text(f'角度: [{arm.angles[0]:.2f}, {arm.angles[1]:.2f}, {arm.angles[2]:.2f}]')
        return arm_links + arm_joints + [end_effector, time_text, angle_text]

    ani = FuncAnimation(fig, animate, frames=len(path), init_func=init, interval=20, blit=True) 


    if save_animation:
        ani.save('arm_motion.gif', writer=PillowWriter(fps=30))  # 使用 PillowWriter 保存为 GIF

    plt.tight_layout()
    plt.show()

    return ani

def main():
    """主函数：设置参数并运行仿真"""
    # 设置中文字体
    plt.rcParams['font.sans-serif'] = ['SimSun']

    # 创建机械臂实例
    arm = ThreeLinkArm(link_lengths=[1.2, 1.0, 0.8])

    # 设置初始角度（所有关节角度为0）
    start_angles = [0.0, 0.0, 0.0]

    # 设置目标位置坐标
    target = [2, 1.5]

    # 设置障碍物位置和大小：[x坐标, y坐标, 半径]
    obstacles = [
        [0.7, 0.3, 0.1],
        [0.8, 1.2, 0.2],
        [1.2, 0.8, 0.2],
        [2.5, 0.5, 0.1]
    ]

    print("正在使用人工势场法规划路径...")
    path, positions = plan_path(arm, start_angles, target, obstacles, max_steps=2000, dt=0.01)

    print("正在可视化机械臂运动...")
    visualize_arm_motion(arm, path, positions, obstacles, target, save_animation=True)

    print("完成！")

if __name__ == "__main__":
    main()