Add spline interpolator to tesseract_common

[mpowelson]

This function uses Eigen to interpolate each column of a TrajArray using a cubic spline.

[marip8]

You should probably check these quantities before using them to make your spline

[mpowelson]

Is there a good/easily readable way to do that? It shouldn't segfault if you add them. The results will just be wrong.

[mpowelson]

Actually, the sizes being mismatched might cause problems. But the others shouldn't.

[marip8]

These arguments just allow you to specify the derivatives of the spline at certain knots, right? If so, some more thorough documentation might make it clearer on how to use this constructor

[mpowelson]

Yeah I haven't played around with these too much and the Eigen unsupported documentation is not great. This example was my attempt at making it clearer. Do you have suggestions as to what you'd like to see?

Honestly, we can't really use the derivatives unless we had some way to distribute a patched Eigen version of at least the broken headers. I don't think we want to add install instructions that say to go modify your system installed Eigen headers.

[marip8]

I would say something like

derivatives is a vector of derivatives of the input variable with which to create the spline. The indices of the input points to which the derivatives correspond is captured in the indices parameter

Should we even include a spline with derivatives if it doesn't work correctly?

[marip8]

The x_vec argument and x_min/x_max parameters (and associated scaling functions) seem a bit unnecessary in my opinion. Is there a compelling reason to use it? It seems like the user will almost always create a new vector of [0, 1, 2, ... n] like you did in the unit test. I think it would be more understandable if x_vec wasn't required in the constructor, and the user could only query the () operator with a value on [0:1]

[mpowelson]

If you were to use this with Trajopt with time parameterization turned on (which coincidentally the interpolate function below is not set up for), you'd get joints with timesteps that are not evenly spaced. Then if you had a trajectory with a bunch of points clustered in time, you'd like to fit a spline to allow of those and then uniformly sample the whole thing in time.

[marip8]

I would propose having the user provide a number on [0,1] when calling the () operator, since that is the common convention for splines anyway. Inferring max and min values from this seemingly unrelated x_vec input and silently scaling the argument to the () operator seems a bit confusing to me. Regarding the time parameterization example, I would assume that the calling code would convert the timestep to be on [0,1] before sampling the spline

[marip8]

@mpowelson you should also add a few checks in the class itself to validate that your spline actually approximates the data correctly. I ran into some issues recently using this library where my input points generated bad splines (mostly due to duplicate or almost duplicate input points)

1. Check that the knots are equal to the input points within some small threshold

  Eigen::Matrix3Xd pts; // input points
  Eigen::Spline3d::KnotVectorType chord_lengths;
  Eigen::ChordLengths(pts, chord_lengths);
  for (Eigen::Index i = 0; i < pts.cols(); ++i)
  {
    const Eigen::Vector3d& pt = spline(chord_lengths(i));
    const Eigen::Vector3d& ref = pts.col(i);
    if (!ref.isApprox(pt))
    {
      return false;
    }
  }

2. Check that the piecewise, linearly interpolated "length" of the input points is equal (within some percentage tolerance) to the length of an arbitrary (large) number of points sampled evenly on the spline from [0:1]. In my case where I accidentally had duplicate input points, the length of my spline was about 2x as long as the piecewise interpolated length of my input points, which caused some serious problems. This is the function I used to estimate the length

double estimateSplineLength(const Eigen::Matrix3Xd& points)
{
  double length = 0;
  for (Eigen::Index i = 0; i < points.cols() - 1; ++i)
  {
    const Eigen::VectorXd& first = points.col(i);
    const Eigen::VectorXd& second = points.col(i + 1);
    length += (second - first).norm();
  }
  return length;
}

[gavanderhoorn]

Might be interesting to take a look at joint_trajectory_controller/include/trajectory_interface/quintic_spline_segment.h?

[johnwason]

Is this designed to retime trajectories for vel/accel/jerk limits?

[johnwason]

Is there any movement on this? We need trapezoidal trajectory profiles for use with robots that have direct position control.

[gavanderhoorn]

You could take a look at hungpham2511/toppra.

It's only acceleration-limited, but generates pretty smooth trajectories.

@johnwason: what brand of robots are you using? Afaik most robots that have "direct position control" also come with adequate filtering.

[johnwason]

Thanks @gavanderhoorn, that link is really useful!

Most robots we are working with will dynamic overload of the command impulse is too large (ABB EGM), or will overshoot (Sawyer). The control provided is very low level.

[gavanderhoorn]

Are you using EGM for those ABBs? Because there are quite some knobs to tune with EGM. IIRC, gains on position setpoints and low-pass filter cutoffs.

[johnwason]

@gavanderhoorn yes, we have been working with the ABB EGM for quite a while. The issue is that if we lower the gains sufficiently to avoid dynamic overload, the performance becomes unusable. The ABB EGM and other similar inputs expect the input profile to pre-filtered for dynamics.

[gavanderhoorn]

I'm aware of the context in which non-smooth trajectories can become problematic. I just wanted to make sure we're not overcomplicating things.

[Levi-Armstrong]

@mpowelson What do you think we should do with this PR?

[mpowelson]

@mpowelson What do you think we should do with this PR?

I guess close it. I opened this when I was playing around with the idea of adding spline paths to trajopt (rather than assuming joint interpolated like we do now). I don't see me working on that any time soon.

That said, this code should still work if someone needed it - with the caveats explained in the comments about some features being broken in the system version of Eigen.

[mpowelson]

@Levi-Armstrong I rebased it for completeness. I still won't be upset if we close this.

[rjoomen]

I think this can be closed.

[Levi-Armstrong]

I have been leaving this open, because I would like to get this merged but not sure if the feature are fixed in the Eigen version on 20.04.

[rjoomen]

I checked, only from Eigen 3.4.0 @mpowelson's PR has been included. Ubuntu 20.04 still is on 3.3.7, unfortunately.
You could add a check to either the CMakeLists.txt or the code to either not include this code or throw an error when Eigen version < 3.4.0. Then this PR could be merged. Because I don't think the official Ubuntu 20.04 repos will ever get Eigen 3.4.