Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

How to use QUINTIC & CUBIC #59

Open
chenguang3312 opened this issue May 19, 2022 · 4 comments
Open

How to use QUINTIC & CUBIC #59

chenguang3312 opened this issue May 19, 2022 · 4 comments

Comments

@chenguang3312
Copy link

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
Copy link
Member

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
Copy link

mohdjawadi commented Apr 27, 2023

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
Copy link
Member

rickertm commented May 6, 2023

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:

::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
Copy link

Thanks I now understand

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants