Best method to add path points

Hi @MatthewPeterKelly, thank you for this awesome project and the description. I'm new to trajectory optimization and want to setup something specific:

Beside the start and end point, I want to reconstruct the trajectory of a vehicle from it's radar reflections. That means, beside the start and end-point, I want to add multiple path-points of the trajectory in between at certain time values. Whats the best way of doing so?

I've started by modifiying the toyCar example, see attachment, in which some points were inserted and assigned to certain trajectory points. In the pathObjective and pathConstraint, the positions and time were assigned. Is this good practice or would you recommend doing it differently?

I defined the path points here:

The first index is the trajectory point where this point is assigned to. The second index is the time in second. Third and fourth index are the x and y positions.

P.s.: The dynamics of the toyCar were adapted to include velocity, steering angle, etc.

========================================================
Edit: Perhaps multiple shooting would be the right approach? Do you have an example on how to implement this?

toyCar.zip

Start by reviewing the toyCar example and the attached toyCar.zip, focusing on the modified pathObjective and pathConstraint where intermediate positions and times are assigned. Compare that setup with the issue's multiple-shooting question; done would be a clear, reproducible example or documented approach for reconstructing a trajectory from intermediate vehicle path points.