How to use QUINTIC & CUBIC

[chenguang3312]

Hi there,
I was confused trying to run rlaxiscontrollerdemo.How to interpolate joint waypoints.
The trajectory planning outputs a series of spatial coordinate points, which are solved by IK to obtain the joint list. We use the interpolation algorithm to calculate the smooth trajectory and send it to the robot.
Does the RTT output trajectory need to be interpolated first, then IK solved, and then interpolated and smoothed the joint path points?
#ifdef CUBIC rl::math::Polynomial<rl::math::Vector> interpolator = rl::math::Polynomial<rl::math::Vector>::CubicFirst( q0, q1, rl::math::Vector::Zero(controller.getDof()), rl::math::Vector::Zero(controller.getDof()), te ); #endif // CUBIC #ifdef QUINTIC rl::math::Polynomial<rl::math::Vector> interpolator = rl::math::Polynomial<rl::math::Vector>::( q0, q1, rl::math::Vector::Zero(controller.getDof()), rl::math::Vector::Zero(controller.getDof()), rl::math::Vector::Zero(controller.getDof()), rl::math::Vector::Zero(controller.getDof()), te );
How to determine cubic & quintic parameters when the interpolation object is a series of joint waypoints.

[rickertm]

You can use an IK solver to find suitable joint configurations for Cartesian waypoints. How to interpolate between these points depends on whether you want an interpolation in joint space or a linear interpolation in Cartesian space.

For an interpolation between two joint configurations in joint space, you can directly use an interpolation such as rl::math::Polynomial::CubicFirst or rl::math::Polynomial::QuinticFirstSecond, which will give you a function with derivatives for velocity and acceleration. The max. velocity/acceleration values are directly related to the duration, you can match this by looking at the maximum values of the derivatives. You can also use rl::math::Spline::TrapezoidalAccelerationAtRest to define max. velocity, acceleration, and jerk values.

To interpolate between multiple joint configurations, you can use rl::math::Spline::CubicFirst, rl::math::Spline::LinearParabolic, rl::math::Spline::LinearParabolicPercentage, rl::math::Spline::LinearQuartic etc.

For a linear interpolation between two joint configurations in Cartesian space, you have to define an interpolation for the position and orientation of the corresponding Cartesian pose. Position is analog to joint interpolation, but for orientation you should use quaternions. The combination of these two interpolations creates a 6D vector (x,y,z,a,b,c) that can be transformed to joint space via the Jacobian inverse. Due to singularities and joint limits, there may not be a viable solution when moving to the target position.

[mohdjawadi]

Please in your description of using any of these rl::math::Spline::CubicFirst, rl::math::Spline::LinearParabolic function etc. Is a little bit confusing. in the demo folders rlInterpolatorDemo and rlAxisControllerDemo for example, only two waypoints q0 and q1 are used for the interpolation as shown in the code snippets below:
rl::math::Polynomial<rl::math::Vector> interpolator = rl::math::Polynomial<rl::math::Vector>::CubicAtRest( q0, q1, vmax, amax, jmax );
So please as the original question suggested, how can we interpolate say multiple waypoints like say q0, q1, q2, q3, q4,q5,q6 with this same function?

Thanks for this amazing library

[rickertm]

A function for the interpolation between several waypoints requires multiple polynomials, therefore the respective static functions in the class rl::math::Spline are required. rl::math::Spline::CubicFirst and rl::math::Spline::CubicNatural define a cubic spline interpolation, whereas rl::math::Spline::LinearParabolic, rl::math::Spline::LinearQuartic etc. define piecewise functions with parabolic/quartic blends.

rlInterpolatorDemo includes an example with rl::math::ArrayX, but you can also use rl::math::Vector:

rl/demos/rlInterpolatorDemo/rlInterpolatorDemo.cpp

Lines 209 to 237 in 2da1f81

	::std::vector<rl::math::Real> x;
	x.push_back(0);
	x.push_back(1);
	x.push_back(3);
	x.push_back(3.5);
	x.push_back(6);
	x.push_back(7);
	::std::vector<rl::math::ArrayX> y;
	y.push_back(rl::math::ArrayX::Constant(1, 0));
	y.push_back(rl::math::ArrayX::Constant(1, 1));
	y.push_back(rl::math::ArrayX::Constant(1, -1));
	y.push_back(rl::math::ArrayX::Constant(1, 3));
	y.push_back(rl::math::ArrayX::Constant(1, 3));
	y.push_back(rl::math::ArrayX::Constant(1, -2));
	rl::math::Spline<rl::math::ArrayX> f0 = rl::math::Spline<rl::math::ArrayX>::CubicFirst(
	x,
	y,
	rl::math::ArrayX::Constant(1, 0),
	rl::math::ArrayX::Constant(1, 0)
	);
	eval(f0);
	rl::math::Spline<rl::math::ArrayX> f1 = rl::math::Spline<rl::math::ArrayX>::CubicNatural(x, y);
	eval(f1);
	rl::math::Spline<rl::math::ArrayX> f2 = rl::math::Spline<rl::math::ArrayX>::LinearParabolic(x, y, static_cast<rl::math::Real>(0.25));
	eval(f2);
	rl::math::Spline<rl::math::ArrayX> f3 = rl::math::Spline<rl::math::ArrayX>::LinearQuartic(x, y, static_cast<rl::math::Real>(0.25));
	eval(f3);
	rl::math::Spline<rl::math::ArrayX> f4 = rl::math::Spline<rl::math::ArrayX>::LinearSextic(x, y, static_cast<rl::math::Real>(0.25));
	eval(f4);

[mohdjawadi]

Thanks I now understand