I am trying to optimize a set of equations simulating pendulums with springs between the initial vector and eachother, in R^3.
It is actually a system of matrices and vectors, but I have written it out like this to make it easier to see each equation separately. In this case there are just two pendulums. The q's(q[0:3] and q[3:6]) are the directions of the pendulums, and the p's the speed of the two points. In this case the length of the pendulums are also set to 1 so the q's actually represent the entire pendulums.
There is a force F pushing down in the z-direction on the final point, and because that point is defined as Q=l[0]*q[:3]+l[1]q[3:], the force is included in both of the z-equations of p.
The terms of k*num/denare the springs reacting to deflection of the angle. The terms are in the form m.acos(q1@q2)/m.sqrt(1-(q1@q2)**2), which I have had some issues with, considering they are 0/0 when the pendulums are in the same direction. Especially in the starting point they were previously both in the same direction as eachother and the initial vector. I do think I fixed it with the help from this answer, and also perturbed the x-values slightly so they don't actually start in a 0/0 position.
The pendulums are also constrained by the equations under "constraints" to be of length 1, this is also the reason of including the terms q*lam in the p-equations(this is how a constrained Hamiltonian system is structured).
However, now I am onto actually implementing the control/input force u. If I run the system without u, it solves and finds the solution and evolution of the differential equations. I have tried implementing the control input with different solvers. When I implement it with m.options.SOLVER 1 and 3 I get the max iterations error. And when I try with solver 2 I get the error:
*** WARNING MESSAGE FROM SUBROUTINE MA27BD *** INFO(1) = 3
MATRIX IS SINGULAR. RANK= 135
Problem with linear solver, INFO: 3
Below is the code used, I tried to make it as barebones as possible, but please ask if something is unclear!
from gekko import GEKKO
import numpy as np
#%% Functions
def get_M(n, d, ml, l):
M = np.zeros((n*d, n*d))
for i in range(n):
for j in range(n):
M[d*i:d*(i+1), d*j:d*(j+1)] = sum(ml[max(i, j):]) * l[i] * l[j] * np.eye(d)
return M
def get_q(theta, phi):
theta = theta*np.pi/180
phi = phi*np.pi/180
x = np.sin(theta)*np.cos(phi)
y = np.sin(theta)*np.sin(phi)
z = np.cos(theta)
return np.array([x, y, z])
#%% Run
m = GEKKO()
n = 2; d = 3; #n number of pendulums, d dimension
k = np.ones(n)*1e3; ml = np.ones(n); l = np.ones(n) #k list of spring constants, ml list of masses, l list of lengths
F = 1 # Force
N = 3 # Number of steps + 1
Minv = np.linalg.inv(get_M(n=n, d=d, ml=ml, l=l)) # Inverted mass matrix
m.time = np.linspace(0, 0.01, N)
q = m.Array(m.Var, n*d, value=0)
q[0].value, q[1].value, q[2].value = get_q(90, 90.001) # Initial values of pendulum 1
q[3].value, q[4].value, q[5].value = get_q(90, 89.999) # Initial values of pendulum 2
p = m.Array(m.Var, n*d, value=0)
lam = m.Array(m.Var, n, value=0)
sv = np.zeros(3)
sv[1] = 1 # [0., 1., 0.] Initial vector, first spring is between this and the first pendulum
# control variables
u = m.MV(value=0)
u.STATUS = 1
# q equations
m.Equations(q[i].dt() == (Minv@p)[i] for i in range(n*d))
# spring terms
s1 = m.Var(lb=0)
m.Equation(s1 >= 1-(q[:3]@sv)**2)
m.Minimize(s1)
num1 = m.Intermediate(m.acos(q[:3]@sv))
den1 = m.Intermediate(m.sqrt(s1))
s2 = m.Var(lb=0)
m.Equation(s2 >= 1-(q[:3]@q[3:])**2)
m.Minimize(s2)
num2 = m.Intermediate(m.acos(q[:3]@q[3:]))
den2 = m.Intermediate(m.sqrt(s2))
# p equations
m.Equation(p[0].dt() == + k[1]*num2/den2*q[3] - q[0]*lam[0]) # x
m.Equation(p[1].dt() == k[0]*num1/den1 + k[1]*num2/den2*q[4] - q[1]*lam[0]) # y
m.Equation(p[2].dt() == - F + u + k[1]*num2/den2*q[5] - q[2]*lam[0]) # z
m.Equation(p[3].dt() == + k[1]*num2/den2*q[0] - q[3]*lam[1]) # x
m.Equation(p[4].dt() == + k[1]*num2/den2*q[1] - q[4]*lam[1]) # y
m.Equation(p[5].dt() == - F + u + k[1]*num2/den2*q[2] - q[5]*lam[1]) # z
# constraints
m.Equation(0.5 * (q[:3]@q[:3] - 1) == 0)
m.Equation(0.5 * (q[3:]@q[3:] - 1) == 0)
m.options.IMODE = 6
m.options.SOLVER = 3
m.Obj((q[5]-2*q[2])**2)
m.solve(disp=1)
Does anyone have any tips of why it's not working? I am quite new to gekko so there might be some glaring mistakes I'm overlooking. Thanks!
