位置和姿态

估算位姿的灵活性 3.1.36

在临近奇异位置和工作空间边界的地方，灵活性接近于0。

通常，严格条件下小于0.001视为灵活性差，宽松条件下小于0.0001视为灵活性差。

• 函数名：lua模块的函数measure_manipulation
• 参数：
  1. joints: [JointPose] 关节角度（弧度），如 {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14}
• 返回：[float] 灵活性，0~1，越大越灵活

示例程序

cpose = {-0.3, -0.3, 0.3, 0, 0, 0}
jpose = kinematics_inverse(cpose)
if not jpose["ok"] then
  error("kinematics_inverse failed")
end
manipulation = measure_manipulation(jpose)
print(manipulation)
if manipulation < 0.001 then
  error("manipulation too low")
end
movel(jpose, 0.1, 0.1, 0, 0)

正解

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

• 函数名：lua模块的函数kinematics_forward
• 参数：
  1. joints: [JointPose] 关节角度（弧度），如 {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14}
• 返回：[KinResult] 笛卡尔位置和姿态，格式为 {x, y, z, Rz, Ry, Rx, ok}，如 {-0.364354, 0.255486, 0.147719, 0.276500, 0.913569, -2.982690, 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"])

反解

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

• 函数名：lua模块的函数kinematics_inverse
• 参数：
  1. vector: [CartesianPose] 工具空间的位置和姿态
  2. joints: [JointPose] 关节空间参考位置。可选，默认为当前反馈关节位置。当反解出现多解时，会取距离 joints 最近的一个解
• 返回：[KinResult] 关节位置，格式为 {j1=a, j2=b, j3=c, j4=d, j5=e, j6=f, ok}，如 {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, 0.125, 6.12})
for k, v in ipairs(p) do
  print(k, v)
end
print(p["ok"])

位姿特征坐标系转换

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

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

• 函数名：lua模块的函数pose_times
• 参数：
  1. a: [Pose] 位姿 $A$
  2. b: [Pose] 位姿 $B$
• 返回：[CartesianPose] 位姿 $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)

位姿的逆

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

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

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

• 函数名：lua模块的函数pose_inverse
• 参数：
  1. a: [Pose] 位姿 $A$
• 返回：[CartesianPose] $A$ 的逆 $A^{-1}$

示例程序

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

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

• 函数名：lua模块的函数pose_add
• 参数：
  1. base: [Pose] 起始位姿
  2. delta: [CartesianPose] 位姿偏移量
  3. frame: [CartesianPose] 位姿偏移量方向，仅姿态部分有效。可选，默认基座方向
• 返回：[CartesianPose] 移动后的位姿

示例程序

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)

LuaApi位姿转为SDK位姿 3.1.24

将LuaApi位姿转为SDK位姿。

• 函数名：lua模块的函数sdk_pose
• 参数：
  1. p: [Pose] Lua API 格式的位姿，如 {j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}
• 返回：[Pose] SDK 格式的位姿，如 {0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}

示例程序

p = {j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}
pose = sdk_pose(p) -- {0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}

SDK位姿转为LuaApi位姿 3.1.24

将SDK位姿转为LuaApi位姿。

• 函数名：lua模块的函数lua_pose
• 参数：
  1. p: [Pose] SDK 格式的位姿，如 {0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}
• 返回：[Pose] Lua API 格式的位姿，如 {j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}

示例程序

p = {0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}
pose = lua_pose(p) -- {j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}