-
-
Notifications
You must be signed in to change notification settings - Fork 411
Description
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))
endNow, 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])
endSurely, 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.