Passivity + singularity-CBF controller (any robot)

Rigid-body dynamics gives you M(q), gravity, and forward dynamics for any robot. solve_control_qp uses them to drive that robot toward a task-space target safely -- guaranteeing passivity of the tracking error and staying away from kinematic singularities, both as constraints in a small QP, not as separate ad-hoc checks.

from dense_armor.dynamics.urdf_dynamics import RigidBodyModel
from dense_armor.dynamics.passivity_cbf_controller import solve_control_qp

model = RigidBodyModel("panda.urdf")
qdd, tau, mu, h = solve_control_qp(model, "panda_hand", q, qd, p_des, pd_des, pdd_des, eps=0.03)

link_name is any link in the URDF; p_des/pd_des/pdd_des are the desired position/velocity/acceleration of that link at the current instant (e.g. from quintic_trajectory). eps is the minimum manipulability index the controller will maintain -- mu (returned) never drops far below it, even when the commanded target would otherwise drive the robot through a singularity.

What the QP actually solves

Every call solves for the joint acceleration qdd that gets closest to a nominal operational-space PD command, subject to two constraints:

• Passivity: Vdot <= 0, where V is the tracking-error storage function -- guarantees the closed loop doesn't inject energy it shouldn't.
• Singularity avoidance: an exponential CBF keeping the manipulability index mu(q) above eps.
• Joint limits: model's own real per-joint position/velocity bounds (parsed from the URDF's <limit> tags), added only for joints the URDF actually declares a limit for.

Both constraints are affine in qdd; their coefficients are extracted by evaluating the constraint function and its gradient at qdd=0 (exact, since the function is affine), not by deriving them by hand.

solve_control_qp

solve_control_qp(model, link_name, q, qd, p_des, pd_des, pdd_des, kp_task=50.0, kd_task=20.0, kd_null=5.0, eps=0.035, ka=(100.0, 20.0), w_reg=1e-06)

Solve one control-tick QP for task-space passivity + singularity avoidance.

Parameters

model : dynamics.urdf_dynamics.RigidBodyModel link_name : str Name of the URDF link whose position is tracked. q, qd : array-like, shape (model.n,) Current joint position/velocity. p_des, pd_des, pdd_des : array-like, shape (3,) Desired task-space position/velocity/acceleration at the current time (e.g. from utility.trajectory.quintic_trajectory, mapped to the tracked link's task space). kp_task, kd_task : float Task-space PD gains for the nominal (unconstrained) command. kd_null : float Joint-velocity damping gain applied in the redundant null space. eps : float Minimum manipulability index the CBF constraint enforces. ka : tuple of float Exponential CBF class-K gains (proportional, derivative). w_reg : float Small regularization weight on the QP's joint-acceleration cost.

Returns

qdd : ndarray, shape (model.n,) Solved joint acceleration. tau : ndarray, shape (model.n,) Corresponding joint torque, from the rigid-body dynamics equation. mu : float Manipulability index at the current configuration. h : float CBF value (mu - eps); negative means the declared floor was crossed.

Real per-joint position/velocity limits from model's own URDF tags are enforced too (Kurtz et al.'s own "joint" CBF constraint type), inert for any joint the URDF declares no limit for.

Source code in dense_armor/dynamics/passivity_cbf_controller.py

def solve_control_qp(model, link_name, q, qd, p_des, pd_des, pdd_des,
                      kp_task=50.0, kd_task=20.0, kd_null=5.0,
                      eps=0.035, ka=(100.0, 20.0), w_reg=1e-6):
    """Solve one control-tick QP for task-space passivity + singularity avoidance.

    Parameters
    ----------
    model : dynamics.urdf_dynamics.RigidBodyModel
    link_name : str
        Name of the URDF link whose position is tracked.
    q, qd : array-like, shape (model.n,)
        Current joint position/velocity.
    p_des, pd_des, pdd_des : array-like, shape (3,)
        Desired task-space position/velocity/acceleration at the current time
        (e.g. from `utility.trajectory.quintic_trajectory`, mapped to the
        tracked link's task space).
    kp_task, kd_task : float
        Task-space PD gains for the nominal (unconstrained) command.
    kd_null : float
        Joint-velocity damping gain applied in the redundant null space.
    eps : float
        Minimum manipulability index the CBF constraint enforces.
    ka : tuple of float
        Exponential CBF class-K gains (proportional, derivative).
    w_reg : float
        Small regularization weight on the QP's joint-acceleration cost.

    Returns
    -------
    qdd : ndarray, shape (model.n,)
        Solved joint acceleration.
    tau : ndarray, shape (model.n,)
        Corresponding joint torque, from the rigid-body dynamics equation.
    mu : float
        Manipulability index at the current configuration.
    h : float
        CBF value (mu - eps); negative means the declared floor was crossed.

    Real per-joint position/velocity limits from `model`'s own URDF <limit>
    tags are enforced too (Kurtz et al.'s own "joint" CBF constraint type),
    inert for any joint the URDF declares no limit for.
    """
    n = model.n
    m, bias, grav, qdd_nom, a1, u1, a2, u2, mu, h, qdd_lb, qdd_ub = _qp_ingredients(
        model, link_name, jnp.asarray(q), jnp.asarray(qd), jnp.asarray(p_des),
        jnp.asarray(pd_des), jnp.asarray(pdd_des), kp_task, kd_task, kd_null,
        eps, ka[0], ka[1])

    p_mat = sparse.csc_matrix(np.eye(n) * (1.0 + w_reg))
    q_vec = -np.asarray(qdd_nom)

    qdd_lb_raw = np.asarray(qdd_lb)
    qdd_ub_raw = np.asarray(qdd_ub)
    # Only add the joint-limit box rows when the URDF actually declares a
    # real limit somewhere -- an all-unbounded robot gets the exact same
    # 2-row QP as before this feature existed. A literally-inert box row
    # (bounds clipped to +/-1e20) still perturbs OSQP's internal scaling by
    # ~1e-4, which would otherwise break the machine-precision cross-check
    # this module was validated with for zero physical reason.
    has_real_limits = not (np.all(np.isneginf(qdd_lb_raw)) and np.all(np.isposinf(qdd_ub_raw)))

    rows_a1a2 = [sparse.csc_matrix(np.asarray(a1).reshape(1, n)),
                 sparse.csc_matrix(np.asarray(a2).reshape(1, n))]
    l_a1a2 = [-1e20, -1e20]
    u_a1a2 = [float(u1), float(u2)]

    if has_real_limits:
        qdd_lb_np = np.clip(qdd_lb_raw, -1e20, 1e20)
        qdd_ub_np = np.clip(qdd_ub_raw, -1e20, 1e20)
        identity_n = sparse.csc_matrix(np.eye(n))
        a_full = sparse.vstack(rows_a1a2 + [identity_n]).tocsc()
        l_full = np.concatenate([l_a1a2, qdd_lb_np])
        u_full = np.concatenate([u_a1a2, qdd_ub_np])
    else:
        a_full = sparse.vstack(rows_a1a2).tocsc()
        l_full = np.array(l_a1a2)
        u_full = np.array(u_a1a2)

    prob = osqp.OSQP()
    prob.setup(p_mat, q_vec, a_full, l_full, u_full, verbose=False, polish=True)
    res = prob.solve()

    if res.info.status_val != osqp.constant("OSQP_SOLVED"):
        # Soft passivity constraint dropped; hard CBF + joint-limit constraints kept.
        if has_real_limits:
            a_cbf = sparse.vstack([sparse.csc_matrix(np.asarray(a2).reshape(1, n)),
                                    sparse.csc_matrix(np.eye(n))]).tocsc()
            l_cbf = np.concatenate([[-1e20], qdd_lb_np])
            u_cbf = np.concatenate([[float(u2)], qdd_ub_np])
        else:
            a_cbf = sparse.csc_matrix(np.asarray(a2).reshape(1, n))
            l_cbf = np.array([-1e20])
            u_cbf = np.array([float(u2)])
        prob = osqp.OSQP()
        prob.setup(p_mat, q_vec, a_cbf, l_cbf, u_cbf, verbose=False, polish=True)
        res = prob.solve()

    qdd = np.asarray(res.x)
    tau = np.asarray(m) @ qdd + np.asarray(bias) + np.asarray(grav)
    return qdd, tau, float(mu), float(h)

---

Details

Two-step promotion, same as RigidBodyModel: Dense-Evolution-Discovery Experiment 61 implemented Kurtz, Wensing & Lin's (2021, arXiv:2109.13349) controller but hardcoded to one Kinova Gen3's kinematics. Experiment 63 replaced the hardcoded calls with RigidBodyModel's API and re-validated on the same three robots RigidBodyModel itself was validated on:

robot	link	min(mu), no CBF	min(mu), CBF eps=0.03
Kinova Gen3 7-DoF	end_effector_link	0.00003	0.02947
Kinova Gen3 6-DoF	bracelet_with_vision_link	0.00006	0.02995
Franka Panda	panda_hand	0.00296	0.02997

Each row drives the named link toward that robot's own true kinematic singularity (mu=0). Without the CBF, the controller reaches it; with it, manipulability stays within 0.1-1.8% of the declared floor in every case.

A real bug, found and fixed: OSQP can report the passivity+CBF QP jointly infeasible (the passivity constraint's coefficients go numerically near-zero exactly when tracking is already good, which combined with a tight CBF margin occasionally leaves no feasible point under OSQP's default tolerances) and return its infeasibility certificate -- a vector with norm in the billions -- as if it were a real solution. Fixed by checking the solver status and, on infeasibility, dropping the soft passivity constraint and re-solving with only the hard, safety-critical CBF constraint -- the regression test test_controller_stays_finite_near_a_documented_infeasible_state reproduces the exact state that triggered this.

Scope: task-space position tracking only (3 DoF). For full 6-DoF (position + orientation) tracking, see six_dof_pbc_cbf_controller. Mimic-joint constraints (e.g. a gripper's two fingers tied together) are modeled by RigidBodyModel -- see coupled joints via mimic -- so a mimic joint contributes no independent column of its own to this controller's QP.

Joint limits, real numbers: Franka Panda joint4 (real range [-3.1416, 0.0]) sitting right at its bound with velocity driving past it -- unconstrained nominal command qdd=-205.8, real CBF box [-7.175, -4.999], solved qdd=-7.175 (the box's own edge). The box rows are only added when a robot's URDF has a real finite limit somewhere: an unconditional (but mathematically inert) row was tried first and rejected, since it measurably perturbed OSQP's internal scaling and broke the machine-precision cross-check above for a robot with no real limits declared.