The square root produces a complex number when the operand is negative. There is a way to handle complex numbers in Gekko such as shown in: Is it possible to use GEKKO for curve fitting if complex numbers can appear in intermediate steps? However, it is also possible to rearrange the equation to avoid the possibility of a complex number and improve convergence. Here is one way to rearrange the equation into an equivalent form that solves reliably.
#m.Equation(rr00 == (-AA2+(AA2**2-4*BB2)**(1/2))/2)
m.Equation((2*rr00+AA2)**2 == AA2**2-4*BB2)
A few other suggestions on the model:
- Use
m.Intermediate() variables where possible to reduce the number of implicitly solved variables and increase the solution performance.
# Input 1
theta21 = m.Intermediate(-math.pi+h2/2-h2*(0.43989*theta25/beta2-0.035014*m.sin(4*math.pi*theta25/beta2)))
A2 = m.Intermediate(2*rr0*rr3*m.cos(theta20)-2*rr1*rr3*m.cos(theta21))
B2 = m.Intermediate(2*rr0*rr3*m.sin(theta20)-2*rr1*rr3*m.sin(theta21))
C2 = m.Intermediate(rr0**2+rr1**2+rr3**2-rr2**2-2*rr0*rr1*(m.cos(theta20)*m.cos(theta21)+m.sin(theta20)*m.sin(theta21)))
K2 = m.Intermediate((-B2-(B2**2-C2**2+A2**2)**(1/2))/(C2-A2))
theta23 = m.Intermediate(m.atan(K2)*2)
theta211 = m.Intermediate(theta23 + math.pi)
AA2 = m.Intermediate(2*rr33*(m.cos(theta200)*m.cos(theta233)+m.sin(theta200)*m.sin(theta233))-2*rr11*(m.cos(theta200)*m.cos(theta211)+m.sin(theta200)*m.sin(theta211)))
BB2 = m.Intermediate(rr11**2+rr33**2-rr22**2-2*rr11*rr33*(m.cos(theta211)*m.cos(theta233)+m.sin(theta211)*m.sin(theta233)))
- Remove variables that have no associated equation or include an equation.
# Remove A22
# Add equations for velocity and acceleration
m.Equation(rr00.dt()==rr000)
m.Equation(rr000.dt()==rr0000)
Here is a complete script that solves successfully in simulation.
from gekko import GEKKO
from tqdm import tqdm
import numpy as np
import math
import matplotlib.pyplot as plt
import pandas as pd
m = GEKKO(remote=False)
m.time=np.linspace(0.000,0.046875,300)
#Variables
t = m.Param(m.time) #Time
h2 = 4.907/180*math.pi #arm 각도범위
beta2 = 40/360*2*math.pi #cam 할부각
rr1 = 292 #length of link1
rr2 = 398 #length of link2
rr3 = 130 #length of link3
rr0 = 429.71 #length of link0
rr11 = 260
rr22 = 639.5
rr33 = 260
theta20 = 112.4853/360*2*math.pi
theta25 = 2*math.pi/3*t
theta200 = math.pi/2
theta233 = 0
rr00 = m.Var() #output 2
rr000 = m.Var() #Velocity of output
rr0000 = m.Var() #acceleration of output
# Input 1
theta21 = m.Intermediate(-math.pi+h2/2-h2*(0.43989*theta25/beta2-0.035014*m.sin(4*math.pi*theta25/beta2)))
A2 = m.Intermediate(2*rr0*rr3*m.cos(theta20)-2*rr1*rr3*m.cos(theta21))
B2 = m.Intermediate(2*rr0*rr3*m.sin(theta20)-2*rr1*rr3*m.sin(theta21))
C2 = m.Intermediate(rr0**2+rr1**2+rr3**2-rr2**2-2*rr0*rr1*(m.cos(theta20)*m.cos(theta21)+m.sin(theta20)*m.sin(theta21)))
K2 = m.Intermediate((-B2-(B2**2-C2**2+A2**2)**(1/2))/(C2-A2))
theta23 = m.Intermediate(m.atan(K2)*2)
theta211 = m.Intermediate(theta23 + math.pi)
AA2 = m.Intermediate(2*rr33*(m.cos(theta200)*m.cos(theta233)+m.sin(theta200)*m.sin(theta233))-2*rr11*(m.cos(theta200)*m.cos(theta211)+m.sin(theta200)*m.sin(theta211)))
BB2 = m.Intermediate(rr11**2+rr33**2-rr22**2-2*rr11*rr33*(m.cos(theta211)*m.cos(theta233)+m.sin(theta211)*m.sin(theta233)))
m.Equation((2*rr00+AA2)**2 == AA2**2-4*BB2)
m.Equation(rr00.dt()==rr000)
m.Equation(rr000.dt()==rr0000)
m.options.IMODE=4
m.options.SOLVER=3
m.solve(disp=True)
df = pd.DataFrame([m.time, rr00.value])
df.to_csv('test.csv',index=None)
plt.plot(m.time, rr00.value)
plt.show()
Here is the solver output:
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 0.0000000e+000 1.63e+006 0.00e+000 0.0 0.00e+000 - 0.00e+000 0.00e+000 0
1 0.0000000e+000 3.10e+005 0.00e+000 -11.0 3.60e+011 - 1.00e+000 6.25e-002h 5
2 0.0000000e+000 1.81e+004 0.00e+000 -11.0 2.74e+009 - 1.00e+000 1.00e+000h 1
3 0.0000000e+000 4.97e+001 0.00e+000 -11.0 1.43e+008 - 1.00e+000 1.00e+000h 1
4 0.0000000e+000 3.77e-004 0.00e+000 -11.0 3.95e+005 - 1.00e+000 1.00e+000h 1
5 0.0000000e+000 6.98e-010 0.00e+000 -11.0 3.00e+000 - 1.00e+000 1.00e+000h 1
Number of Iterations....: 5
(scaled) (unscaled)
Objective...............: 0.0000000000000000e+000 0.0000000000000000e+000
Dual infeasibility......: 0.0000000000000000e+000 0.0000000000000000e+000
Constraint violation....: 4.0017713930875105e-010 6.9849193096160889e-010
Complementarity.........: 0.0000000000000000e+000 0.0000000000000000e+000
Overall NLP error.......: 4.0017713930875105e-010 6.9849193096160889e-010
Number of objective function evaluations = 10
Number of objective gradient evaluations = 6
Number of equality constraint evaluations = 11
Number of inequality constraint evaluations = 0
Number of equality constraint Jacobian evaluations = 6
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations = 5
Total CPU secs in IPOPT (w/o function evaluations) = 0.045
Total CPU secs in NLP function evaluations = 0.175
EXIT: Optimal Solution Found.
The solution was found.
The final value of the objective function is 0.
---------------------------------------------------
Solver : IPOPT (v3.12)
Solution time : 0.24009999999999998 sec
Objective : 0.
Successful solution
---------------------------------------------------
As you modify the problem, also try the APOPT solver with m.options.SOLVER=1 to see if that improves performance. I changed the IMODE to simulation (4) because there are zero degrees of freedom. You will need to change it back to IMODE=6 if you introduce decision variables and an objective function.
