#AEAI04008. 低切换代价的逆解选择

低切换代价的逆解选择

题目描述

多关节机械臂从当前姿态切换到某个逆解候选时,每个关节都有速度限制。给出当前关节值、各关节速度上限、控制周期,以及若干逆解候选的姿态误差和安全距离,请选择一个安全且切换代价最低的候选。

候选必须满足 pose_err <= eps_errsafety >= safety_min。对每个通过筛选的候选,设各关节差值为 diff[j] = |q[j] - cur[j]|,所需周期数为:

max(1, max(ceil(diff[j] / (vlim[j] * period))))

排序时依次比较周期数、关节差值总和、最大单关节差值、编号。

输入格式

第一行输入关节数 J 和候选数 M

第二行输入 J 个当前关节值。

第三行输入 J 个关节速度上限。

第四行输入 period eps_err safety_min

接下来 M 行,每行输入:

id pose_err safety q1 q2 ... qJ

输出格式

若存在可用候选,输出:

id total_diff max_diff cycles

其中 total_diffmax_diff 保留 6 位小数。

若没有可用候选,输出 NO_SAFE_IK

判定规则

使用标准输出比较。姿态误差和安全距离筛选都包含边界;排序关键字固定为周期数、总差值、最大差值、编号。

数据范围与测试提示

  • 正式测试会覆盖样例没有出现的边界输入,请不要依赖样例规模或样例顺序写特判。
  • 输出只包含题目要求的字段,不要输出调试信息;浮点结果按题面要求保留 6 位小数。
  • 安全距离和姿态误差筛选都包含等号边界,隐藏数据会检查刚好满足阈值的候选。
  • 排序关键字必须依次为周期数、总差值、最大差值、编号,不能只比较总差值。
  • 无安全逆解输出 NO_SAFE_IK;周期数至少为 1,误差和安全距离等于阈值时仍可用。

样例

输入样例 1

2 2
0 0
10 10
0.1 0.2 0.5
1 0.1 0.8 1 1
2 0.1 0.9 2 0

输出样例 1

1 2.000000 1.000000 1

输入样例 2

2 2
0 0
1 1
1 0.1 0.5
1 0.2 0.9 1 1
2 0.05 0.1 0 0

输出样例 2

NO_SAFE_IK