437 lines
15 KiB
Python
437 lines
15 KiB
Python
|
import os
|
|||
|
|
import sys
|
|||
|
|
import numpy as np
|
|||
|
|
import pinocchio as pin
|
|||
|
|
import matplotlib.pyplot as plt
|
|||
|
|
from mpl_toolkits.mplot3d import Axes3D
|
|||
|
|
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
|
|||
|
|
from omegaconf import OmegaConf
|
|||
|
|
|
|||
|
|
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
|
|||
|
|
from core.sew_mapper import SEWMapper, fk_update, pose_of_frame, rot_error_deg
|
|||
|
|
|
|||
|
|
|
|||
|
|
def safe_normalize(v, eps=1e-8):
|
|||
|
|
"""Return v / ||v||, or None if the norm is too small."""
|
|||
|
|
n = np.linalg.norm(v)
|
|||
|
|
if n < eps:
|
|||
|
|
return None
|
|||
|
|
return v / n
|
|||
|
|
|
|||
|
|
|
|||
|
|
def check_once(mapper, q_m, names, visualize=True):
|
|||
|
|
import numpy as np
|
|||
|
|
import open3d as o3d
|
|||
|
|
from utils.drwa import o3d_frame, o3d_lineset, o3d_sphere
|
|||
|
|
|
|||
|
|
m_model, m_data = mapper.m_model, mapper.m_data
|
|||
|
|
s_model, s_data = mapper.s_model, mapper.s_data
|
|||
|
|
|
|||
|
|
q_s, dbg = mapper.retargetting(q_m)
|
|||
|
|
|
|||
|
|
# 主端 FK
|
|||
|
|
fk_update(m_model, m_data, q_m)
|
|||
|
|
pS_m, RS_m = pose_of_frame(m_model, m_data, names["m_shoulder"])
|
|||
|
|
pE_m, REf_m = pose_of_frame(m_model, m_data, names["m_elbow"])
|
|||
|
|
pW_m, RW_m = pose_of_frame(m_model, m_data, names["m_wrist"])
|
|||
|
|
p_m, R_m = pose_of_frame(m_model, m_data, names["m_ee"])
|
|||
|
|
|
|||
|
|
# 从端 FK
|
|||
|
|
fk_update(s_model, s_data, q_s)
|
|||
|
|
pS_s, RS_s = pose_of_frame(s_model, s_data, names["s_shoulder"])
|
|||
|
|
pE_s, REf_s = pose_of_frame(s_model, s_data, names["s_elbow"])
|
|||
|
|
pW_s, RW_s = pose_of_frame(s_model, s_data, names["s_wrist"])
|
|||
|
|
p_s, R_s = pose_of_frame(s_model, s_data, names["s_ee"])
|
|||
|
|
|
|||
|
|
# 误差
|
|||
|
|
pos_err = np.linalg.norm(p_m - p_s) * 1000.0 # mm
|
|||
|
|
rot_err = rot_error_deg(R_m, R_s) # degree
|
|||
|
|
print(f"rot_err: {rot_err}, rot_err: {rot_err}")
|
|||
|
|
|
|||
|
|
# 是否发生 reach 裁剪
|
|||
|
|
clipped = dbg["d_s"] < np.linalg.norm(
|
|||
|
|
m_data.oMf[m_model.getFrameId(names["m_wrist"])].translation
|
|||
|
|
- m_data.oMf[m_model.getFrameId(names["m_shoulder"])].translation) - 1e-9 \
|
|||
|
|
or dbg["d_s"] > mapper.L1 + mapper.L2 - mapper.eps_clip + 1e-9
|
|||
|
|
|
|||
|
|
if visualize:
|
|||
|
|
geoms = []
|
|||
|
|
geoms.append(o3d_frame(np.eye(3), np.zeros(3), size=0.1))
|
|||
|
|
|
|||
|
|
pts_m = np.vstack([pS_m, pE_m, pW_m, p_m])
|
|||
|
|
geoms.append(o3d_lineset(pts_m, color=(1.0, 0.0, 0.0)))
|
|||
|
|
geoms += [
|
|||
|
|
o3d_sphere(pS_m, 0.016, (1.0,0.4,0.4)),
|
|||
|
|
o3d_sphere(pE_m, 0.014, (1.0,0.3,0.3)),
|
|||
|
|
o3d_sphere(pW_m, 0.012, (1.0,0.2,0.2)),
|
|||
|
|
o3d_sphere(p_m, 0.012, (1.0,0.0,0.0)),
|
|||
|
|
o3d_frame(R_m, p_m, size=0.07)
|
|||
|
|
]
|
|||
|
|
# 从臂（蓝）
|
|||
|
|
pts_s = np.vstack([pS_s, pE_s, pW_s, p_s])
|
|||
|
|
geoms.append(o3d_lineset(pts_s, color=(0.0, 0.4, 1.0)))
|
|||
|
|
geoms += [
|
|||
|
|
o3d_sphere(pS_s, 0.016, (0.4,0.6,1.0)),
|
|||
|
|
o3d_sphere(pE_s, 0.014, (0.3,0.5,1.0)),
|
|||
|
|
o3d_sphere(pW_s, 0.012, (0.2,0.4,1.0)),
|
|||
|
|
o3d_sphere(p_s, 0.012, (0.0,0.2,1.0)),
|
|||
|
|
o3d_frame(R_s, p_s, size=0.07)
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
geoms.append(o3d_sphere(dbg["pW_s_ref"], 0.008, (0.2, 1.0, 0.2)))
|
|||
|
|
o3d.visualization.draw_geometries(geoms, window_name="SEW Retargeting (Master=Red, Slave=Blue)")
|
|||
|
|
return dict(pos_mm=pos_err, rot_deg=rot_err, clipped=clipped, dbg=dbg, q_s=q_s)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def random_qm(mapper, N=50, margin=0.2):
|
|||
|
|
lb = []; ub = []
|
|||
|
|
for j in mapper.m_model.joints[1:]: # skip universe
|
|||
|
|
if j.nq == 1: # 1-DoF revolute
|
|||
|
|
jid = j.id
|
|||
|
|
iq = j.idx_q
|
|||
|
|
lb.append(mapper.m_model.lowerPositionLimit[iq])
|
|||
|
|
ub.append(mapper.m_model.upperPositionLimit[iq])
|
|||
|
|
lb = np.array(lb); ub = np.array(ub)
|
|||
|
|
rng = (ub - lb)
|
|||
|
|
lb2 = lb + margin * rng
|
|||
|
|
ub2 = ub - margin * rng
|
|||
|
|
|
|||
|
|
qs = []
|
|||
|
|
for _ in range(N):
|
|||
|
|
v = lb2 + np.random.rand(len(lb2)) * (ub2 - lb2)
|
|||
|
|
q_full = pin.neutral(mapper.m_model)
|
|||
|
|
k = 0
|
|||
|
|
for j in mapper.m_model.joints[1:]:
|
|||
|
|
if j.nq == 1:
|
|||
|
|
q_full[j.idx_q] = v[k]; k += 1
|
|||
|
|
qs.append(q_full)
|
|||
|
|
return qs
|
|||
|
|
|
|||
|
|
|
|||
|
|
def validation(mapper):
|
|||
|
|
q_m0 = pin.neutral(mapper.m_model)
|
|||
|
|
names = {
|
|||
|
|
"m_shoulder": "master_shoulder",
|
|||
|
|
"m_elbow": "master_forearm",
|
|||
|
|
"m_wrist": "master_wrist",
|
|||
|
|
"m_ee": "master_ee",
|
|||
|
|
"s_shoulder": "slave_shoulder",
|
|||
|
|
"s_elbow": "slave_forearm",
|
|||
|
|
"s_wrist": "slave_wrist",
|
|||
|
|
"s_ee": "slave_ee"
|
|||
|
|
}
|
|||
|
|
res0 = check_once(mapper, q_m0, names)
|
|||
|
|
print("[Neutral] pos_err = %.3f mm, rot_err = %.3f deg, clipped=%s" %
|
|||
|
|
(res0["pos_mm"], res0["rot_deg"], res0["clipped"]))
|
|||
|
|
|
|||
|
|
# 2) 随机多组验证
|
|||
|
|
N = 100
|
|||
|
|
samples = random_qm(mapper, N=N, margin=0.1)
|
|||
|
|
pos_errs = []; rot_errs = []; n_clip = 0
|
|||
|
|
for q_m in samples:
|
|||
|
|
r = check_once(mapper, q_m, names)
|
|||
|
|
pos_errs.append(r["pos_mm"])
|
|||
|
|
rot_errs.append(r["rot_deg"])
|
|||
|
|
n_clip += int(r["clipped"])
|
|||
|
|
|
|||
|
|
pos_errs = np.array(pos_errs)
|
|||
|
|
rot_errs = np.array(rot_errs)
|
|||
|
|
|
|||
|
|
|
|||
|
|
# =======================================================
|
|||
|
|
# 辅助函数：让 3D 轴等比例，图更“几何化”
|
|||
|
|
# =======================================================
|
|||
|
|
def set_equal_aspect_3d(ax, pts):
|
|||
|
|
"""pts: (N,3) numpy array."""
|
|||
|
|
pts = np.asarray(pts)
|
|||
|
|
|
|||
|
|
# 如果有 NaN 或 Inf，直接返回，不调坐标轴
|
|||
|
|
if not np.all(np.isfinite(pts)):
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
x_min, y_min, z_min = pts.min(axis=0)
|
|||
|
|
x_max, y_max, z_max = pts.max(axis=0)
|
|||
|
|
max_range = max(x_max - x_min, y_max - y_min, z_max - z_min)
|
|||
|
|
if max_range == 0:
|
|||
|
|
max_range = 1.0 # 防止除零
|
|||
|
|
|
|||
|
|
x_mid = 0.5 * (x_max + x_min)
|
|||
|
|
y_mid = 0.5 * (y_max + y_min)
|
|||
|
|
z_mid = 0.5 * (z_max + z_min)
|
|||
|
|
r = 0.6 * max_range
|
|||
|
|
ax.set_xlim(x_mid - r, x_mid + r)
|
|||
|
|
ax.set_ylim(y_mid - r, y_mid + r)
|
|||
|
|
ax.set_zlim(z_mid - r, z_mid + r)
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
# =======================================================
|
|||
|
|
# 绘图（论文插图专用）
|
|||
|
|
# =======================================================
|
|||
|
|
|
|||
|
|
def plot_sew_geometry(pS_m, pE_m, pW_m,
|
|||
|
|
pS_s, pE_s, pW_s_ref,
|
|||
|
|
xhat_m, n_m, n_ref, phi_m):
|
|||
|
|
"""
|
|||
|
|
Fig. SEW Geometry：
|
|||
|
|
- Master/Slave S-E-W
|
|||
|
|
- Shoulder-Wrist 方向 x̂_m
|
|||
|
|
- 上臂平面法向 n_m
|
|||
|
|
- 参考法向 n_ref
|
|||
|
|
"""
|
|||
|
|
|
|||
|
|
fig = plt.figure(figsize=(6, 6))
|
|||
|
|
ax = fig.add_subplot(111, projection='3d')
|
|||
|
|
|
|||
|
|
# Master skeleton (粗实体＋虚线前臂)
|
|||
|
|
ax.plot([pS_m[0], pE_m[0]], [pS_m[1], pE_m[1]], [pS_m[2], pE_m[2]],
|
|||
|
|
'r-', lw=2.5, label='Master upper arm')
|
|||
|
|
ax.plot([pE_m[0], pW_m[0]], [pE_m[1], pW_m[1]], [pE_m[2], pW_m[2]],
|
|||
|
|
'r--', lw=2.0, label='Master forearm')
|
|||
|
|
|
|||
|
|
# Slave skeleton (蓝色)
|
|||
|
|
ax.plot([pS_s[0], pE_s[0]], [pS_s[1], pE_s[1]], [pS_s[2], pE_s[2]],
|
|||
|
|
'b-', lw=2.5, label='Slave upper arm')
|
|||
|
|
ax.plot([pE_s[0], pW_s_ref[0]], [pE_s[1], pW_s_ref[1]], [pE_s[2], pW_s_ref[2]],
|
|||
|
|
'b--', lw=2.0, label='Slave forearm')
|
|||
|
|
|
|||
|
|
# --- Master 上臂平面 patch（S_m, E_m, W_m） ---
|
|||
|
|
tri = np.vstack([pS_m, pE_m, pW_m])
|
|||
|
|
area = np.linalg.norm(np.cross(pE_m - pS_m, pW_m - pS_m))
|
|||
|
|
if area > 1e-8:
|
|||
|
|
# 使用 Poly3DCollection 画简单三角面片，避免 qhull
|
|||
|
|
verts = [tri]
|
|||
|
|
poly = Poly3DCollection(verts, alpha=0.08,
|
|||
|
|
facecolor='r', edgecolor='none')
|
|||
|
|
ax.add_collection3d(poly)
|
|||
|
|
ax.plot_trisurf(tri[:, 0], tri[:, 1], tri[:, 2],
|
|||
|
|
color='r', alpha=0.08, edgecolor='none')
|
|||
|
|
|
|||
|
|
# Vectors at S_m
|
|||
|
|
arrow_len = 0.12
|
|||
|
|
ax.quiver(*pS_m, *(arrow_len * xhat_m),
|
|||
|
|
color='g', lw=2.0, normalize=False, label=r'$\hat{x}_m$')
|
|||
|
|
ax.quiver(*pS_m, *(arrow_len * n_m),
|
|||
|
|
color='m', lw=2.0, normalize=False, label=r'$n_m$')
|
|||
|
|
ax.quiver(*pS_m, *(arrow_len * n_ref),
|
|||
|
|
color='c', lw=2.0, normalize=False, label=r'$n_{\mathrm{ref}}$')
|
|||
|
|
|
|||
|
|
# Labels
|
|||
|
|
ax.text(*pS_m, r'$S_m$', fontsize=10)
|
|||
|
|
ax.text(*pE_m, r'$E_m$', fontsize=10)
|
|||
|
|
ax.text(*pW_m, r'$W_m$', fontsize=10)
|
|||
|
|
ax.text(*pS_s, r'$S_s$', fontsize=10)
|
|||
|
|
ax.text(*pE_s, r'$E_s$', fontsize=10)
|
|||
|
|
ax.text(*pW_s_ref, r'$W_s$', fontsize=10)
|
|||
|
|
|
|||
|
|
# 视角和轴
|
|||
|
|
pts_all = np.vstack([pS_m, pE_m, pW_m, pS_s, pE_s, pW_s_ref])
|
|||
|
|
set_equal_aspect_3d(ax, pts_all)
|
|||
|
|
ax.view_init(elev=22, azim=52)
|
|||
|
|
ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z")
|
|||
|
|
|
|||
|
|
title_deg = phi_m * 180.0 / np.pi
|
|||
|
|
ax.set_title(r"SEW geometry (swivel $\phi_m = %.1f^\circ$)" % title_deg)
|
|||
|
|
ax.legend(loc='upper right', fontsize=8)
|
|||
|
|
plt.tight_layout()
|
|||
|
|
plt.show()
|
|||
|
|
|
|||
|
|
|
|||
|
|
def plot_swivel_angle(pS_m, n_ref, n_m, xhat_m):
|
|||
|
|
"""
|
|||
|
|
Fig. Swivel Angle:
|
|||
|
|
可视化 n_ref → n_m 绕 x̂_m 的旋转，并画出圆弧 φ_m。
|
|||
|
|
"""
|
|||
|
|
|
|||
|
|
fig = plt.figure(figsize=(6, 6))
|
|||
|
|
ax = fig.add_subplot(111, projection='3d')
|
|||
|
|
|
|||
|
|
# 以 S_m 为圆心，在垂直于 xhat_m 的平面内画单位圆
|
|||
|
|
# 构造平面正交基 b1, b2
|
|||
|
|
tmp = np.array([1.0, 0.0, 0.0])
|
|||
|
|
if abs(np.dot(tmp, xhat_m)) > 0.9:
|
|||
|
|
tmp = np.array([0.0, 1.0, 0.0])
|
|||
|
|
b1 = tmp - np.dot(tmp, xhat_m) * xhat_m
|
|||
|
|
b1 = b1 / np.linalg.norm(b1)
|
|||
|
|
b2 = np.cross(xhat_m, b1)
|
|||
|
|
|
|||
|
|
# 用 n_ref, n_m 在该平面上算出对应角度
|
|||
|
|
# 这里假设 n_ref 已经在平面内
|
|||
|
|
r = 0.12
|
|||
|
|
# Swivel angle
|
|||
|
|
cos_phi = np.dot(n_ref, n_m)
|
|||
|
|
sin_phi = np.dot(xhat_m, np.cross(n_ref, n_m))
|
|||
|
|
phi = np.arctan2(sin_phi, cos_phi)
|
|||
|
|
|
|||
|
|
# 圆弧
|
|||
|
|
ts = np.linspace(0.0, phi, 80)
|
|||
|
|
arc_pts = []
|
|||
|
|
for t in ts:
|
|||
|
|
v = np.cos(t) * n_ref + np.sin(t) * np.cross(xhat_m, n_ref)
|
|||
|
|
v = v / np.linalg.norm(v)
|
|||
|
|
arc_pts.append(pS_m + r * v)
|
|||
|
|
arc_pts = np.asarray(arc_pts)
|
|||
|
|
ax.plot(arc_pts[:, 0], arc_pts[:, 1], arc_pts[:, 2], 'k-', lw=2.0, label=r'$\phi_m$')
|
|||
|
|
|
|||
|
|
# 向量
|
|||
|
|
ax.quiver(*pS_m, *(r * n_ref), color='c', lw=2.0, normalize=False, label=r'$n_{\mathrm{ref}}$')
|
|||
|
|
ax.quiver(*pS_m, *(r * n_m), color='m', lw=2.0, normalize=False, label=r'$n_m$')
|
|||
|
|
ax.quiver(*pS_m, *(r * xhat_m), color='g', lw=2.0, normalize=False, label=r'$\hat{x}_m$')
|
|||
|
|
|
|||
|
|
ax.text(*pS_m, r'$S_m$', fontsize=10)
|
|||
|
|
|
|||
|
|
pts_all = np.vstack([arc_pts, pS_m])
|
|||
|
|
set_equal_aspect_3d(ax, pts_all)
|
|||
|
|
ax.view_init(elev=20, azim=45)
|
|||
|
|
ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z")
|
|||
|
|
ax.set_title(r"Swivel angle definition")
|
|||
|
|
ax.legend(loc='upper right', fontsize=8)
|
|||
|
|
plt.tight_layout()
|
|||
|
|
plt.show()
|
|||
|
|
|
|||
|
|
|
|||
|
|
def plot_two_sphere_sew(pS_s, pW_s_ref, L1, L2, pE_s):
|
|||
|
|
"""
|
|||
|
|
Fig. Two-sphere intersection:
|
|||
|
|
- 球心 S_s, W_s，半径 L1, L2
|
|||
|
|
- 交点 E_s
|
|||
|
|
- 高亮三角形 S-E-W
|
|||
|
|
"""
|
|||
|
|
|
|||
|
|
fig = plt.figure(figsize=(6, 6))
|
|||
|
|
ax = fig.add_subplot(111, projection='3d')
|
|||
|
|
|
|||
|
|
# 球面
|
|||
|
|
u, v = np.mgrid[0:2*np.pi:40j, 0:np.pi:20j]
|
|||
|
|
sphere1 = pS_s.reshape(3,1,1) + L1 * np.array([
|
|||
|
|
np.cos(u)*np.sin(v),
|
|||
|
|
np.sin(u)*np.sin(v),
|
|||
|
|
np.cos(v)
|
|||
|
|
])
|
|||
|
|
sphere2 = pW_s_ref.reshape(3,1,1) + L2 * np.array([
|
|||
|
|
np.cos(u)*np.sin(v),
|
|||
|
|
np.sin(u)*np.sin(v),
|
|||
|
|
np.cos(v)
|
|||
|
|
])
|
|||
|
|
ax.plot_surface(sphere1[0], sphere1[1], sphere1[2],
|
|||
|
|
alpha=0.15, color='r', edgecolor='none')
|
|||
|
|
ax.plot_surface(sphere2[0], sphere2[1], sphere2[2],
|
|||
|
|
alpha=0.15, color='b', edgecolor='none')
|
|||
|
|
|
|||
|
|
# S-E-W 三角形
|
|||
|
|
tri = np.vstack([pS_s, pE_s, pW_s_ref])
|
|||
|
|
ax.plot_trisurf(tri[:, 0], tri[:, 1], tri[:, 2],
|
|||
|
|
color='k', alpha=0.08, edgecolor='none')
|
|||
|
|
ax.plot([pS_s[0], pE_s[0], pW_s_ref[0], pS_s[0]],
|
|||
|
|
[pS_s[1], pE_s[1], pW_s_ref[1], pS_s[1]],
|
|||
|
|
[pS_s[2], pE_s[2], pW_s_ref[2], pS_s[2]],
|
|||
|
|
'k-', lw=2.0)
|
|||
|
|
|
|||
|
|
# 关键点
|
|||
|
|
ax.scatter(*pE_s, color='k', s=50, label=r'$E_s$')
|
|||
|
|
ax.scatter(*pS_s, color='r', s=40, label=r'$S_s$')
|
|||
|
|
ax.scatter(*pW_s_ref, color='b', s=40, label=r'$W_s$')
|
|||
|
|
|
|||
|
|
ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z")
|
|||
|
|
ax.set_title("Two-sphere intersection for SEW elbow")
|
|||
|
|
|
|||
|
|
pts_all = np.vstack([pS_s, pE_s, pW_s_ref])
|
|||
|
|
set_equal_aspect_3d(ax, pts_all)
|
|||
|
|
ax.view_init(elev=20, azim=40)
|
|||
|
|
ax.legend(loc='upper right', fontsize=8)
|
|||
|
|
plt.tight_layout()
|
|||
|
|
plt.show()
|
|||
|
|
|
|||
|
|
|
|||
|
|
# =======================================================
|
|||
|
|
# 主测试逻辑
|
|||
|
|
# =======================================================
|
|||
|
|
|
|||
|
|
def main():
|
|||
|
|
conf = OmegaConf.load("../config/config.yaml")
|
|||
|
|
|
|||
|
|
master_model, _, _ = pin.buildModelsFromUrdf(str(conf.master_urdf))
|
|||
|
|
slave_model, _, _ = pin.buildModelsFromUrdf(str(conf.slave_urdf))
|
|||
|
|
|
|||
|
|
mapper = SEWMapper(
|
|||
|
|
master_model=master_model,
|
|||
|
|
slave_model=slave_model,
|
|||
|
|
m_shoulder_frame=conf.m_shoulder_frame,
|
|||
|
|
m_elbow_frame=conf.m_elbow_frame,
|
|||
|
|
m_wrist_frame=conf.m_wrist_frame,
|
|||
|
|
m_ee_frame=conf.m_ee_frame,
|
|||
|
|
s_shoulder_frame=conf.s_shoulder_frame,
|
|||
|
|
s_elbow_frame=conf.s_elbow_frame,
|
|||
|
|
s_wrist_frame=conf.s_wrist_frame,
|
|||
|
|
s_ee_frame=conf.s_ee_frame,
|
|||
|
|
slave_joint_names=conf.sew_mapper.slave_joint_names,
|
|||
|
|
up_dir=np.array(conf.sew_mapper.up_dir),
|
|||
|
|
eps_clip=conf.sew_mapper.eps_clip,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 选一组姿态示例（这里用 neutral）
|
|||
|
|
# q_m = pin.neutral(mapper.m_model)
|
|||
|
|
samples = random_qm(mapper, N=500, margin=0.1)
|
|||
|
|
for q_m in samples:
|
|||
|
|
q_s, dbg = mapper.retargetting(q_m)
|
|||
|
|
|
|||
|
|
# 主端点
|
|||
|
|
fk_update(mapper.m_model, mapper.m_data, q_m)
|
|||
|
|
pS_m, _ = pose_of_frame(mapper.m_model, mapper.m_data, conf.m_shoulder_frame)
|
|||
|
|
pE_m, _ = pose_of_frame(mapper.m_model, mapper.m_data, conf.m_elbow_frame)
|
|||
|
|
pW_m, _ = pose_of_frame(mapper.m_model, mapper.m_data, conf.m_wrist_frame)
|
|||
|
|
|
|||
|
|
# 从端（SEW 参考）
|
|||
|
|
pS_s = mapper.pS_s_fixed
|
|||
|
|
pE_s = dbg["pE_s"]
|
|||
|
|
pW_s_ref = dbg["pW_s_ref"]
|
|||
|
|
|
|||
|
|
# 几何向量
|
|||
|
|
r_m = pW_m - pS_m
|
|||
|
|
xhat_m = safe_normalize(r_m)
|
|||
|
|
if xhat_m is None:
|
|||
|
|
raise RuntimeError("Shoulder–wrist vector is zero; check FK / frames.")
|
|||
|
|
|
|||
|
|
# master 上臂平面法向
|
|||
|
|
n_m_raw = np.cross(pE_m - pS_m, pW_m - pS_m)
|
|||
|
|
n_m = safe_normalize(n_m_raw)
|
|||
|
|
|
|||
|
|
# 参考法向（胸部 up 向量投影到垂直于 xhat_m 的平面）
|
|||
|
|
nref_tilde = mapper.up - (mapper.up @ xhat_m) * xhat_m
|
|||
|
|
n_ref = safe_normalize(nref_tilde)
|
|||
|
|
|
|||
|
|
phi_m = dbg["phi_m"]
|
|||
|
|
|
|||
|
|
degenerate_plane = (n_m is None or n_ref is None)
|
|||
|
|
if degenerate_plane:
|
|||
|
|
print("[Warn] S–E–W nearly collinear, swivel angle undefined for this posture.")
|
|||
|
|
# 用一个合理的默认法向，方便画 SEW overall 图；swivel 角度取 0
|
|||
|
|
if n_ref is None:
|
|||
|
|
# 如果连 n_ref 都 degenerate，就随便取一个与 xhat_m 垂直的向量
|
|||
|
|
tmp = np.array([1.0, 0.0, 0.0])
|
|||
|
|
if abs(np.dot(tmp, xhat_m)) > 0.9:
|
|||
|
|
tmp = np.array([0.0, 1.0, 0.0])
|
|||
|
|
n_ref = safe_normalize(tmp - np.dot(tmp, xhat_m) * xhat_m)
|
|||
|
|
if n_m is None:
|
|||
|
|
n_m = n_ref.copy()
|
|||
|
|
phi_m = 0.0 # 退化情形下设成 0 仅用于示意
|
|||
|
|
|
|||
|
|
# 绘制三类图
|
|||
|
|
print("Plotting SEW Geometry...")
|
|||
|
|
plot_sew_geometry(pS_m, pE_m, pW_m,
|
|||
|
|
pS_s, pE_s, pW_s_ref,
|
|||
|
|
xhat_m, n_m, n_ref, phi_m)
|
|||
|
|
|
|||
|
|
if not degenerate_plane:
|
|||
|
|
print("Plotting Swivel Angle Geometry...")
|
|||
|
|
plot_swivel_angle(pS_m, n_ref, n_m, xhat_m)
|
|||
|
|
else:
|
|||
|
|
print("Skip swivel-angle figure for this degenerate posture (S–E–W nearly collinear).")
|
|||
|
|
|
|||
|
|
print("Plotting Two-Sphere Intersection...")
|
|||
|
|
plot_two_sphere_sew(pS_s, pW_s_ref, mapper.L1, mapper.L2, pE_s)
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == '__main__':
|
|||
|
|
main()
|