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
|