ros-controls / ros_control

Generic and simple controls framework for ROS
http://wiki.ros.org/ros_control
BSD 3-Clause "New" or "Revised" License
469 stars 307 forks source link

limit_jerk question in speed_limiter #503

Open YuLin1226 opened 2 years ago

YuLin1226 commented 2 years ago

Hi,

I'm now reading the controller source code and trying to customize my controller.

In the speed_limiter.cpp, I have a question about the dt2, i don't get that why it is 2*dt*dt?

why is it not just dt*dt by applying finite-difference methods?

Thanks.

double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt)
    {
        const double tmp = v;
        if (has_jerk_limits)
        {
            const double dv  = v  - v0;
            const double dv0 = v0 - v1;
            const double dt2 = 2. * dt * dt; // why 2?
            const double da_min = min_jerk * dt2;
            const double da_max = max_jerk * dt2;
            const double da = clamp(dv - dv0, da_min, da_max);
            v = v0 + dv0 + da;
        }
        return tmp != 0.0 ? v / tmp : 1.0;
    }