Issues with m.if2, m.abs2 in implementing a contact switch - gekko

Question

This is a continuation of a prior question with a slightly different emphasis.
In summary, the prior solution helped with the implementation of the model inputs. The below model is working and provides a solution for the full contact condition. The framework and basic mechanics are in place for the variable contact constraint. However, no solution is available for when the contact switch variable is applied to the geometric constraints, whether as a Param or FV. (Note: no issues when applied to the dynamic equations).
One thing I've noted is that the m.if2 output is not correct in the [0] position. Below is the output of the switch-related variables:
adiff= [0.1, 0.10502512563, 0.11005025126, 0.11507537688, 0.12010050251,...
bdiff= [1.0, 0.99497487437, 0.98994974874, 0.98492462312, 0.97989949749,...
swtch= [0.1, 0.10449736118, 0.10894421858, 0.11334057221, 0.11768642206,...
c= [0.0, 1.000000005, 1.000000005, 1.000000005, 1.000000005, 1.000000005,...
Based on the logic swtch = adiff*bdiff and m.if2(swtch-thres,0,1), c[0] should be ~1.0. I've played with these parameters and haven't found a way to affect that first cell. I can't say for sure that this initial position is causing issues, but this seems like an erroneous output regardless.
Second, given that the m.if() outputs as approximately 0 and 1, I've attempted to soften the geometric constraint as: m.abs2({constraint}) <= {tol}. Even in the case when a generous tolerance is applied and c is excluded, this fails to produce a solution (whereas the hard constraint will).
Any suggestions for correcting either issue are appreciated.
Lastly, in the prior post, the use of m.integral() for setting the value of c was suggested. I'm unclear if that entails using if2 as well. If you can expand on implementing a switch that enables at t=a and switches off at t=b using an integral, that would be appreciated.
Full code:
###Import Libraries
import math
import matplotlib
matplotlib.use("TkAgg")
import matplotlib.animation as animation
import numpy as np
from gekko import GEKKO
###Defining a model
m = GEKKO(remote=True)
v = 1 #set walking speed (m/s)
L1 = .5 #set thigh length (m)
L2 = .5 #set shank length (m)
M = 75 #set mass (kg)
#################################
###Define secondary parameters
D = L1 + L2 #leg length parameter
pi = math.pi #define pi
g = 9.81 #define gravity
###Define initial and final conditions and limits
xmin = -D; xmax = D
xdotmin = .5*v; xdotmax = 1.5*v
ymin = 0*D; ymax = 5*D
q1min = -pi/2; q1max = pi/2
q2min = -pi/2; q2max = -.01
tfmin = .25; tfmax = 10
#amin = 0; amax = .45 #limits for FVs (future capability)
#bmin = .55; bmax = 1
###Defining the time parameter (0, 1)
N = 200
t = np.linspace(0,1,N)
m.time = t
###Final time Fixed Variable
TF = m.FV(1,lb=tfmin,ub=tfmax); TF.STATUS = 1
end_loc = len(m.time)-1
###Defining initial and final condition vectors
init = np.zeros(len(m.time))
final = np.zeros(len(m.time))
init[1] = 1
final[-1] = 1
init = m.Param(value=init)
final = m.Param(value=final)
###Parameters
M = m.Param(value=M) #cart mass
L1 = m.Param(value=L1) #link 1 length
L2 = m.Param(value=L2) #link 1 length
g = m.Const(value=g) #gravity
###Control Input Manipulated Variable
u = m.MV(0, lb=-70, ub=70); u.STATUS = 1
###Ground Contact Fixed Variables
#as fixed variables (future state)
#a = m.FV(0,lb=amin,ub=amax); a.STATUS = 1 #equates to the unscaled time when contact first occurs
#b = m.FV(1,lb=bmin,ub=bmax); b.STATUS = 1 #equates to the unscaled time when contact last occurs
#as fixed parameter
a = m.Param(value=-.1) #a<0 to drive m.time-a positive
b = m.Param(value=1)
###State Variables
x, y, xdot, ydot, q1, q2 = m.Array(m.Var, 6)
#Define BCs
m.free_initial(x)
m.free_final(x)
m.free_initial(xdot)
m.free_final(xdot)
m.free_initial(y)
m.free_initial(ydot)
#Define Limits
y.LOWER = ymin; y.UPPER = ymax
x.LOWER = xmin; x.UPPER = xmax
xdot.LOWER = xdotmin; xdot.UPPER = xdotmax
q1.LOWER = q1min; q1.UPPER = q1max
q2.LOWER = q2min; q2.UPPER = q2max
###Intermediates
xdot_int = m.Intermediate(final*m.integral(xdot)) #for average velocity constraint
adiff = m.Param(m.time-a.VALUE) #positive if m.time>a
bdiff = m.Param(b.VALUE-m.time) #positive if m.time<b
swtch = m.Intermediate(adiff*bdiff) #positive if m.time > a AND m.time < b
thres = .001
c = m.if2(swtch-thres,0,1) #c=0 if swtch <0, c=1 if swtch >0
###Defining the State Space Model
m.Equation(xdot.dt()/TF == -c*u*(L1*m.sin(q1)
+L2*m.sin(q1+q2))
/(M*L1*L2*m.sin(q2)))
m.Equation(ydot.dt()/TF == c*u*(L1*m.cos(q1)
+L2*m.cos(q1+q2))
/(M*L1*L2*m.sin(q2))-g)
m.Equation(x.dt()/TF == xdot)
m.Equation(y.dt()/TF == ydot)
m.periodic(y) #initial and final y position must be equal
m.periodic(ydot) #initial and final y velocity must be equal
m.periodic(xdot) #initial and final x velocity must be equal
m.Equation(m.abs2(xdot_int*final - v*final) <= .02) #soft constraint for average velocity ~= v
###Geometric constraints
#with no contact switch, this works
m.Equation(x + L1*m.sin(q1) + L2*m.sin(q1+q2) == 0) #x geometric constraint when in contact
m.Equation(y - L1*m.cos(q1) - L2*m.cos(q1+q2) == 0) #y geometric constraint when in contact
#soft constraint for contact switch. Produces no solution, with or without c, abs2 or abs3:
#m.Equation(c*m.abs2(x + L1*m.sin(q1) + L2*m.sin(q1+q2)) <= .01) #x geometric constraint when in contact
#.Equation(c*m.abs2(y - L1*m.cos(q1) - L2*m.cos(q1+q2)) <= .01) #y geometric constraint when in contact
###Objectives
#Maximize stride length
m.Maximize(100*final*x)
m.Minimize(100*init*x)
#Minimize torque
m.Obj(0.01*u**2)
###Solve
m.options.IMODE = 6
m.options.SOLVER = 3
m.solve()
###Scale time vector
m.time = np.multiply(TF, m.time)
###Display Outputs
print("adiff=", adiff.VALUE)
print("bdiff=", bdiff.VALUE)
print("swtch=", swtch.VALUE)
print("c=", c.VALUE)
########################################
####Plotting the results
import matplotlib.pyplot as plt
plt.close('all')
fig1 = plt.figure()
fig2 = plt.figure()
fig3 = plt.figure()
fig4 = plt.figure()
ax1 = fig1.add_subplot()
ax2 = fig2.add_subplot(221)
ax3 = fig2.add_subplot(222)
ax4 = fig2.add_subplot(223)
ax5 = fig2.add_subplot(224)
ax6 = fig3.add_subplot()
ax7 = fig4.add_subplot(121)
ax8 = fig4.add_subplot(122)
ax1.plot(m.time,u.value,'m',lw=2)
ax1.legend([r'$u$'],loc=1)
ax1.set_title('Control Input')
ax1.set_ylabel('Torque (N-m)')
ax1.set_xlabel('Time (s)')
ax1.set_xlim(m.time[0],m.time[-1])
ax1.grid(True)
ax2.plot(m.time,x.value,'r',lw=2)
ax2.set_ylabel('X Position (m)')
ax2.set_xlabel('Time (s)')
ax2.legend([r'$x$'],loc='upper left')
ax2.set_xlim(m.time[0],m.time[-1])
ax2.grid(True)
ax2.set_title('Mass X-Position')
ax3.plot(m.time,xdot.value,'g',lw=2)
ax3.set_ylabel('X Velocity (m/s)')
ax3.set_xlabel('Time (s)')
ax3.legend([r'$xdot$'],loc='upper left')
ax3.set_xlim(m.time[0],m.time[-1])
ax3.grid(True)
ax3.set_title('Mass X-Velocity')
ax4.plot(m.time,y.value,'r',lw=2)
ax4.set_ylabel('Y Position (m)')
ax4.set_xlabel('Time (s)')
ax4.legend([r'$y$'],loc='upper left')
ax4.set_xlim(m.time[0],m.time[-1])
ax4.grid(True)
ax4.set_title('Mass Y-Position')
ax5.plot(m.time,ydot.value,'g',lw=2)
ax5.set_ylabel('Y Velocity (m/s)')
ax5.set_xlabel('Time (s)')
ax5.legend([r'$ydot$'],loc='upper left')
ax5.set_xlim(m.time[0],m.time[-1])
ax5.grid(True)
ax5.set_title('Mass Y-Velocity')
ax6.plot(x.value, y.value,'g',lw=2)
ax6.set_ylabel('Y-Position (m)')
ax6.set_xlabel('X-Position (m)')
ax6.legend([r'$mass coordinate$'],loc='upper left')
ax6.set_xlim(x.value[0],x.value[-1])
ax6.set_ylim(0,1.1)
ax6.grid(True)
ax6.set_title('Mass Position')
ax7.plot(m.time,q1.value,'r',lw=2)
ax7.set_ylabel('q1 Position (rad)')
ax7.set_xlabel('Time (s)')
ax7.legend([r'$q1$'],loc='upper left')
ax7.set_xlim(m.time[0],m.time[-1])
ax7.grid(True)
ax7.set_title('Hip Joint Angle')
ax8.plot(m.time,q2.value,'r',lw=2)
ax8.set_ylabel('q2 Position (rad)')
ax8.set_xlabel('Time (s)')
ax8.legend([r'$q2$'],loc='upper left')
ax8.set_xlim(m.time[0],m.time[-1])
ax8.grid(True)
ax8.set_title('Knee Joint Angle')
plt.show()

Answer 1

The m.if2() function is a Mathematical Program with Complementarity Constraints (MPCC). It does not use a binary variable like the m.if3() function and therefore can be solved with any NLP solver, such as IPOPT. The disadvantage is that it has a saddle point at the switching condition and often gets stuck at the local solution. One way to overcome this issue is to use m.if3() with the IPOPT solver for initialization and then switch to the APOPT solver to generate an exact MINLP solution.
m.options.SOLVER=3 # IPOPT
m.solve()
m.options.SOLVER=1 # APOPT
m.options.TIME_SHIFT = 0 # don't update initial conditions
m.solve()
Additional information on MPCCs and binary conditional statements is in the Design Optimization course section on Logical Conditions.