乐白帮助

位置和姿态

正解

cpose = 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"])

反解

jpose = kinematics_inverse(vector, joints)

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

  • 参数
    • vector。工具空间的位置和姿态。
    • joints。关节空间参考位置。可选，默认为当前反馈关节位置。当反解出现多解时，会取距离 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"])

位姿特征坐标系转换

c = pose_times(a, b)

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

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

  • 参数
    • a。位姿 AA。
    • b。位姿 BB。
  • 返回
    • c。位姿 C=ABC = 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)

位姿的逆

c = pose_inverse(a)

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

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

  • 参数
    • a。位姿 AA。
  • 返回
    • c。AA 的逆 A−1A^{-1}。
示例程序
  • 已知用户坐标系位姿 a（可以为某个示教点），然后示教出另一个点 b，用该方法可以求出 b 相对于 a 的描述。
  • 计算出该结果之后，当用户坐标系发生改变，但示教点相对位置不变时，无需重新示教。
  • AX=B→X=A−1BAX=B \to X=A^{-1}B
function axb(a, b)
  return pose_times(pose_inverse(a), b)
end

预落杯 = {-0.159,-0.342,-0.0391,-2.97,-0.017,-3.14}
落杯 = {-0.044,-0.0036,-0.0004,2.89,0,0}
local 相对落杯 = axb(预落杯, 落杯)
print(pose_times(预落杯, 相对落杯))
print(落杯)

位姿的加 3.1.13

pose = pose_add(base, delta, frame)

从base位置，往frame方向移动delta后的位姿

  • 参数
    • base。起始位姿。
    • delta。位姿偏移量。
    • frame。位姿偏移量方向，仅姿态部分有效。可空，默认基座方向
  • 返回
    • pose。移动后的位姿。
示例程序
base = {-0.4,0,0.1,-1.57,0,1.57}
frame = {0,0,0,0,-0.78,0} -- 绕y轴旋转45°，使z轴斜向上
delta = {0,0,0.1,0,0,0} -- 沿z轴方向移动0.1m
pose = pose_add(base, delta, frame)
movej(base, 0.4, 0.1)
movel(pose, 0.4, 0.1)