位置和姿态

从资源库加载路点位姿

pose = lebai:load_pose(name, dir)
  • 参数

    • name 名称
    • dir 工作空间名称。可选,默认根目录
  • 返回

    • pose 相对于基座的位姿
JSON-RPC
// 请求:
{ "jsonrpc": "2.0", "method": "load_pose", "params": [{"name": "", "dir": ""}], "id": 1 }
// 响应:
{ "jsonrpc": "2.0", "result": {"kind": ""}, "id": 1 }

从资源库加载特征坐标系

frame = lebai:load_frame(name, dir)
  • 参数

    • name 名称
    • dir 工作空间名称。可选,默认根目录
  • 返回

    • frame 相对于基座的偏移量
JSON-RPC
// 请求:
{ "jsonrpc": "2.0", "method": "load_frame", "params": [{"name": "", "dir": ""}], "id": 1 }
// 响应:
{ "jsonrpc": "2.0", "result": {"position": {}, "rotation": {}}, "id": 1 }

正解

cart = lebai:kinematics_forward(pose)

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

  • 参数

    • pose 位姿
  • 返回

    • cart 笛卡尔空间位姿
JSON-RPC
// 请求:
{ "jsonrpc": "2.0", "method": "get_forward_kin", "params": [{"kind": ""}], "id": 1 }
// 响应:
{ "jsonrpc": "2.0", "result": {"position": {}, "rotation": {}}, "id": 1 }

反解

joints = lebai:kinematics_inverse(pose, joints)

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

  • 参数

    • pose 位姿
    • joints 关节空间参考位置。可选,默认为当前反馈关节位置。当反解出现多解时,会取距离 joints 最近的一个解。
  • 返回

    • joints 关节空间位姿
JSON-RPC
// 请求:
{ "jsonrpc": "2.0", "method": "get_inverse_kin", "params": [{"kind": ""}], "id": 1 }
// 响应:
{ "jsonrpc": "2.0", "result": {"joint": [0]}, "id": 1 }

位姿特征坐标系转换

c = pose_trans(a, b)

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

  • 参数

    • a 位姿 AA
    • b 位姿 BB
  • 返回

    • c 位姿 C=ABC = AB
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 }

位姿的逆

c = lebai:pose_inverse(a)

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

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

  • 参数

    • a 位姿 AA
  • 返回

    • c AA 的逆 A1A^{-1}
JSON-RPC
// 请求:
{ "jsonrpc": "2.0", "method": "get_pose_inverse", "params": [{"kind": ""}], "id": 1 }
// 响应:
{ "jsonrpc": "2.0", "result": {"position": {}, "rotation": {}}, "id": 1 }

位姿的加

pose = lebai:pose_add(base, frame, delta)

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

  • 参数

    • base。起始位姿。
    • frame。移动方向。
    • delta。移动距离。
  • 返回

    • pose。移动后的位姿。
示例程序
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, rx=-0.78, rx=0} -- z轴斜向上45°
delta = {x=0, y=0, z=0.1, rz=0, rx=0, rx=0} -- 沿z轴方向移动0.1m
pose = lebai:pose_add(base, frame, delta)
lebai:movej(base, 0.4, 0.1)
lebai:movel(pose, 0.4, 0.1)
上次更新: