//  [python]
def lm_penalty_optimal_capsule(vertices: torch.Tensor, nruns=5, vis=None):
    best_p1, best_p2, best_r = None, None, None
    best_cost = np.inf
    for i in range(nruns):
        try:
            # p1_0 = torch.tensor([0.0, 0.0, -10.0])
            p1_0 = torch.randn(3)
            p1_0 = 0.2 * p1_0 / torch.norm(p1_0)
            # p2_0 = torch.tensor([0.0, 0.0, 10.0])
            # p2_0 = 0.5 * torch.randn(3)
            p2_0 = -p1_0
            # r_0 = torch.tensor([10])
            r_0 = torch.abs(torch.randn(1))
            x = torch.cat((p1_0, p2_0, r_0), dim=0)

            def fg(x, mu, vertices):
                p1, p2, r = x[None, 0:3], x[None, 3:6], x[6:7]
                dists = point_capsule_distance_batch(
                    vertices,
                    p1.expand(vertices.shape[0], -1),
                    p2.expand(vertices.shape[0], -1),
                    r.expand(vertices.shape[0]),
                )
                return torch.cat((
                    capsule_volume_batch(p1, p2, r),
                    torch.clamp(mu * dists, min=0),
                ))

            Jfn = torch.func.jacfwd(fg, argnums=0)

            margin = 1e-3
            xtol = 1e-6
            mu = 0.1
            outer_step = 0
            satisfied = False
            while not satisfied:
                inner_step = 0
                converged = False
                while not converged:
                    p1vis = np.array([x[0], x[1], x[2]], dtype=np.float64)
                    p2vis = np.array([x[3], x[4], x[5]], dtype=np.float64)
                    rvis = x[6].item()
                    h = np.linalg.norm(p2vis - p1vis)
                    capsule_material = meshcat.geometry.MeshToonMaterial(color=0x8888FF, opacity=0.4)
                    vis["p1"].set_object(meshcat.geometry.Sphere(rvis), capsule_material)
                    vis["p1"].set_transform(meshcat.transformations.translation_matrix(p1vis))
                    vis["p2"].set_object(meshcat.geometry.Sphere(rvis), capsule_material)
                    vis["p2"].set_transform(meshcat.transformations.translation_matrix(p2vis))
                    vis["cyl"].set_object(meshcat.geometry.Cylinder(h, rvis), capsule_material)
                    T = np.eye(4)
                    T[:3, 3] = (p1vis + p2vis) / 2
                    v = p2vis - p1vis
                    if np.linalg.norm(v) > 1e-6:
                        v = v / np.linalg.norm(v)
                    else:
                        v = np.array([0, 0, 1])
                    vx, vy, vz = v
                    sign = 1 if vz > 0 else -1
                    a = -1 / (sign + vz)
                    b = vx * vy * a
                    t1 = np.array([1 + sign * vx * vx * a, sign * b, -sign * vx])
                    t2 = np.array([b, sign + vy * vy * a, -vy])
                    T[:3, 0] = t1
                    T[:3, 1] = v  # Meshcat uses y as axis of rotational symmetry
                    T[:3, 2] = t2
                    vis["cyl"].set_transform(T)

                    J = Jfn(x, mu, vertices)
                    A = J.t() @ J
                    # lmbd = max(0, torch.min(torch.real(torch.linalg.eigvals(A))).item())
                    A += (1e-6) * torch.eye(J.shape[1])
                    b = -J.t() @ fg(x, mu, vertices)
                    dx = torch.linalg.solve(A, b)
                    x = x + 1e-2 / mu * dx
                    if torch.norm(dx) < xtol:
                        converged = True
                        print(f"Converged in {inner_step} steps")

                    if inner_step > 5000:
                        print(f"Did not converge in {inner_step} inner LM steps")
                        break

                    inner_step += 1
                    mins = torch.min(vertices, dim=0)[0]
                    maxs = torch.max(vertices, dim=0)[0]
                    x = torch.clamp(
                        x,
                        min=torch.cat([mins, mins, torch.tensor([0.01])]),
                        max=torch.cat([maxs, maxs, torch.tensor([1])]),
                    )

                if torch.all(fg(x, 1, vertices)[1:] <= margin):
                    satisfied = True
                    print(f"Satisfied in {outer_step} outer steps")

                if outer_step > 100:
                    print(f"Did not satisfy in {outer_step} outer steps")
                    break

                mu *= 2
                outer_step += 1

            if satisfied:
                p1, p2, r = x[0:3], x[3:6], x[6]
                cost = capsule_volume_batch(p1.reshape(-1, 3), p2.reshape(-1, 3), r.reshape(-1))
                if cost < best_cost:
                    best_p1, best_p2, best_r = p1, p2, r
                    best_cost = cost

        except Exception as e:
            print(e)

    return best_p1, best_p2, best_r + margin