1. 简介
CS在进行一些二次开发时需要用到正逆解运算，以及坐标系偏移。本文借助代码介绍对应脚本的使用方法。
2. 相关指令
2.1 获取当前关节角度
get_actual_joint_positions()功能：
该指令用于获取所有关节的实际关节角位置。实际关节角位置以弧度为单位，并以长度为 6 的矢量返回。
参数：
无
返回值：
list 型数据：[Base,Shoulder,Elbow,Wrist1,Wrist2,Wrist3]，以弧度为单位的当前实际关节角位置。
示例：
j=get_actual_joint_positions()2.2 获取当前TCP位姿
get_actual_tcp_pose()功能：
该指令用于获取当前 TCP 位姿。返回 6D 位姿——6D 位姿表示基架中规定的工具位置和方位。此姿态的计算基于实际机器人编码器读数。
参数：
无
返回值：
当前实际 TCP 的位姿，格式为 [x,y,z,Rx,Ry,Rz]，list 型数据，其中 x、y、z 单位为 m，Rx、Ry、Rz 单位为 rad。
示例：
b=get_actual_tcp_pose()2.3 逆解计算
get_inverse_kin(p, qnear='', tcp='active tcp')功能：
该指令用于运动学逆解计算，即从笛卡尔空间到关节空间的转换。如果定义了qnear，则返回最接近 qnear 的解。否则，将返回最接近当前关节位置的解。如果未提供 tcp，则将使用控制器的当前激活 tcp。
参数：
p：位姿，格式为 [x,y,z,Rx,Ry,Rz]，list 型数据，其中 x、y、z 单位为 m，Rx、Ry、Rz 单位为 rad；
qnear：最接近的关节角度，缺省值为当前机器人关节角，list 型数据（可选参数）；
tcp：工具形状（可选参数）。
返回值：
list 型数据，关节角度，单位：弧度。
示例：
po=[0.4,‑0.2,0.4,‑3.1,2.8,‑1.8]
qn=[‑0.2,‑1.6,‑1.4,‑1.5,1.5,‑1.6]
v=get_inverse_kin(po,qnear=qn)2.4 正解计算
get_forward_kin(q='', tcp='active tcp')功能：
该指令用于正运动学计算，即通过关节角度获取位姿，使用当前系统激活的 TCP数据。如果未提供关节位置矢量，则将使用机器人本体的当前关节角度。如果未提供 tcp，则将使用控制器的当前激活 tcp。
参数：
q：机器人的关节角度，缺省值为当前机器人关节角度，单位为弧度，list 型数据（可选参数）。
tcp：工具形状（可选参数）。
返回值：
机器人位姿，格式为 [x,y,z,Rx,Ry,Rz]，list 型数据，其中 x、y、z 单位为 m，Rx、Ry、Rz 单位为 rad。
示例：
p_j=[‑0.2,‑1.6,‑1.4,‑1.5,1.5,0]
p_p=get_forward_kin(p_j)2.5 位姿变换
pose_trans(p_from, p_from_to)功能：
该指令用于姿态变换。第一个自变量 p_from 用于变换第二个自变量 p_from_to，然后返回结果。这意味着结果是从 p_from 坐标系开始，然后坐标系移动 p_from_to 生成的姿态。如果姿态被视为变换矩阵，则如下所示：
T_world->to=T_world->from * T_from->to
T_x->to=T_x->from * T_from->to
参数：
p_from：起始姿态，格式为 [x,y,z,Rx,Ry,Rz]，list 型数据，其中 x、y、z 单位为 m，Rx、Ry、Rz 单位为 rad；
p_from_to：相对于起始姿态的姿态变化，格式为 [x,y,z,Rx,Ry,Rz]，list 型数据，其中 x、y、z 单位为 m，Rx、Ry、Rz 单位为 rad。
返回值：
生成的姿态，格式为 [x,y,z,Rx,Ry,Rz]，list 型数据，其中 x、y、z 单位为 m，Rx、Ry、Rz 单位为 rad。
示例：
pf=[0.4,‑0.2,0.4,‑3.1,2.8,‑2.3]
pft=[‑0.01,‑1.5,‑0.002,‑1.5,1.5,‑2.3]
tp=pose_trans(pf,pft)2.6 位姿求逆
pose_inv(p_from)功能：
该指令用于计算并返回位姿的逆。
参数：
p_from：工具位姿，格式为 [x,y,z,Rx,Ry,Rz]，list 型数据，其中 x、y、z 单位为 m，Rx、Ry、Rz 单位为 rad。
返回值：
p_from 的逆，格式为 [x,y,z,Rx,Ry,Rz]，list 型数据，其中 x、y、z 单位为 m，Rx、Ry、Rz 单位为 rad。
示例：
pf=[0.4,‑0.2,0.4,‑3.1,2.8,‑2.3]
inv_pose=pose_inv(pf)3. 代码示例
#!/usr/bin/env python3
# SPDX-License-Identifier: MIT
# Copyright (c) 2025, Elite Robots.
#
# 使用 40011 明文请求端口实现运动学示例
# 等价于 SDK 版本的 kinematics_example，无需任何 SDK 依赖
#
# 功能：
# 1. 获取当前关节角度 (get_actual_joint_positions)
# 2. 获取当前 TCP 位姿 (get_actual_tcp_pose)
# 3. 正解 FK: get_forward_kin(q) → TCP 位姿
# 4. 逆解 IK: get_inverse_kin(p) → 关节角度
#
# 用法：
# python kinematics_via_port.py --robot-ip 192.168.1.200
import argparse
import ast
import socket
import sys
# ─────────────────────────────────────────────
# 40011 端口通讯工具
# ─────────────────────────────────────────────
def send_request(ip: str, req_id: int, cmd: str, timeout: float = 5.0):
"""
向 40011 端口发送一条请求，返回 (result_list, error_str)。
格式：req {id}{cmd}\n ← id 与 cmd 之间无空格！
成功响应：{id} success: {value}
失败响应：{id} failure: {reason}
"""
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.settimeout(timeout)
try:
sock.connect((ip, 40011))
payload = f"req {req_id}{cmd}\n"
sock.sendall(payload.encode("utf-8"))
resp = b""
while True:
chunk = sock.recv(4096)
if not chunk:
break
resp += chunk
if b"\n" in resp:
break
finally:
sock.close()
resp_str = resp.decode("utf-8", errors="replace").strip()
if "failure" in resp_str.lower():
return None, resp_str
# 按第一个 ':' 分割
parts = resp_str.split(":", 1)
if len(parts) < 2:
return None, f"unexpected response: {resp_str}"
val_str = parts[1].strip()
# 尝试解析为列表（可能是双重嵌套 [[...]]）
try:
val = ast.literal_eval(val_str)
if isinstance(val, list):
# 展开双重嵌套
if len(val) == 1 and isinstance(val[0], list):
val = val[0]
return [float(x) for x in val], None
return float(val), None
except Exception:
pass
# 降级：手动去括号解析
try:
clean = val_str.replace("[", "").replace("]", "").replace(" ", "")
return [float(x) for x in clean.split(",") if x], None
except Exception as e:
return None, f"parse error: {e}, raw: {resp_str}"
# ─────────────────────────────────────────────
# 位姿工具函数
# ─────────────────────────────────────────────
def pose_inv(ip: str, req_id_ref: list, p: list) -> tuple:
"""
调用机器人 pose_inv(p)，返回 (result, next_req_id, error)。
req_id_ref 是一个单元素列表，用于在函数内自增 ID。
"""
p_native = [float(x) for x in p]
result, err = send_request(ip, req_id_ref[0], f"pose_inv({p_native})")
req_id_ref[0] += 1
return result, err
def pose_trans(ip: str, req_id_ref: list, p1: list, p2: list) -> tuple:
"""
调用机器人 pose_trans(p1, p2)，返回 (result, error)。
"""
p1_native = [float(x) for x in p1]
p2_native = [float(x) for x in p2]
result, err = send_request(ip, req_id_ref[0], f"pose_trans({p1_native},{p2_native})")
req_id_ref[0] += 1
return result, err
def offs(ip: str, req_id_ref: list, p: list, x: float, y: float, z: float,
user: list = None) -> tuple:
"""
等价于 EliteScript 的 Offs 函数：
def Offs(p, x, y, z, user=[0,0,0,0,0,0]):
out = p.copy()
out_in_user = pose_trans(pose_inv(user), out)
out_in_user[0] += x
out_in_user[1] += y
out_in_user[2] += z
out = pose_trans(user, out_in_user)
return out
参数：
ip 机器人 IP
req_id_ref [int]，请求 ID 容器（自动递增）
p 目标位姿 [x,y,z,Rx,Ry,Rz]，基坐标系，单位 m/rad
x, y, z 在用户坐标系下的偏移量，单位 m
user 用户坐标系位姿，默认 [0,0,0,0,0,0]（即基坐标系）
返回：(偏移后位姿, error_str)
"""
if user is None:
user = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Step 1: pose_inv(user)
inv_user, err = pose_inv(ip, req_id_ref, user)
if err:
return None, f"pose_inv failed: {err}"
# Step 2: out_in_user = pose_trans(inv_user, p)
out_in_user, err = pose_trans(ip, req_id_ref, inv_user, p)
if err:
return None, f"pose_trans(inv_user, p) failed: {err}"
# Step 3: 在用户坐标系下施加偏移
out_in_user[0] += float(x)
out_in_user[1] += float(y)
out_in_user[2] += float(z)
# Step 4: out = pose_trans(user, out_in_user)
out, err = pose_trans(ip, req_id_ref, user, out_in_user)
if err:
return None, f"pose_trans(user, out_in_user) failed: {err}"
return out, None
def log_vector(prefix: str, vec):
vals = ", ".join(f"{v:.6f}" for v in vec)
print(f"{prefix} [{vals}]")
# ─────────────────────────────────────────────
# 主程序
# ─────────────────────────────────────────────
def main():
ip = "172.16.15.45"
req_id = 1 # 请求 ID，每次递增
print(f"[INFO] 连接机器人: {ip}:40011")
# ── Step 1: 获取当前关节角度 ──────────────────
joints, err = send_request(ip, req_id, "get_actual_joint_positions()")
req_id += 1
if err:
print(f"[FATAL] 获取关节角度失败: {err}")
sys.exit(1)
print("[INFO] 获取实际关节角度成功")
log_vector("Current Joints:", joints)
# ── Step 2: 获取当前 TCP 位姿 ─────────────────
tcp, err = send_request(ip, req_id, "get_actual_tcp_pose()")
req_id += 1
if err:
print(f"[FATAL] 获取 TCP 位姿失败: {err}")
sys.exit(1)
print("[INFO] 获取实际 TCP 位姿成功")
log_vector("Current TCP Pose:", tcp)
# ── Step 3: 正解 FK ───────────────────────────
# get_forward_kin(q) → TCP 位姿
# 参数必须是 Python 原生 float 列表
joints_native = [float(x) for x in joints]
fk_cmd = f"get_forward_kin({joints_native})"
print(fk_cmd)
fk_pose, err = send_request(ip, req_id, fk_cmd)
req_id += 1
if err:
print(f"[FATAL] 正解 FK 失败: {err}")
sys.exit(1)
print("[INFO] 正解 FK 成功")
log_vector("FK Pose:", fk_pose)
# ── Step 4: 逆解 IK ───────────────────────────
# get_inverse_kin(p, qnear='', tcp='active tcp')
# 以当前 TCP 位姿为目标，当前关节角为参考解
tcp_native = [float(x) for x in tcp]
joints_ref = [float(x) for x in joints]
ik_cmd = f"get_inverse_kin({tcp_native}, {joints_ref})"
print(ik_cmd)
ik_joints, err = send_request(ip, req_id, ik_cmd)
req_id += 1
if err:
print(f"[FATAL] 逆解 IK 失败: {err}")
sys.exit(1)
print("[INFO] 逆解 IK 成功")
log_vector("IK Result Joints:", ik_joints)
# ── 汇总输出（与 SDK 版本格式一致）──────────
log_vector("Current TCP Pose:", tcp)
log_vector("FK Pose:", fk_pose)
log_vector("IK Result Joints:", ik_joints)
log_vector("Current Joints:", joints)
# ── Step 5: Offs 演示 ─────────────────────────────────
# 在当前 TCP 位姿基础上，在基坐标系 X 方向偏移 +50mm
print("[INFO] 测试 Offs: 在基坐标系 X 方向偏移 +50mm")
req_id_ref = [req_id]
tcp_native = [float(v) for v in tcp]
offs_result, err = offs(ip, req_id_ref, tcp_native, x=0.05, y=0.0, z=0.0)
req_id = req_id_ref[0]
if err:
print(f"[ERROR] Offs 失败: {err}")
else:
log_vector("Offs Result (X+50mm):", offs_result)
if __name__ == "__main__":
main()输出将如下所示：
FK Pose: [-0.203082, -0.255123, 0.284660, 2.043829, 0.427645, 1.090979]
IK Result Joints: [0.200621, -1.609931, 1.694720, -0.711048, -0.785667, -0.000485]
Current Joints: [0.200621, -1.609931, 1.694719, -0.711047, -0.785668, -0.000485]
[INFO] 测试 Offs: 在基坐标系 X 方向偏移 +50mm
Offs Result (X+50mm): [-0.153082, -0.255123, 0.284660, 2.043828, 0.427645, 1.090978]