Add spline interpolator to tesseract_common

[mpowelson]

This function uses Eigen to interpolate each column of a TrajArray using a cubic 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.