Skip to content

Separate acceleration and deceleration limits. #17

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.

Sign up for GitHub

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

Merged
merged 3 commits into from
Jan 29, 2016
Merged

Separate acceleration and deceleration limits. #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0ee3d03
Separated acceleration and deceleration limits so deceleration is not…
p6chen Jan 28, 2016
54d054e
Fixing declaration order, and scope limiting variables.
p6chen Jan 29, 2016
a482337
Added proper clamping of velocities for positive/negative transitions.
p6chen Jan 29, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 9 additions & 1 deletion diff_drive_controller/include/diff_drive_controller/speed_limiter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ namespace diff_drive_controller
* \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
* \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
* \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
* \param [in] min_deceleration Minimum deceleration [m/s^2], usually <= 0
* \param [in] max_deceleration Maximum deceleration [m/s^2], usually >= 0
*/
SpeedLimiter(
bool has_velocity_limits = false,
Expand All @@ -66,7 +68,9 @@ namespace diff_drive_controller
double min_acceleration = 0.0,
double max_acceleration = 0.0,
double min_jerk = 0.0,
double max_jerk = 0.0);
double max_jerk = 0.0,
double min_deceleration = 0.0,
double max_deceleration = 0.0);

/**
* \brief Limit the velocity and acceleration
Expand Down Expand Up @@ -122,6 +126,10 @@ namespace diff_drive_controller
// Jerk limits:
double min_jerk;
double max_jerk;

// Deceleration limits
double min_deceleration; // Minimum acceleration used to slow down vehicle to zero velocity.
double max_deceleration; // Maximum acceleration used to slow down vehicle to zero velocity.
};

} // namespace diff_drive_controller
Expand Down
4 changes: 4 additions & 0 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -247,6 +247,8 @@ namespace diff_drive_controller
controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
controller_nh.param("linear/x/max_deceleration" , limiter_lin_.max_deceleration , limiter_lin_.max_acceleration );
controller_nh.param("linear/x/min_deceleration" , limiter_lin_.min_deceleration , limiter_lin_.min_acceleration );
controller_nh.param("linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk );
controller_nh.param("linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk );

Expand All @@ -257,6 +259,8 @@ namespace diff_drive_controller
controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
controller_nh.param("angular/z/max_deceleration" , limiter_ang_.max_deceleration , limiter_ang_.max_acceleration );
controller_nh.param("angular/z/min_deceleration" , limiter_ang_.min_deceleration , limiter_ang_.min_acceleration );
controller_nh.param("angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk );
controller_nh.param("angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk );

Expand Down
50 changes: 44 additions & 6 deletions diff_drive_controller/src/speed_limiter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,9 @@ namespace diff_drive_controller
double min_acceleration,
double max_acceleration,
double min_jerk,
double max_jerk
double max_jerk,
double min_deceleration,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

move them up, just below the _acceleration ones

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I had them at the end intentionally to keep backwards compatibility of any code that may have invoked the constructor with the old arguments. Having them at the end allows the new params to be default initialized.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

OK, then declared them after max_jerk to address my other/similar comment below

double max_deceleration
)
: has_velocity_limits(has_velocity_limits)
, has_acceleration_limits(has_acceleration_limits)
Expand All @@ -69,6 +71,8 @@ namespace diff_drive_controller
, max_acceleration(max_acceleration)
, min_jerk(min_jerk)
, max_jerk(max_jerk)
, min_deceleration(min_deceleration)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

move them up, just below the _acceleration ones (here also keeps the order of declaration, which should silence some compiler warnings)

, max_deceleration(max_deceleration)
{
}

Expand Down Expand Up @@ -101,11 +105,45 @@ namespace diff_drive_controller

if (has_acceleration_limits)
{
const double dv_min = min_acceleration * dt;
const double dv_max = max_acceleration * dt;

const double dv = clamp(v - v0, dv_min, dv_max);

double dv_min, dv_max, dv;

if (v0 >= 0.0 && v >= 0.0)
{
// Moving in positive direction
dv_min = min_deceleration * dt;
dv_max = max_acceleration * dt;
}
else if (v0 <= 0.0 && v <= 0.0)
{
// Moving in the negative direction
dv_min = min_acceleration * dt;
dv_max = max_deceleration * dt;
}
else
{
// Transitioning from positive to negative velocity
if (v0 > 0)
{
dv_max = v0;
dv_min = min_deceleration * dt;
if (v0 + dv < 0)
{
dv_min = min_acceleration * (dt - v0 / min_deceleration);
}
}
// Transitioning from negative to positive velocity
else if (v0 < 0)
{
dv_min = v0;
dv_max = max_deceleration * dt;
if (v0 + dv > 0)
{
dv_max = max_acceleration * (dt - v0 / max_deceleration);
}
}
}

dv = clamp(v - v0, dv_min, dv_max);
v = v0 + dv;
}

Expand Down