Full 6-DoF passivity + singularity-CBF controller

Passivity + singularity-CBF controller tracks a link's position only. six_dof_pbc_cbf_controller.solve_control_qp extends the same QP to the link's full pose -- position and orientation together -- using its 6xN spatial Jacobian instead of only the translational one.

from dense_armor.dynamics.urdf_dynamics import RigidBodyModel
from dense_armor.dynamics.six_dof_pbc_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,
                                    r_des, w_des, wd_des, eps=0.03)

r_des is the desired orientation (rotation matrix) of the tracked link; w_des/wd_des are its desired angular velocity/acceleration in world frame. Everything else matches passivity_cbf_controller.solve_control_qp -- same passivity + singularity-avoidance QP structure, same real per-joint limit CBF.

The attitude error

Orientation error uses Lee, Leok & McClamroch (2010)'s SO(3) formula:

e_R = 0.5 * vee(R_des^T @ R - R^T @ R_des)

Smooth everywhere and zero iff R == R_des, unlike a roll-pitch-yaw based error, which has a real gimbal-lock singularity this formulation avoids.

solve_control_qp

solve_control_qp(model, link_name, q, qd, p_des, pd_des, pdd_des, r_des, w_des, wd_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 full 6-DoF passivity + singularity avoidance.

Parameters

model : dynamics.urdf_dynamics.RigidBodyModel link_name : str Name of the URDF link whose full pose 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. r_des : array-like, shape (3, 3) Desired orientation (rotation matrix) of the tracked link. w_des, wd_des : array-like, shape (3,) Desired angular velocity/acceleration (world frame). 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, same as passivity_cbf_controller.solve_control_qp.

Source code in dense_armor/dynamics/six_dof_pbc_cbf_controller.py

def solve_control_qp(model, link_name, q, qd, p_des, pd_des, pdd_des, r_des, w_des, wd_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 full 6-DoF passivity + singularity avoidance.

    Parameters
    ----------
    model : dynamics.urdf_dynamics.RigidBodyModel
    link_name : str
        Name of the URDF link whose full pose 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.
    r_des : array-like, shape (3, 3)
        Desired orientation (rotation matrix) of the tracked link.
    w_des, wd_des : array-like, shape (3,)
        Desired angular velocity/acceleration (world frame).
    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, same as `passivity_cbf_controller.solve_control_qp`.
    """
    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), jnp.asarray(r_des),
        jnp.asarray(w_des), jnp.asarray(wd_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)
    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"):
        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()

        if has_real_limits and res.info.status_val != osqp.constant("OSQP_SOLVED"):
            # A real second-level infeasibility: the 6-DoF manipulability
            # measure (spatial Jacobian, includes orientation) can be much
            # smaller than the 3-DoF one at the same configuration (a real
            # wrist-singularity-adjacent case, not an artifact), so the CBF's
            # required recovery rate can exceed what the velocity-limit box
            # allows -- CBF+box jointly infeasible. The CBF is the harder
            # safety constraint (prevents an actual kinematic singularity,
            # not just a momentary rate-limit overshoot), so drop the box
            # and keep only the CBF -- feasible in R^n as long as a2 != 0.
            a_cbf2 = sparse.csc_matrix(np.asarray(a2).reshape(1, n))
            l_cbf2 = np.array([-1e20])
            u_cbf2 = np.array([float(u2)])
            prob = osqp.OSQP()
            prob.setup(p_mat, q_vec, a_cbf2, l_cbf2, u_cbf2, 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

Promoted from Dense-Evolution-Discovery Experiment 65, built directly on Experiment 63's RigidBodyModel-based controller (passivity_cbf_controller.py here). Validated two ways:

• Exact gravity compensation at zero error: at the exact desired pose (position and orientation) with zero velocity, the solved qdd is zero and tau equals gravity compensation exactly, to machine precision (1e-9) -- a correctness check of the whole pipeline (spatial Jacobian, rotation error, task-space Lambda), not just "doesn't crash".
• Real closed-loop convergence: a 10cm position offset plus a 30-degree orientation offset (about world z), RK4-integrated under the real rigid-body dynamics for 1000 control ticks at 5 physics substeps each, converges to position error 1e-6 m and orientation error 1e-4.

Validated on three robots, the same singular configurations passivity_cbf_controller.py uses, through the full 6-DoF pipeline instead:

robot	link	mu	h (mu - eps)	qdd norm
Kinova Gen3 6-DoF	bracelet_with_vision_link	0.017607	-0.012393	162.60
Franka Panda	panda_hand	0.032751	0.002751	5.00

A real second-level infeasibility, found and fixed by this validation. The 6-DoF (spatial) manipulability measure can be well below the 3-DoF one at the same configuration (0.0176 vs 0.1128 for Gen3 6-DoF here -- a real wrist-singularity-adjacent case the position-only measure doesn't see), and h can already be negative before the QP solves. When the CBF's required recovery rate exceeds the velocity-limit box, CBF+box is jointly infeasible -- the single-level fallback this module inherited from passivity_cbf_controller.py (drop passivity, keep CBF+box) isn't enough there, and silently returned OSQP's infeasibility certificate as qdd (norm in the billions). Fixed with a third-level fallback specific to this module: if CBF+box is still infeasible, drop the box too and keep only the CBF -- guaranteed feasible in R^n as long as the manipulability gradient is nonzero, since the CBF (preventing an actual kinematic singularity) is the harder safety constraint of the two.

Scope: otherwise inherits passivity_cbf_controller.py's joint-limit CBF unchanged; see that module's own Details section for its own real numbers and its (single-level) OSQP-infeasibility fix.