3-DOF planar arm with Hill-type muscles

def _f_pe(l_ce, l_opt):
    """Parallel elastic element force (exponential toe region)."""
    strain = (l_ce - l_opt) / max(l_opt, 1e-6)
    if strain > 0:
        return _K_PE * strain * strain
    return 0.0

def _fl_active(l_ce, l_opt):
    """Active force-length relationship (Gaussian approximation)."""
    x = (l_ce / max(l_opt, 1e-6) - 1.0) / _WIDTH
    return max(1.0 - x * x, 0.0)

def musculoskeletal_arm_3dof_rhs(t, y):
    q  = y[0:3]
    dq = y[3:6]
    lce = np.maximum(y[6:12], 1e-4)

    dy = np.empty(12)

    # --- Muscle forces and CE dynamics ---
    tau_muscle = np.zeros(_N_JOINTS)
    for m in range(_N_MUSCLES):
        fl = _fl_active(lce[m], _L_OPT[m])

        # Tendon length (simplified: l_mt ~ l_slack + l_opt, constant path)
        l_mt = _L_SLACK[m] + _L_OPT[m]
        l_tendon = l_mt - lce[m]
        k_tendon = 35.0 * _F_MAX[m] / max(_L_SLACK[m], 1e-6)
        f_tendon = max(k_tendon * (l_tendon - _L_SLACK[m]), 0.0)
        f_tendon += _DAMPING * (-dy[6 + m] if m > 0 else 0.0)  # approx

        # CE force at current v_ce (we solve for v_ce from force balance)
        # F_CE + F_PE = F_tendon  ->  a*fl*fv*Fmax + f_pe = f_tendon
        f_pe = _f_pe(lce[m], _L_OPT[m]) * _F_MAX[m]
        target_ce_force = max(f_tendon - f_pe, 0.0)

        # Invert Hill equation for v_ce:
        # target = a * fl * fv(v) * Fmax
        a_fl_fmax = _A_TONIC * fl * _F_MAX[m]
        if a_fl_fmax > 1e-6:
            ratio = target_ce_force / a_fl_fmax
            # From fv = (1+v)/(1-v/0.25), solve for v:
            # ratio*(1 - v/0.25) = 1 + v  =>  v = (ratio - 1) / (ratio/0.25 + 1)
            ratio = min(ratio, 1.39)  # cap at lengthening limit
            v_norm = (ratio - 1.0) / (ratio / 0.25 + 1.0)
            v_ce = v_norm * _L_OPT[m] * _V_MAX
        else:
            # Very low activation: CE extends passively under tendon pull
            v_ce = 0.01 * (f_tendon - f_pe) / max(_F_MAX[m], 1.0)

        dy[6 + m] = v_ce

        # Torque contribution
        f_muscle = f_tendon
        j = _JOINT_MAP[m]
        tau_muscle[j] += _MOMENT[m] * f_muscle

    # --- Joint dynamics: q'' = M^{-1} * (tau - C*q' - G) ---
    # Diagonal inertia (simplified)
    M_diag = _I_LINK.copy()

    # Gravity torques
    G_torque = np.empty(3)
    for j in range(3):
        G_torque[j] = -_M_LINK[j] * _G * _L_LINK[j] * np.sin(q[j])

    # Viscous damping (simplified Coriolis/centrifugal substitute)
    C_damp = 0.5 * dq

    ddq = (tau_muscle - C_damp - G_torque) / M_diag

    dy[0:3] = dq
    dy[3:6] = ddq

    return dy