Skip to content

Nonlinear Modeling: user-defined functions with vector inputs *and* output #1914

@ferrolho

Description

@ferrolho

Consider the following user-defined functions with vector inputs:

function my_dynamics(x::Vararg{T}) where T
    q = [i for i in x[0*dof+1:1*dof]]
    v = [i for i in x[1*dof+1:2*dof]]
    τ = [i for i in x[2*dof+1:3*dof]]

    state = statecache[T]
    set_configuration!(state, q)
    set_velocity!(state, v)

    result = dynamicsresultcache[T]
    dynamics!(result, state, τ)

    euler_q_next, euler_v_next = EulerStep(q, v, result.v̇, sim_Δt)
end

register(model, :my_dynamics, 3*dof, my_dynamics, autodiff=true)

This function does not return a single scalar; in fact, it returns a tuple comprised of two arrays.

Ideally, I'd like to be able to define some set of constraints with the following (non-working) syntax:

for i in 1:sim_num_knots-1
    q = configurations[i]
    v = velocities[i]
    τ = torques[i]

    q_next = configurations[i+1]
    v_next = velocities[i+1]
    
    @NLconstraint(model, my_dynamics(q..., v..., τ...) == (q_next, v_next))
end

Now, I am aware of the current NL interface limitations and know this is not currently possible. Moreover, I have been trying to find an example of such a situation and some workaround with the methods currently available in the NL interface but without success...

For that matter, my current workaround has ended up resembling a horrible, horrible abomination:

function my_dynamics(x::Vararg{T}) where T
    q = [i for i in x[0*dof+1:1*dof]]
    v = [i for i in x[1*dof+1:2*dof]]
    τ = [i for i in x[2*dof+1:3*dof]]

    state = statecache[T]
    set_configuration!(state, q)
    set_velocity!(state, v)

    result = dynamicsresultcache[T]
    dynamics!(result, state, τ)

    euler_q_next, euler_v_next = EulerStep(q, v, result.v̇, sim_Δt)

    vcat(euler_q_next, euler_v_next)
end

register(model, :my_dynamics, 3*dof, my_dynamics, autodiff=true)

my_dynamics1(x...) = my_dynamics(x...)[1]
my_dynamics2(x...) = my_dynamics(x...)[2]
my_dynamics3(x...) = my_dynamics(x...)[3]
my_dynamics4(x...) = my_dynamics(x...)[4]
my_dynamics5(x...) = my_dynamics(x...)[5]
my_dynamics6(x...) = my_dynamics(x...)[6]
my_dynamics7(x...) = my_dynamics(x...)[7]

my_dynamics11(x...) = my_dynamics(x...)[7+1]
my_dynamics12(x...) = my_dynamics(x...)[7+2]
my_dynamics13(x...) = my_dynamics(x...)[7+3]
my_dynamics14(x...) = my_dynamics(x...)[7+4]
my_dynamics15(x...) = my_dynamics(x...)[7+5]
my_dynamics16(x...) = my_dynamics(x...)[7+6]
my_dynamics17(x...) = my_dynamics(x...)[7+7]

register(model, :my_dynamics1, 3*dof, my_dynamics1, autodiff=true)
register(model, :my_dynamics2, 3*dof, my_dynamics2, autodiff=true)
register(model, :my_dynamics3, 3*dof, my_dynamics3, autodiff=true)
register(model, :my_dynamics4, 3*dof, my_dynamics4, autodiff=true)
register(model, :my_dynamics5, 3*dof, my_dynamics5, autodiff=true)
register(model, :my_dynamics6, 3*dof, my_dynamics6, autodiff=true)
register(model, :my_dynamics7, 3*dof, my_dynamics7, autodiff=true)

register(model, :my_dynamics11, 3*dof, my_dynamics11, autodiff=true)
register(model, :my_dynamics12, 3*dof, my_dynamics12, autodiff=true)
register(model, :my_dynamics13, 3*dof, my_dynamics13, autodiff=true)
register(model, :my_dynamics14, 3*dof, my_dynamics14, autodiff=true)
register(model, :my_dynamics15, 3*dof, my_dynamics15, autodiff=true)
register(model, :my_dynamics16, 3*dof, my_dynamics16, autodiff=true)
register(model, :my_dynamics17, 3*dof, my_dynamics17, autodiff=true)

for i in 1:sim_num_knots-1
    q = configurations[i]
    v = velocities[i]
    τ = torques[i]

    q_next = configurations[i+1]
    v_next = velocities[i+1]
    
    @NLconstraint(model, my_dynamics1(q..., v..., τ...) == q_next[1])
    @NLconstraint(model, my_dynamics2(q..., v..., τ...) == q_next[2])
    @NLconstraint(model, my_dynamics3(q..., v..., τ...) == q_next[3])
    @NLconstraint(model, my_dynamics4(q..., v..., τ...) == q_next[4])
    @NLconstraint(model, my_dynamics5(q..., v..., τ...) == q_next[5])
    @NLconstraint(model, my_dynamics6(q..., v..., τ...) == q_next[6])
    @NLconstraint(model, my_dynamics7(q..., v..., τ...) == q_next[7])

    @NLconstraint(model, my_dynamics11(q..., v..., τ...) == v_next[1])
    @NLconstraint(model, my_dynamics12(q..., v..., τ...) == v_next[2])
    @NLconstraint(model, my_dynamics13(q..., v..., τ...) == v_next[3])
    @NLconstraint(model, my_dynamics14(q..., v..., τ...) == v_next[4])
    @NLconstraint(model, my_dynamics15(q..., v..., τ...) == v_next[5])
    @NLconstraint(model, my_dynamics16(q..., v..., τ...) == v_next[6])
    @NLconstraint(model, my_dynamics17(q..., v..., τ...) == v_next[7])
end

Surely, there must be a better way...

As such, I am opening this issue here, with the hope that someone, somewhere, has been through this and can perhaps give me some pointers to some minimum working examples, or some suggestions on how to tackle this and improve the snippet above...

Thanks in advance, and apologies for this terrible code.

P.s. I was unsure whether I should have opened this issue as a "Bug report" or as a "Feature request". I hope it isn't a big deal in the case I chose wrongly.

Metadata

Metadata

Assignees

No one assigned

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions