controller_allocation_issue.jl

using OSQP.MathOptInterfaceOSQP
using SimpleQP
using StaticArrays
using RigidBodyDynamics
using MomentumBasedControl: ContactPoint
using MathOptInterface
const MOI = MathOptInterface

function defaultoptimizer()
    optimizer = OSQPOptimizer()
    MOI.set!(optimizer, OSQPSettings.Verbose(), false)
    MOI.set!(optimizer, OSQPSettings.EpsAbs(), 1e-8)
    MOI.set!(optimizer, OSQPSettings.EpsRel(), 1e-16)
    MOI.set!(optimizer, OSQPSettings.MaxIter(), 10000)
    MOI.set!(optimizer, OSQPSettings.AdaptiveRhoInterval(), 25) # required for deterministic behavior
    optimizer
end

function build_model(mechanism, optimizer, Δt = 0.01)
    state = MechanismState(mechanism)
    model = Model(optimizer)
    
    qnext = [Variable(model) for _ in 1:num_positions(state)]
    vnext = [Variable(model) for _ in 1:num_velocities(state)]
    u = [Variable(model) for _ in 1:num_velocities(state)]
    
    H = let state = state
        Parameter(mass_matrix(state), model) do H
            mass_matrix!(H, state)
        end
    end
    c = let state = state, result = DynamicsResult(mechanism)
        Parameter(result.dynamicsbias, model) do c
            dynamics_bias!(result, state)
        end
    end
    
    body = findbody(mechanism, "foot")
    contact_point = ContactPoint{4}(
        Point3D(default_frame(body), 0., 0, 0),
        FreeVector3D(default_frame(body), 0., 0, 1),
        1.0,
        state,
        model)
    
    path_to_root = path(state.mechanism, body, root_body(state.mechanism))
    J = let state = state, path_to_root = path_to_root
        Parameter(geometric_jacobian(state, path_to_root), model) do J
            geometric_jacobian!(J, state, path_to_root)
        end
    end
    τ_ext = @expression transpose(angular(J)) * angular(contact_point.wrench_world) + transpose(linear(J)) * linear(contact_point.wrench_world)
    
    @constraint(model, H * (vnext - velocity(state)) == Δt * (u - c - τ_ext))
    
    model, state, u
end

urdf = "hopper.urdf"
mechanism = parse_urdf(Float64, urdf)
model, state, u = build_model(mechanism, defaultoptimizer())
set_configuration!(state, [1, 1.])
solve!(model)
@allocated solve!(model)