Potential issue with indexing of time-optimal trajectory smoothing

[colormotor]

Hi, I was testing the code for trajectory smoothing, and in my case I require the code to track densely sampled trajctories rather than a limited number of way-points (for example points along a circle). Currently the code in panda-py forces the max_deviation in the Path constructor to zero (which works fine), but this will result in jerky motions that treat each pair of samples as a segment. This can be avoided by increasing max_deviation to a suitable value. However, doing so results in the Trajectory::getTrajectorySegmentIndex function returning a wrong index for a given timestep. I believe this is due to a mismatch in the length of the smoothed trajectory and the input path.

I'm still testing this, but one way to avoid the issue is to store a segment index into each PathSegment when constructing the Path object, and then using the following code for Trajectory::getTrajectorySegmentIndex:

size_t Trajectory::getTrajectorySegmentIndex(double time) {
  size_t idx = 0;
  auto t = getTrajectorySegment(time);
  double s = t->path_pos_;
  return path_.getPathSegment(s)->index;
}

[JeanElsner]

Hi, that's a nice catch! Admittedly I've only used that functionality to go through a very limited number of waypoints, mostly from A to B. I will investigate once I have some time. At our lab we're also using more advanced motion generators (e.g. using dual quaternions for screw motions) that may be more suitable to your application. We will probably make them available for panda-py after the current IROS deadline. @greinerth ;-)