512 lines
16 KiB
Python
512 lines
16 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
ablation_haptic.py
|
||
|
||
Quantitative ablation for the haptic rendering module.
|
||
|
||
Modes:
|
||
- baseline : full method (energy tank + F/V filtering + alpha smoothing + torque LPF)
|
||
- no_tank : energy tank disabled (no passivity enforcement)
|
||
- no_filter : tank enabled but F/V filtering + alpha smoothing + torque LPF disabled
|
||
|
||
Run:
|
||
python test/ablation_haptic.py
|
||
from the project root, with config/config.yaml following the same
|
||
structure as demo.py and test_sew.py.
|
||
"""
|
||
|
||
import os
|
||
import sys
|
||
import time
|
||
from typing import Dict
|
||
|
||
import numpy as np
|
||
import pinocchio as pin
|
||
|
||
import matplotlib.pyplot as plt
|
||
from omegaconf import OmegaConf
|
||
|
||
# Make sure project root is on sys.path
|
||
THIS_DIR = os.path.dirname(__file__)
|
||
PROJECT_ROOT = os.path.abspath(os.path.join(THIS_DIR, ".."))
|
||
if PROJECT_ROOT not in sys.path:
|
||
sys.path.append(PROJECT_ROOT)
|
||
|
||
from core.sew_mapper import SEWMapper
|
||
from core.interaction_estimater import InteractionEstimator
|
||
from core.haptic_render import HapticRenderer
|
||
|
||
|
||
# -------------------- Utilities --------------------
|
||
|
||
def dummy_master_measure(step: int, model: pin.Model) -> np.ndarray:
|
||
"""
|
||
Synthetic master joint measurement used for offline ablation.
|
||
Start from neutral and add small sinusoidal motions on the first few joints.
|
||
"""
|
||
q = pin.neutral(model)
|
||
n_use = min(model.nq, 6)
|
||
for i in range(n_use):
|
||
q[i] = 0.5 * np.sin(0.01 * step + 0.7 * i)
|
||
return q
|
||
|
||
|
||
def dummy_slave_dynamics(q_cmd: np.ndarray, model: pin.Model) -> np.ndarray:
|
||
"""
|
||
Simple slave "execution" model: follow the commanded joint positions
|
||
with a small Gaussian disturbance, to emulate interaction / noise.
|
||
"""
|
||
q_cmd = np.asarray(q_cmd, dtype=float).reshape(-1,)
|
||
noise = 0.001 * np.random.randn(model.nq)
|
||
return q_cmd + noise
|
||
|
||
|
||
def load_config():
|
||
"""
|
||
Load config/config.yaml in the project root (same style as demo.py / test_sew.py).
|
||
"""
|
||
cand = os.path.join(PROJECT_ROOT, "config", "config.yaml")
|
||
if not os.path.exists(cand):
|
||
raise FileNotFoundError(f"config.yaml not found at: {cand}")
|
||
conf = OmegaConf.load(cand)
|
||
print(f"[INFO] Loaded config from: {cand}")
|
||
return conf
|
||
|
||
|
||
def build_modules(conf):
|
||
# Build models from URDF
|
||
master_model = pin.buildModelFromUrdf(str(conf.master_urdf))
|
||
slave_model = pin.buildModelFromUrdf(str(conf.slave_urdf))
|
||
|
||
# SEW mapper
|
||
sew = 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, dtype=float),
|
||
eps_clip=float(conf.sew_mapper.eps_clip),
|
||
)
|
||
|
||
# Interaction estimator on slave side
|
||
estimator = InteractionEstimator(
|
||
model=slave_model,
|
||
chest_frame_name=conf.s_base_frame,
|
||
ee_frame_name=conf.s_ee_frame,
|
||
lambda_damp=float(conf.interaction_est.lambda_damp),
|
||
)
|
||
|
||
return master_model, slave_model, sew, estimator
|
||
|
||
|
||
|
||
# -------------------- Haptic renderer variants --------------------
|
||
|
||
class HapticRendererNoTank(HapticRenderer):
|
||
"""
|
||
Variant that completely disables the energy tank:
|
||
- directly maps interaction wrench to master joint torques through CJ_m^T
|
||
- no passivity enforcement
|
||
"""
|
||
|
||
def _compute_feedback_tau(self,
|
||
q_m: np.ndarray,
|
||
qd_m: np.ndarray,
|
||
CF_int_slave_C: np.ndarray,
|
||
V_slave_C: np.ndarray | None,
|
||
dt: float):
|
||
CF = np.asarray(CF_int_slave_C, dtype=float).reshape(6,)
|
||
# Chest Jacobian on master side
|
||
CJ_m = self.CJ_master.chest_jacobian(q_m, qd_m)
|
||
tau_fb_m_raw = CJ_m.T @ CF
|
||
alpha = 1.0
|
||
return tau_fb_m_raw, alpha
|
||
|
||
|
||
def make_renderer(conf, master_model: pin.Model, mode: str) -> HapticRenderer:
|
||
"""
|
||
Construct different haptic renderers for ablation.
|
||
"""
|
||
common_kwargs = dict(
|
||
master_model=master_model,
|
||
chest_frame_name=conf.m_base_frame,
|
||
ee_frame_name=conf.m_ee_frame,
|
||
feedback_strength=float(conf.haptic_render.feedback_strength),
|
||
E_init=float(conf.haptic_render.E_init),
|
||
E_max=float(conf.haptic_render.E_max),
|
||
alpha_floor=float(conf.haptic_render.alpha_floor),
|
||
alpha_ceil=float(conf.haptic_render.alpha_ceil),
|
||
E0=float(conf.haptic_render.E0),
|
||
)
|
||
|
||
if mode == "baseline":
|
||
renderer = HapticRenderer(**common_kwargs)
|
||
|
||
# baseline：能量罐 + 强滤波 + 平滑 + 扭矩低通（“管得很严”）
|
||
renderer.tank.tp.force_alpha = 0.05
|
||
renderer.tank.tp.vel_alpha = 0.05
|
||
renderer.tank.tp.alpha_smooth = 0.02
|
||
renderer.tank.tp.power_deadzone = 0.0
|
||
renderer.tau_alpha = 0.05
|
||
|
||
elif mode == "no_tank":
|
||
renderer = HapticRendererNoTank(**common_kwargs)
|
||
|
||
# no_tank：完全不做能量控制，也不做扭矩低通，暴露最差情况
|
||
renderer.tau_alpha = 1.0
|
||
|
||
elif mode == "no_filter":
|
||
renderer = HapticRenderer(**common_kwargs)
|
||
|
||
# no_filter：保留能量罐，但不给它任何滤波/平滑能力
|
||
renderer.tank.tp.force_alpha = 1.0
|
||
renderer.tank.tp.vel_alpha = 1.0
|
||
renderer.tank.tp.alpha_smooth = 1.0
|
||
renderer.tank.tp.power_deadzone = 0.0
|
||
renderer.tau_alpha = 1.0
|
||
|
||
else:
|
||
raise ValueError(f"Unknown ablation mode: {mode}")
|
||
|
||
return renderer
|
||
|
||
|
||
# -------------------- Teleoperation simulation (offline) --------------------
|
||
|
||
def simulate_teleop(conf,
|
||
master_model: pin.Model,
|
||
slave_model: pin.Model,
|
||
sew: SEWMapper,
|
||
estimator: InteractionEstimator,
|
||
renderer: HapticRenderer,
|
||
Ts: float = 0.002,
|
||
steps: int = 800) -> Dict[str, np.ndarray]:
|
||
"""
|
||
Offline simulation similar to demo.py, but without real-time delays.
|
||
Returns logs for computing quantitative metrics.
|
||
"""
|
||
|
||
# States
|
||
q_slave = pin.neutral(slave_model)
|
||
dq_slave = np.zeros(slave_model.nv)
|
||
q_slave_prev = q_slave.copy()
|
||
|
||
q_m_init = dummy_master_measure(0, master_model)
|
||
pre_q_m = q_m_init.copy()
|
||
pre_qd_m = np.zeros_like(q_m_init)
|
||
|
||
# Logs
|
||
log_tau_fb = []
|
||
log_alpha = []
|
||
log_E = []
|
||
log_tau_int_norm = []
|
||
log_qd_m = []
|
||
ee_traj_world = []
|
||
|
||
# Harder virtual wall to highlight differences
|
||
d_wall = 0.35
|
||
k_wall = 20000.0
|
||
|
||
for k in range(steps):
|
||
# --- master state ---
|
||
q_m = dummy_master_measure(k, master_model)
|
||
if k == 0:
|
||
qd_m = np.zeros_like(q_m)
|
||
qdd_m = np.zeros_like(q_m)
|
||
else:
|
||
qd_m = (q_m - pre_q_m) / Ts
|
||
qdd_m = (qd_m - pre_qd_m) / Ts
|
||
|
||
pre_q_m = q_m
|
||
pre_qd_m = qd_m
|
||
log_qd_m.append(qd_m.copy())
|
||
|
||
# --- SEW retargetting: master -> desired slave ---
|
||
q_des_slave, _ = sew.retargetting(q_m, q_slave)
|
||
|
||
# --- slave motion (with small noise) ---
|
||
q_slave = dummy_slave_dynamics(q_des_slave, slave_model)
|
||
dq_slave = (q_slave - q_slave_prev) / Ts
|
||
q_slave_prev = q_slave.copy()
|
||
qdd_slave = np.zeros(slave_model.nv)
|
||
|
||
# --- virtual wall on slave side ---
|
||
CJ_env = estimator._chest_jacobian(q_slave, dq_slave)
|
||
|
||
oTC = estimator.data.oMf[estimator.fid_C] # ^wT_C
|
||
oTEE = estimator.data.oMf[estimator.fid_EE] # ^wT_EE
|
||
|
||
R_wc = oTC.rotation
|
||
p_wc = oTC.translation
|
||
R_cw = R_wc.T
|
||
p_cw = -R_cw @ p_wc
|
||
|
||
p_we = oTEE.translation
|
||
ee_traj_world.append(p_we.copy())
|
||
p_ce = R_cw @ p_we + p_cw
|
||
x_ce = float(p_ce[0])
|
||
|
||
if x_ce > d_wall:
|
||
delta = x_ce - d_wall
|
||
Fx = -k_wall * delta
|
||
CF_env = np.array([Fx, 0, 0, 0, 0, 0], dtype=float)
|
||
else:
|
||
CF_env = np.zeros(6, dtype=float)
|
||
|
||
tau_env = CJ_env.T @ CF_env
|
||
|
||
# --- interaction estimation on slave ---
|
||
tau_model = estimator._tau_model(q_slave, dq_slave, qdd_slave)
|
||
tau_meas = tau_model + tau_env
|
||
tau_int, CF_int, CJ = estimator.estimate(q_slave, dq_slave, qdd_slave, tau_meas)
|
||
|
||
log_tau_int_norm.append(np.linalg.norm(tau_int))
|
||
|
||
V_slave_C = CJ @ dq_slave # 6 x 1
|
||
|
||
# --- haptic rendering on master side ---
|
||
tau_cmd_m, tau_fb_m, alpha = renderer.render_tau(
|
||
q_m=q_m,
|
||
qd_m=qd_m,
|
||
qdd_m=qdd_m,
|
||
CF_int_slave_C=CF_int,
|
||
V_slave_C=V_slave_C,
|
||
tau_ff_fric=None,
|
||
dt=Ts,
|
||
)
|
||
|
||
log_tau_fb.append(tau_fb_m.copy())
|
||
log_alpha.append(alpha)
|
||
# not all variants really use the tank, but querying E is safe
|
||
log_E.append(getattr(renderer.tank, "E", 0.0))
|
||
|
||
# stack
|
||
log_tau_fb = np.vstack(log_tau_fb)
|
||
log_alpha = np.asarray(log_alpha)
|
||
log_E = np.asarray(log_E)
|
||
log_tau_int_norm = np.asarray(log_tau_int_norm)
|
||
log_qd_m = np.vstack(log_qd_m)
|
||
ee_traj_world = np.vstack(ee_traj_world)
|
||
|
||
return dict(
|
||
tau_fb=log_tau_fb,
|
||
alpha=log_alpha,
|
||
E=log_E,
|
||
tau_int_norm=log_tau_int_norm,
|
||
qd_m=log_qd_m,
|
||
ee_traj_world=ee_traj_world,
|
||
)
|
||
|
||
|
||
# -------------------- Metric computation --------------------
|
||
|
||
def compute_metrics(tau_fb: np.ndarray,
|
||
qd_m: np.ndarray,
|
||
E: np.ndarray,
|
||
Ts: float) -> Dict[str, float]:
|
||
"""
|
||
Compute quantitative metrics for ablation:
|
||
- rms / peak torque
|
||
- torque spike count / rate
|
||
- positive power injection ratio
|
||
- (optional) energy range if tank is used
|
||
"""
|
||
# torque norms
|
||
tau_norm = np.linalg.norm(tau_fb, axis=1)
|
||
rms_tau = float(np.sqrt(np.mean(tau_norm ** 2)))
|
||
peak_tau = float(np.max(tau_norm))
|
||
|
||
# torque spikes: ||Δτ||_∞ > threshold
|
||
dtau = np.diff(tau_fb, axis=0)
|
||
dtau_inf = np.max(np.abs(dtau), axis=1)
|
||
spike_th = 5.0 # Nm, can be adjusted to your system scale
|
||
n_spikes = int(np.sum(dtau_inf > spike_th))
|
||
spike_rate = n_spikes / (len(dtau_inf) * Ts)
|
||
|
||
# positive power injection ratio: tau_fb^T * qd_m > 0
|
||
P = np.sum(tau_fb * qd_m, axis=1)
|
||
P_inject_ratio = float(np.mean(P > 0.0))
|
||
|
||
metrics = dict(
|
||
rms_tau=rms_tau,
|
||
peak_tau=peak_tau,
|
||
n_spikes=n_spikes,
|
||
spike_rate=spike_rate,
|
||
P_inject_ratio=P_inject_ratio,
|
||
)
|
||
|
||
if np.any(E != 0.0):
|
||
metrics["E_min"] = float(np.min(E))
|
||
metrics["E_max"] = float(np.max(E))
|
||
|
||
return metrics
|
||
|
||
|
||
def plot_ablation_results(all_logs, all_metrics, Ts):
|
||
|
||
modes = ["baseline", "no_tank", "no_filter"]
|
||
colors = {
|
||
"baseline": "tab:blue",
|
||
"no_tank": "tab:red",
|
||
"no_filter": "tab:green",
|
||
}
|
||
|
||
# -------------------------------------------
|
||
# 1) Torque feedback trajectories (norm vs time)
|
||
# -------------------------------------------
|
||
plt.figure(figsize=(8,4))
|
||
for mode in modes:
|
||
tau_norm = np.linalg.norm(all_logs[mode]["tau_fb"], axis=1)
|
||
t = np.arange(len(tau_norm)) * Ts
|
||
plt.plot(t, tau_norm, label=mode, color=colors[mode])
|
||
plt.xlabel("Time [s]")
|
||
plt.ylabel(r"$\|\tau_{\mathrm{fb}}(t)\|\ \mathrm{[N\cdot m]}$")
|
||
plt.title("Feedback Torque Norm Over Time")
|
||
plt.legend()
|
||
plt.grid(True, alpha=0.3)
|
||
plt.tight_layout()
|
||
plt.savefig("logs_ablation/fig_tau_fb_norm.png", dpi=300)
|
||
|
||
# -------------------------------------------
|
||
# 2) Alpha(t) (energy tank scaling)
|
||
# -------------------------------------------
|
||
plt.figure(figsize=(8,4))
|
||
for mode in modes:
|
||
alpha = all_logs[mode]["alpha"]
|
||
t = np.arange(len(alpha)) * Ts
|
||
plt.plot(t, alpha, label=mode, color=colors[mode])
|
||
plt.xlabel("Time [s]")
|
||
plt.ylabel(r"$\alpha(t)$")
|
||
plt.title("Energy Tank Scaling Factor")
|
||
plt.ylim([-0.1, 1.1])
|
||
plt.legend()
|
||
plt.grid(True, alpha=0.3)
|
||
plt.tight_layout()
|
||
plt.savefig("logs_ablation/fig_alpha.png", dpi=300)
|
||
|
||
# -------------------------------------------
|
||
# 3) Tank Energy E(t)
|
||
# -------------------------------------------
|
||
plt.figure(figsize=(8,4))
|
||
for mode in modes:
|
||
E = all_logs[mode]["E"]
|
||
t = np.arange(len(E)) * Ts
|
||
plt.plot(t, E, label=mode, color=colors[mode])
|
||
plt.xlabel("Time [s]")
|
||
plt.ylabel(r"$E(t)$")
|
||
plt.title("Energy Tank Level")
|
||
plt.legend()
|
||
plt.grid(True, alpha=0.3)
|
||
plt.tight_layout()
|
||
plt.savefig("logs_ablation/fig_energy.png", dpi=300)
|
||
|
||
# -------------------------------------------
|
||
# 4) Bar charts: RMS / Peak / Spikes / P>0
|
||
# -------------------------------------------
|
||
metrics_list = ["rms_tau", "peak_tau", "n_spikes", "P_inject_ratio"]
|
||
metric_names = [
|
||
"RMS Torque",
|
||
"Peak Torque",
|
||
"# Torque Spikes",
|
||
"Positive Power Ratio",
|
||
]
|
||
|
||
plt.figure(figsize=(9,4))
|
||
for i, key in enumerate(metrics_list):
|
||
plt.subplot(1,4,i+1)
|
||
vals = [all_metrics[m][key] for m in modes]
|
||
plt.bar(modes, vals, color=[colors[m] for m in modes])
|
||
plt.title(metric_names[i])
|
||
plt.xticks(rotation=45)
|
||
plt.grid(True, alpha=0.3)
|
||
plt.tight_layout()
|
||
plt.savefig("logs_ablation/fig_metrics_barchart.png", dpi=300)
|
||
|
||
print("[INFO] All plots saved to logs_ablation/")
|
||
|
||
|
||
|
||
# -------------------- Main --------------------
|
||
|
||
def main():
|
||
conf = load_config()
|
||
master_model, slave_model, sew, estimator = build_modules(conf)
|
||
|
||
Ts = 0.002
|
||
steps = 800
|
||
|
||
modes = ["baseline", "no_tank", "no_filter"]
|
||
all_logs = {}
|
||
all_metrics = {}
|
||
|
||
print(f"dt = {Ts:.4f} s, steps = {steps}")
|
||
|
||
for mode in modes:
|
||
print(f"\n========== Running haptic ablation: {mode} ==========")
|
||
renderer = make_renderer(conf, master_model, mode)
|
||
renderer.reset_tank(float(conf.haptic_render.E0))
|
||
|
||
# fix random seed so that each mode sees the same slave noise
|
||
np.random.seed(0)
|
||
|
||
t0 = time.time()
|
||
logs = simulate_teleop(
|
||
conf,
|
||
master_model,
|
||
slave_model,
|
||
sew,
|
||
estimator,
|
||
renderer,
|
||
Ts=Ts,
|
||
steps=steps,
|
||
)
|
||
elapsed = time.time() - t0
|
||
|
||
all_logs[mode] = logs
|
||
|
||
metrics = compute_metrics(
|
||
tau_fb=logs["tau_fb"],
|
||
qd_m=logs["qd_m"],
|
||
E=logs["E"],
|
||
Ts=Ts,
|
||
)
|
||
all_metrics[mode] = metrics
|
||
|
||
print(
|
||
f"[{mode}] done in {elapsed:.3f} s | "
|
||
f"rms_tau={metrics['rms_tau']:.3f}, "
|
||
f"peak_tau={metrics['peak_tau']:.3f}, "
|
||
f"spikes={metrics['n_spikes']}, "
|
||
f"spike_rate={metrics['spike_rate']:.3f} 1/s, "
|
||
f"P>0={metrics['P_inject_ratio']:.3f}, "
|
||
f"E_range="
|
||
f"{'[{:.2f},{:.2f}]'.format(metrics['E_min'], metrics['E_max']) if 'E_min' in metrics else 'N/A'}"
|
||
)
|
||
|
||
# Save logs for further plotting if needed
|
||
save_dir = os.path.join(PROJECT_ROOT, "logs_ablation")
|
||
os.makedirs(save_dir, exist_ok=True)
|
||
save_path = os.path.join(save_dir, "haptics_ablation_results.npz")
|
||
|
||
# pack as a single dict and rely on pickle when loading
|
||
np.savez(
|
||
save_path,
|
||
all_logs=all_logs,
|
||
all_metrics=all_metrics,
|
||
Ts=Ts,
|
||
)
|
||
|
||
print(f"\n[INFO] Ablation logs and metrics saved to:\n {save_path}")
|
||
plot_ablation_results(all_logs, all_metrics, Ts)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|