Path planning using waypoint and linear interpolation


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. :slightly_smiling_face:


## 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 =, 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