Best method to add path points

[ChrGri]

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

[MatthewPeterKelly]

I'm going to try to rephrase your question, to check my understanding:

What is the best way to implement a path objective that ties a trajectory to some data set, sampled along the trajectory?

A few different thoughts:

(1) There is a whole field of research tied to using trajectory optimization for state estimation for measurements from some dynamical system. Think reconstructing the path of a gymnast doing a vault using markers and a camera. I'm not an expert in this, but there are definitely lots of papers that you could find with details.

(2) If you want to use direct collocation, and tie your discretization to the data set, then you could manually add a constraint between each collocation point and the data points in the set. This is a bit of a hack, but I think it would work, possibly with some minor adjustments to the codebase (I don't have time now to dig into the details).

(3) The "standard" way to do this would be to have the path objective interpolate data from the table, which means that you could decouple the collocation points from the points in the dataset. You have to be careful though: it is really easy to accidentally introduce discontinuous gradients into the NLP solver, which cause convergence trouble. If you read Bett's book (cited in my tutorial paper, and maybe the user guide for OptimTraj), he discusses this in some detail. If I recall, you need to ensure that the collocation points don't jump between different data points during the solve. I think that you get this for free if you don't allow time scaling (which I think is not a feature in OptimTraj anyway). Then you need to use method-consistent interpolation so that you're actually comparing the correct value on the trajectory (eg. don't use lilnear interpolation in your objective, but then use a cubic method for your discretization).

Good luck!

[glennlolo]

Hi @ChrGri,

I had the same problem for one of my project where i wanted to constrain the path taken by the "toy car" demo.

I only used the PathConstraint "Cpb" in order to constrain the "x" and "y" states at specific times. The Matrix of "Cpb" is of the size 2 and the length of the time vector, as their is one constraint for x state and one for y state. at other times I initialized the values at 0 to not have any influence.

It seems to work but i don't have the background in this research field to be sure of this "simple" solution, i let @MatthewPeterKelly to correct me if this approach is wrong.

This method has also its flaws as you don't have control on the time of the collocation points. I opted to get the closest one discretized by the method. I think the method describes by Matthew before handle this problem but i'm not good enough to understand the last one ^^

Cheers