Got burrs by calling interpolateCubicNatural method

[truewq]

Hi I'm using tinyspline to interpolate for robot end effector way point. The following is my code:

include "ros/ros.h"

include "tinysplinecxx.h"

include <nav_msgs/Path.h>

int main(int argc, char **argv) { ros::init(argc, argv, "tiny_nurbs"); ros::NodeHandle n; ros::Publisher path_pub = n.advertise("nurbs",1, true);

// Setup control points. std::vector ctrlp ; ctrlp.push_back(0.471); ctrlp.push_back(0.168); ctrlp.push_back(0);

ctrlp.push_back(0.432); ctrlp.push_back(0.152); ctrlp.push_back(0);

ctrlp.push_back(0.413); ctrlp.push_back(0.184); ctrlp.push_back(0);

ctrlp.push_back(0.406); ctrlp.push_back(0.184); ctrlp.push_back(0);

ctrlp.push_back(0.397); ctrlp.push_back(0.183); ctrlp.push_back(0);

ctrlp.push_back(0.388); ctrlp.push_back(0.183); ctrlp.push_back(0);

ctrlp.push_back(0.208); ctrlp.push_back(0.168); ctrlp.push_back(0);

tinyspline::BSpline spline = tinyspline::BSpline::interpolateCubicNatural(ctrlp, 3);

// Evaluate spline at u = 0.4 using 'eval'. std::vector result = spline.eval(0.4).result(); std::cout << "x = " << result[0] << ", y = " << result[1]<<", z = " << result[2] << std::endl;

float f = 0.0; ros::Rate loop_rate(0.2); while (ros::ok()) { nav_msgs::Path path;

  path.header.stamp=ros::Time::now();
  path.header.frame_id="/world_frame";
  for (uint32_t i = 0; i <= 100; ++i)
  {
      double u = double(i)/100.0;
      std::vector<tinyspline::real> result = spline.eval(u).result();
      double x = result[0];
      double y = result[1];
      double z = result[2];
      std::cout << "gen x = " << result[0] << ", y = " << result[1]<<", z = " << result[2] << std::endl;

      geometry_msgs::PoseStamped this_pose_stamped;
      this_pose_stamped.pose.position.x = x;
      this_pose_stamped.pose.position.y = y;
      this_pose_stamped.pose.position.z = z;

      this_pose_stamped.pose.orientation.x = 0;
      this_pose_stamped.pose.orientation.y = 0;
      this_pose_stamped.pose.orientation.z = 0;
      this_pose_stamped.pose.orientation.w = 1;

      this_pose_stamped.header.stamp=ros::Time::now();;
      this_pose_stamped.header.frame_id="/world_frame";

      path.poses.push_back(this_pose_stamped);
  }
  path_pub.publish(path);

  loop_rate.sleep();
  f += 0.04;

}

return 0; }

I got a strange burr in the penultimate curve. Please click the link of the picture shows the burr. https://pic2.zhimg.com/v2-812d990236890202e0815db0f3a4c2e9_b.png

Could you please tell me why I got this burr? If I change the distance between the last and the penultimate data point, then the burr would become smoother or even disappear. How can I solve this problem?

Thanks.

[msteinbeck]

Hi @truewq,

Maybe tinyspline::BSpline::interpolateCatmullRom solves your issue. If alpha is 0.5 (the default value), the interpolated spline has no cusps (see https://splines.readthedocs.io/en/latest/euclidean/catmull-rom-properties.html#Cusps-and-Self-Intersections for more details). Could you please also visualize the interpolated control points.

[truewq]

Hi @msteinbeck

I have tried the tinyspline::BSpline::interpolateCatmullRom. It indeed solves the cusps issue, but in the same time I lose C-2 continuity of the interpolate curve. I have printed the control points, please check picture in the image: https://pic1.zhimg.com/v2-d9d8eaccf7947d1d481fd44c6979f394_b.png

I'm wondering if the centripetal parameterization used with B-splines could solve this issue. (see https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/INT-APP/CURVE-INT-global.html)

Could you give me some advise?

[msteinbeck]

but in the same time I lose C-2 continuity of the interpolate curve.

The splines connecting the points to be interpolated are C-2 continuous. Only the join points (the points to be interpolated) are C-1 continuous. If you need a spline which is C-2 continuous at any point, you need to use tinyspline::BSpline::interpolateCubicNatural. The cusps of tinyspline::BSpline::interpolateCubicNatural occur when neighboring data points (the points to be interpolated) are too close together. You could "merge" such points in order to remove the cusps (a similar approach is used in tinyspline::BSpline::interpolateCatmullRom, see https://github.com/msteinbeck/tinyspline/blob/master/src/tinyspline.c#L967).

I'm wondering if the centripetal parameterization used with B-splines could solve this issue. (see https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/INT-APP/CURVE-INT-global.html)

Maybe this is a viable alternative. That said, global interpolation is not yet available in TinySpline (PRs are welcome).

[truewq]

@msteinbeck Thanks for your patient explanation. I will pre-process the data points and use tinyspline::BSpline::interpolateCubicNatural.

I use https://github.com/orbingol/NURBS-Python, the fitting.interpolate_curve(points, degree, centripetal=True) method got a ideal cubic Bspline curve.