数学和机器人学

kinematics_forward

正解。

kinematics_forward(joints)

通过机器人正向运动学将关角度转成笛卡尔位置和姿态。

• 参数

  • joints 关节角度：{j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14}

• 返回 {x, y, z, Rz, Ry, Rx, ok=true}

  • 如 {-0.36435443592898153, 0.25548551781114875, 0.1477188562117689, 0.27649950521632694, 0.913569441486944, -2.9826903521006582, ok=true}
  • ok解算是否成功 true：成功 false：失败

示例程序

p = kinematics_forward({j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14})
for k,v in ipairs(p) do
  print(k,v)
end
print(p["ok"])

kinematics_inverse

反解。

kinematics_inverse(vector, joints)

通过机器人逆向运动学反解，将笛卡尔位置和姿态转换成关节角度。计算结果与当前 TCP 设置和当前关节位置有关。

• 参数

  • vector。工具空间的位置和姿态。
  • joints 2.3.5。关节空间参考位置。可选，默认为当前反馈关节位置。当反解出现多解时，会取距离 joints 最近的一个解。

• 返回 {j1=a, j2=b, j3=c, j4=d, j5=e, j6=f, ok=true}

  • 对应关节空间 如 {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14, ok=true}
  • ok解算是否成功 true：成功 false：失败

示例程序

p = kinematics_inverse({1.12,2.12,3.12,4.12,.125,6.12})
for k,v in ipairs(p) do
  print(k,v)
end
print(p["ok"])

pose_times 2.1

位姿乘法。

pose_times(a, b)

算法分别将 a 和 b 转为 4×4 的齐次矩阵 $A$ 和 $B$，然后求矩阵乘法 $C=AB$，最后将 $C$ 转为位姿表示法返回。

物理意义相当于，以 a 为用户坐标系 $\{A\}$，b 是相对于坐标系 $\{A\}$ 的位姿描述。最后返回相对于机器人世界坐标系的位姿描述，该结果可用于 move 系指令。

• 参数

  • a。位姿 $A$。
  • b。位姿 $B$。

• 返回

  • c。位姿 $C=AB$。

示例程序

local res = pose_times({-0.159,-0.342,-0.0391,-2.97,-0.017,-3.14}, {-0.044,-0.0036,-0.0004,3.89,0,0})
movej(res, 1, 1, 0, 1)

pose_inverse 2.1

位姿的逆。

pose_inverse(a)

用于求位姿 a 对应的齐次矩阵 $A$，$A$ 的逆的位姿描述。可用于求解位姿方程。

齐次矩阵的逆等于其转置 $A^{-1}=A^T$ 。

• 参数

  • a。位姿 $A$。

• 返回

  • c。$A$ 的逆 $A^{-1}$。

示例程序

• 已知用户坐标系位姿 a（可以为某个示教点），然后示教出另一个点 b，用该方法可以求出 b 相对于 a 的描述。
• 计算出该结果之后，当用户坐标系发生改变，但示教点相对位置不变时，无需重新示教。
• $AX=B\to X=A^{-1}B$

function axb(a, b)
  return pose_times(pose_inverse(a), b)
end

local 相对落杯 = axb(预落杯, 落杯)
相对落杯.base = a
movej(相对落杯, 1, 1, 0, 1)