Get indexes or times of passed in joint values

[pbeeson]

I have a feature request. Is it possible in your implementation to be able to get out the times associated with the passed in points? Because right now, I get back a spline, which I can query for joint values at certain times, but short of creating a 1 KHz sampling for a trajectory, there's no way to really guarantee straight line motion. Additionally, many robot controllers are going to not work well with such a dense trajectory. I'd prefer to be able to just query what the times are for the passed joint values that were used to solve the problem. If these moved slightly no huge problem. I wrote some thing that goes through looking for the times where the joints occur but with complex trajectories the spline isn't exactly equal to the values passed in and you could end up finding something that matches best at the wrong place/time in the trajectory. Having something that keeps track of the indicies associated with the original knot points would be great.

[hungpham2511]

Thanks for the suggestion! Actually toppra computes the times associated with each waypoint, from which it returns the final spline/trajectory. Hence what you are suggesting can be implemented easily, as it is only a matter of exposing that array to the end-user.

I will implement this feature later today.

[pbeeson]

Great! Just another (related) quick question. Is there a particular functional reason why you default to 'not-a-knot' when creating the final trajectory in compute_trajectory? This seems to be one reason why I think my original joint values passed in are being used to generate the times but aren't necessarily being exactly met by the final curve. Also it makes the final point in the trajectory not necessarily have 0 velocity. Just curious as to the thinking there.

[hungpham2511]

Is there a particular functional reason why you default to 'not-a-knot' when creating the final trajectory in compute_trajectory?

In my experiences with scipy.CubicSpline, 'not-a-knot' option leads to more natural-looking splines. 'clamped' option tends to lead to oscillating splines, which are not very desirable.

This seems to be one reason why I think my original joint values passed in are being used to generate the times but aren't necessarily being exactly met by the final curve.

This should not happen though. You can check the newly updated kinematics.py example in the lastest commit, the plotted waypoints lie exactly on the output trajectory. Also from this commit you can use return_data option with compute_trajectory to object the time stamps of the waypoints.

[pbeeson]

Seems to work

[mrunaljsarvaiya]

Referring to the source code, return_data=True returns an additional variable K, which are the controllable velocities. How does one get the time stamps for each input position?

[mrunaljsarvaiya]

@hungpham2511 Could you please link me to the variable where these time stamps are stored?

[hungpham2511]

@hungpham2511 Could you please link me to the variable where these time stamps are stored?

Hi, the timestamps of each index can be found here https://github.com/hungpham2511/toppra/blob/766a2c736319639c7b0176959d2baa624ef74415/toppra/parametrizer.py#L55

[mrunaljsarvaiya]

@hungpham2511 Could you please link me to the variable where these time stamps are stored?

Hi, the timestamps of each index can be found here

https://github.com/hungpham2511/toppra/blob/766a2c736319639c7b0176959d2baa624ef74415/toppra/parametrizer.py#L55

Thank you!

[mrunaljsarvaiya]

@hungpham2511 Could you please link me to the variable where these time stamps are stored?

Hi, the timestamps of each index can be found here

https://github.com/hungpham2511/toppra/blob/766a2c736319639c7b0176959d2baa624ef74415/toppra/parametrizer.py#L55

Could you confirm that this is the correct way to get the time values?

For example if the inputs are initialized as:

waypts = np.array([[0], [1], [10]])
path = ta.SplineInterpolator(np.linspace(0, 1, n), waypts)

To get the timestamp for waypts[1], in one of the parametrizer calls in topppra/toppra/parametrizer.py, I would first look for the gridpoint index (idx) that corresponds to path(0.5) (the linspace call places waypts[1] at 0.5). The corresponding timestamp would then be self._ts[idx]? There's also no gurantee that the path(0.5) would exist in the list of gridpoints right? So the code would need to look for the closest value

[hungpham2511]

Yes you are right. The list of gridpoints is can be supplied manually as well. In this way you can actually include any point you want.