控制器 API

三类控制器共享 get_task_space_state(q, v) 遥测接口，但主控制律的数据契约不同： 经典阻抗和 HQP-AC 接收世界轴对齐的任务空间线量；SE(3) Lie 控制器接收 body 运动参考与 EE body wrench。坐标约定与推导见 SE(3) Lie 群阻抗控制器。

任务空间阻抗（CIC）

任务空间动力学控制器（平动+姿态阻抗，动力学一致映射，零空间阻尼）/ Task-space dynamics controller (translation + rotation impedance; dynamics-consistent mapping; null damping)

源代码位于： src/compliant_docking/control/task_space.py

class TaskSpaceController:
    """
    任务空间动力学控制器（平动+姿态阻抗，动力学一致映射，零空间阻尼）/
    Task-space dynamics controller (translation + rotation impedance; dynamics-consistent mapping; null damping)
    """
    def __init__(self, robot_model: pin.Model, dt: float,
                 impedance: ImpedanceConfig | None = None,
                 ee_frame: str = "cylinder_link",
                 frictionloss: np.ndarray | None = None,
                 damping: np.ndarray | None = None,
                 friction_integral_gain: float | None = None,
                 friction_mode: str = "torque",
                 friction_tau_scale: float = 2.0):
        """
        初始化控制器：设定 Pinocchio 模型、步长与阻抗参数 /
        Initialize controller: set Pinocchio model, time step and impedance params

        Args:
            robot_model: Pinocchio 模型 / Pinocchio model
            dt: 控制步长 [s] / control time step
            impedance: 阻抗参数（None 时取 ImpedanceConfig 默认值） / impedance params
            ee_frame: 末端 frame 名（由模型/场景决定；默认值为组合 URDF 的
                ``cylinder_link``，即 iiwa14 + 公头圆柱场景的历史名称） /
                end-effector frame name (decided by model/scene; default is the
                historical name of the iiwa combined URDF)
            frictionloss: 关节摩擦损耗幅值 [N·m]（nq 维；None 时取零向量）。
                Pinocchio 的 MJCF/URDF 导入不保留 frictionloss，需由调用方
                从组装 MjModel 的 dof_frictionloss 传入；控制器以前馈补偿，
                模式由 friction_mode 选择（默认 "torque"，见下） /
                joint friction-loss magnitudes for feedforward compensation
            friction_integral_gain: 任务空间积分增益 [N/(m·s)]，用于克服
                静摩擦死区（前馈在零速时消失）。None 时自动：摩擦非零取
                150.0，否则 0（iiwa14 零摩擦路径行为不变）

        Note:
            重力置零由 compliant_docking.models.load_pin_model 负责（加载时统一处理）/
            gravity zeroing is owned by compliant_docking.models.load_pin_model

        Note:
            末端 frame 由 ee_frame 决定，方法内所有矩阵/向量维数均按
            ``model.nq`` 泛化（iiwa14 nq=7 时与历史实现数值逐位一致）。
        """
        self.model = robot_model
        self.data = self.model.createData()
        self.dt = dt
        self.impedance = impedance or ImpedanceConfig()

        self.Kp = np.diag([0.] * 3)
        self.Kd = np.diag([0.] * 3)

        # 摩擦前馈幅值（零向量 = 无补偿，行为与历史实现一致）
        self.frictionloss = (np.zeros(self.model.nq) if frictionloss is None
                             else np.asarray(frictionloss, dtype=float).reshape(self.model.nq))
        # MuJoCo 的 dof_damping 产生被动广义力 -damping*qdot；Pinocchio 模型不
        # 保存该项，故以 +damping*qdot 前馈补偿，和 frictionloss 一样由场景组装模型提供。
        self.damping = (np.zeros(self.model.nq) if damping is None
                        else np.asarray(damping, dtype=float).reshape(self.model.nq))
        self._friction_v0 = 0.01  # tanh 平滑化速度阈值 [rad/s]
        # 摩擦前馈模式："velocity"（τ_ff=f·tanh(q̇/v₀)，零速时补偿消失，
        # 低速任务易发粘滑）或 "torque"（τ_ff=f·tanh(τ_pre/τ₀)，用补偿前
        # 力矩方向决定摩擦方向，力矩一出即被抬过静摩擦阈值；τ₀=f/scale）
        validate_friction_mode(friction_mode)
        self.friction_mode = friction_mode
        self.friction_tau_scale = float(friction_tau_scale)

        # 静摩擦死区的积分补偿（速度前馈在零速时消失，I 项负责稳态残差；
        # 摩擦为零时增益恒 0，历史行为不变）。积分力限幅 ±10N 防饱和。
        if friction_integral_gain is None:
            self._ki = 150.0 if np.any(self.frictionloss) else 0.0
        else:
            self._ki = float(friction_integral_gain)
        self._i_clamp_force = 10.0  # [N]
        self._i_err = np.zeros(3)

        # 末端 frame id（由 ee_frame 参数决定，方法内统一复用）
        self.end_effector_id = self.model.getFrameId(ee_frame)

        # 以下限位/速度上限数组为 iiwa14 专用经验值（仅 _apply_limits 使用，主回路不调用）
        self.q_min = np.array([-2.96706, -2.0944, -2.96706, -2.0944, -2.96706, -2.0944, -3.05433])
        self.q_max = np.array([2.96706, 2.0944, 2.96706, 2.0944, 2.96706, 2.0944, 3.05433])
        self.v_max = np.array([1.4835, 1.4835, 1.7453, 1.3090, 2.2689, 2.3562, 2.3562])

        # 期望初始姿态（固定朝向），用于 log3 误差
        self.initial_orientation = np.array([
            [1,  0,  0],
            [0, -1,  0],
            [0,  0, -1]])

    def get_task_space_state(
            self, q: np.ndarray, v: np.ndarray,
    ) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        """
        计算当前末端位置与线速度（世界系）/
        Compute current end-effector position and linear velocity (world frame)
        """
        pin.forwardKinematics(self.model, self.data, q)
        pin.updateFramePlacements(self.model, self.data)

        H = self.data.oMf[self.end_effector_id]
        current_pos = H.translation
        current_ori = H.rotation

        J = pin.computeFrameJacobian(self.model, self.data, q, self.end_effector_id, _FRAME_REFERENCE)
        J_pos = J[:3, :]

        current_vel = J_pos @ v  # 末端线速度（线速度雅可比 J_pos 乘关节速度）

        return current_pos, current_vel, current_ori

    def get_task_space_state_with_orientation(self, q: np.ndarray, v: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        """
        返回末端位置、线/角速度，以及相对于期望姿态的李代数姿态误差 /
        Return EE position, linear/angular velocity, and orientation error (log map)
        """
        pin.forwardKinematics(self.model, self.data, q)
        pin.updateFramePlacements(self.model, self.data)

        H = self.data.oMf[self.end_effector_id]
        current_pos = H.translation
        current_rot = H.rotation

        # 姿态误差：log(Rd Rc^T)
        orientation_error = pin.log3(self.initial_orientation @ current_rot.T)

        J = pin.computeFrameJacobian(self.model, self.data, q, self.end_effector_id, _FRAME_REFERENCE)
        J_pos = J[:3, :]
        J_rot = J[3:, :]

        current_vel_pos = J_pos @ v  # 末端线速度
        current_vel_rot = J_rot @ v  # 末端角速度

        return current_pos, current_vel_pos, orientation_error, current_vel_rot

    def compute_control_task_space_with_orientation_and_imp(self, q: np.ndarray, v: np.ndarray,
                       pos_des: np.ndarray, vel_des: np.ndarray,
                       acc_des: np.ndarray, current_pos: np.ndarray,
                       current_vel: np.ndarray,
                       force_ext: np.ndarray, torque_ext: np.ndarray) -> np.ndarray:
        """
        任务空间控制（含姿态 + 阻抗 + 外力补偿）：输出关节力矩 /
        Task-space control (orientation + impedance + external force): output joint torques

        Args:
            q: 当前关节状态 / current joints
            v: 当前关节状态 / current joints
            pos_des: 期望项 / desired
            vel_des: 期望项 / desired
            acc_des: 期望项 / desired
            current_pos: 实际项 / current
            current_vel: 实际项 / current
            force_ext: 外力 / external
            torque_ext: 外力 / external

        Returns:
            tau: 关节力矩 / joint torques
        """
        # 1) 获取任务空间状态：当前位置、姿态误差（log 映射）、线/角速度
        pos_cur, vel_pos_cur, ori_err, vel_rot_cur = self.get_task_space_state_with_orientation(q, v)
        # 姿态的速度误差（希望角速度为0）：当前角速度取负
        vel_rot_err = -vel_rot_cur

        # 2) 计算末端 frame 原点的世界轴对齐雅可比及同参考系 J_dot。
        pin.forwardKinematics(self.model, self.data, q, v)
        pin.computeJointJacobiansTimeVariation(self.model, self.data, q, v)
        pin.updateFramePlacements(self.model, self.data)
        J = pin.getFrameJacobian(self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)
        J_pos = J[:3, :]
        J_rot = J[3:, :]

        # 3) 机器人动力学项：广义质量矩阵 M 及其伪逆，权重矩阵 W（此处取单位阵）
        M = pin.crba(self.model, self.data, q)  # 质量矩阵
        M_inv = pinv(M)

        # 4) 雅可比的时间变化项 J_dot（用于前馈/补偿项）
        J_dot = pin.getFrameJacobianTimeVariation(
            self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)

        # 5) 科氏/离心项：C(q, v)·v（转为一维向量表示广义力）
        pin.computeCoriolisMatrix(self.model, self.data, q, v)
        C = self.data.C
        C = C @ v.reshape(-1, 1)  # 广义科氏/离心项乘以速度，得到广义力形式
        C = C.reshape(self.model.nq)

        # 6) 平动阻抗参数与外力（参数由 ImpedanceConfig 集中管理） /
        # 6) Translational impedance params and external force (owned by ImpedanceConfig)
        force_ext = np.array(force_ext).reshape(3)
        imp = self.impedance

        # 期望外力（此处为0，可根据任务需要设置）
        force_desired = np.array([0, 0, 0])

        # 7) 平动阻抗：Md (xdd - xdd_des) + Dd (xd - xd_des) + Kd (x - x_des) = F_ext - F_des
        #    整理得到期望操作空间加速度/力输入 u_pos
        if self._ki > 0.0:
            # 积分补偿（摩擦非零时启用）：积累位置误差产生额外恢复力，
            # 上限 ±_i_clamp_force N 防积分饱和。接触后（|f_ext|>1N）冻结
            # 积累，避免 I 项在对接预紧上持续加力
            if np.linalg.norm(force_ext) < 1.0:
                self._i_err = np.clip(self._i_err + (pos_des - current_pos) * self.dt,
                                      -self._i_clamp_force / self._ki,
                                      self._i_clamp_force / self._ki)
            force_integral = self._ki * self._i_err
        else:
            force_integral = np.zeros(3)
        u_pos = (acc_des + (force_ext + force_integral - force_desired
                            - imp.d * (current_vel - vel_des)
                            - imp.k * (current_pos - pos_des)) / imp.m)

        # 8) 姿态阻抗：类似 PD，在角速度误差与姿态误差上施加控制 /
        # 8) Rotational impedance: PD-like control on orientation/angular-velocity errors
        u_rot = (imp.k_rot * (ori_err) + imp.d_rot * (vel_rot_err)) / imp.m_rot

        # 拼接平动与旋转的任务输入（6维）
        u = np.concatenate([u_pos, u_rot])

        # 10) 组合雅可比并计算标准操作空间惯性/动力学一致广义逆。
        J_full = np.vstack([J_pos, J_rot])
        Lambda = pinv(J_full @ M_inv @ J_full.T)
        J_bar = M_inv @ J_full.T @ Lambda

        # 11) 零空间阻尼：抑制未约束自由度的速度振荡（阻尼由 ImpedanceConfig 提供） /
        # 11) Null-space damping (coefficient from ImpedanceConfig)
        D_null = imp.null_damping * np.eye(self.model.nq)
        v_null = v
        N = np.eye(self.model.nq) - J_bar @ J_full
        null_term2 = -N.T @ D_null @ v_null.reshape(self.model.nq)

        # 12) 合成关节力矩：主任务项（含前馈与科氏/离心补偿）+ 零空间阻尼
        tau = J_full.T @ Lambda @ (u - J_dot @ v) + C + null_term2

        # 13) 关节摩擦前馈补偿（Pinocchio 模型不含 frictionloss，仿真侧有）：
        #     共享 helper（control/friction.py），SE(3) Lie 控制器同源复用
        tau = friction_feedforward(tau, v, self.frictionloss, self.damping,
                                   mode=self.friction_mode,
                                   tau_scale=self.friction_tau_scale,
                                   v0=self._friction_v0)

        return tau

    def compute_control_task_space_with_orientation_and_imp2(self, q: np.ndarray, v: np.ndarray,
                       pos_des: np.ndarray, vel_des: np.ndarray,
                       acc_des: np.ndarray, current_pos: np.ndarray,
                       current_vel: np.ndarray,
                       force_ext: np.ndarray, torque_ext: np.ndarray | None = None) -> np.ndarray:
        pos_cur, vel_pos_cur, ori_err, vel_rot_cur = self.get_task_space_state_with_orientation(q, v)

        vel_rot_err = -vel_rot_cur

        pin.forwardKinematics(self.model, self.data, q, v)
        pin.computeJointJacobiansTimeVariation(self.model, self.data, q, v)
        pin.updateFramePlacements(self.model, self.data)
        J = pin.getFrameJacobian(self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)

        M = pin.crba(self.model, self.data, q)

        J_dot = pin.getFrameJacobianTimeVariation(
            self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)

        pin.computeCoriolisMatrix(self.model, self.data, q, v)
        C = self.data.C
        C = C @ v.reshape(-1, 1)
        C = C.reshape(self.model.nq)

        force_ext = np.array(force_ext).reshape(3)
        m = 10
        d = 20
        k = 100

        force_desired = np.array([0, 0, 0])

        u_pos = acc_des + (-force_desired - d * (current_vel - vel_des) - k * (current_pos - pos_des)) / m
        u_rot = 1 * (vel_rot_err) + 1 * (ori_err)

        u = np.concatenate([u_pos, u_rot])

        lambda_ = pinv(J)

        tau = M @ (lambda_ @ (u - J_dot @ v)) + C

        return tau

    def compute_control_task_space_with_orientation(self, q: np.ndarray, v: np.ndarray,
                       pos_des: np.ndarray, vel_des: np.ndarray,
                       acc_des: np.ndarray, current_pos: np.ndarray,
                       current_vel: np.ndarray) -> np.ndarray:
        """
        任务空间控制（含姿态 PD，但不显式使用外力）/
        Task-space control (with orientation PD; no explicit external force)
        """
        pos_cur, vel_pos_cur, ori_err, vel_rot_cur = self.get_task_space_state_with_orientation(q, v)

        vel_rot_err = -vel_rot_cur

        pin.forwardKinematics(self.model, self.data, q, v)
        pin.computeJointJacobiansTimeVariation(self.model, self.data, q, v)
        pin.updateFramePlacements(self.model, self.data)
        J = pin.getFrameJacobian(self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)
        J_pos = J[:3, :]
        J_rot = J[3:, :]

        M = pin.crba(self.model, self.data, q)
        M_inv = pinv(M)

        J_dot = pin.getFrameJacobianTimeVariation(
            self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)

        pin.computeCoriolisMatrix(self.model, self.data, q, v)
        C = self.data.C
        C = C @ v.reshape(-1, 1)
        C = C.reshape(self.model.nq)

        u_pos = acc_des + 20 * (vel_des - current_vel) + 100 * (pos_des - current_pos)
        u_rot = 20 * (vel_rot_err) + 100 * (ori_err)

        u = np.concatenate([u_pos, u_rot])

        J_full = np.vstack([J_pos, J_rot])
        lambda_ = M @ M_inv.T @ J_full.T @ pinv(J_full @ M_inv @ M @ M_inv.T @ J_full.T)

        D_null = 1.2 * np.eye(self.model.nq)
        v_null = v
        N = (np.eye(self.model.nq) - lambda_ @ J_full @ M_inv)
        null_term2 = -N @ D_null @ v_null.reshape(self.model.nq)

        tau = lambda_ @ (u - J_dot @ v + J_full @ M_inv @ (C)) + null_term2

        return tau

    def compute_control_task_space(self, q: np.ndarray, v: np.ndarray,
                       pos_des: np.ndarray, vel_des: np.ndarray,
                       acc_des: np.ndarray, current_pos: np.ndarray,
                       current_vel: np.ndarray) -> np.ndarray:
        """
        仅平动任务空间控制（不包含姿态）/
        Task-space control for translation only (no orientation)
        """
        pos_cur, vel_cur, _ = self.get_task_space_state(q, v)

        pin.forwardKinematics(self.model, self.data, q, v)
        pin.computeJointJacobiansTimeVariation(self.model, self.data, q, v)
        pin.updateFramePlacements(self.model, self.data)
        J = pin.getFrameJacobian(self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)
        J_pos = J[:3, :]

        M = pin.crba(self.model, self.data, q)
        M_inv = pinv(M)

        lambda_ = M @ M_inv.T @ J_pos.T @ pinv(J_pos @ M_inv @ M @ M_inv.T @ J_pos.T)

        J_dot_full = pin.getFrameJacobianTimeVariation(
            self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)

        J_dot_full = J_dot_full[:3, :]

        pin.computeCoriolisMatrix(self.model, self.data, q, v)
        C = self.data.C
        C = C @ v.reshape(-1, 1)
        C = C.reshape(self.model.nq)

        u = acc_des + 20 * (vel_des - current_vel) + 100 * (pos_des - current_pos)

        # 保留调用以维持对 self.data 的任何副作用（与原实现一致）
        self.manipulability_gradient(q)

        D_null = 1.2 * np.eye(self.model.nq)
        v_null = v
        N = (np.eye(self.model.nq) - lambda_ @ J_pos @ M_inv)
        null_term2 = -N @ D_null @ v_null.reshape(self.model.nq)

        tau = lambda_ @ (u - J_dot_full @ v + J_pos @ M_inv @ (C)) + null_term2

        return tau

    def _apply_limits(self, tau: np.ndarray, q: np.ndarray, v: np.ndarray) -> np.ndarray:
        """
        软限位与速度缩放（示例）：约束越界趋势并按速度限制缩放力矩 /
        Soft joint limits and velocity scaling (example implementation)
        """
        k_limit = 100.0
        tau_limit = np.zeros_like(tau)
        for i in range(len(q)):
            if q[i] < self.q_min[i]:
                tau_limit[i] = k_limit * (self.q_min[i] - q[i])
            elif q[i] > self.q_max[i]:
                tau_limit[i] = k_limit * (self.q_max[i] - q[i])

        v_scale = np.minimum(1.0, self.v_max / (np.abs(v) + 1e-6))
        tau = tau * v_scale

        return tau + tau_limit

    def manipulability_gradient(self, q, delta=1e-6):
        """
        计算操控度梯度（ Yoshikawa 指标 det(JJ^T) 的数值梯度 ）/
        Compute manipulability gradient (numerical) for det(JJ^T)
        """
        grad = np.zeros_like(q)
        J_current = pin.computeFrameJacobian(
            self.model, self.data, q, self.end_effector_id, _FRAME_REFERENCE)[:3, :]
        manipulability_current = np.linalg.det(J_current @ J_current.T)

        for i in range(len(q)):
            q_delta = q.copy()
            q_delta[i] += delta
            J_delta = pin.computeFrameJacobian(
                self.model, self.data, q_delta, self.end_effector_id, _FRAME_REFERENCE)[:3, :]
            manipulability_delta = np.linalg.det(J_delta @ J_delta.T)
            grad[i] = (manipulability_delta - manipulability_current) / delta
        return grad

方法：

__init__

__init__(
    robot_model: Model,
    dt: float,
    impedance: ImpedanceConfig | None = None,
    ee_frame: str = "cylinder_link",
    frictionloss: ndarray | None = None,
    damping: ndarray | None = None,
    friction_integral_gain: float | None = None,
    friction_mode: str = "torque",
    friction_tau_scale: float = 2.0,
)

初始化控制器：设定 Pinocchio 模型、步长与阻抗参数 / Initialize controller: set Pinocchio model, time step and impedance params

参数：

名称	类型	描述	默认
robot_model	Model	Pinocchio 模型 / Pinocchio model	必需
dt	float	控制步长 [s] / control time step	必需
impedance	ImpedanceConfig | None	阻抗参数（None 时取 ImpedanceConfig 默认值） / impedance params	None
ee_frame	str	末端 frame 名（由模型/场景决定；默认值为组合 URDF 的 cylinder_link，即 iiwa14 + 公头圆柱场景的历史名称） / end-effector frame name (decided by model/scene; default is the historical name of the iiwa combined URDF)	'cylinder_link'
frictionloss	ndarray | None	关节摩擦损耗幅值 [N·m]（nq 维；None 时取零向量）。 Pinocchio 的 MJCF/URDF 导入不保留 frictionloss，需由调用方 从组装 MjModel 的 dof_frictionloss 传入；控制器以前馈补偿， 模式由 friction_mode 选择（默认 "torque"，见下） / joint friction-loss magnitudes for feedforward compensation	None
friction_integral_gain	float | None	任务空间积分增益 [N/(m·s)]，用于克服 静摩擦死区（前馈在零速时消失）。None 时自动：摩擦非零取 150.0，否则 0（iiwa14 零摩擦路径行为不变）	None

Note

重力置零由 compliant_docking.models.load_pin_model 负责（加载时统一处理）/ gravity zeroing is owned by compliant_docking.models.load_pin_model

Note

末端 frame 由 ee_frame 决定，方法内所有矩阵/向量维数均按 model.nq 泛化（iiwa14 nq=7 时与历史实现数值逐位一致）。

源代码位于： src/compliant_docking/control/task_space.py

def __init__(self, robot_model: pin.Model, dt: float,
             impedance: ImpedanceConfig | None = None,
             ee_frame: str = "cylinder_link",
             frictionloss: np.ndarray | None = None,
             damping: np.ndarray | None = None,
             friction_integral_gain: float | None = None,
             friction_mode: str = "torque",
             friction_tau_scale: float = 2.0):
    """
    初始化控制器：设定 Pinocchio 模型、步长与阻抗参数 /
    Initialize controller: set Pinocchio model, time step and impedance params

    Args:
        robot_model: Pinocchio 模型 / Pinocchio model
        dt: 控制步长 [s] / control time step
        impedance: 阻抗参数（None 时取 ImpedanceConfig 默认值） / impedance params
        ee_frame: 末端 frame 名（由模型/场景决定；默认值为组合 URDF 的
            ``cylinder_link``，即 iiwa14 + 公头圆柱场景的历史名称） /
            end-effector frame name (decided by model/scene; default is the
            historical name of the iiwa combined URDF)
        frictionloss: 关节摩擦损耗幅值 [N·m]（nq 维；None 时取零向量）。
            Pinocchio 的 MJCF/URDF 导入不保留 frictionloss，需由调用方
            从组装 MjModel 的 dof_frictionloss 传入；控制器以前馈补偿，
            模式由 friction_mode 选择（默认 "torque"，见下） /
            joint friction-loss magnitudes for feedforward compensation
        friction_integral_gain: 任务空间积分增益 [N/(m·s)]，用于克服
            静摩擦死区（前馈在零速时消失）。None 时自动：摩擦非零取
            150.0，否则 0（iiwa14 零摩擦路径行为不变）

    Note:
        重力置零由 compliant_docking.models.load_pin_model 负责（加载时统一处理）/
        gravity zeroing is owned by compliant_docking.models.load_pin_model

    Note:
        末端 frame 由 ee_frame 决定，方法内所有矩阵/向量维数均按
        ``model.nq`` 泛化（iiwa14 nq=7 时与历史实现数值逐位一致）。
    """
    self.model = robot_model
    self.data = self.model.createData()
    self.dt = dt
    self.impedance = impedance or ImpedanceConfig()

    self.Kp = np.diag([0.] * 3)
    self.Kd = np.diag([0.] * 3)

    # 摩擦前馈幅值（零向量 = 无补偿，行为与历史实现一致）
    self.frictionloss = (np.zeros(self.model.nq) if frictionloss is None
                         else np.asarray(frictionloss, dtype=float).reshape(self.model.nq))
    # MuJoCo 的 dof_damping 产生被动广义力 -damping*qdot；Pinocchio 模型不
    # 保存该项，故以 +damping*qdot 前馈补偿，和 frictionloss 一样由场景组装模型提供。
    self.damping = (np.zeros(self.model.nq) if damping is None
                    else np.asarray(damping, dtype=float).reshape(self.model.nq))
    self._friction_v0 = 0.01  # tanh 平滑化速度阈值 [rad/s]
    # 摩擦前馈模式："velocity"（τ_ff=f·tanh(q̇/v₀)，零速时补偿消失，
    # 低速任务易发粘滑）或 "torque"（τ_ff=f·tanh(τ_pre/τ₀)，用补偿前
    # 力矩方向决定摩擦方向，力矩一出即被抬过静摩擦阈值；τ₀=f/scale）
    validate_friction_mode(friction_mode)
    self.friction_mode = friction_mode
    self.friction_tau_scale = float(friction_tau_scale)

    # 静摩擦死区的积分补偿（速度前馈在零速时消失，I 项负责稳态残差；
    # 摩擦为零时增益恒 0，历史行为不变）。积分力限幅 ±10N 防饱和。
    if friction_integral_gain is None:
        self._ki = 150.0 if np.any(self.frictionloss) else 0.0
    else:
        self._ki = float(friction_integral_gain)
    self._i_clamp_force = 10.0  # [N]
    self._i_err = np.zeros(3)

    # 末端 frame id（由 ee_frame 参数决定，方法内统一复用）
    self.end_effector_id = self.model.getFrameId(ee_frame)

    # 以下限位/速度上限数组为 iiwa14 专用经验值（仅 _apply_limits 使用，主回路不调用）
    self.q_min = np.array([-2.96706, -2.0944, -2.96706, -2.0944, -2.96706, -2.0944, -3.05433])
    self.q_max = np.array([2.96706, 2.0944, 2.96706, 2.0944, 2.96706, 2.0944, 3.05433])
    self.v_max = np.array([1.4835, 1.4835, 1.7453, 1.3090, 2.2689, 2.3562, 2.3562])

    # 期望初始姿态（固定朝向），用于 log3 误差
    self.initial_orientation = np.array([
        [1,  0,  0],
        [0, -1,  0],
        [0,  0, -1]])

get_task_space_state

get_task_space_state(
    q: ndarray, v: ndarray
) -> tuple[ndarray, ndarray, ndarray]

计算当前末端位置与线速度（世界系）/ Compute current end-effector position and linear velocity (world frame)

源代码位于： src/compliant_docking/control/task_space.py

def get_task_space_state(
        self, q: np.ndarray, v: np.ndarray,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    计算当前末端位置与线速度（世界系）/
    Compute current end-effector position and linear velocity (world frame)
    """
    pin.forwardKinematics(self.model, self.data, q)
    pin.updateFramePlacements(self.model, self.data)

    H = self.data.oMf[self.end_effector_id]
    current_pos = H.translation
    current_ori = H.rotation

    J = pin.computeFrameJacobian(self.model, self.data, q, self.end_effector_id, _FRAME_REFERENCE)
    J_pos = J[:3, :]

    current_vel = J_pos @ v  # 末端线速度（线速度雅可比 J_pos 乘关节速度）

    return current_pos, current_vel, current_ori

get_task_space_state_with_orientation

get_task_space_state_with_orientation(
    q: ndarray, v: ndarray
) -> tuple[ndarray, ndarray, ndarray]

返回末端位置、线/角速度，以及相对于期望姿态的李代数姿态误差 / Return EE position, linear/angular velocity, and orientation error (log map)

源代码位于： src/compliant_docking/control/task_space.py

def get_task_space_state_with_orientation(self, q: np.ndarray, v: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    返回末端位置、线/角速度，以及相对于期望姿态的李代数姿态误差 /
    Return EE position, linear/angular velocity, and orientation error (log map)
    """
    pin.forwardKinematics(self.model, self.data, q)
    pin.updateFramePlacements(self.model, self.data)

    H = self.data.oMf[self.end_effector_id]
    current_pos = H.translation
    current_rot = H.rotation

    # 姿态误差：log(Rd Rc^T)
    orientation_error = pin.log3(self.initial_orientation @ current_rot.T)

    J = pin.computeFrameJacobian(self.model, self.data, q, self.end_effector_id, _FRAME_REFERENCE)
    J_pos = J[:3, :]
    J_rot = J[3:, :]

    current_vel_pos = J_pos @ v  # 末端线速度
    current_vel_rot = J_rot @ v  # 末端角速度

    return current_pos, current_vel_pos, orientation_error, current_vel_rot

compute_control_task_space_with_orientation_and_imp

compute_control_task_space_with_orientation_and_imp(
    q: ndarray,
    v: ndarray,
    pos_des: ndarray,
    vel_des: ndarray,
    acc_des: ndarray,
    current_pos: ndarray,
    current_vel: ndarray,
    force_ext: ndarray,
    torque_ext: ndarray,
) -> ndarray

任务空间控制（含姿态 + 阻抗 + 外力补偿）：输出关节力矩 / Task-space control (orientation + impedance + external force): output joint torques

参数：

名称	类型	描述	默认
q	ndarray	当前关节状态 / current joints	必需
v	ndarray	当前关节状态 / current joints	必需
pos_des	ndarray	期望项 / desired	必需
vel_des	ndarray	期望项 / desired	必需
acc_des	ndarray	期望项 / desired	必需
current_pos	ndarray	实际项 / current	必需
current_vel	ndarray	实际项 / current	必需
force_ext	ndarray	外力 / external	必需
torque_ext	ndarray	外力 / external	必需

返回：

名称	类型	描述
tau	ndarray	关节力矩 / joint torques

源代码位于： src/compliant_docking/control/task_space.py

def compute_control_task_space_with_orientation_and_imp(self, q: np.ndarray, v: np.ndarray,
                   pos_des: np.ndarray, vel_des: np.ndarray,
                   acc_des: np.ndarray, current_pos: np.ndarray,
                   current_vel: np.ndarray,
                   force_ext: np.ndarray, torque_ext: np.ndarray) -> np.ndarray:
    """
    任务空间控制（含姿态 + 阻抗 + 外力补偿）：输出关节力矩 /
    Task-space control (orientation + impedance + external force): output joint torques

    Args:
        q: 当前关节状态 / current joints
        v: 当前关节状态 / current joints
        pos_des: 期望项 / desired
        vel_des: 期望项 / desired
        acc_des: 期望项 / desired
        current_pos: 实际项 / current
        current_vel: 实际项 / current
        force_ext: 外力 / external
        torque_ext: 外力 / external

    Returns:
        tau: 关节力矩 / joint torques
    """
    # 1) 获取任务空间状态：当前位置、姿态误差（log 映射）、线/角速度
    pos_cur, vel_pos_cur, ori_err, vel_rot_cur = self.get_task_space_state_with_orientation(q, v)
    # 姿态的速度误差（希望角速度为0）：当前角速度取负
    vel_rot_err = -vel_rot_cur

    # 2) 计算末端 frame 原点的世界轴对齐雅可比及同参考系 J_dot。
    pin.forwardKinematics(self.model, self.data, q, v)
    pin.computeJointJacobiansTimeVariation(self.model, self.data, q, v)
    pin.updateFramePlacements(self.model, self.data)
    J = pin.getFrameJacobian(self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)
    J_pos = J[:3, :]
    J_rot = J[3:, :]

    # 3) 机器人动力学项：广义质量矩阵 M 及其伪逆，权重矩阵 W（此处取单位阵）
    M = pin.crba(self.model, self.data, q)  # 质量矩阵
    M_inv = pinv(M)

    # 4) 雅可比的时间变化项 J_dot（用于前馈/补偿项）
    J_dot = pin.getFrameJacobianTimeVariation(
        self.model, self.data, self.end_effector_id, _FRAME_REFERENCE)

    # 5) 科氏/离心项：C(q, v)·v（转为一维向量表示广义力）
    pin.computeCoriolisMatrix(self.model, self.data, q, v)
    C = self.data.C
    C = C @ v.reshape(-1, 1)  # 广义科氏/离心项乘以速度，得到广义力形式
    C = C.reshape(self.model.nq)

    # 6) 平动阻抗参数与外力（参数由 ImpedanceConfig 集中管理） /
    # 6) Translational impedance params and external force (owned by ImpedanceConfig)
    force_ext = np.array(force_ext).reshape(3)
    imp = self.impedance

    # 期望外力（此处为0，可根据任务需要设置）
    force_desired = np.array([0, 0, 0])

    # 7) 平动阻抗：Md (xdd - xdd_des) + Dd (xd - xd_des) + Kd (x - x_des) = F_ext - F_des
    #    整理得到期望操作空间加速度/力输入 u_pos
    if self._ki > 0.0:
        # 积分补偿（摩擦非零时启用）：积累位置误差产生额外恢复力，
        # 上限 ±_i_clamp_force N 防积分饱和。接触后（|f_ext|>1N）冻结
        # 积累，避免 I 项在对接预紧上持续加力
        if np.linalg.norm(force_ext) < 1.0:
            self._i_err = np.clip(self._i_err + (pos_des - current_pos) * self.dt,
                                  -self._i_clamp_force / self._ki,
                                  self._i_clamp_force / self._ki)
        force_integral = self._ki * self._i_err
    else:
        force_integral = np.zeros(3)
    u_pos = (acc_des + (force_ext + force_integral - force_desired
                        - imp.d * (current_vel - vel_des)
                        - imp.k * (current_pos - pos_des)) / imp.m)

    # 8) 姿态阻抗：类似 PD，在角速度误差与姿态误差上施加控制 /
    # 8) Rotational impedance: PD-like control on orientation/angular-velocity errors
    u_rot = (imp.k_rot * (ori_err) + imp.d_rot * (vel_rot_err)) / imp.m_rot

    # 拼接平动与旋转的任务输入（6维）
    u = np.concatenate([u_pos, u_rot])

    # 10) 组合雅可比并计算标准操作空间惯性/动力学一致广义逆。
    J_full = np.vstack([J_pos, J_rot])
    Lambda = pinv(J_full @ M_inv @ J_full.T)
    J_bar = M_inv @ J_full.T @ Lambda

    # 11) 零空间阻尼：抑制未约束自由度的速度振荡（阻尼由 ImpedanceConfig 提供） /
    # 11) Null-space damping (coefficient from ImpedanceConfig)
    D_null = imp.null_damping * np.eye(self.model.nq)
    v_null = v
    N = np.eye(self.model.nq) - J_bar @ J_full
    null_term2 = -N.T @ D_null @ v_null.reshape(self.model.nq)

    # 12) 合成关节力矩：主任务项（含前馈与科氏/离心补偿）+ 零空间阻尼
    tau = J_full.T @ Lambda @ (u - J_dot @ v) + C + null_term2

    # 13) 关节摩擦前馈补偿（Pinocchio 模型不含 frictionloss，仿真侧有）：
    #     共享 helper（control/friction.py），SE(3) Lie 控制器同源复用
    tau = friction_feedforward(tau, v, self.frictionloss, self.damping,
                               mode=self.friction_mode,
                               tau_scale=self.friction_tau_scale,
                               v0=self._friction_v0)

    return tau

HQP-AC

HQP-AC 控制器：约束 QP 主任务 + 自适应刚度 + 零空间奇异性规避/关节位姿阻抗。

与 TaskSpaceController 鸭子类型兼容：提供同签名的 compute_control_task_space_with_orientation_and_imp 与 get_task_space_state，可在 experiments/run_docking.py 中直接互换。

源代码位于： src/compliant_docking/control/hqp_ac.py

class HQPAdaptiveController:
    """HQP-AC 控制器：约束 QP 主任务 + 自适应刚度 + 零空间奇异性规避/关节位姿阻抗。

    与 TaskSpaceController 鸭子类型兼容：提供同签名的
    ``compute_control_task_space_with_orientation_and_imp`` 与
    ``get_task_space_state``，可在 experiments/run_docking.py 中直接互换。
    """

    def __init__(self, robot_model: pin.Model, dt: float,
                 config: HQPConfig | None = None,
                 ee_frame: str = "cylinder_link",
                 r_des: np.ndarray | None = None,
                 frictionloss: np.ndarray | None = None,
                 damping: np.ndarray | None = None,
                 impedance: ImpedanceConfig | None = None,
                 friction_mode: str = "torque",
                 friction_tau_scale: float = 2.0,
                 force_source: str = "sensor",
                 observer_kp: float = 20.0,
                 observer_ki: float = 40.0,
                 preload_force: float = 0.0,
                 preload_ramp_s: float = 1.5,
                 preload_axis: np.ndarray | None = None,
                 contact_deadband: float = 0.0):
        """初始化控制器：预解析限位并预建两个 ProxQP 实例（主任务/零空间）。

        Args:
            robot_model: Pinocchio 模型（重力置零由 load_pin_model 负责）
            dt: 控制步长 [s]
            config: HQPConfig 参数（None 时取默认值）
            ee_frame: 末端 frame 名（与 TaskSpaceController 同口径）
            r_des: 期望姿态（世界系 3×3）；None 时取
                ``[[1,0,0],[0,-1,0],[0,0,-1]]``（与 TaskSpaceController
                的 initial_orientation 同口径）
            frictionloss: 关节摩擦损耗幅值 [N·m]（nv 维；None 时取零向量）。
                Pinocchio 导入器不保留 MJCF frictionloss，需由调用方从组装
                MjModel 的 dof_frictionloss 传入；以前馈并入 ĥ（同时进入力矩
                硬约束与输出力矩），模式由 friction_mode 选择（默认 "torque"）

        Note:
            QP 实例复用策略：proxsuite 支持 ``qp.update(...)`` 原地更新 H/g/C/u，
            两个实例在 __init__ 各建一次，之后每个控制步只 update+solve，
            不再重新构造。
        """
        self.model = robot_model
        self.data = self.model.createData()
        self.dt = dt
        self.config = config or HQPConfig()
        if impedance is not None:
            # 场景阻抗覆盖（ImpedanceOverride 的 k/d/k_rot/d_rot 可选字段 → K0）
            from dataclasses import replace

            cfg = self.config
            K0 = cfg.K0.copy()
            for i, val in enumerate((impedance.k, impedance.k, impedance.k,
                                     impedance.k_rot, impedance.k_rot, impedance.k_rot)):
                if val is not None:
                    K0[i] = float(val)
            self.config = replace(cfg, K0=K0)
        cfg = self.config

        self.frame_id = self.model.getFrameId(ee_frame)
        self.n = self.model.nv

        # 摩擦前馈幅值（零向量 = 无补偿，行为与历史实现一致）
        self.frictionloss = (np.zeros(self.n) if frictionloss is None
                             else np.asarray(frictionloss, dtype=float).reshape(self.n))
        self.damping = (np.zeros(self.n) if damping is None
                        else np.asarray(damping, dtype=float).reshape(self.n))
        self._friction_v0 = 0.01  # tanh 平滑化速度阈值 [rad/s]
        # 摩擦前馈模式："velocity"（τ_ff=f·tanh(q̇/v₀)）或 "torque"
        # （τ_ff=f·tanh(τ_pre/τ₀)，用补偿前力矩方向治零速死区；τ₀=f/scale），
        # 与 TaskSpaceController 同语义
        if friction_mode not in ("velocity", "torque"):
            raise ValueError(f"friction_mode 不支持 {friction_mode!r}，可选 'velocity' 或 'torque'")
        self.friction_mode = friction_mode
        self.friction_tau_scale = float(friction_tau_scale)

        # 外力来源："sensor"（run 循环传入的 F/T 读数）或 "observer"
        # （PI 动量观测器估计，无传感器方案，Ren & Shan 2026 Eq.23-25）
        if force_source not in ("sensor", "observer"):
            raise ValueError(f"force_source 不支持 {force_source!r}，可选 'sensor' 或 'observer'")
        self.force_source = force_source
        self._observer: MomentumObserver | None = None
        if force_source == "observer":
            self._observer = MomentumObserver(
                robot_model, ee_frame, self.dt, kp=observer_kp, ki=observer_ki,
                frictionloss=self.frictionloss, damping=self.damping)

        # 接触预紧力跟踪（世界系）：检测到接触后按斜坡施加 preload_force·
        # preload_axis 的任务力（论文 Eq.26 中以期望接触力替代 F̂_ext 的推论），
        # 解决纯阻抗"轻触即停"无预紧的问题
        self.preload_force = float(preload_force)
        self.preload_ramp_s = max(float(preload_ramp_s), 1e-3)
        self.preload_axis = (np.zeros(3) if preload_axis is None else
                             np.asarray(preload_axis, dtype=float).reshape(3))
        self._preload_val = 0.0
        # 接触检测死区 [N]：|F| 低于该值不作接触/软化（观测器残差含模型
        # 失配噪声，deadband 防止自由空间误软化；传感器模式噪声 mN 级，
        # 默认 0 行为不变）
        self.contact_deadband = float(contact_deadband)

        # 期望姿态（世界系 3×3）
        if r_des is None:
            self.r_des = np.array([[1.0, 0.0, 0.0],
                                   [0.0, -1.0, 0.0],
                                   [0.0, 0.0, -1.0]])
        else:
            self.r_des = np.array(r_des, dtype=float)

        # 限位预解析：q_min/q_max、v_max/v_min（≤0 的轴视作无限制）、τ_max/τ_min
        self.q_min = np.array(self.model.lowerPositionLimit, dtype=float).copy()
        self.q_max = np.array(self.model.upperPositionLimit, dtype=float).copy()
        self._v_max = np.array(self.model.velocityLimit, dtype=float).copy()
        self._v_max[self._v_max <= 0.0] = np.inf
        if cfg.torque_limit is None:
            self._tau_max = np.array(self.model.effortLimit, dtype=float).copy()
        else:
            self._tau_max = np.full(self.n, float(cfg.torque_limit))
        self._tau_min = -self._tau_max

        # 自适应刚度上下界（对角向量）
        self._K0 = np.array(cfg.K0, dtype=float).reshape(6)
        self._K_min = cfg.K_min_ratio * self._K0

        # 预建两个 ProxQP 实例：n 变量、0 等式、6n 不等式
        # （速度 2n + 位置 2n + 力矩 2n，见 _constraint_matrices）
        self._n_in = 6 * self.n
        self._qp_main = _DenseQP(self.n, 0, self._n_in)
        self._qp_null = _DenseQP(self.n, 0, self._n_in)
        self._l_inf = np.full(self._n_in, -np.inf)
        for qp in (self._qp_main, self._qp_null):
            qp.settings.eps_abs = cfg.eps_abs
            qp.init(np.eye(self.n), np.zeros(self.n), None, None,
                    np.zeros((self._n_in, self.n)), self._l_inf, np.zeros(self._n_in))

        # 诊断状态
        self.q_col: np.ndarray | None = None  # Eq.(41) 关节位姿阻抗参考（首次调用捕获）
        self.n_solver_failures = 0
        self.last_K_r: np.ndarray | None = None
        self._solve_time_sum = 0.0
        self._n_solves = 0

    # ------------------------------------------------------------------
    # 只读诊断属性
    # ------------------------------------------------------------------
    @property
    def last_solve_time_ms(self) -> float:
        """主+零空间 QP 求解耗时滚动均值 [ms]（累计平均；尚无求解时为 0.0）。"""
        if self._n_solves == 0:
            return 0.0
        return 1e3 * self._solve_time_sum / self._n_solves

    # ------------------------------------------------------------------
    # 状态接口（与 TaskSpaceController 同签名，供 run_simulation 复用）
    # ------------------------------------------------------------------
    def get_task_space_state(self, q: np.ndarray, v: np.ndarray) -> tuple:
        """返回末端位置、线速度（世界系）与旋转矩阵（世界系 3×3）。"""
        pin.forwardKinematics(self.model, self.data, q)
        pin.updateFramePlacements(self.model, self.data)
        H = self.data.oMf[self.frame_id]
        J = np.array(pin.computeFrameJacobian(
            self.model, self.data, q, self.frame_id, _FRAME_REFERENCE))
        return H.translation.copy(), J[:3, :] @ v, H.rotation.copy()

    # ------------------------------------------------------------------
    # 内部计算（论文公式逐条对应）
    # ------------------------------------------------------------------
    def _adaptive_stiffness(self, F: float) -> np.ndarray:
        """Eq.(26)(27)：按接触力幅值自适应的参考刚度（6 维对角向量）。"""
        alpha = 1.0 / (1.0 + np.exp(-self.config.k_alpha * F))
        return np.clip((1.0 - alpha) * self._K0, self._K_min, self._K0)

    @staticmethod
    def _reference_damping(Lambda: np.ndarray, K_r: np.ndarray) -> np.ndarray:
        """Eq.(29)：D_r = Λ^(1/2)K_r^(1/2) + K_r^(1/2)Λ^(1/2)。

        K_r 为对角向量（开方取逐元素 sqrt）；Λ 的对称正定平方根经
        ``eigh`` 实现（特征值截断到非负，数值稳健且恒为实矩阵）。
        """
        w, V = np.linalg.eigh(Lambda)
        sqrt_L = (V * np.sqrt(np.clip(w, 0.0, None))) @ V.T
        sqrt_K = np.diag(np.sqrt(np.clip(K_r, 0.0, None)))
        return sqrt_L @ sqrt_K + sqrt_K @ sqrt_L

    def _manipulability(self, q: np.ndarray) -> float:
        """Eq.(38)：ω = sqrt(det(J Jᵀ))（6 维、世界轴对齐 frame 原点雅可比）。"""
        J = np.array(pin.computeFrameJacobian(
            self.model, self.data, q, self.frame_id, _FRAME_REFERENCE))
        return float(np.sqrt(max(np.linalg.det(J @ J.T), 0.0)))

    def _manipulability_gradient(self, q: np.ndarray, delta: float = 1e-6) -> np.ndarray:
        """Eq.(39)：可操作度梯度的数值差分（写法参照 task_space.manipulability_gradient）。"""
        grad = np.zeros(self.n)
        w0 = self._manipulability(q)
        for i in range(self.n):
            q_delta = q.copy()
            q_delta[i] += delta
            grad[i] = (self._manipulability(q_delta) - w0) / delta
        return grad

    def _constraint_matrices(self, q: np.ndarray, v: np.ndarray,
                             M: np.ndarray, h: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
        """Eq.(34)-(36)：ZOH 时域 dt_p 下的关节位置/速度/力矩硬约束。

        全部堆叠为 ``C_mat q̈ ≤ u``（下界侧乘 -1 并入，行下界 l = -inf）：
        行 0..n-1        速度上限：v + dt_p·q̈ ≤ v_max
        行 n..2n-1       速度下限：-(v + dt_p·q̈) ≤ v_max
        行 2n..3n-1      位置上限：q + v·dt_p + ½dt_p²·q̈ ≤ q_max
        行 3n..4n-1      位置下限：-(q + v·dt_p + ½dt_p²·q̈) ≤ -q_min
        行 4n..5n-1      力矩上限：M·q̈ ≤ τ_max - ĥ
        行 5n..6n-1      力矩下限：-M·q̈ ≤ ĥ - τ_min
        其中 ĥ = C(q,v)·v（科氏/离心广义力）。
        """
        n = self.n
        dt_p = self.config.dt_p
        eye = np.eye(n)
        C_mat = np.zeros((self._n_in, n))
        u = np.zeros(self._n_in)
        C_mat[:n] = dt_p * eye
        u[:n] = self._v_max - v
        C_mat[n:2 * n] = -dt_p * eye
        u[n:2 * n] = self._v_max + v
        half_dt2 = 0.5 * dt_p * dt_p
        C_mat[2 * n:3 * n] = half_dt2 * eye
        u[2 * n:3 * n] = self.q_max - q - v * dt_p
        C_mat[3 * n:4 * n] = -half_dt2 * eye
        u[3 * n:4 * n] = q + v * dt_p - self.q_min
        C_mat[4 * n:5 * n] = M
        u[4 * n:5 * n] = self._tau_max - h
        C_mat[5 * n:6 * n] = -M
        u[5 * n:6 * n] = h - self._tau_min
        return C_mat, u

    def _solve_qp(self, qp, H: np.ndarray, g: np.ndarray,
                  C_mat: np.ndarray, u: np.ndarray) -> np.ndarray | None:
        """更新并求解一个 ProxQP 实例；非 solved 返回 None（调用方回退）。"""
        qp.update(H, g, None, None, C_mat, self._l_inf, u)
        qp.solve()
        if qp.results.info.status == _QP_SOLVED:
            return np.array(qp.results.x)
        return None

    # ------------------------------------------------------------------
    # 主入口：与 TaskSpaceController.compute_control_task_space_with_orientation_and_imp
    # 同名同签名（刻意的鸭子类型约定，便于 run_docking 直接换控制器）
    # ------------------------------------------------------------------
    def compute_control_task_space_with_orientation_and_imp(
            self, q: np.ndarray, v: np.ndarray,
            pos_des: np.ndarray, vel_des: np.ndarray,
            acc_des: np.ndarray, current_pos: np.ndarray,
            current_vel: np.ndarray,
            force_ext: np.ndarray, torque_ext: np.ndarray) -> np.ndarray:
        """HQP-AC 控制律：返回关节力矩 τ = M·q̈_c + ĥ。

        参数与 TaskSpaceController 同名方法完全一致 / Same signature as the
        TaskSpaceController method of the same name.

        Args:
            q: 关节状态
            v: 关节状态
            pos_des: 期望任务位置
            vel_des: 期望任务速度
            acc_des: 期望任务加速度
            current_pos: 实际末端位置
            current_vel: 实际末端线速度
            force_ext: 世界系末端外力（3 维）
            torque_ext: 世界系末端外力矩（3 维）

        Returns:
            关节力矩 τ（n 维）

        Note:
            回退语义 / Fallback: 主 QP 非 solved → 该步主任务分量回退为
            无约束最小二乘解（min‖Jq̈-(target-J̇q̇)‖²），并跳过零空间 QP；
            零空间 QP 非 solved → 零空间分量取 0。
            每次失败 ``n_solver_failures`` 自增 1。

        Note:
            直接使用 F/T 传感器输入（无传感器动量观测器留作后续）。
        """
        q = np.array(q, dtype=float).reshape(self.n)
        v = np.array(v, dtype=float).reshape(self.n)
        pos_des = np.array(pos_des, dtype=float).reshape(3)
        vel_des = np.array(vel_des, dtype=float).reshape(3)
        acc_des = np.array(acc_des, dtype=float).reshape(3)
        current_pos = np.array(current_pos, dtype=float).reshape(3)
        current_vel = np.array(current_vel, dtype=float).reshape(3)
        force_ext = np.array(force_ext, dtype=float).reshape(3)
        torque_ext = np.array(torque_ext, dtype=float).reshape(3)

        # 外力源切换：observer 模式下用 PI 动量观测器的"纯接触"估计
        # （残差扣除已知耗散模型）替代 F/T 传感器（论文 §3.2.1 无传感器方案）；
        # 观测器由 run 循环按步调用 update。摩擦不经此通道——它已在 ĥ 中前馈
        if self._observer is not None:
            force_ext = self._observer.force_contact
            torque_ext = self._observer.wrench_contact[3:]

        # Eq.(41) 的 q_col：首次调用捕获关节位姿
        if self.q_col is None:
            self.q_col = q.copy()

        # 1) FK + 世界轴对齐 frame 原点雅可比及同参考系时间导数。
        pin.forwardKinematics(self.model, self.data, q, v)
        pin.computeJointJacobiansTimeVariation(self.model, self.data, q, v)
        pin.updateFramePlacements(self.model, self.data)
        R_cur = np.array(self.data.oMf[self.frame_id].rotation)
        J = np.array(pin.getFrameJacobian(
            self.model, self.data, self.frame_id, _FRAME_REFERENCE))
        J_dot = np.array(pin.getFrameJacobianTimeVariation(
            self.model, self.data, self.frame_id, _FRAME_REFERENCE))
        J_rot = J[3:, :]

        # 2) 动力学项：M（对称化）、ĥ = C(q,v)·v
        M = np.array(pin.crba(self.model, self.data, q))
        M = np.triu(M) + np.triu(M, 1).T  # crba 只保证上三角，显式对称化
        pin.computeCoriolisMatrix(self.model, self.data, q, v)
        h = np.array(self.data.C) @ v
        # 关节摩擦前馈（Pinocchio 模型不含 frictionloss，仿真侧有）：
        # 并入 ĥ 使力矩硬约束与输出力矩自动一致；torque 模式用补偿前 ĥ
        # 的方向决定摩擦方向（与 TaskSpaceController 同语义）
        if self.friction_mode == "torque":
            h = h + self.frictionloss * np.tanh(
                h * self.friction_tau_scale / np.maximum(self.frictionloss, 1e-9))
        else:
            h = h + self.frictionloss * np.tanh(v / self._friction_v0)
        h = h + self.damping * v

        # 任务空间惯性 Λ 及其逆（Λ⁻¹ = J M⁻¹ Jᵀ，pinv 稳健化后对称化）
        M_inv = np.linalg.pinv(M)
        A_task = J @ M_inv @ J.T
        Lambda = np.linalg.pinv(A_task)
        Lambda = 0.5 * (Lambda + Lambda.T)

        # 3) 误差与自适应刚度：Eq.(26)(27)
        #    姿态误差取 log(R_cur·R_desᵀ)（"当前相对期望"，与 e_pos=current-desired
        #    同向），保证 ė_p = Δν = [ẋ-ẋ_d; ω] 严格成立，使 Eq.(29)(31) 的统一式
        #    F_r = -D_r·Δν - K_r·e_p 在平动/旋转两通道都是耗散阻抗
        #    （ë_p + Λ⁻¹D_r·ė_p + Λ⁻¹K_r·e_p = 0）。注意：TaskSpaceController 的
        #    ori_err = log(R_des·R_curᵀ) 与 u_rot = +k_rot·ori_err + d_rot·(-ω)
        #    在代数上与本处约定完全等价（相差一个整体负号）。
        e_pos = current_pos - pos_des
        e_ori = pin.log3(R_cur @ self.r_des.T)
        delta_nu = np.concatenate([current_vel - vel_des, J_rot @ v])  # 期望角速度为 0
        e_p = np.concatenate([e_pos, e_ori])
        F_contact = float(np.linalg.norm(np.concatenate([force_ext, torque_ext])))
        F_contact_eff = max(F_contact - self.contact_deadband, 0.0)
        K_r = self._adaptive_stiffness(F_contact_eff)
        self.last_K_r = K_r

        # 4) 参考阻尼与广义力：Eq.(29)
        D_r = self._reference_damping(Lambda, K_r)
        F_r = -D_r @ delta_nu - K_r * e_p

        # 5) 主任务 QP（Eq.31）：min ‖Jq̈ + J̇q̇ - target‖²，target = ν̇_r + Λ⁻¹F_r
        #    接触预紧项：检测到接触（|F|>1N）后按斜坡施加 preload_force·axis
        #    的任务力（推入对接方向），脱离接触则回落，力目标
        #    F_pre = preload · ramp 加入期望操作空间广义力
        if self.preload_force > 0.0:
            in_contact = F_contact > 1.0
            rate = self.preload_force / self.preload_ramp_s * self.dt
            if in_contact:
                self._preload_val = min(self.preload_force, self._preload_val + rate)
            else:
                self._preload_val = max(0.0, self._preload_val - rate)
        F_pre6 = np.concatenate([self._preload_val * self.preload_axis,
                                 np.zeros(3)])
        nu_dot_r = np.concatenate([acc_des, np.zeros(3)])
        target = nu_dot_r + A_task @ (F_r + F_pre6)
        H_main = 2.0 * (J.T @ J) + _H_REG * np.eye(self.n)
        g_main = 2.0 * (J.T @ (J_dot @ v - target))

        # 6) 硬约束（Eq.34-36，ZOH 时域 dt_p）
        C_mat, u = self._constraint_matrices(q, v, M, h)

        t_start = time.perf_counter()
        a_m = self._solve_qp(self._qp_main, H_main, g_main, C_mat, u)
        t_main = time.perf_counter() - t_start
        if a_m is not None:
            q_ddot_m = a_m
        else:
            # 回退：无约束最小二乘（主任务分量）
            self.n_solver_failures += 1
            q_ddot_m = np.linalg.lstsq(J, target - J_dot @ v, rcond=None)[0]
            self._solve_time_sum += t_main
            self._n_solves += 1
            return M @ q_ddot_m + h  # 主 QP 失败 → 跳过零空间

        # 7) 零空间 QP（Eq.38-43）：奇异性规避 + 关节位姿阻抗
        omega = self._manipulability(q)
        q_ddot_sa = np.zeros(self.n)
        if omega < self.config.omega_th:
            grad_w = self._manipulability_gradient(q)
            norm_gw = float(np.linalg.norm(grad_w))
            if norm_gw > 1e-12:
                q_ddot_sa = ((self.config.omega_th - omega) / self.config.omega_th) \
                    * grad_w / norm_gw
        q_ddot_ji = -self.config.D_ji @ v - self.config.K_ji @ (q - self.q_col)
        a_null = self.config.k_sa * q_ddot_sa + self.config.k_ji * q_ddot_ji

        J_pinv = M_inv @ J.T @ Lambda  # Eq.(43)：J# = M⁻¹JᵀΛ
        N = np.eye(self.n) - J_pinv @ J
        H_null = (2.0 + _H_REG) * np.eye(self.n)
        g_null = -2.0 * a_null
        u_null = u - C_mat @ q_ddot_m

        t_null_start = time.perf_counter()
        a_n = self._solve_qp(self._qp_null, H_null, g_null, C_mat @ N, u_null)
        self._solve_time_sum += t_main + (time.perf_counter() - t_null_start)
        self._n_solves += 1
        if a_n is not None:
            q_ddot_n = a_n
        else:
            # 回退：零空间分量取 0
            self.n_solver_failures += 1
            q_ddot_n = np.zeros(self.n)

        # 8) 合成：q̈_c = q̈*_m + N·q̈*_null，τ = M·q̈_c + ĥ
        q_ddot_c = q_ddot_m + N @ q_ddot_n
        return M @ q_ddot_c + h

    def update_momentum_observer(self, q: np.ndarray, v: np.ndarray,
                                 tau_applied: np.ndarray) -> None:
        """以步进后的关节状态与实际施加力矩推进一步动量观测器。

        仅 force_source == "observer" 时有效（其余为 no-op）；
        τ_applied 应为限幅后的实际力矩（上一控制周期）。
        """
        if self._observer is not None:
            self._observer.update(q, v, tau_applied)

    @property
    def observed_wrench(self) -> np.ndarray | None:
        """观测器外力/外力矩估计（世界系 6 维）；未启用时为 None。"""
        return None if self._observer is None else self._observer.wrench

属性

observed_wrench property

observed_wrench: ndarray | None

观测器外力/外力矩估计（世界系 6 维）；未启用时为 None。

last_solve_time_ms property

last_solve_time_ms: float

主+零空间 QP 求解耗时滚动均值 [ms]（累计平均；尚无求解时为 0.0）。

方法：

__init__

__init__(
    robot_model: Model,
    dt: float,
    config: HQPConfig | None = None,
    ee_frame: str = "cylinder_link",
    r_des: ndarray | None = None,
    frictionloss: ndarray | None = None,
    damping: ndarray | None = None,
    impedance: ImpedanceConfig | None = None,
    friction_mode: str = "torque",
    friction_tau_scale: float = 2.0,
    force_source: str = "sensor",
    observer_kp: float = 20.0,
    observer_ki: float = 40.0,
    preload_force: float = 0.0,
    preload_ramp_s: float = 1.5,
    preload_axis: ndarray | None = None,
    contact_deadband: float = 0.0,
)

初始化控制器：预解析限位并预建两个 ProxQP 实例（主任务/零空间）。

参数：

名称	类型	描述	默认
robot_model	Model	Pinocchio 模型（重力置零由 load_pin_model 负责）	必需
dt	float	控制步长 [s]	必需
config	HQPConfig | None	HQPConfig 参数（None 时取默认值）	None
ee_frame	str	末端 frame 名（与 TaskSpaceController 同口径）	'cylinder_link'
r_des	ndarray | None	期望姿态（世界系 3×3）；None 时取 [[1,0,0],[0,-1,0],[0,0,-1]]（与 TaskSpaceController 的 initial_orientation 同口径）	None
frictionloss	ndarray | None	关节摩擦损耗幅值 [N·m]（nv 维；None 时取零向量）。 Pinocchio 导入器不保留 MJCF frictionloss，需由调用方从组装 MjModel 的 dof_frictionloss 传入；以前馈并入 ĥ（同时进入力矩 硬约束与输出力矩），模式由 friction_mode 选择（默认 "torque"）	None

Note

QP 实例复用策略：proxsuite 支持 qp.update(...) 原地更新 H/g/C/u， 两个实例在 init 各建一次，之后每个控制步只 update+solve， 不再重新构造。

源代码位于： src/compliant_docking/control/hqp_ac.py

def __init__(self, robot_model: pin.Model, dt: float,
             config: HQPConfig | None = None,
             ee_frame: str = "cylinder_link",
             r_des: np.ndarray | None = None,
             frictionloss: np.ndarray | None = None,
             damping: np.ndarray | None = None,
             impedance: ImpedanceConfig | None = None,
             friction_mode: str = "torque",
             friction_tau_scale: float = 2.0,
             force_source: str = "sensor",
             observer_kp: float = 20.0,
             observer_ki: float = 40.0,
             preload_force: float = 0.0,
             preload_ramp_s: float = 1.5,
             preload_axis: np.ndarray | None = None,
             contact_deadband: float = 0.0):
    """初始化控制器：预解析限位并预建两个 ProxQP 实例（主任务/零空间）。

    Args:
        robot_model: Pinocchio 模型（重力置零由 load_pin_model 负责）
        dt: 控制步长 [s]
        config: HQPConfig 参数（None 时取默认值）
        ee_frame: 末端 frame 名（与 TaskSpaceController 同口径）
        r_des: 期望姿态（世界系 3×3）；None 时取
            ``[[1,0,0],[0,-1,0],[0,0,-1]]``（与 TaskSpaceController
            的 initial_orientation 同口径）
        frictionloss: 关节摩擦损耗幅值 [N·m]（nv 维；None 时取零向量）。
            Pinocchio 导入器不保留 MJCF frictionloss，需由调用方从组装
            MjModel 的 dof_frictionloss 传入；以前馈并入 ĥ（同时进入力矩
            硬约束与输出力矩），模式由 friction_mode 选择（默认 "torque"）

    Note:
        QP 实例复用策略：proxsuite 支持 ``qp.update(...)`` 原地更新 H/g/C/u，
        两个实例在 __init__ 各建一次，之后每个控制步只 update+solve，
        不再重新构造。
    """
    self.model = robot_model
    self.data = self.model.createData()
    self.dt = dt
    self.config = config or HQPConfig()
    if impedance is not None:
        # 场景阻抗覆盖（ImpedanceOverride 的 k/d/k_rot/d_rot 可选字段 → K0）
        from dataclasses import replace

        cfg = self.config
        K0 = cfg.K0.copy()
        for i, val in enumerate((impedance.k, impedance.k, impedance.k,
                                 impedance.k_rot, impedance.k_rot, impedance.k_rot)):
            if val is not None:
                K0[i] = float(val)
        self.config = replace(cfg, K0=K0)
    cfg = self.config

    self.frame_id = self.model.getFrameId(ee_frame)
    self.n = self.model.nv

    # 摩擦前馈幅值（零向量 = 无补偿，行为与历史实现一致）
    self.frictionloss = (np.zeros(self.n) if frictionloss is None
                         else np.asarray(frictionloss, dtype=float).reshape(self.n))
    self.damping = (np.zeros(self.n) if damping is None
                    else np.asarray(damping, dtype=float).reshape(self.n))
    self._friction_v0 = 0.01  # tanh 平滑化速度阈值 [rad/s]
    # 摩擦前馈模式："velocity"（τ_ff=f·tanh(q̇/v₀)）或 "torque"
    # （τ_ff=f·tanh(τ_pre/τ₀)，用补偿前力矩方向治零速死区；τ₀=f/scale），
    # 与 TaskSpaceController 同语义
    if friction_mode not in ("velocity", "torque"):
        raise ValueError(f"friction_mode 不支持 {friction_mode!r}，可选 'velocity' 或 'torque'")
    self.friction_mode = friction_mode
    self.friction_tau_scale = float(friction_tau_scale)

    # 外力来源："sensor"（run 循环传入的 F/T 读数）或 "observer"
    # （PI 动量观测器估计，无传感器方案，Ren & Shan 2026 Eq.23-25）
    if force_source not in ("sensor", "observer"):
        raise ValueError(f"force_source 不支持 {force_source!r}，可选 'sensor' 或 'observer'")
    self.force_source = force_source
    self._observer: MomentumObserver | None = None
    if force_source == "observer":
        self._observer = MomentumObserver(
            robot_model, ee_frame, self.dt, kp=observer_kp, ki=observer_ki,
            frictionloss=self.frictionloss, damping=self.damping)

    # 接触预紧力跟踪（世界系）：检测到接触后按斜坡施加 preload_force·
    # preload_axis 的任务力（论文 Eq.26 中以期望接触力替代 F̂_ext 的推论），
    # 解决纯阻抗"轻触即停"无预紧的问题
    self.preload_force = float(preload_force)
    self.preload_ramp_s = max(float(preload_ramp_s), 1e-3)
    self.preload_axis = (np.zeros(3) if preload_axis is None else
                         np.asarray(preload_axis, dtype=float).reshape(3))
    self._preload_val = 0.0
    # 接触检测死区 [N]：|F| 低于该值不作接触/软化（观测器残差含模型
    # 失配噪声，deadband 防止自由空间误软化；传感器模式噪声 mN 级，
    # 默认 0 行为不变）
    self.contact_deadband = float(contact_deadband)

    # 期望姿态（世界系 3×3）
    if r_des is None:
        self.r_des = np.array([[1.0, 0.0, 0.0],
                               [0.0, -1.0, 0.0],
                               [0.0, 0.0, -1.0]])
    else:
        self.r_des = np.array(r_des, dtype=float)

    # 限位预解析：q_min/q_max、v_max/v_min（≤0 的轴视作无限制）、τ_max/τ_min
    self.q_min = np.array(self.model.lowerPositionLimit, dtype=float).copy()
    self.q_max = np.array(self.model.upperPositionLimit, dtype=float).copy()
    self._v_max = np.array(self.model.velocityLimit, dtype=float).copy()
    self._v_max[self._v_max <= 0.0] = np.inf
    if cfg.torque_limit is None:
        self._tau_max = np.array(self.model.effortLimit, dtype=float).copy()
    else:
        self._tau_max = np.full(self.n, float(cfg.torque_limit))
    self._tau_min = -self._tau_max

    # 自适应刚度上下界（对角向量）
    self._K0 = np.array(cfg.K0, dtype=float).reshape(6)
    self._K_min = cfg.K_min_ratio * self._K0

    # 预建两个 ProxQP 实例：n 变量、0 等式、6n 不等式
    # （速度 2n + 位置 2n + 力矩 2n，见 _constraint_matrices）
    self._n_in = 6 * self.n
    self._qp_main = _DenseQP(self.n, 0, self._n_in)
    self._qp_null = _DenseQP(self.n, 0, self._n_in)
    self._l_inf = np.full(self._n_in, -np.inf)
    for qp in (self._qp_main, self._qp_null):
        qp.settings.eps_abs = cfg.eps_abs
        qp.init(np.eye(self.n), np.zeros(self.n), None, None,
                np.zeros((self._n_in, self.n)), self._l_inf, np.zeros(self._n_in))

    # 诊断状态
    self.q_col: np.ndarray | None = None  # Eq.(41) 关节位姿阻抗参考（首次调用捕获）
    self.n_solver_failures = 0
    self.last_K_r: np.ndarray | None = None
    self._solve_time_sum = 0.0
    self._n_solves = 0

compute_control_task_space_with_orientation_and_imp

compute_control_task_space_with_orientation_and_imp(
    q: ndarray,
    v: ndarray,
    pos_des: ndarray,
    vel_des: ndarray,
    acc_des: ndarray,
    current_pos: ndarray,
    current_vel: ndarray,
    force_ext: ndarray,
    torque_ext: ndarray,
) -> ndarray

HQP-AC 控制律：返回关节力矩 τ = M·q̈_c + ĥ。

参数与 TaskSpaceController 同名方法完全一致 / Same signature as the TaskSpaceController method of the same name.

参数：

名称	类型	描述	默认
q	ndarray	关节状态	必需
v	ndarray	关节状态	必需
pos_des	ndarray	期望任务位置	必需
vel_des	ndarray	期望任务速度	必需
acc_des	ndarray	期望任务加速度	必需
current_pos	ndarray	实际末端位置	必需
current_vel	ndarray	实际末端线速度	必需
force_ext	ndarray	世界系末端外力（3 维）	必需
torque_ext	ndarray	世界系末端外力矩（3 维）	必需

返回：

类型	描述
ndarray	关节力矩 τ（n 维）

Note

回退语义 / Fallback: 主 QP 非 solved → 该步主任务分量回退为 无约束最小二乘解（min‖Jq̈-(target-J̇q̇)‖²），并跳过零空间 QP； 零空间 QP 非 solved → 零空间分量取 0。 每次失败 n_solver_failures 自增 1。

Note

直接使用 F/T 传感器输入（无传感器动量观测器留作后续）。

源代码位于： src/compliant_docking/control/hqp_ac.py

def compute_control_task_space_with_orientation_and_imp(
        self, q: np.ndarray, v: np.ndarray,
        pos_des: np.ndarray, vel_des: np.ndarray,
        acc_des: np.ndarray, current_pos: np.ndarray,
        current_vel: np.ndarray,
        force_ext: np.ndarray, torque_ext: np.ndarray) -> np.ndarray:
    """HQP-AC 控制律：返回关节力矩 τ = M·q̈_c + ĥ。

    参数与 TaskSpaceController 同名方法完全一致 / Same signature as the
    TaskSpaceController method of the same name.

    Args:
        q: 关节状态
        v: 关节状态
        pos_des: 期望任务位置
        vel_des: 期望任务速度
        acc_des: 期望任务加速度
        current_pos: 实际末端位置
        current_vel: 实际末端线速度
        force_ext: 世界系末端外力（3 维）
        torque_ext: 世界系末端外力矩（3 维）

    Returns:
        关节力矩 τ（n 维）

    Note:
        回退语义 / Fallback: 主 QP 非 solved → 该步主任务分量回退为
        无约束最小二乘解（min‖Jq̈-(target-J̇q̇)‖²），并跳过零空间 QP；
        零空间 QP 非 solved → 零空间分量取 0。
        每次失败 ``n_solver_failures`` 自增 1。

    Note:
        直接使用 F/T 传感器输入（无传感器动量观测器留作后续）。
    """
    q = np.array(q, dtype=float).reshape(self.n)
    v = np.array(v, dtype=float).reshape(self.n)
    pos_des = np.array(pos_des, dtype=float).reshape(3)
    vel_des = np.array(vel_des, dtype=float).reshape(3)
    acc_des = np.array(acc_des, dtype=float).reshape(3)
    current_pos = np.array(current_pos, dtype=float).reshape(3)
    current_vel = np.array(current_vel, dtype=float).reshape(3)
    force_ext = np.array(force_ext, dtype=float).reshape(3)
    torque_ext = np.array(torque_ext, dtype=float).reshape(3)

    # 外力源切换：observer 模式下用 PI 动量观测器的"纯接触"估计
    # （残差扣除已知耗散模型）替代 F/T 传感器（论文 §3.2.1 无传感器方案）；
    # 观测器由 run 循环按步调用 update。摩擦不经此通道——它已在 ĥ 中前馈
    if self._observer is not None:
        force_ext = self._observer.force_contact
        torque_ext = self._observer.wrench_contact[3:]

    # Eq.(41) 的 q_col：首次调用捕获关节位姿
    if self.q_col is None:
        self.q_col = q.copy()

    # 1) FK + 世界轴对齐 frame 原点雅可比及同参考系时间导数。
    pin.forwardKinematics(self.model, self.data, q, v)
    pin.computeJointJacobiansTimeVariation(self.model, self.data, q, v)
    pin.updateFramePlacements(self.model, self.data)
    R_cur = np.array(self.data.oMf[self.frame_id].rotation)
    J = np.array(pin.getFrameJacobian(
        self.model, self.data, self.frame_id, _FRAME_REFERENCE))
    J_dot = np.array(pin.getFrameJacobianTimeVariation(
        self.model, self.data, self.frame_id, _FRAME_REFERENCE))
    J_rot = J[3:, :]

    # 2) 动力学项：M（对称化）、ĥ = C(q,v)·v
    M = np.array(pin.crba(self.model, self.data, q))
    M = np.triu(M) + np.triu(M, 1).T  # crba 只保证上三角，显式对称化
    pin.computeCoriolisMatrix(self.model, self.data, q, v)
    h = np.array(self.data.C) @ v
    # 关节摩擦前馈（Pinocchio 模型不含 frictionloss，仿真侧有）：
    # 并入 ĥ 使力矩硬约束与输出力矩自动一致；torque 模式用补偿前 ĥ
    # 的方向决定摩擦方向（与 TaskSpaceController 同语义）
    if self.friction_mode == "torque":
        h = h + self.frictionloss * np.tanh(
            h * self.friction_tau_scale / np.maximum(self.frictionloss, 1e-9))
    else:
        h = h + self.frictionloss * np.tanh(v / self._friction_v0)
    h = h + self.damping * v

    # 任务空间惯性 Λ 及其逆（Λ⁻¹ = J M⁻¹ Jᵀ，pinv 稳健化后对称化）
    M_inv = np.linalg.pinv(M)
    A_task = J @ M_inv @ J.T
    Lambda = np.linalg.pinv(A_task)
    Lambda = 0.5 * (Lambda + Lambda.T)

    # 3) 误差与自适应刚度：Eq.(26)(27)
    #    姿态误差取 log(R_cur·R_desᵀ)（"当前相对期望"，与 e_pos=current-desired
    #    同向），保证 ė_p = Δν = [ẋ-ẋ_d; ω] 严格成立，使 Eq.(29)(31) 的统一式
    #    F_r = -D_r·Δν - K_r·e_p 在平动/旋转两通道都是耗散阻抗
    #    （ë_p + Λ⁻¹D_r·ė_p + Λ⁻¹K_r·e_p = 0）。注意：TaskSpaceController 的
    #    ori_err = log(R_des·R_curᵀ) 与 u_rot = +k_rot·ori_err + d_rot·(-ω)
    #    在代数上与本处约定完全等价（相差一个整体负号）。
    e_pos = current_pos - pos_des
    e_ori = pin.log3(R_cur @ self.r_des.T)
    delta_nu = np.concatenate([current_vel - vel_des, J_rot @ v])  # 期望角速度为 0
    e_p = np.concatenate([e_pos, e_ori])
    F_contact = float(np.linalg.norm(np.concatenate([force_ext, torque_ext])))
    F_contact_eff = max(F_contact - self.contact_deadband, 0.0)
    K_r = self._adaptive_stiffness(F_contact_eff)
    self.last_K_r = K_r

    # 4) 参考阻尼与广义力：Eq.(29)
    D_r = self._reference_damping(Lambda, K_r)
    F_r = -D_r @ delta_nu - K_r * e_p

    # 5) 主任务 QP（Eq.31）：min ‖Jq̈ + J̇q̇ - target‖²，target = ν̇_r + Λ⁻¹F_r
    #    接触预紧项：检测到接触（|F|>1N）后按斜坡施加 preload_force·axis
    #    的任务力（推入对接方向），脱离接触则回落，力目标
    #    F_pre = preload · ramp 加入期望操作空间广义力
    if self.preload_force > 0.0:
        in_contact = F_contact > 1.0
        rate = self.preload_force / self.preload_ramp_s * self.dt
        if in_contact:
            self._preload_val = min(self.preload_force, self._preload_val + rate)
        else:
            self._preload_val = max(0.0, self._preload_val - rate)
    F_pre6 = np.concatenate([self._preload_val * self.preload_axis,
                             np.zeros(3)])
    nu_dot_r = np.concatenate([acc_des, np.zeros(3)])
    target = nu_dot_r + A_task @ (F_r + F_pre6)
    H_main = 2.0 * (J.T @ J) + _H_REG * np.eye(self.n)
    g_main = 2.0 * (J.T @ (J_dot @ v - target))

    # 6) 硬约束（Eq.34-36，ZOH 时域 dt_p）
    C_mat, u = self._constraint_matrices(q, v, M, h)

    t_start = time.perf_counter()
    a_m = self._solve_qp(self._qp_main, H_main, g_main, C_mat, u)
    t_main = time.perf_counter() - t_start
    if a_m is not None:
        q_ddot_m = a_m
    else:
        # 回退：无约束最小二乘（主任务分量）
        self.n_solver_failures += 1
        q_ddot_m = np.linalg.lstsq(J, target - J_dot @ v, rcond=None)[0]
        self._solve_time_sum += t_main
        self._n_solves += 1
        return M @ q_ddot_m + h  # 主 QP 失败 → 跳过零空间

    # 7) 零空间 QP（Eq.38-43）：奇异性规避 + 关节位姿阻抗
    omega = self._manipulability(q)
    q_ddot_sa = np.zeros(self.n)
    if omega < self.config.omega_th:
        grad_w = self._manipulability_gradient(q)
        norm_gw = float(np.linalg.norm(grad_w))
        if norm_gw > 1e-12:
            q_ddot_sa = ((self.config.omega_th - omega) / self.config.omega_th) \
                * grad_w / norm_gw
    q_ddot_ji = -self.config.D_ji @ v - self.config.K_ji @ (q - self.q_col)
    a_null = self.config.k_sa * q_ddot_sa + self.config.k_ji * q_ddot_ji

    J_pinv = M_inv @ J.T @ Lambda  # Eq.(43)：J# = M⁻¹JᵀΛ
    N = np.eye(self.n) - J_pinv @ J
    H_null = (2.0 + _H_REG) * np.eye(self.n)
    g_null = -2.0 * a_null
    u_null = u - C_mat @ q_ddot_m

    t_null_start = time.perf_counter()
    a_n = self._solve_qp(self._qp_null, H_null, g_null, C_mat @ N, u_null)
    self._solve_time_sum += t_main + (time.perf_counter() - t_null_start)
    self._n_solves += 1
    if a_n is not None:
        q_ddot_n = a_n
    else:
        # 回退：零空间分量取 0
        self.n_solver_failures += 1
        q_ddot_n = np.zeros(self.n)

    # 8) 合成：q̈_c = q̈*_m + N·q̈*_null，τ = M·q̈_c + ĥ
    q_ddot_c = q_ddot_m + N @ q_ddot_n
    return M @ q_ddot_c + h

update_momentum_observer

update_momentum_observer(
    q: ndarray, v: ndarray, tau_applied: ndarray
) -> None

以步进后的关节状态与实际施加力矩推进一步动量观测器。

仅 force_source == "observer" 时有效（其余为 no-op）； τ_applied 应为限幅后的实际力矩（上一控制周期）。

源代码位于： src/compliant_docking/control/hqp_ac.py

def update_momentum_observer(self, q: np.ndarray, v: np.ndarray,
                             tau_applied: np.ndarray) -> None:
    """以步进后的关节状态与实际施加力矩推进一步动量观测器。

    仅 force_source == "observer" 时有效（其余为 no-op）；
    τ_applied 应为限幅后的实际力矩（上一控制周期）。
    """
    if self._observer is not None:
        self._observer.update(q, v, tau_applied)

SE(3) Lie 群阻抗

SE3LieImpedanceController(
    robot_model,
    dt,
    config=None,
    ee_frame="cylinder_link",
    frictionloss=None,
    damping=None,
    friction_mode="torque",
    friction_tau_scale=2.0,
    A=None,
    D=None,
    K=None,
)

• get_task_space_state(q, v)：返回世界系位置、线速度与姿态，供统一遥测使用；
• get_body_state(q, v)：返回 EE 位姿、LOCAL Jacobian 与其时间导数；
• compute_control(q, v, T_d, V_d, Vdot_d, F_body, F_d=None)：执行 Eq. 44–66 标称控制律并返回关节力矩；运动量和 wrench 均为线量在前；
• latest_diagnostics：最近一步的 lam、lam_dot、任务矩阵条件数、wrench 范数与力矩范数等诊断，不承载控制状态。

A、D、K 可直接注入一般 6×6 矩阵；未提供时由 SE3ImpedanceConfig 的对角参数构造。

Lie 群数学工具

公开函数均采用 Pinocchio 的线量在前排列：twist [v; ω]、wrench [f; n]。

lie_se3.py — SE(3)/SO(3) 微分指数映射（dexp）闭式工具集

实现 Kim et al. 2025（IEEE T-RO, Vol. 41）"Impedance Control Design Framework Using Commutative Map Between SE(3) and se(3)" Section II-C / II-D（Eq. 26-43）：

• Lemma 1（Eq. 26-27）：SO(3) 的 dexp / dexp⁻¹
• Lemma 2（Eq. 28-31）：SE(3) 的 dexp / dexp⁻¹（含旋转-平移耦合块 C_ξ(η)、D_ξ(η)）
• Lemma 3（Eq. 34-35）：d/dt dexp_ξ = C_ξ(ξ̇) 与 d/dt dexp_ξ⁻¹ = D_ξ(ξ̇)
• Lemma 4（Eq. 36-43）：SE(3) 的 d/dt dexp_λ 与 d/dt dexp_λ⁻¹（解析实现）

约定（论文 Eq. 1-5，与 Pinocchio Motion 一致）：

• twist V = [v; ω]（线量在前、角量在后），wrench F = [f; n]
• ceiling form [V] = [[ω], v; 0, 0]
• ad_V = [[ω], [v]; 0, [ω]]，Ad_T = [R, [r]R; 0, R]
• 标量系数（Eq. 9）：α = sinθ/θ、β = 2(1-cosθ)/θ²、γ = α/β = (θ/2)cot(θ/2)

dexp 约定（本模块实现的是论文 Eq. 17a/22 的右平凡化版本）::

vee(Ṫ T⁻¹) = dexp(λ) λ̇，   T = Exp(λ)

即 dexp 把指数坐标速度映射为空间（spatial）twist。论文控制器（Eq. 48）中 相对位姿 T̃ 以当前末端系 {b} 为参考"空间系"，因此 Ṽ = dexp_λ λ̇ 恰是用 {b} 系表达的相对 twist——接线见 control/se3_impedance.py。

数值稳定性：θ→0 时 (1-α)/θ² 等标量系数存在灾难性消去，统一在 θ < _TAYLOR_EPS 时切换显式 Taylor 展开（含 Γ1..Γ5 与 2(1-γ/β)/θ⁴）。 dexp⁻¹ 族函数在 ‖ξ‖ ≥ 2π 抛 ValueError（论文 Lemma 1：定义域 ‖ξ‖ < 2π， 2kπ 处 β→0 奇异；控制器经 log6 取得的 λ 满足 ‖ξ‖ < π，不会触界）。

函数：

skew3

skew3(x: ndarray) -> ndarray

⌈x⌉ ∈ so(3)（论文 Eq. 1）。

源代码位于： src/compliant_docking/control/lie_se3.py

def skew3(x: np.ndarray) -> np.ndarray:
    """⌈x⌉ ∈ so(3)（论文 Eq. 1）。"""
    x = np.asarray(x, dtype=float).reshape(3)
    return np.array([
        [0.0, -x[2], x[1]],
        [x[2], 0.0, -x[0]],
        [-x[1], x[0], 0.0],
    ])

vee3

vee3(S: ndarray) -> ndarray

so(3) → R³，skew3 的逆算子。

源代码位于： src/compliant_docking/control/lie_se3.py

def vee3(S: np.ndarray) -> np.ndarray:
    """so(3) → R³，skew3 的逆算子。"""
    S = np.asarray(S, dtype=float).reshape(3, 3)
    return np.array([S[2, 1], S[0, 2], S[1, 0]])

hat4

hat4(V: ndarray) -> ndarray

[V] ∈ se(3) 的 4×4 ceiling form（论文 Eq. 2），V = [v; ω]。

源代码位于： src/compliant_docking/control/lie_se3.py

def hat4(V: np.ndarray) -> np.ndarray:
    """[V] ∈ se(3) 的 4×4 ceiling form（论文 Eq. 2），V = [v; ω]。"""
    V = np.asarray(V, dtype=float).reshape(6)
    M = np.zeros((4, 4))
    M[:3, :3] = skew3(V[3:])
    M[:3, 3] = V[:3]
    return M

vee4

vee4(M: ndarray) -> ndarray

se(3) 4×4 矩阵 → [v; ω]，hat4 的逆算子。

源代码位于： src/compliant_docking/control/lie_se3.py

def vee4(M: np.ndarray) -> np.ndarray:
    """se(3) 4×4 矩阵 → [v; ω]，hat4 的逆算子。"""
    M = np.asarray(M, dtype=float).reshape(4, 4)
    return np.concatenate([M[:3, 3], vee3(M[:3, :3])])

ad6

ad6(V: ndarray) -> ndarray

伴随算子 ad_V（论文 Eq. 3-4），V = [v; ω]。

源代码位于： src/compliant_docking/control/lie_se3.py

def ad6(V: np.ndarray) -> np.ndarray:
    """伴随算子 ad_V（论文 Eq. 3-4），V = [v; ω]。"""
    V = np.asarray(V, dtype=float).reshape(6)
    w_skew = skew3(V[3:])
    return np.block([
        [w_skew, skew3(V[:3])],
        [np.zeros((3, 3)), w_skew],
    ])

adjoint

adjoint(T) -> ndarray

伴随变换 Ad_T（论文 Eq. 5）。

T = ^A T_B（B 在 A 中的位姿）时满足 V_A = Ad_T V_B。

源代码位于： src/compliant_docking/control/lie_se3.py

def adjoint(T) -> np.ndarray:
    """伴随变换 Ad_T（论文 Eq. 5）。

    T = ^A T_B（B 在 A 中的位姿）时满足 ``V_A = Ad_T V_B``。
    """
    R, p = _Rp(T)
    return np.block([
        [R, skew3(p) @ R],
        [np.zeros((3, 3)), R],
    ])

adjoint_wrench

adjoint_wrench(T) -> ndarray

wrench 变换矩阵 Ad_T^{-T}（co-adjoint 作用）。

T = ^A T_B 时满足 F_A = Ad_T^{-T} F_B 且功率不变 V_Aᵀ F_A = V_Bᵀ F_B。显式形式 [[R, 0], [[p]R, R]] 的左下块即矩平移项 p × f。

源代码位于： src/compliant_docking/control/lie_se3.py

def adjoint_wrench(T) -> np.ndarray:
    """wrench 变换矩阵 ``Ad_T^{-T}``（co-adjoint 作用）。

    T = ^A T_B 时满足 ``F_A = Ad_T^{-T} F_B`` 且功率不变
    ``V_Aᵀ F_A = V_Bᵀ F_B``。显式形式 ``[[R, 0], [[p]R, R]]``
    的左下块即矩平移项 ``p × f``。
    """
    R, p = _Rp(T)
    return np.block([
        [R, np.zeros((3, 3))],
        [skew3(p) @ R, R],
    ])

dexp_so3

dexp_so3(xi: ndarray) -> ndarray

dexp_ξ（论文 Eq. 26，右平凡化）：vee(Ṙ Rᵀ) = dexp_ξ ξ̇。

源代码位于： src/compliant_docking/control/lie_se3.py

def dexp_so3(xi: np.ndarray) -> np.ndarray:
    """dexp_ξ（论文 Eq. 26，右平凡化）：``vee(Ṙ Rᵀ) = dexp_ξ ξ̇``。"""
    xi = np.asarray(xi, dtype=float).reshape(3)
    c = _coeffs(float(np.linalg.norm(xi)))
    S = skew3(xi)
    return np.eye(3) + 0.5 * c.beta * S + c.g1 * (S @ S)

dexp_inv_so3

dexp_inv_so3(xi: ndarray) -> ndarray

dexp_ξ⁻¹（论文 Eq. 27），dexp_so3 的矩阵逆。

源代码位于： src/compliant_docking/control/lie_se3.py

def dexp_inv_so3(xi: np.ndarray) -> np.ndarray:
    """dexp_ξ⁻¹（论文 Eq. 27），dexp_so3 的矩阵逆。"""
    xi = np.asarray(xi, dtype=float).reshape(3)
    c = _coeffs(float(np.linalg.norm(xi)), inverse=True)
    S = skew3(xi)
    return np.eye(3) - 0.5 * S + c.g4 * (S @ S)

dexp_dot_so3

dexp_dot_so3(
    xi: ndarray, xi_dot: ndarray
) -> ndarray

d/dt dexp_ξ（论文 Lemma 3 / Eq. 34）= C_ξ(ξ̇)。

源代码位于： src/compliant_docking/control/lie_se3.py

def dexp_dot_so3(xi: np.ndarray, xi_dot: np.ndarray) -> np.ndarray:
    """d/dt dexp_ξ（论文 Lemma 3 / Eq. 34）= C_ξ(ξ̇)。"""
    return _C_block(xi, xi_dot)

dexp_inv_dot_so3

dexp_inv_dot_so3(
    xi: ndarray, xi_dot: ndarray
) -> ndarray

d/dt dexp_ξ⁻¹（论文 Lemma 3 / Eq. 35）= D_ξ(ξ̇)。

源代码位于： src/compliant_docking/control/lie_se3.py

def dexp_inv_dot_so3(xi: np.ndarray, xi_dot: np.ndarray) -> np.ndarray:
    """d/dt dexp_ξ⁻¹（论文 Lemma 3 / Eq. 35）= D_ξ(ξ̇)。"""
    return _D_block(xi, xi_dot)

dexp_se3

dexp_se3(lambda_: ndarray) -> ndarray

dexp_λ（论文 Lemma 2 / Eq. 28）。

满足右平凡化微分关系 vee(Ṫ T⁻¹) = dexp_λ λ̇（λ = [η; ξ]）。 注意右上耦合块 C_ξ(η) 不可省略——block_diag(dexp_ξ, dexp_ξ) 会丢掉 旋转-平移耦合。

源代码位于： src/compliant_docking/control/lie_se3.py

def dexp_se3(lambda_: np.ndarray) -> np.ndarray:
    """dexp_λ（论文 Lemma 2 / Eq. 28）。

    满足右平凡化微分关系 ``vee(Ṫ T⁻¹) = dexp_λ λ̇``（λ = [η; ξ]）。
    注意右上耦合块 C_ξ(η) 不可省略——block_diag(dexp_ξ, dexp_ξ) 会丢掉
    旋转-平移耦合。
    """
    lam = np.asarray(lambda_, dtype=float).reshape(6)
    eta, xi = lam[:3], lam[3:]
    theta = float(np.linalg.norm(xi))
    c = _coeffs(theta)
    S = skew3(xi)
    E = np.eye(3) + 0.5 * c.beta * S + c.g1 * (S @ S)
    out = np.zeros((6, 6))
    out[:3, :3] = E
    out[:3, 3:] = _C_block(xi, eta, coeffs=c)
    out[3:, 3:] = E
    return out

dexp_inv_se3

dexp_inv_se3(lambda_: ndarray) -> ndarray

dexp_λ⁻¹（论文 Lemma 2 / Eq. 29），dexp_se3 的矩阵逆。

源代码位于： src/compliant_docking/control/lie_se3.py

def dexp_inv_se3(lambda_: np.ndarray) -> np.ndarray:
    """dexp_λ⁻¹（论文 Lemma 2 / Eq. 29），dexp_se3 的矩阵逆。"""
    lam = np.asarray(lambda_, dtype=float).reshape(6)
    eta, xi = lam[:3], lam[3:]
    theta = float(np.linalg.norm(xi))
    c = _coeffs(theta, inverse=True)
    S = skew3(xi)
    E = np.eye(3) - 0.5 * S + c.g4 * (S @ S)
    out = np.zeros((6, 6))
    out[:3, :3] = E
    out[:3, 3:] = _D_block(xi, eta)
    out[3:, 3:] = E
    return out

dexp_dot_se3

dexp_dot_se3(
    lambda_: ndarray, lambda_dot: ndarray
) -> ndarray

d/dt dexp_λ（论文 Lemma 4 / Eq. 36），解析实现。

源代码位于： src/compliant_docking/control/lie_se3.py

def dexp_dot_se3(lambda_: np.ndarray, lambda_dot: np.ndarray) -> np.ndarray:
    """d/dt dexp_λ（论文 Lemma 4 / Eq. 36），解析实现。"""
    lam = np.asarray(lambda_, dtype=float).reshape(6)
    lam_dot = np.asarray(lambda_dot, dtype=float).reshape(6)
    eta, xi = lam[:3], lam[3:]
    eta_dot, xi_dot = lam_dot[:3], lam_dot[3:]
    diag = _C_block(xi, xi_dot)
    out = np.zeros((6, 6))
    out[:3, :3] = diag
    out[:3, 3:] = _C_dot_block(xi, xi_dot, eta, eta_dot)
    out[3:, 3:] = diag
    return out

dexp_inv_dot_se3

dexp_inv_dot_se3(
    lambda_: ndarray, lambda_dot: ndarray
) -> ndarray

d/dt dexp_λ⁻¹（论文 Lemma 4 / Eq. 37），解析实现。

源代码位于： src/compliant_docking/control/lie_se3.py

def dexp_inv_dot_se3(lambda_: np.ndarray, lambda_dot: np.ndarray) -> np.ndarray:
    """d/dt dexp_λ⁻¹（论文 Lemma 4 / Eq. 37），解析实现。"""
    lam = np.asarray(lambda_, dtype=float).reshape(6)
    lam_dot = np.asarray(lambda_dot, dtype=float).reshape(6)
    eta, xi = lam[:3], lam[3:]
    eta_dot, xi_dot = lam_dot[:3], lam_dot[3:]
    diag = _D_block(xi, xi_dot)
    out = np.zeros((6, 6))
    out[:3, :3] = diag
    out[:3, 3:] = _D_dot_block(xi, xi_dot, eta, eta_dot)
    out[3:, 3:] = diag
    return out

Wrench 坐标与参考点变换

transform_wrench(
    force, torque, p_source, R_source, p_target, R_target
) -> tuple[np.ndarray, np.ndarray]

wrench_to_body(
    force, torque, p_source, R_source, T_target
) -> np.ndarray

transform_wrench 返回 target frame 表达、target 原点参考的 (force, torque)； wrench_to_body 是面向控制器的便利封装，返回 [f_E; n_E]。两者都会保留 参考点变更产生的 (p_source-p_target)×f 力矩项，不负责更改传感器读数符号。

摩擦与关节阻尼前馈

friction.py — 关节摩擦/阻尼前馈的共享实现。

从 TaskSpaceController 已验证逻辑中逐位抽取（tests/test_friction_comp.py 与 slow 回归锚点保证行为不变），供 task_space 与 SE(3) Lie 阻抗控制器 共用，保证 iiwa14 / FR3 上多控制器对比的摩擦补偿公平性。

背景：Pinocchio 的 URDF/MJCF 导入不保留 MuJoCo 的 dof_frictionloss 与 dof_damping（被动广义力 -f·sign(q̇)、-d·q̇），故以前馈补偿：

• torque 模式（默认）：τ_ff = f·tanh(τ_pre·scale/f)，用补偿前力矩 方向决定摩擦方向（τ₀=f/scale），零速静摩擦死区一出发即被抬过阈值；
• velocity 模式：τ_ff = f·tanh(q̇/v₀)，零速时补偿消失，低速易粘滑。

属性

FRICTION_MODES module-attribute

FRICTION_MODES = ('velocity', 'torque')

函数：

validate_friction_mode

validate_friction_mode(mode: str) -> None

源代码位于： src/compliant_docking/control/friction.py

def validate_friction_mode(mode: str) -> None:
    if mode not in FRICTION_MODES:
        raise ValueError(f"friction_mode 不支持 {mode!r}，可选 'velocity' 或 'torque'")

friction_feedforward

friction_feedforward(
    tau_pre: ndarray,
    v: ndarray,
    frictionloss: ndarray,
    damping: ndarray,
    mode: str = "torque",
    tau_scale: float = 2.0,
    v0: float = 0.01,
) -> ndarray

在逆动力学力矩上叠加摩擦/阻尼前馈，返回补偿后力矩。

参数：

名称	类型	描述	默认
tau_pre	ndarray	补偿前关节力矩（摩擦力矩模式用其方向）	必需
v	ndarray	关节速度	必需
frictionloss	ndarray	摩擦损耗幅值（零向量 = 无补偿）	必需
damping	ndarray	关节阻尼系数（前馈 +damping·v）	必需
mode	str	"torque"（默认）或 "velocity"	'torque'
tau_scale	float	torque 模式的力矩-摩擦换算 scale（τ₀=f/scale）	2.0
v0	float	velocity 模式的 tanh 平滑速度阈值 [rad/s]	0.01

源代码位于： src/compliant_docking/control/friction.py

def friction_feedforward(tau_pre: np.ndarray, v: np.ndarray,
                         frictionloss: np.ndarray, damping: np.ndarray,
                         mode: str = "torque", tau_scale: float = 2.0,
                         v0: float = 0.01) -> np.ndarray:
    """在逆动力学力矩上叠加摩擦/阻尼前馈，返回补偿后力矩。

    Args:
        tau_pre: 补偿前关节力矩（摩擦力矩模式用其方向）
        v: 关节速度
        frictionloss: 摩擦损耗幅值（零向量 = 无补偿）
        damping: 关节阻尼系数（前馈 +damping·v）
        mode: "torque"（默认）或 "velocity"
        tau_scale: torque 模式的力矩-摩擦换算 scale（τ₀=f/scale）
        v0: velocity 模式的 tanh 平滑速度阈值 [rad/s]
    """
    validate_friction_mode(mode)
    if mode == "torque":
        tau = tau_pre + frictionloss * np.tanh(
            tau_pre * tau_scale / np.maximum(frictionloss, 1e-9))
    else:
        tau = tau_pre + frictionloss * np.tanh(v / v0)
    return tau + damping * v

PI 动量观测器

无传感器外力估计：PI 动量观测器（世界系 LWA 口径）。

用法（每控制步，τ_applied 为实际施加——限幅后的力矩）： tau_ext = obs.update(q, v, tau_applied) force = obs.force # 世界系 3 维外力估计（同一步的 J 映射）

源代码位于： src/compliant_docking/control/momentum_observer.py

class MomentumObserver:
    """无传感器外力估计：PI 动量观测器（世界系 LWA 口径）。

    用法（每控制步，τ_applied 为实际施加——限幅后的力矩）：
        tau_ext = obs.update(q, v, tau_applied)
        force = obs.force   # 世界系 3 维外力估计（同一步的 J 映射）
    """

    def __init__(self, robot_model: pin.Model, ee_frame: str, dt: float, *,
                 kp: float = 20.0, ki: float = 40.0,
                 integral_limit: float = 20.0,
                 frictionloss: np.ndarray | None = None,
                 damping: np.ndarray | None = None,
                 friction_v0: float = 0.01):
        """
        Args:
            kp: 观测器比例增益 [s⁻¹]（K_p 量纲 1/s）
            ki: 观测器积分增益 [s⁻¹]（K_i 1/s²）
            integral_limit: 积分项 ∫Δp 的逐关节限幅 [N·m·s]，防饱和
            frictionloss: 已知关节耗散模型（与仿真同源）。提供后
                ``force_contact`` 从残差中扣除耗散项，得到不含摩擦污染的
                纯接触力估计——控制器的前馈已补偿同一模型，残差中的耗散
                分量对力控而言是可减去的已知项
            damping: 已知关节耗散模型（与仿真同源）。提供后
                ``force_contact`` 从残差中扣除耗散项，得到不含摩擦污染的
                纯接触力估计——控制器的前馈已补偿同一模型，残差中的耗散
                分量对力控而言是可减去的已知项
            friction_v0: tanh 平滑化速度阈值 [rad/s]（与控制器一致）
        """
        self.model = robot_model
        self.data = robot_model.createData()
        self.dt = float(dt)
        self.kp = float(kp)
        self.ki = float(ki)
        self.integral_limit = float(integral_limit)
        self.frame_id = robot_model.getFrameId(ee_frame)
        n = robot_model.nv
        self.frictionloss = (np.zeros(n) if frictionloss is None
                             else np.asarray(frictionloss, dtype=float).reshape(n))
        self.damping = (np.zeros(n) if damping is None
                        else np.asarray(damping, dtype=float).reshape(n))
        self.friction_v0 = float(friction_v0)

        self.momentum_hat = np.zeros(n)
        self.integral_err = np.zeros(n)
        self.tau_ext = np.zeros(n)
        self._wrench = np.zeros(6)
        self._wrench_contact = np.zeros(6)
        self._started = False

    @property
    def wrench(self) -> np.ndarray:
        """最新外力/外力矩估计（世界系 LWA，6 维，含摩擦污染）。"""
        return self._wrench.copy()

    @property
    def force(self) -> np.ndarray:
        """最新外力估计（世界系，3 维，含摩擦污染）。"""
        return self._wrench[:3].copy()

    @property
    def wrench_contact(self) -> np.ndarray:
        """扣除已知耗散模型后的纯接触力/力矩估计（世界系 LWA，6 维）。"""
        return self._wrench_contact.copy()

    @property
    def force_contact(self) -> np.ndarray:
        """扣除已知耗散模型后的纯接触力估计（世界系，3 维）。"""
        return self._wrench_contact[:3].copy()

    def reset(self, q: np.ndarray, v: np.ndarray) -> None:
        """以当前状态初始化动量估计（避免初值阶跃）。"""
        M = pin.crba(self.model, self.data, q)
        self.momentum_hat = np.array(M) @ np.asarray(v, dtype=float).reshape(-1)
        self.integral_err[:] = 0.0
        self.tau_ext[:] = 0.0
        self._started = True

    def update(self, q: np.ndarray, v: np.ndarray, tau_applied: np.ndarray) -> np.ndarray:
        """推进一步观测器，返回关节外力矩估计 τ̃_ext（n 维）。

        Args:
            q: 当前关节状态（步进后）
            v: 当前关节状态（步进后）
            tau_applied: 上一控制周期实际施加的关节力矩（限幅后）
        """
        q = np.asarray(q, dtype=float).reshape(-1)
        v = np.asarray(v, dtype=float).reshape(-1)
        tau_applied = np.asarray(tau_applied, dtype=float).reshape(-1)
        if not self._started:
            self.reset(q, v)

        # p = M q̇ 与 β = g - Cᵀq̇（精确模型项；重力置零时 g=0）
        M = np.array(pin.crba(self.model, self.data, q))
        M = np.triu(M) + np.triu(M, 1).T
        pin.computeCoriolisMatrix(self.model, self.data, q, v)
        g = pin.computeGeneralizedGravity(self.model, self.data, q)
        beta = np.asarray(g) - np.array(self.data.C).T @ v
        p = M @ v

        # PI 残差：误差 e = p - p̂（真减估），r = K_p·e + K_i·∫e（积分限幅防饱和）。
        # 注意符号方向：该形式误差系统 ė = τ_e - r 稳定（特征多项式
        # s² + K_p·s + K_i 全负实部）；若按 Δp = p̃ - p 正馈则为鞍点发散
        delta_p = p - self.momentum_hat
        self.integral_err = np.clip(
            self.integral_err + self.dt * delta_p,
            -self.integral_limit, self.integral_limit)
        self.tau_ext = self.kp * delta_p + self.ki * self.integral_err

        # 动量估计递推：p̂̇ = τ_applied - β + r
        self.momentum_hat = self.momentum_hat + self.dt * (
            tau_applied - beta + self.tau_ext)

        # 任务空间映射：F̂ = (J Jᵀ)⁻¹ J τ̃_ext（世界系 LWA）
        pin.computeJointJacobians(self.model, self.data, q)
        pin.updateFramePlacement(self.model, self.data, self.frame_id)
        J = np.array(pin.getFrameJacobian(
            self.model, self.data, self.frame_id, _FRAME_REFERENCE))
        self._wrench = np.linalg.solve(J @ J.T + 1e-9 * np.eye(6), J @ self.tau_ext)
        # 纯接触估计：扣除已知耗散模型（控制器前馈补偿的同一项）
        tau_diss = (self.frictionloss * np.tanh(v / self.friction_v0)
                    + self.damping * v)
        self._wrench_contact = np.linalg.solve(
            J @ J.T + 1e-9 * np.eye(6), J @ (self.tau_ext - tau_diss))
        return self.tau_ext.copy()

属性

wrench property

wrench: ndarray

最新外力/外力矩估计（世界系 LWA，6 维，含摩擦污染）。

force property

force: ndarray

最新外力估计（世界系，3 维，含摩擦污染）。

wrench_contact property

wrench_contact: ndarray

扣除已知耗散模型后的纯接触力/力矩估计（世界系 LWA，6 维）。

force_contact property

force_contact: ndarray

扣除已知耗散模型后的纯接触力估计（世界系，3 维）。

方法：

__init__

__init__(
    robot_model: Model,
    ee_frame: str,
    dt: float,
    *,
    kp: float = 20.0,
    ki: float = 40.0,
    integral_limit: float = 20.0,
    frictionloss: ndarray | None = None,
    damping: ndarray | None = None,
    friction_v0: float = 0.01,
)

参数：

名称	类型	描述	默认
kp	float	观测器比例增益 [s⁻¹]（K_p 量纲 1/s）	20.0
ki	float	观测器积分增益 [s⁻¹]（K_i 1/s²）	40.0
integral_limit	float	积分项 ∫Δp 的逐关节限幅 [N·m·s]，防饱和	20.0
frictionloss	ndarray | None	已知关节耗散模型（与仿真同源）。提供后 force_contact 从残差中扣除耗散项，得到不含摩擦污染的 纯接触力估计——控制器的前馈已补偿同一模型，残差中的耗散 分量对力控而言是可减去的已知项	None
damping	ndarray | None	已知关节耗散模型（与仿真同源）。提供后 force_contact 从残差中扣除耗散项，得到不含摩擦污染的 纯接触力估计——控制器的前馈已补偿同一模型，残差中的耗散 分量对力控而言是可减去的已知项	None
friction_v0	float	tanh 平滑化速度阈值 [rad/s]（与控制器一致）	0.01

源代码位于： src/compliant_docking/control/momentum_observer.py

def __init__(self, robot_model: pin.Model, ee_frame: str, dt: float, *,
             kp: float = 20.0, ki: float = 40.0,
             integral_limit: float = 20.0,
             frictionloss: np.ndarray | None = None,
             damping: np.ndarray | None = None,
             friction_v0: float = 0.01):
    """
    Args:
        kp: 观测器比例增益 [s⁻¹]（K_p 量纲 1/s）
        ki: 观测器积分增益 [s⁻¹]（K_i 1/s²）
        integral_limit: 积分项 ∫Δp 的逐关节限幅 [N·m·s]，防饱和
        frictionloss: 已知关节耗散模型（与仿真同源）。提供后
            ``force_contact`` 从残差中扣除耗散项，得到不含摩擦污染的
            纯接触力估计——控制器的前馈已补偿同一模型，残差中的耗散
            分量对力控而言是可减去的已知项
        damping: 已知关节耗散模型（与仿真同源）。提供后
            ``force_contact`` 从残差中扣除耗散项，得到不含摩擦污染的
            纯接触力估计——控制器的前馈已补偿同一模型，残差中的耗散
            分量对力控而言是可减去的已知项
        friction_v0: tanh 平滑化速度阈值 [rad/s]（与控制器一致）
    """
    self.model = robot_model
    self.data = robot_model.createData()
    self.dt = float(dt)
    self.kp = float(kp)
    self.ki = float(ki)
    self.integral_limit = float(integral_limit)
    self.frame_id = robot_model.getFrameId(ee_frame)
    n = robot_model.nv
    self.frictionloss = (np.zeros(n) if frictionloss is None
                         else np.asarray(frictionloss, dtype=float).reshape(n))
    self.damping = (np.zeros(n) if damping is None
                    else np.asarray(damping, dtype=float).reshape(n))
    self.friction_v0 = float(friction_v0)

    self.momentum_hat = np.zeros(n)
    self.integral_err = np.zeros(n)
    self.tau_ext = np.zeros(n)
    self._wrench = np.zeros(6)
    self._wrench_contact = np.zeros(6)
    self._started = False

reset

reset(q: ndarray, v: ndarray) -> None

以当前状态初始化动量估计（避免初值阶跃）。

源代码位于： src/compliant_docking/control/momentum_observer.py

def reset(self, q: np.ndarray, v: np.ndarray) -> None:
    """以当前状态初始化动量估计（避免初值阶跃）。"""
    M = pin.crba(self.model, self.data, q)
    self.momentum_hat = np.array(M) @ np.asarray(v, dtype=float).reshape(-1)
    self.integral_err[:] = 0.0
    self.tau_ext[:] = 0.0
    self._started = True

update

update(
    q: ndarray, v: ndarray, tau_applied: ndarray
) -> ndarray

推进一步观测器，返回关节外力矩估计 τ̃_ext（n 维）。

参数：

名称	类型	描述	默认
q	ndarray	当前关节状态（步进后）	必需
v	ndarray	当前关节状态（步进后）	必需
tau_applied	ndarray	上一控制周期实际施加的关节力矩（限幅后）	必需

源代码位于： src/compliant_docking/control/momentum_observer.py

def update(self, q: np.ndarray, v: np.ndarray, tau_applied: np.ndarray) -> np.ndarray:
    """推进一步观测器，返回关节外力矩估计 τ̃_ext（n 维）。

    Args:
        q: 当前关节状态（步进后）
        v: 当前关节状态（步进后）
        tau_applied: 上一控制周期实际施加的关节力矩（限幅后）
    """
    q = np.asarray(q, dtype=float).reshape(-1)
    v = np.asarray(v, dtype=float).reshape(-1)
    tau_applied = np.asarray(tau_applied, dtype=float).reshape(-1)
    if not self._started:
        self.reset(q, v)

    # p = M q̇ 与 β = g - Cᵀq̇（精确模型项；重力置零时 g=0）
    M = np.array(pin.crba(self.model, self.data, q))
    M = np.triu(M) + np.triu(M, 1).T
    pin.computeCoriolisMatrix(self.model, self.data, q, v)
    g = pin.computeGeneralizedGravity(self.model, self.data, q)
    beta = np.asarray(g) - np.array(self.data.C).T @ v
    p = M @ v

    # PI 残差：误差 e = p - p̂（真减估），r = K_p·e + K_i·∫e（积分限幅防饱和）。
    # 注意符号方向：该形式误差系统 ė = τ_e - r 稳定（特征多项式
    # s² + K_p·s + K_i 全负实部）；若按 Δp = p̃ - p 正馈则为鞍点发散
    delta_p = p - self.momentum_hat
    self.integral_err = np.clip(
        self.integral_err + self.dt * delta_p,
        -self.integral_limit, self.integral_limit)
    self.tau_ext = self.kp * delta_p + self.ki * self.integral_err

    # 动量估计递推：p̂̇ = τ_applied - β + r
    self.momentum_hat = self.momentum_hat + self.dt * (
        tau_applied - beta + self.tau_ext)

    # 任务空间映射：F̂ = (J Jᵀ)⁻¹ J τ̃_ext（世界系 LWA）
    pin.computeJointJacobians(self.model, self.data, q)
    pin.updateFramePlacement(self.model, self.data, self.frame_id)
    J = np.array(pin.getFrameJacobian(
        self.model, self.data, self.frame_id, _FRAME_REFERENCE))
    self._wrench = np.linalg.solve(J @ J.T + 1e-9 * np.eye(6), J @ self.tau_ext)
    # 纯接触估计：扣除已知耗散模型（控制器前馈补偿的同一项）
    tau_diss = (self.frictionloss * np.tanh(v / self.friction_v0)
                + self.damping * v)
    self._wrench_contact = np.linalg.solve(
        J @ J.T + 1e-9 * np.eye(6), J @ (self.tau_ext - tau_diss))
    return self.tau_ext.copy()