Mismatch between ipopt and fatrop

[maximvochten]

For difficult problems there is still a mismatch between Fatrop and IPOPT. Below you can find such a challenging problem.

Fatrop does not appear to converge for this problem and returns all zeros for the solution. See the attached output: fatrop_output.txt and ipopt_output.txt

import numpy as np
import rockit
import casadi as cas

solver = 'fatrop' # {options: 'fatrop', 'ipopt'}

data = np.array([[-1.00338665e-01, 5.25791787e-01, 1.01274970e+00],
                 [-8.97279569e-02, 5.19482902e-01, 1.01274967e+00],
                 [-7.90349921e-02, 5.13313382e-01, 1.01274962e+00],
                 [-6.82011033e-02, 5.07394713e-01, 1.01274960e+00],
                 [-5.72345225e-02, 5.01725572e-01, 1.01274958e+00],
                 [-4.61262435e-02, 4.96339626e-01, 1.01274956e+00],
                 [-3.48478939e-02, 4.91319254e-01, 1.01274955e+00],
                 [-2.34020210e-02, 4.86694065e-01, 1.01274955e+00],
                 [-1.17851527e-02, 4.82524894e-01, 1.01274957e+00],
                 [-2.23721351e-05, 4.79352736e-01, 1.01275073e+00]])
dt = 0.01
rms_error_traj = 1e-3

''' System dynamics '''

ocp = rockit.Ocp(T=1)
N = np.size(data,0)

# States
p_obj = ocp.state(3) # position trajectory (xyz-coordinates)
R_t = ocp.state(3,3) # moving frame (Frenet-Serret frame) as a 9x1 vector

# Controls
invars = ocp.control(3) # invariants (velocity, curvature rate, torsion rate)
i_vel = invars[0] # object translation speed
i_kappa = invars[1] # curvature rate of moving frame
i_tau = invars[2] # torsion rate of moving frame

# Parameters
p_obj_m = ocp.parameter(3,grid='control+') # measured position trajectory (xyz-coordinates)
h = ocp.parameter(1,1) # step-size (can be a discrete step in time or arclength)

# Represent the velocities in the moving frame
omega = cas.vertcat(i_tau,0,i_kappa) 
omega_norm = cas.norm_2(omega)
v = cas.vertcat(i_vel,0,0)

# Integrate velocities (body-fixed twist) to find change in rotation and translation
deltaR = np.eye(3) + cas.sin(omega_norm @ h)/omega_norm*cas.skew(omega) + (1-cas.cos(omega_norm @ h))/omega_norm**2 * cas.mtimes(cas.skew(omega),cas.skew(omega))
deltaP = (np.eye(3)-deltaR) @ cas.skew(omega) @ v/omega_norm**2 + omega @ omega.T @ v/omega_norm**2*h

# Apply change in rotation and translation
R_t_plus1 = R_t @ deltaR
p_obj_plus1 = R_t @ deltaP + p_obj
ocp.set_next(p_obj,p_obj_plus1)
ocp.set_next(R_t,R_t_plus1)

''' Constraints and objectives '''

# Orthonormality of rotation matrix (only needed for one timestep, property is propagated by integrator)  
matrix = R_t.T @ R_t - np.eye(3)
tril_vec = cas.vertcat(matrix[0,0], matrix[1,1], matrix[2,2], matrix[1,0], matrix[2,0], matrix[2,1])
ocp.subject_to(ocp.at_t0(tril_vec == 0.))

# Measurement fitting constraint
ek = cas.dot(p_obj - p_obj_m, p_obj - p_obj_m) # squared position error
running_ek = ocp.state() # running sum of squared error
ocp.subject_to(ocp.at_t0(running_ek == 0))
ocp.set_next(running_ek, running_ek + ek)
total_ek = ocp.state() # total sum of squared error
ocp.set_next(total_ek, total_ek)
ocp.subject_to(ocp.at_tf(total_ek == running_ek + ek))
ocp.subject_to(total_ek/N/rms_error_traj**2 < 1)

# total_ek = ocp.sum(ek)
# ocp.add_objective(total_ek/N)

# Minimize moving frame invariants to deal with singularities and noise
objective_reg = ocp.sum(cas.dot(invars[1:3],invars[1:3]))
objective = objective_reg/(N-1)
ocp.add_objective(objective)

""" Solver definition """

if solver=='fatrop':
    ocp.method(rockit.external_method('fatrop',N=N-1))
    ocp._method.set_expand(True) 
    #ocp._method.
elif solver == 'ipopt':
    ocp.method(rockit.MultipleShooting(N=N-1))
    ocp.solver('ipopt', {'expand':True, 'ipopt.mu_init':1e2})

""" Solving the OCP """

# Initialize states and control
ocp.set_initial(R_t, np.eye(3))
ocp.set_initial(p_obj, data.T)
ocp.set_initial(invars, [1, 1e-1, 1e-1])

# Set values parameters
ocp.set_value(p_obj_m, data.T)
ocp.set_value(h, dt)

# Solve the OCP
sol = ocp.solve_limited()
invariants = np.array([sol.sample(invars[i], grid='control')[1] for i in range(3)]).T
print(invariants)

[ImW1ll]

Hi, Sorry I don't have an answer for your problem. I was simply wondering if you had run in the issue of this type : HPIPM: specified structure of A does not correspond to what the interface can handle. And what workaround had you found if you faced one. Thank you!

[lvanroye]

As discussed privately with Maxim, this specific problem can be solved by setting the "linsol_lu_fact_tol" to a very small value (for example 1e-12).

[maximvochten]

This was done by modifying the solver options as follows for the Rockit interface:

if solver=='fatrop':
    ocp.method(rockit.external_method('fatrop',N=N-1))
    ocp._method.set_expand(True) 
    ocp = ocp._transcribed
    ocp._method.set_option("linsol_lu_fact_tol", 1e-12) # add this line
    ocp._method.set_option("tol", 1e-6)