Hello all! I am pretty new to Julia, and am trying to implement a matrix form model for the dynamics. This is what I have written so far:
using JuMP #, MadNLP
import Ipopt
#Number of mesh point
n = 10
#Simulation parameters
const g = [-1; 0; 0]
const m_wet = 2.0
const m_dry = 1.0
const T_min = 0.3
const T_max = 5
const δ_max = 20
const θ_max = 90
const γ_gs = 20
const ω_max = 60
const J_B = 1e-2 * [1 0 0 ; 0 1 0 ; 0 0 1]
const r_TB = [-1e-2 0 0]
#Made up value - α
const I_sp = 1 # s
#Initial and final conditions
r₀ = [4 4 0]
v_f = [-1exp(-1) 0 0]
ω₀ = [0 0 0]
q_f = [1 0 0 0]
#Initial calculations
αₘ = 1 / I_sp / -9.80665
#Defining model
model = Model(Ipopt.Optimizer)
#Defining statez variables
@variable(model, r[1:n, 1:3])
@variable(model, q[1:n, 1:4])
@variable(model, v[1:n, 1:3])
@variable(model, ω[1:n, 1:3])
@variable(model, m_dry ≤ m[1:n] ≤ m_wet)
@variable(model, 1e-4 ≤ t_f)
#Defining control variables
@variable(model, T_B[1:n, 1:3])
@constraint(model, T_min^2 .≤ sum(T_B.^2, dims=2) .≤ T_max^2)
#Dynamic equation expressions
#Matrix Functions
function C_IB_func(q)
C = [(1-2*(q[2]^2+q[3]^2)) 2*(q[1]*q[2] - q[4]*q[3]) 2*(q[1]*q[3] + q[4]*q[2]);
2*(q[1]*q[2] + q[4]*q[3]) (1-2*(q[1]^2+q[3]^2)) 2*(q[2]*q[3] - q[4]*q[1]);
2*(q[1]*q[3] - q[4]*q[2]) 2*(q[2]*q[3] + q[4]*q[1]) (1-2*(q[1]^2+q[2]^2))]
return C
end
function Omega(ξ)
A = [0 -ξ[1] -ξ[2] -ξ[3];
ξ[1] 0 ξ[3] -ξ[2];
ξ[2] -ξ[3] 0 ξ[2];
ξ[3] ξ[2] -ξ[1] 0;]
return A
end
@expression(model, Δτ, t_f/(n-1)) # is this the best way to do that?
@expression(model, δm[j=1:n], -αₘ * sqrt(sum(T_B[j, :].^2)))
function δv_calculations(q, T, g, m)
C = C_IB_func(q)
matrixres = g + C*T./m
return matrixres
end
@expression(model, δv[j=1:n, k=1:3], δv_calculations(q[j,:], T_B[j,:], g, m[j])[k])
function δq_calculations(ω, q)
return 0.5 * Omega(ω) * q
end
@expression(model, δq[j=1:n, k=1:4], δq_calculations(ω[j, :], q[j,:])[k])
function ξcross(ξ)
M = [0 -ξ[3] ξ[2];
ξ[3] 0 -ξ[1];
-ξ[2] ξ[1] 0]
return M
end
function δω_calculations(J_B, r_TB, T, ω)
A = ξcross(r_TB)*T - ξcross(ω)*J_B*ω
B = inv(J_B) * A
return B
end
@expression(model, δω[j=1:n, k=1:3], δω_calculations(J_B, r_TB, T_B[j, :], ω[j, :])[k])
integration_rule = "trapezoidal"
for j=2:n
i = j-1
for k=1:3
@NLconstraint(model, -r[j, k] + r[i, k] + 0.5 * Δτ * (v[j, k] + v[i, k]) == 0)
@NLconstraint(model, -ω[j, k] + ω[i, k] + 0.5 * Δτ * (δω[j, k] + δω[i, k]) == 0)
@NLconstraint(model, -v[j, k] + v[i, k] + 0.5 * Δτ * (δv[j, k] + δv[i, k]) == 0)
end
for k=1:4
@NLconstraint(model, -q[j, k] + q[i, k] + 0.5 * Δτ * (δq[j, k] + δq[i, k]) == 0)
end
@NLconstraint(model, -m[j] + m[i] + 0.5 * Δτ * (δm[j] + δm[i]) == 0)
end
@objective(model, Max, m[n])
optimize!(model)
Even though I don’t get any errors defining the problem, whenever I try to run the last optimization stage, I get the error that:
“The Jacobian for the equality constraints contains an invalid number”
I have read in this same forum that this is normally due to a division by zero, but I couldn’t find any points where I have done this.
I have identified the problem comes from the last non linear constraint (the one related to m). Could it be from including the norm in the previous constraint? Is there a better way to define this?
Thank you for your time!!
