# haptic_render.py # -*- coding: utf-8 -*- from __future__ import annotations from dataclasses import dataclass import numpy as np try: import pinocchio as pin except ModuleNotFoundError: # Pure supervisor tests do not require Pinocchio. pin = None # ===== Final applied-port energy supervision ===== @dataclass class TankParams: E_min: float = 0.5 E_max: float = 20.0 alpha_floor: float = 0.0 alpha_ceil: float = 1.0 power_epsilon: float = 1e-12 # Legacy fields are kept so existing configuration/ablation code can load. # They are deliberately not used for energy accounting: filtering, a power # dead-zone, or alpha smoothing at this stage would make the tank account a # different signal from the torque actually applied at the master port. force_alpha: float = 0.2 vel_alpha: float = 0.2 alpha_smooth: float = 0.1 power_deadzone: float = 0.0 @dataclass(frozen=True) class AppliedPortDiagnostics: """Snapshot of one final-port projection/accounting step.""" tau_candidate: np.ndarray tau_applied: np.ndarray rho: float E_before: float E_preclip: float E_after: float candidate_power: float power: float fail_safe_active: bool class AppliedPortEnergySupervisor: """Passivity supervisor at the final master haptic-torque port. Positive ``tau_app @ qd_m`` is power delivered by the device to the operator and therefore discharges the tank. Negative power charges the tank. The candidate torque passed to :meth:`apply` must already include all nominal scaling, filtering, rate limiting, and actuator saturation. The returned torque is final: downstream code must not filter, rescale, rate-limit, or saturate it. The projection is radial, ``tau_app = alpha * tau_candidate``. This keeps a previously saturated candidate inside its actuator limits while imposing the exact one-step energy budget. ``alpha_floor`` is never allowed to override the energy-safe upper bound. """ def __init__(self, tank: TankParams | None = None, E0: float = 2.0): self.tp = TankParams() if tank is None else tank if not np.isfinite(self.tp.E_min) or not np.isfinite(self.tp.E_max): raise ValueError("E_min and E_max must be finite") if self.tp.E_max < self.tp.E_min: raise ValueError("E_max must be greater than or equal to E_min") if not 0.0 <= self.tp.alpha_ceil <= 1.0: raise ValueError("alpha_ceil must lie in [0, 1]") if not 0.0 <= self.tp.alpha_floor <= self.tp.alpha_ceil: raise ValueError("alpha_floor must lie in [0, alpha_ceil]") if self.tp.power_epsilon < 0.0: raise ValueError("power_epsilon must be non-negative") self.E = float(np.clip(E0, self.tp.E_min, self.tp.E_max)) self.alpha = 1.0 self.last_alpha = 1.0 self.last_power = 0.0 self.last_energy_before = self.E self.last_energy_after = self.E self.last_tau_candidate = np.empty(0, dtype=float) self.last_tau_applied = np.empty(0, dtype=float) self.fail_safe_active = False self.last_diagnostics: AppliedPortDiagnostics | None = None def reset(self, E0: float | None = None): if E0 is None: E0 = self.tp.E_min self.E = float(np.clip(E0, self.tp.E_min, self.tp.E_max)) self.alpha = 1.0 self.last_alpha = 1.0 self.last_power = 0.0 self.last_energy_before = self.E self.last_energy_after = self.E self.last_tau_candidate = np.empty(0, dtype=float) self.last_tau_applied = np.empty(0, dtype=float) self.fail_safe_active = False self.last_diagnostics = None def _record(self, tau_candidate: np.ndarray, tau_applied: np.ndarray, rho: float, E_before: float, E_preclip: float, E_after: float, candidate_power: float, power: float, fail_safe_active: bool) -> AppliedPortDiagnostics: """Record one result while retaining legacy scalar/state attributes.""" diagnostics = AppliedPortDiagnostics( tau_candidate=tau_candidate.copy(), tau_applied=tau_applied.copy(), rho=float(rho), E_before=float(E_before), E_preclip=float(E_preclip), E_after=float(E_after), candidate_power=float(candidate_power), power=float(power), fail_safe_active=bool(fail_safe_active), ) self.alpha = diagnostics.rho self.last_alpha = diagnostics.rho self.last_power = diagnostics.power self.last_energy_before = diagnostics.E_before self.last_energy_after = diagnostics.E_after self.last_tau_candidate = diagnostics.tau_candidate.copy() self.last_tau_applied = diagnostics.tau_applied.copy() self.fail_safe_active = diagnostics.fail_safe_active self.last_diagnostics = diagnostics return diagnostics def _zero_fail_safe( self, tau_candidate: np.ndarray, ) -> tuple[np.ndarray, AppliedPortDiagnostics]: tau_zero = np.zeros_like(tau_candidate, dtype=float) diagnostics = self._record( tau_candidate=tau_candidate, tau_applied=tau_zero, rho=0.0, E_before=self.E, E_preclip=self.E, E_after=self.E, candidate_power=0.0, power=0.0, fail_safe_active=True, ) return tau_zero, diagnostics def apply(self, tau_raw: np.ndarray, qd: np.ndarray, dt: float) -> tuple[np.ndarray, AppliedPortDiagnostics]: """Return the final safe torque and diagnostics, accounting exactly once. ``tau_raw`` is the already shaped and actuator-limited candidate at the master joint port. It may come directly from a mapping such as ``A.T @ tau_s`` or ``J_m.T @ F``. ``qd`` is the velocity at that same port. The returned ``tau_app`` must be sent unchanged to the haptic output (apart from an emergency zero-output fail-safe). """ tau_candidate = np.asarray(tau_raw, dtype=float).reshape(-1,) qd_m = np.asarray(qd, dtype=float).reshape(-1,) if tau_candidate.shape != qd_m.shape: raise ValueError( "tau_raw and qd must have identical one-dimensional shapes" ) if ( tau_candidate.size == 0 or not np.isfinite(dt) or dt <= 0.0 or not np.all(np.isfinite(tau_candidate)) or not np.all(np.isfinite(qd_m)) ): return self._zero_fail_safe(tau_candidate) energy_before = self.E nominal_power = float(np.dot(tau_candidate, qd_m)) if not np.isfinite(nominal_power): return self._zero_fail_safe(tau_candidate) rho = self.tp.alpha_ceil # Only device-to-human power consumes stored energy. alpha_floor is a # preference, not a safety constraint, and cannot force an unsafe gain. if nominal_power > 0.0: available = max(0.0, energy_before - self.tp.E_min) rho_energy = available / (nominal_power * dt) rho = min(rho, max(0.0, rho_energy)) tau_app = rho * tau_candidate applied_power = float(np.dot(tau_app, qd_m)) # This is the sole tank update. It uses the exact torque returned to # the caller and the exact master velocity supplied for this sample. energy_preclip = energy_before - applied_power * dt self.E = float(np.clip(energy_preclip, self.tp.E_min, self.tp.E_max)) diagnostics = self._record( tau_candidate=tau_candidate, tau_applied=tau_app, rho=rho, E_before=energy_before, E_preclip=energy_preclip, E_after=self.E, candidate_power=nominal_power, power=applied_power, fail_safe_active=False, ) return tau_app, diagnostics def project_and_account(self, tau_candidate: np.ndarray, qd_m: np.ndarray, dt: float) -> tuple[float, np.ndarray]: """Compatibility wrapper around :meth:`apply`. Returns ``(alpha, tau_app)``. The wrapper invokes ``apply`` exactly once, so it cannot introduce a second tank update. """ tau_app, diagnostics = self.apply(tau_candidate, qd_m, dt) return diagnostics.rho, tau_app def observe_master(self, *_args, **_kwargs): """Reject the former second accounting path.""" raise RuntimeError( "observe_master() is disabled: use apply() exactly once " "at the final applied master-torque port" ) def enforce(self, *_args, **_kwargs): """Reject wrench/twist-port enforcement from the former implementation.""" raise RuntimeError( "enforce(CF, VC, dt) is disabled: passivity is enforced on final " "master tau_app and qd_m via apply()" ) # Backward-compatible class name for existing imports. EnergyTankPOPC = AppliedPortEnergySupervisor # ===== 主端胸腔雅可比 ===== class ChestJacobianMaster: def __init__(self, model: pin.Model, chest_frame_name: str, ee_frame_name: str): if pin is None: raise ModuleNotFoundError( "Pinocchio is required to construct ChestJacobianMaster" ) self.model = model self.data = model.createData() self.fid_C = model.getFrameId(chest_frame_name) self.fid_EE = model.getFrameId(ee_frame_name) def chest_jacobian(self, q_m, qd_m): pin.forwardKinematics(self.model, self.data, q_m, qd_m) pin.updateFramePlacements(self.model, self.data) # LOCAL_WORLD_ALIGNED expresses the twist at the EE point in axes # parallel to the world frame. The spatial-vector convention used by # this project is [linear; angular]. Jw = pin.computeFrameJacobian(self.model, self.data, q_m, self.fid_EE, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) # Rotate both vector blocks into chest axes, but do not translate the # reference point: the wrench paired with this Jacobian still acts at # the EE point. A full spatial adjoint here would introduce a spurious # moment-arm term between the chest origin and EE point. oTC = self.data.oMf[self.fid_C] # ^wT_C R_cw = oTC.rotation.T rotate_axes = np.zeros((6, 6)) rotate_axes[:3, :3] = R_cw rotate_axes[3:, 3:] = R_cw CJ_m = rotate_axes @ Jw return CJ_m # ===== 主端力反馈渲染 ===== class HapticRenderer: """Render interaction wrench as final, passivity-supervised master torque. The haptic path has one strict ordering: ``J_m.T @ F`` (direct baseline) -> optional nominal low-pass -> nominal sign/strength scaling -> torque-rate limiting -> actuator torque saturation -> final applied-port energy projection and single accounting update. No filtering or limiting is performed after the energy projection. The second returned torque is therefore the exact ``tau_app`` used in ``tau_app @ qd_m`` for the tank update. """ def __init__(self, master_model: pin.Model, chest_frame_name: str, ee_frame_name: str, feedback_strength: float, E_init: float, E_max: float, alpha_floor: float, alpha_ceil: float, E0: float = 2.0, torque_limit: float | np.ndarray | None = None, torque_rate_limit: float | np.ndarray | None = None, tau_filter_alpha: float = 0.2): # 约定 feedback_strength > 0，反作用时乘上一个负号 self.feedback_strength = float(feedback_strength) if not np.isfinite(self.feedback_strength): raise ValueError("feedback_strength must be finite") self.CJ_master = ChestJacobianMaster(master_model, chest_frame_name, ee_frame_name) tank_params = TankParams( E_min=E_init, E_max=E_max, alpha_floor=alpha_floor, alpha_ceil=alpha_ceil, ) self.tank = AppliedPortEnergySupervisor(tank_params, E0) # Nominal shaping state. All of this is upstream of the energy # projection. Public tau_alpha is retained for existing scripts. self.tau_alpha = float(tau_filter_alpha) if not 0.0 <= self.tau_alpha <= 1.0: raise ValueError("tau_filter_alpha must lie in [0, 1]") self.torque_limit = torque_limit self.torque_rate_limit = torque_rate_limit self._tau_fb_state = np.zeros(master_model.nv) self._tau_applied_prev = np.zeros(master_model.nv) self.last_rate_limit_active = False self.last_torque_saturation_active = False def reset_tank(self, E0: float = None): self.tank.reset(E0) self._tau_fb_state[:] = 0.0 self._tau_applied_prev[:] = 0.0 self.last_rate_limit_active = False self.last_torque_saturation_active = False # ---------- 主端逆动力学：计算 tau_master_current ---------- def _inverse_dynamics(self, q_m: np.ndarray, qd_m: np.ndarray, qdd_m: np.ndarray, tau_ff_fric: np.ndarray | None = None) -> np.ndarray: """ 主端动力学模型： τ = M(q) qdd + C(q,qd) qd + g(q) + τ_ff_fric """ q_m = np.asarray(q_m, dtype=float).reshape(-1,) qd_m = np.asarray(qd_m, dtype=float).reshape(-1,) qdd_m = np.asarray(qdd_m, dtype=float).reshape(-1,) if tau_ff_fric is not None: tau_ff_fric = np.asarray(tau_ff_fric, dtype=float).reshape(-1,) model = self.CJ_master.model data = self.CJ_master.data # 惯量矩阵 M(q) M = pin.crba(model, data, q_m) M = (M + M.T) - np.diag(M.diagonal()) # 数值对称化 # 非线性项（科氏/离心 + 重力） nle = pin.nonLinearEffects(model, data, q_m, qd_m) # = C(q,qd)qd + g(q) g = pin.computeGeneralizedGravity(model, data, q_m) Cqd = nle - g tau = M @ qdd_m + Cqd + g if tau_ff_fric is not None: tau = tau + tau_ff_fric return tau def compute_tau_master_current(self, q_m: np.ndarray, qd_m: np.ndarray, qdd_m: np.ndarray, tau_ff_fric: np.ndarray | None = None) -> np.ndarray: """ 对外接口：给定主端 (q, qd, qdd)，计算 τ_master_current = M qdd + C qd + g + τ_ff_fric """ return self._inverse_dynamics(q_m, qd_m, qdd_m, tau_ff_fric) # ---------- 从端交互力 → 主端反馈扭矩 ---------- @staticmethod def _symmetric_limit_vector(value: float | np.ndarray | None, size: int, name: str) -> np.ndarray: if value is None: return np.full(size, np.inf, dtype=float) limit = np.asarray(value, dtype=float) if limit.ndim == 0: limit = np.full(size, float(limit), dtype=float) else: limit = limit.reshape(-1,) if limit.size != size: raise ValueError(f"{name} must be scalar or have {size} entries") if np.any(np.isnan(limit)) or np.any(limit < 0.0): raise ValueError(f"{name} must contain non-negative values") return limit def direct_from_CF(self, q_m: np.ndarray, qd_m: np.ndarray, CF_int_slave_C: np.ndarray) -> np.ndarray: """Return the unmodified baseline mapping ``J_m.T @ F``. This interface intentionally applies no sign convention, nominal gain, filter, rate limit, saturation, or energy supervision. """ CF = np.asarray(CF_int_slave_C, dtype=float).reshape(6,) CJ_m = self.CJ_master.chest_jacobian(q_m, qd_m) return np.asarray(CJ_m.T @ CF, dtype=float).reshape(-1,) # Explicitly named alias for experiment/baseline code. render_direct_from_CF = direct_from_CF 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): """Legacy extension hook. The base implementation returns the direct mapping and ``None`` to request final applied-port supervision. Existing no-tank subclasses that override this method and return a numeric alpha continue to bypass only the energy projection; all upstream nominal actuator shaping still applies. ``V_slave_C`` is retained solely for call compatibility and is never used for tank accounting. """ del V_slave_C, dt return self.direct_from_CF(q_m, qd_m, CF_int_slave_C), None def _shape_and_supervise(self, tau_direct: np.ndarray, qd_m: np.ndarray, dt: float, bypass_energy_projection: bool = False ) -> tuple[np.ndarray, float]: """Apply the strictly ordered haptic output pipeline.""" tau_saturated, valid = self._shape_candidate(tau_direct, qd_m, dt) if not valid: tau_zero, diagnostics = self.tank.apply(tau_direct, qd_m, dt) self._tau_applied_prev = tau_zero.copy() return tau_zero, diagnostics.rho # Final energy projection and the only accounting update. if bypass_energy_projection: tau_app = tau_saturated.copy() alpha = 1.0 else: tau_app, diagnostics = self.tank.apply(tau_saturated, qd_m, dt) alpha = diagnostics.rho # Nothing may modify tau_app after projection. Only state capture and # addition of the independent inverse-dynamics command occur downstream. self.commit_applied(tau_app) return tau_app, float(alpha) def _shape_candidate( self, tau_direct: np.ndarray, qd_m: np.ndarray, dt: float, ) -> tuple[np.ndarray, bool]: """Return the final upstream candidate without applying a supervisor. This split is used by the formal PO/PC baseline. The caller must pass the candidate through exactly one supervisor and then invoke :meth:`commit_applied`; no additional filtering or limiting is allowed. """ tau_direct = np.asarray(tau_direct, dtype=float).reshape(-1,) qd_m = np.asarray(qd_m, dtype=float).reshape(-1,) if tau_direct.shape != qd_m.shape: raise ValueError("direct haptic torque and qd_m must have equal shapes") if self._tau_fb_state.shape != tau_direct.shape: self._tau_fb_state = np.zeros_like(tau_direct) if self._tau_applied_prev.shape != tau_direct.shape: self._tau_applied_prev = np.zeros_like(tau_direct) # Invalid samples must not contaminate nominal filter/rate-limit state. if ( not np.isfinite(dt) or dt <= 0.0 or not np.all(np.isfinite(tau_direct)) or not np.all(np.isfinite(qd_m)) ): self.last_rate_limit_active = False self.last_torque_saturation_active = False return np.zeros_like(tau_direct), False # 1) Optional nominal torque filtering. a = float(self.tau_alpha) if not 0.0 <= a <= 1.0: raise ValueError("tau_alpha must lie in [0, 1]") self._tau_fb_state = (1.0 - a) * self._tau_fb_state + a * tau_direct # 2) Nominal sign and strength scaling. tau_nominal = -self.feedback_strength * self._tau_fb_state # 3) Per-joint rate limiting relative to the previously applied output. rate_limit = self._symmetric_limit_vector( self.torque_rate_limit, tau_nominal.size, "torque_rate_limit" ) max_delta = rate_limit * dt tau_rate_limited = np.clip( tau_nominal, self._tau_applied_prev - max_delta, self._tau_applied_prev + max_delta, ) self.last_rate_limit_active = bool( np.any(np.abs(tau_rate_limited - tau_nominal) > 1e-12) ) # 4) Per-joint symmetric actuator saturation. torque_limit = self._symmetric_limit_vector( self.torque_limit, tau_nominal.size, "torque_limit" ) tau_saturated = np.clip(tau_rate_limited, -torque_limit, torque_limit) self.last_torque_saturation_active = bool( np.any(np.abs(tau_saturated - tau_rate_limited) > 1e-12) ) return tau_saturated, True def shape_mapped_reaction_candidate( self, tau_environment_on_master: np.ndarray, qd_m: np.ndarray, dt: float, ) -> tuple[np.ndarray, bool]: """Prepare the common upstream candidate for an external supervisor.""" reaction = np.asarray( tau_environment_on_master, dtype=float, ).reshape(-1) return self._shape_candidate(-reaction, qd_m, dt) def commit_applied(self, tau_applied: np.ndarray) -> None: """Record the exact supervisor output used at the master port.""" applied = np.asarray(tau_applied, dtype=float).reshape(-1) if applied.shape != self._tau_applied_prev.shape: raise ValueError("tau_applied has an unexpected shape") if not np.all(np.isfinite(applied)): raise ValueError("tau_applied must contain only finite values") self._tau_applied_prev = applied.copy() # ---------- 高层接口：算 tau_master_current + 反馈 ---------- def render_mapped_reaction( self, tau_environment_on_master: np.ndarray, qd_m: np.ndarray, dt: float, *, supervise_energy: bool = True, ) -> tuple[np.ndarray, float]: """Shape an arbitrary generalized reaction such as ``A.T @ tau_s``. ``tau_environment_on_master`` already has the desired physical sign: it is the environment-on-device reaction and should oppose penetration. The historical renderer pipeline stores robot-on-environment action internally and inserts its own leading minus sign, so the conversion is performed exactly once here. The returned torque is the final applied haptic output; callers must not filter or limit it downstream. """ reaction = np.asarray( tau_environment_on_master, dtype=float, ).reshape(-1,) return self._shape_and_supervise( -reaction, qd_m, dt, bypass_energy_projection=not supervise_energy, ) def render_tau(self, q_m: np.ndarray, qd_m: np.ndarray, qdd_m: np.ndarray, CF_int_slave_C: np.ndarray, V_slave_C: np.ndarray | None, dt: float, tau_ff_fric: np.ndarray | None = None, ): """ 高层接口：给定 (q, qd, qdd) 和从端交互力，返回最终主端总扭矩。 输入： - q_m, qd_m, qdd_m: 主端关节位置/速度/加速度（用于逆动力学） - CF_int_slave_C: 从端 InteractionEstimator 估计出的 C F_int（6×1） - V_slave_C: 仅为旧调用兼容而保留；能量只按主端 tau_app^T qd_m 计算 - tau_ff_fric: 主端摩擦前馈（可为 None） - dt: 控制周期（秒） 输出： - tau_cmd_m: 主端最终下发的总扭矩 = tau_master_current + tau_app - tau_app: 最终反馈扭矩，也是能量记账使用的精确力矩 - alpha: 最终能量投影的径向缩放系数 """ # 1) 主端逆动力学扭矩 tau_master_current = self._inverse_dynamics(q_m, qd_m, qdd_m, tau_ff_fric) tau_master_current = np.asarray(tau_master_current, dtype=float).reshape(-1,) # 2) Render and account the haptic contribution exactly once. tau_app, alpha = self.render_from_CF( q_m, qd_m, CF_int_slave_C=CF_int_slave_C, V_slave_C=V_slave_C, dt=dt, ) # 3) tau_app is not modified after projection. tau_cmd_m = tau_master_current + tau_app return tau_cmd_m, tau_app, alpha # 兼容旧接口：只关心反馈扭矩 def render_from_CF(self, q_m: np.ndarray, qd_m: np.ndarray, CF_int_slave_C: np.ndarray, V_slave_C: np.ndarray | None, dt: float): """ 兼容旧接口：返回最终施加的反馈扭矩与能量投影系数。 """ tau_direct, legacy_alpha = self._compute_feedback_tau( q_m, qd_m, CF_int_slave_C, V_slave_C, dt, ) return self._shape_and_supervise( tau_direct, qd_m, dt, bypass_energy_projection=legacy_alpha is not None, )