位置和姿态

估算位姿的灵活性 3.1.36

计算关节位姿的可达性（0~1，越大越灵活）。在临近奇异位置和工作空间边界的地方灵活性接近于0。

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

• 函数名：[Robot]下的方法measure_manipulation
• 参数：
  1. p: [JointPose] 关节位姿（弧度）
• 返回：[float] 灵活性，0~1，越大越灵活

示例程序

cpose = {-0.3, -0.3, 0.3, 0, 0, 0}
jpose = lebai:kinematics_inverse(cpose)
manipulation = lebai:measure_manipulation(jpose)
print(manipulation)
if manipulation < 0.001 then
  error("manipulation too low")
end

cpose = [-0.3, -0.3, 0.3, 0, 0, 0]
jpose = lebai.kinematics_inverse(cpose)
manipulation = lebai.measure_manipulation(jpose)
print(manipulation)
if manipulation < 0.001:
    raise RuntimeError("manipulation too low")

判断是否到达目标位姿

判断机器人当前位姿是否在目标位姿附近（关节误差 < 0.1°，位置偏差 < 5mm，角度偏差 < 1°）。

• 函数名：[Robot]下的方法in_pose
• 参数：
  1. p: [Pose] 目标位姿，关节位姿（list）或笛卡尔位姿（dict）二选一
• 返回：[bool] 是否已到达目标位姿

示例程序

p = {0, -0.7854, 1.5708, -0.7855, 1.5708, 0}
ret = lebai:in_pose(p)
print(ret)

p = [0, -0.7854, 1.5708, -0.7855, 1.5708, 0]
ret = lebai.in_pose(p)
print(ret)

从资源库加载路点位姿

读取已保存的位姿。

• 函数名：[Robot]下的方法load_pose
• 参数：
  1. name: [str] 位置名称
  2. dir: [str] 位置所在文件名称。可选，默认根目录
  3. raw_pose: 返回的位姿类型。可选，默认 False 返回关节位姿；设置为 True 时按保存时的原始格式返回关节位姿或笛卡尔位姿。
• 返回：[Pose] 对应位置库里的关节位置信息

JSON-RPC

// 请求：
{ "jsonrpc": "2.0", "method": "load_pose", "params": [{"name": "", "dir": ""}], "id": 1 }
// 响应：
{ "jsonrpc": "2.0", "result": {"kind": ""}, "id": 1 }

示例程序

pose = lebai:load_pose("test_pose")

pose = lebai.load_pose("test_pose")

保存路点位姿到资源库中

保存位姿到机器人存储。pose 为 None 时保存当前位置。

• 函数名：[Robot]下的方法save_pose
• 参数：
  1. name: [str] 位置名称
  2. pose: [Pose] 待保存的位姿，关节位姿（list）或笛卡尔位姿（dict）二选一。可选，默认None保存当前位置
  3. dir: [str] 位置所在文件名称。可选，默认根目录
  4. refer: [JointPose] 参考关节位姿。可选，默认None不保存参考关节位姿
• 返回：无

JSON-RPC

// 请求：
{ "jsonrpc": "2.0", "method": "save_pose", "params": [{"name": "", "dir": "", "data": {}}], "id": 1 }
// 响应：
{ "jsonrpc": "2.0", "result": {}, "id": 1 }

示例程序

lebai:save_pose("target_tcp_pose", {x = -0.46, y = -0.121, z = 0.13, rz = -1.57, ry = 0, rx = 1.57})

lebai.save_pose("target_tcp_pose", {'x': -0.46, 'y': -0.121, 'z': 0.13, 'rz': -1.57, 'ry': 0, 'rx': 1.57})

从资源库加载特征坐标系

读取已保存的坐标系。

• 函数名：[Robot]下的方法load_frame
• 参数：
  1. name: [str] 名称
  2. dir: [str] 工作空间名称。可选，默认根目录
• 返回：[CartesianPose] 相对于基座的偏移量

JSON-RPC

// 请求：
{ "jsonrpc": "2.0", "method": "load_frame", "params": [{"name": "", "dir": ""}], "id": 1 }
// 响应：
{ "jsonrpc": "2.0", "result": {"position": {}, "rotation": {}}, "id": 1 }

示例程序

frame = lebai:load_frame("frame1")
print(frame)

frame = lebai.load_frame("frame1")
print(frame)

正解

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

• 函数名：[Robot]下的方法kinematics_forward
• 参数：
  1. p: [Pose] 位姿参数，关节位姿（list）或笛卡尔位姿（dict）二选一
• 返回：[CartesianPose] 笛卡尔空间位置和姿态

JSON-RPC

// 请求：
{ "jsonrpc": "2.0", "method": "get_forward_kin", "params": [{"kind": ""}], "id": 1 }
// 响应：
{ "jsonrpc": "2.0", "result": {"position": {}, "rotation": {}}, "id": 1 }

示例程序

jpose = {0, -0.7854, 1.5708, -0.7855, 1.5708, 0}
cart = lebai:kinematics_forward(jpose)

jpose = [0, -0.7854, 1.5708, -0.7855, 1.5708, 0]
cart = lebai.kinematics_forward(jpose)

反解

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

• 函数名：[Robot]下的方法kinematics_inverse
• 参数：
  1. p: [CartesianPose] 笛卡尔位置和姿态
  2. refer: [JointPose] 反解参考的关节位置。可选，默认为当前反馈关节位置。当反解出现多解时，会取距离 refer 最近的一个解
• 返回：[JointPose] 关节位置

JSON-RPC

// 请求：
{ "jsonrpc": "2.0", "method": "get_inverse_kin", "params": [{"kind": ""}], "id": 1 }
// 响应：
{ "jsonrpc": "2.0", "result": {"joint": [0]}, "id": 1 }

示例程序

cpose = {x = -0.465, y = -0.121, z = 0.13, rz = -1.57, ry = 0, rx = 1.57}
jpose = {0, -0.7854, 1.5708, -0.7855, 1.5708, 0}
joints = lebai:kinematics_inverse(cpose, jpose)

cpose = {'x': -0.465, 'y': -0.121, 'z': 0.13, 'rz': -1.57, 'ry': 0, 'rx': 1.57}
jpose = [0, -0.7854, 1.5708, -0.7855, 1.5708, 0]
joints = lebai.kinematics_inverse(cpose, jpose)

位姿特征坐标系转换

求 a 系下 b 的相对位姿。以 a 为用户坐标系 $\{A\}$，b 是相对于坐标系 $\{A\}$ 的位姿描述。最后返回相对于机器人世界坐标系的位姿描述。

• 函数名：[Robot]下的方法pose_trans
• 参数：
  1. a: [Pose] 位姿 $A$
  2. b: [Pose] 位姿 $B$
• 返回：[CartesianPose] 位姿 $C = AB$，是 b 相对于机器人世界坐标系的位姿描述

JSON-RPC

// 请求：
{ "jsonrpc": "2.0", "method": "get_pose_trans", "params": [{"from": {"kind": ""}, "from_to": {"kind": ""}}], "id": 1 }
// 响应：
{ "jsonrpc": "2.0", "result": {"position": {}, "rotation": {}}, "id": 1 }

示例程序

a = {x = 0.3, y = 0, z = 0.36, rz = 0, ry = 0, rx = 1.57}
b = {x = 0.35, y = 0, z = 0.36, rz = 0, ry = 0, rx = 1.57}
c = lebai:pose_trans(a, b)

a = {'x': 0.3, 'y': 0, 'z': 0.36, 'rz': 0, 'ry': 0, 'rx': 1.57}
b = {'x': 0.35, 'y': 0, 'z': 0.36, 'rz': 0, 'ry': 0, 'rx': 1.57}
c = lebai.pose_trans(a, b)

位姿的逆

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

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

• 函数名：[Robot]下的方法pose_inverse
• 参数：
  1. p: [Pose] 位姿，关节位姿（list）或笛卡尔位姿（dict）二选一
• 返回：[CartesianPose] $A$ 的逆矩阵对应的笛卡尔位置和位姿

JSON-RPC

// 请求：
{ "jsonrpc": "2.0", "method": "get_pose_inverse", "params": [{"kind": ""}], "id": 1 }
// 响应：
{ "jsonrpc": "2.0", "result": {"position": {}, "rotation": {}}, "id": 1 }

示例程序

a = {x = 0.3, y = 0, z = 0.36, rz = 0, ry = 0, rx = 1.57}
inverse_a = lebai:pose_inverse(a)

a = {'x': 0.3, 'y': 0, 'z': 0.36, 'rz': 0, 'ry': 0, 'rx': 1.57}
inverse_a = lebai.pose_inverse(a)

位姿的加 3.1.13

位姿叠加：在 pose 基础上增加偏移 delta。从base位置，往frame方向移动delta后的位姿。

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

示例程序

base = {x=-0.4, y=0, z=0.1, rz=-1.57, ry=0, rx=1.57}
frame = {x=0, y=0, z=0, rz=0, ry=-0.78, rx=0} -- 绕y轴旋转45°，使z轴斜向上
delta = {x=0, y=0, z=0.1, rz=0, ry=0, rx=0} -- 沿z轴方向移动0.1m
pose = lebai:pose_add(base, delta, frame)
lebai:movej(base, 0.4, 0.1)
lebai:movel(pose, 0.4, 0.1)

base = {'x': -0.4, 'y': 0, 'z': 0.1, 'rz': -1.57, 'ry': 0, 'rx': 1.57}
frame = {'x': 0, 'y': 0, 'z': 0, 'rz': 0, 'ry': -0.78, 'rx': 0}  # 绕y轴旋转45°，使z轴斜向上
delta = {'x': 0, 'y': 0, 'z': 0.1, 'rz': 0, 'ry': 0, 'rx': 0}  # 沿z轴方向移动0.1m
pose = lebai.pose_add(base, delta, frame)
lebai.movej(base, 0.4, 0.1)
lebai.movel(pose, 0.4, 0.1)