# Path planning using waypoint and linear interpolation

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
``````