48 lines
2.0 KiB
Python
48 lines
2.0 KiB
Python
import os
|
||
import re
|
||
import subprocess
|
||
import numpy as np
|
||
|
||
root_path = os.path.join(os.path.dirname(__file__), "../..")
|
||
|
||
def get_pose(joint_positions, base_link="PELVIS_S", target_link="R_FINGER_TIP",
|
||
exec_path=os.path.join(root_path, "cmake-build-debug/example/solve_fk")):
|
||
joint_str = [str(j) for j in joint_positions]
|
||
cmd = [exec_path, base_link, target_link, *joint_str]
|
||
res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=True, text=True)
|
||
lines = res.stdout.splitlines()
|
||
start_idx = next((i for i, l in enumerate(lines) if re.search(r"变换矩阵|T\s*:", l)), None)
|
||
if start_idx is None or start_idx + 4 >= len(lines):
|
||
raise RuntimeError("未在 solve_fk 输出中找到 4×4 变换矩阵:\n" + res.stdout)
|
||
try:
|
||
mat = np.array([[float(x) for x in lines[start_idx + 1 + r].split()] for r in range(4)], dtype=np.float64)
|
||
if mat.shape != (4, 4):
|
||
raise ValueError
|
||
except Exception:
|
||
raise RuntimeError("矩阵解析失败，原始输出:\n" + res.stdout)
|
||
return mat
|
||
|
||
|
||
def solve_ik(pose, base_link="PELVIS_S", target_link="R_WRIST_R_S",
|
||
exec_path=os.path.join(root_path, "cmake-build-debug/example/solve_ik")):
|
||
[x, y, z, rx, ry, rz] = pose
|
||
cmd = [exec_path, base_link, target_link, f"{x}", f"{y}", f"{z}", f"{rx}", f"{ry}", f"{rz}"]
|
||
print(" ".join(cmd))
|
||
res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=True)
|
||
lines = res.stdout.splitlines()
|
||
success_line = next((l for l in lines if "[IK Solve]" in l), "")
|
||
success = "success=true" in success_line.lower()
|
||
if not success:
|
||
return False, None, None
|
||
|
||
def _parse(tag):
|
||
pats = rf"^{tag}\s*:\s*(.+)$"
|
||
for l in lines:
|
||
m = re.match(pats, l.strip())
|
||
if m:
|
||
return [float(v) for v in m.group(1).split()]
|
||
raise RuntimeError(f"success=true 但未找到 {tag}: 行！\n{res.stdout}")
|
||
|
||
left_q = _parse("left")
|
||
right_q = _parse("right")
|
||
return True, left_q, right_q |