310 lines
9.9 KiB
Python
310 lines
9.9 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
测试脚本：验证 InteractionEstimator + HapticRenderer 的正确性，
|
||
并绘制论文中使用的误差和能量罐曲线。
|
||
|
||
运行：
|
||
python test_haptic_render.py
|
||
|
||
依赖：
|
||
- pinocchio
|
||
- numpy
|
||
- matplotlib
|
||
- omegaconf
|
||
"""
|
||
|
||
import numpy as np
|
||
import pinocchio as pin
|
||
from omegaconf import OmegaConf
|
||
import matplotlib.pyplot as plt
|
||
|
||
import os, sys
|
||
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
|
||
from core.interaction_estimater import InteractionEstimator
|
||
from core.haptic_render import HapticRenderer, TankParams
|
||
|
||
|
||
def moving_average(x, window: int = 7):
|
||
"""简单滑动平均，用于平滑 alpha 等曲线。"""
|
||
if window <= 1:
|
||
return x
|
||
kernel = np.ones(window, dtype=float) / float(window)
|
||
# 使用 same 保持长度一致
|
||
return np.convolve(x, kernel, mode="same")
|
||
|
||
|
||
def build_model_with_frames():
|
||
"""
|
||
备用：构造一个 6-DoF 示例机械臂，并添加：
|
||
- chest_link: 挂在关节1
|
||
- ee_link: 挂在末端关节
|
||
目前脚本直接从 URDF 加载，不一定用得到。
|
||
"""
|
||
model = pin.buildSampleModelManipulator()
|
||
chest_joint_id = 1
|
||
chest_frame_name = "chest_link"
|
||
model.addFrame(pin.Frame(
|
||
chest_frame_name,
|
||
chest_joint_id,
|
||
chest_joint_id,
|
||
pin.SE3.Identity(),
|
||
pin.FrameType.OP_FRAME
|
||
))
|
||
ee_joint_id = model.njoints - 1
|
||
ee_frame_name = "ee_link"
|
||
model.addFrame(pin.Frame(
|
||
ee_frame_name,
|
||
ee_joint_id,
|
||
ee_joint_id,
|
||
pin.SE3.Identity(),
|
||
pin.FrameType.OP_FRAME
|
||
))
|
||
return model, chest_frame_name, ee_frame_name
|
||
|
||
|
||
def main():
|
||
conf = OmegaConf.load("./config/config.yaml")
|
||
np.random.seed(0)
|
||
|
||
# 1) 构造主/从模型（这里用真实 URDF）
|
||
master_model, _, _ = pin.buildModelsFromUrdf(str(conf.master_urdf))
|
||
slave_model, _, _ = pin.buildModelsFromUrdf(str(conf.slave_urdf))
|
||
|
||
# 从端交互估计器
|
||
est_slave = InteractionEstimator(
|
||
slave_model,
|
||
chest_frame_name=conf.s_base_frame,
|
||
ee_frame_name=conf.s_ee_frame,
|
||
lambda_damp=conf.interaction_est.lambda_damp,
|
||
)
|
||
|
||
# 主端渲染器
|
||
renderer = HapticRenderer(
|
||
master_model,
|
||
chest_frame_name=conf.m_base_frame,
|
||
ee_frame_name=conf.m_ee_frame,
|
||
feedback_strength=conf.haptic_render.feedback_strength,
|
||
E_init=conf.haptic_render.E_init,
|
||
E_max=conf.haptic_render.E_max,
|
||
alpha_floor=conf.haptic_render.alpha_floor,
|
||
alpha_ceil=conf.haptic_render.alpha_ceil,
|
||
E0=conf.haptic_render.E0,
|
||
)
|
||
|
||
nq_s, nv_s = slave_model.nq, slave_model.nv
|
||
nq_m, nv_m = master_model.nq, master_model.nv
|
||
|
||
dt = 0.002
|
||
n_steps = 500 # 足够画出平滑曲线
|
||
|
||
print("========== TEST START ==========")
|
||
print(f"slave nq={nq_s}, master nq={nq_m}, dt={dt}s, steps={n_steps}")
|
||
print("--------------------------------")
|
||
|
||
# -------- 统计量 / 曲线数据 --------
|
||
times = []
|
||
err_tau_int_hist = []
|
||
err_CF_hist = []
|
||
err_tau_fb_hist = []
|
||
E_hist = []
|
||
alpha_hist = []
|
||
|
||
max_err_tau_int = 0.0
|
||
max_err_CF = 0.0
|
||
max_err_tau_fb = 0.0
|
||
|
||
for k in range(n_steps):
|
||
t = k * dt
|
||
times.append(t)
|
||
|
||
# 2) 随机生成从端状态（可改成真实记录或特定轨迹）
|
||
q_s = 0.2 * (np.random.rand(nq_s) - 0.5) # [-0.1,0.1]
|
||
qd_s = 0.1 * (np.random.rand(nv_s) - 0.5)
|
||
qdd_s = np.zeros_like(qd_s)
|
||
|
||
# 从端模型扭矩（M qdd + C qd + g）
|
||
tau_model_s = est_slave._tau_model(q_s, qd_s, qdd_s, tau_ff_fric=None)
|
||
|
||
# 从端胸腔雅可比 C J_s
|
||
CJ_s = est_slave._chest_jacobian(q_s, qd_s) # 6 x nv_s
|
||
|
||
# 3) 人为构造“真实”交互扳手 C F_true
|
||
# 也可以改成随时间变化的模式，例如正弦。
|
||
CF_true = np.array([10.0, 0.0, 0.0, # 10 N 沿 Cx
|
||
0.0, 0.0, 0.0])
|
||
|
||
# 真 · 交互关节力矩
|
||
tau_int_true = CJ_s.T @ CF_true
|
||
|
||
# 构造测量力矩：tau_meas = tau_model + tau_int_true
|
||
tau_meas_s = tau_model_s + tau_int_true
|
||
|
||
# 4) 调用 InteractionEstimator 估计
|
||
tau_int_est, CF_int_est, CJ_s_est = est_slave.estimate(
|
||
q_s, qd_s, qdd_s,
|
||
tau_meas=tau_meas_s,
|
||
tau_ff_fric=None
|
||
)
|
||
|
||
# 从端末端在 C 系的速度扭量（用于能量罐功率）
|
||
V_slave_C = CJ_s @ qd_s
|
||
|
||
# 5) 主端状态（此处用从端状态代替，仅做算法验证）
|
||
q_m = q_s.copy()
|
||
qd_m = qd_s.copy()
|
||
|
||
tau_fb_m_est, alpha = renderer.render_from_CF(
|
||
q_m, qd_m,
|
||
CF_int_slave_C=CF_int_est,
|
||
V_slave_C=V_slave_C,
|
||
dt=dt
|
||
)
|
||
|
||
# 主端胸腔雅可比，用于构造“真 · 反馈扭矩”（不经过能量罐）
|
||
CJ_m = renderer.CJ_master.chest_jacobian(q_m, qd_m)
|
||
tau_fb_m_true = CJ_m.T @ CF_true
|
||
|
||
# -------- 误差计算 --------
|
||
err_tau_int = np.linalg.norm(tau_int_est - tau_int_true)
|
||
err_CF = np.linalg.norm(CF_int_est - CF_true)
|
||
err_tau_fb = np.linalg.norm(tau_fb_m_est - tau_fb_m_true)
|
||
|
||
err_tau_int_hist.append(err_tau_int)
|
||
err_CF_hist.append(err_CF)
|
||
err_tau_fb_hist.append(err_tau_fb)
|
||
|
||
max_err_tau_int = max(max_err_tau_int, err_tau_int)
|
||
max_err_CF = max(max_err_CF, err_CF)
|
||
max_err_tau_fb = max(max_err_tau_fb, err_tau_fb)
|
||
|
||
# 能量罐状态
|
||
E_hist.append(renderer.tank.E)
|
||
alpha_hist.append(alpha)
|
||
|
||
# ---- 每隔若干步打印一次 ----
|
||
if k % 100 == 0:
|
||
print(f"\n--- Step {k} (t = {t:.3f} s) ---")
|
||
print(f"‣ ||tau_int_true|| = {np.linalg.norm(tau_int_true):.4e}")
|
||
print(f" ||tau_int_est - tau_int_true|| = {err_tau_int:.4e}")
|
||
print(f"‣ ||CF_true|| = {np.linalg.norm(CF_true):.4e}")
|
||
print(f" ||CF_int_est - CF_true|| = {err_CF:.4e}")
|
||
print(f"‣ ||tau_fb_m_true|| = {np.linalg.norm(tau_fb_m_true):.4e}")
|
||
print(f" ||tau_fb_m_est - tau_fb_m_true|| = {err_tau_fb:.4e}")
|
||
print(f" Energy tank: E = {renderer.tank.E:.4f}, alpha = {alpha:.4f}")
|
||
|
||
# ---------- 数值结果摘要 ----------
|
||
print("\n========== SUMMARY ==========")
|
||
print(f"max ||tau_int_est - tau_int_true|| = {max_err_tau_int:.4e}")
|
||
print(f"max ||CF_int_est - CF_true|| = {max_err_CF:.4e}")
|
||
print(f"max ||tau_fb_est - tau_fb_true|| = {max_err_tau_fb:.4e}")
|
||
|
||
tol_tau_int = 1e-3
|
||
tol_CF = 1e-3
|
||
tol_tau_fb = 1e-3
|
||
if max_err_tau_int < tol_tau_int and max_err_CF < tol_CF and max_err_tau_fb < tol_tau_fb:
|
||
print("[PASS] 所有误差均在容许范围内。")
|
||
else:
|
||
print("[WARN] 误差超出阈值，请检查算法或考虑调小阻尼 lambda_damp。")
|
||
|
||
# =====================================================
|
||
# 绘图部分（适合论文呈现）
|
||
# =====================================================
|
||
|
||
times = np.array(times)
|
||
err_tau_int_hist = np.array(err_tau_int_hist)
|
||
err_CF_hist = np.array(err_CF_hist)
|
||
err_tau_fb_hist = np.array(err_tau_fb_hist)
|
||
E_hist = np.array(E_hist)
|
||
alpha_hist = np.array(alpha_hist)
|
||
|
||
# 对 alpha 做轻微平滑（论文图更清晰）
|
||
alpha_smooth = moving_average(alpha_hist, window=7)
|
||
|
||
# ------------------ 误差子图（3 个独立子图） ------------------
|
||
fig_err, (ax1, ax2, ax3) = plt.subplots(
|
||
3, 1, sharex=True, figsize=(6, 7)
|
||
)
|
||
|
||
# (a) Interaction torque estimation error
|
||
ax1.plot(
|
||
times,
|
||
err_tau_int_hist,
|
||
color="C0",
|
||
label=r"$\|\tau_{\mathrm{int}}^{\mathrm{est}}-\tau_{\mathrm{int}}^{\mathrm{true}}\|$",
|
||
)
|
||
ax1.set_title("(a) Interaction torque estimation error")
|
||
ax1.set_ylabel(r"Error norm $\|\cdot\|$")
|
||
ax1.set_yscale("log")
|
||
ax1.grid(True, linestyle="--", linewidth=0.5)
|
||
ax1.legend(loc="lower right")
|
||
|
||
# (b) Interaction wrench estimation error
|
||
ax2.plot(
|
||
times,
|
||
err_CF_hist,
|
||
color="C1",
|
||
label=r"$\|{}^{C}F_{\mathrm{int}}^{\mathrm{est}}-{}^{C}F_{\mathrm{true}}\|$",
|
||
)
|
||
ax2.set_title("(b) Interaction wrench estimation error")
|
||
ax2.set_ylabel(r"Error norm $\|\cdot\|$")
|
||
ax2.set_yscale("log")
|
||
ax2.grid(True, linestyle="--", linewidth=0.5)
|
||
ax2.legend(loc="lower right")
|
||
|
||
# (c) Feedback torque rendering error
|
||
ax3.plot(
|
||
times,
|
||
err_tau_fb_hist,
|
||
color="C2",
|
||
label=r"$\|\tau_{\mathrm{fb}}^{\mathrm{est}}-\tau_{\mathrm{fb}}^{\mathrm{true}}\|$",
|
||
)
|
||
ax3.set_title("(c) Haptic feedback torque error")
|
||
ax3.set_xlabel(r"Time $t$ [s]")
|
||
ax3.set_ylabel(r"Error norm $\|\cdot\|$")
|
||
ax3.set_yscale("log")
|
||
ax3.grid(True, linestyle="--", linewidth=0.5)
|
||
ax3.legend(loc="lower right")
|
||
|
||
fig_err.tight_layout()
|
||
# fig_err.savefig("figs/haptic_errors_3subplots.png", dpi=300, bbox_inches="tight")
|
||
|
||
# ------------------ 能量罐 + 缩放系数 ------------------
|
||
fig_tank, ax1_t = plt.subplots(figsize=(6, 4))
|
||
|
||
ax1_t.set_title("Energy tank dynamics and scaling factor")
|
||
|
||
l1 = ax1_t.plot(
|
||
times,
|
||
E_hist,
|
||
color="C0",
|
||
label=r"Tank energy $E[k]$",
|
||
)
|
||
ax1_t.set_xlabel(r"Time $t$ [s]")
|
||
ax1_t.set_ylabel(r"Energy $E$ [J]")
|
||
ax1_t.grid(True, linestyle="--", linewidth=0.5)
|
||
|
||
ax2_t = ax1_t.twinx()
|
||
l2 = ax2_t.plot(
|
||
times,
|
||
alpha_smooth,
|
||
color="C1",
|
||
linestyle="--",
|
||
label=r"Scaling $\alpha[k]$ (smoothed)",
|
||
)
|
||
ax2_t.set_ylabel(r"Scaling factor $\alpha$")
|
||
|
||
# Legend 合并并右下角防遮挡
|
||
lines = l1 + l2
|
||
labels = [l.get_label() for l in lines]
|
||
ax1_t.legend(lines, labels, loc="lower right")
|
||
|
||
fig_tank.tight_layout()
|
||
# fig_tank.savefig("figs/haptic_tank_smooth.png", dpi=300, bbox_inches="tight")
|
||
|
||
plt.show()
|
||
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|