Spikes in the Integral force plot due to time calculation

[Fabian Gerlinghaus]

Hey Samir,

part of the problem evidently was the calculation of the time for the integral_force:

// Take a time step
data_->integral_gain_time_pre_ = data_->integral_gain_time_curr_;
data_->integral_gain_time_curr_ = sutil::CSystemClock::getSysTime();
double tmp_int_dt = data_->integral_gain_time_curr_ - data_-

>integral_gain_time_pre_;
tmp_int_dt /= data_->integral_gain_time_constt_;

Once I set it to a fixed value, the spikes in the integral gain torque disappeared:

data_->integral_force_ *= 0.02;

Maybe something is wrong witht the Clock? What do you think?

Best, Fabian

[Samir Menon]

It's not a problem with the clock. It is probably a problem with the Operating System not being real-time. Every now and then a servo loop will take a bit longer.

Please plot a histogram of the time taken between servo ticks.

[Fabian Gerlinghaus]

OK, I improved the shield strategy and gave an entire core to the scl process alone. This improved the spike problem in the calculated torque. But it's still not perfect. There is some weird fluctuation in the the calculation of the time interval (see histogram), so I will just hard set the time interval to two ms and live a happy life.

The question is, whether you want to incorporate this as a feature into the task, or whether you prefer that I make a custom version of the OpPosPIDATask that I can freely fool around with, Samir. Please let me know.

[Fabian Gerlinghaus]

Histogram for the calculated time intervals.

[Fabian Gerlinghaus]

Calculated torque from the integral component much better with the improved cpu set shielding strategy.

[Samir Menon]

Thanks for adding the figures. Yes. Please create your custom task. You can copy it over and add whatever changes you require! :)

[Samir Menon]

Please resolve after you've finished your custom modifications. Thanks.

[Fabian Gerlinghaus]

Sure will do.

Btw, just noticed something: In the mails with Torsten you said this is your formula:

IntegralForce(t) = IntegralForce(t-1) + integral_gain_ * pos_err * (t_curr - t_pre) / integral_gain_time_constt_;

If I'm not mistaken what you effectively implemented, however, is this:

IntegralForce(t) = (IntegralForce(t-1) + integral_gain_ * pos_err) * (t_curr - t_pre) / integral_gain_time_constt_;

[Fabian Gerlinghaus]

When controlling the KUKA LWR. tmp_int_dt needs to be hard set to the cycle time.