Hello,
INTERFACE: Python
GOAL: Generate trajectory and steering control angle from a generic path (no time constraints) defined by waypoints for a kinematic bicycle model.
ISSUE: Solver failure (status =1). Stationary residuals: nan.
I have identified the problem in my cost function because this problem arise when I start using ca.if_else construct in the cost function.
QUESTION_1: Can I use the ca.if_else construct within the acados framework?
QUESTION_2: If not, is there another way to incorporate this logic in acados?
Note: I have tested this using CasADi, and I am able to compute the gradient of my cost function.
Thank you. 
CODE EXAMPLE:
## distance line from segment AB to point C
def distance(A, B, P):
# Vector from A to B
AB = B - A
# Vector from A to P
AP = P - A
# Magnitude of AB vector (the distance between A and B)
AB_length = ca.sqrt(AB[0]**2 + AB[1]**2)
# Normalize the line segment vector
AB_normalized = AB / AB_length
# Project point onto AB (scalar projection)
scalar_projection = ca.dot(AP, AB_normalized)
# Ensure scalar projection lies within the line segment
scalar_projection = ca.fmin(ca.fmax(scalar_projection, 0), AB_length)
# Find the closest point on the line to P
closest_point = A + scalar_projection * AB_normalized
# Distance from P to the closest point on the line
distance = ca.sqrt((P[0] - closest_point[0])**2 + (P[1] - closest_point[1])**2)
return distance, scalar_projection / AB_length # Normalize scalar projection to [0, 1] interval
## find the closest point to the given point
def closest_point(waypoints:np.array, point):
min_distance = 1e6 #ca.inf
for i in range(len(waypoints)-1):
A = waypoints[i]
B = waypoints[i+1]
dist, s = distance(A, B, point)
min_distance = ca.if_else(dist < min_distance, dist, min_distance)
return min_distance