抓取伪代码.md

伪代码描述：

1. 初始化参数： 设置学习率，角度转换常数等。 加载手指的工作空间点。 定义球的半径和中心位置。 加载并构建机械手指的模型。 设置手指关节的初始位置和角度范围。

2. 生成候选接触点： 在球的表面上生成候选接触点。 将候选接触点划分到三个象限。

3. 检查候选接触点是否在手指的工作空间内： 使用 alphaShape 检查每个象限的候选接触点是否在对应手指的工作空间内。 初始化三个可达的接触点集合：reachable_points1, reachable_points2, reachable_points3。

4. 确保形成稳定的抓取三角形： 设置稳定阀值。 遍历所有可能的接触点组合。 检查每个组合的接触点是否共线。 计算每个组合的接触点质心与球心之间的距离。 更新最小距离和最佳接触点组合。 如果找到稳定的抓取三角形，则输出最佳接触点组合和质心与球心之间的距离。

5. 显示结果并绘图： 绘制球和所有候选接触点。 标记出最佳接触点组合。 绘制机械臂初始位姿及末端姿态。

6. 求解逆运动学： 初始化逆运动学求解标志位。 使用反向运动学算法计算每个手指的关节角度，使其末端位置达到最佳接触点。 迭代计算直到所有手指都到达目标位置或者达到最大迭代次数。

7. 再次绘制机器人保持图像： 标记出最佳接触点。 绘制球和机械臂末端姿态。

辅助函数： - iscollinear(p1, p2, p3): 检查三个点是否共线。 - is_point_in_workspace(dt, point): 检查点是否在工作空间的凸包内。 - find_reachable_points_dt(points_sector, dt): 使用 Delaunay 三角剖分检查点是否在工作空间内。 - find_reachable_points_shp(points_sector, shp): 使用 alphaShape 检查点是否在工作空间内。