Skip to content

Commit

Permalink
Replace with TractionLimiter
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Nov 1, 2024
1 parent 2c35a8c commit 55ee7c6
Show file tree
Hide file tree
Showing 3 changed files with 473 additions and 185 deletions.
55 changes: 26 additions & 29 deletions include/control_toolbox/speed_limiter.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 PAL Robotics S.L.
// Copyright 2022 Pixel Robotics.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -13,7 +13,7 @@
// limitations under the License.

/*
* Author: Enrique Fernández
* Author: Tony Najjar
*/

#ifndef CONTROL_TOOLBOX__SPEED_LIMITER_HPP_
Expand All @@ -28,65 +28,58 @@ class SpeedLimiter
public:
/**
* \brief Constructor
* \param [in] has_velocity_limits if true, applies velocity limits
* \param [in] has_acceleration_limits if true, applies acceleration limits
* \param [in] has_jerk_limits if true, applies jerk limits
* \param [in] min_velocity Minimum velocity [m/s], usually <= 0
* \param [in] max_velocity Maximum velocity [m/s], usually >= 0
* \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
* \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_velocity Minimum velocity [m/s] or [rad/s]
* \param [in] max_velocity Maximum velocity [m/s] or [rad/s]
* \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2]
* \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2]
* \param [in] min_deceleration Minimum deceleration [m/s^2] or [rad/s^2]
* \param [in] max_deceleration Maximum deceleration [m/s^2] or [rad/s^2]
* \param [in] min_jerk Minimum jerk [m/s^3] (applied for both signs of jerk)
* \param [in] max_jerk Maximum jerk [m/s^3] (applied for both signs of jerk)
*/
SpeedLimiter(
bool has_velocity_limits = false, bool has_acceleration_limits = false,
bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN,
double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN,
double max_jerk = NAN);
double min_velocity = NAN, double max_velocity = NAN, double min_acceleration = NAN,
double max_acceleration = NAN, double min_deceleration = NAN, double max_deceleration = NAN,
double min_jerk = NAN, double max_jerk = NAN);

/**
* \brief Limit the velocity and acceleration
* \param [in, out] v Velocity [m/s]
* \param [in] v0 Previous velocity to v [m/s]
* \param [in] v1 Previous velocity to v0 [m/s]
* \param [in, out] v Velocity [m/s] or [rad/s]
* \param [in] v0 Previous velocity to v [m/s] or [rad/s]
* \param [in] v1 Previous velocity to v0 [m/s] or [rad/s]
* \param [in] dt Time step [s]
* \return Limiting factor (1.0 if none)
*/
double limit(double & v, double v0, double v1, double dt);

/**
* \brief Limit the velocity
* \param [in, out] v Velocity [m/s]
* \param [in, out] v Velocity [m/s] or [rad/s]
* \return Limiting factor (1.0 if none)
*/
double limit_velocity(double & v);

/**
* \brief Limit the acceleration
* \param [in, out] v Velocity [m/s]
* \param [in] v0 Previous velocity [m/s]
* \param [in, out] v Velocity [m/s] or [rad/s]
* \param [in] v0 Previous velocity [m/s] or [rad/s]
* \param [in] dt Time step [s]
* \return Limiting factor (1.0 if none)
*/
double limit_acceleration(double & v, double v0, double dt);

/**
* \brief Limit the jerk
* \param [in, out] v Velocity [m/s]
* \param [in] v0 Previous velocity to v [m/s]
* \param [in] v1 Previous velocity to v0 [m/s]
* \param [in, out] v Velocity [m/s] or [rad/s]
* \param [in] v0 Previous velocity to v [m/s] or [rad/s]
* \param [in] v1 Previous velocity to v0 [m/s] or [rad/s]
* \param [in] dt Time step [s]
* \return Limiting factor (1.0 if none)
* \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
*/
double limit_jerk(double & v, double v0, double v1, double dt);

private:
// Enable/Disable velocity/acceleration/jerk limits:
bool has_velocity_limits_;
bool has_acceleration_limits_;
bool has_jerk_limits_;

// Velocity limits:
double min_velocity_;
double max_velocity_;
Expand All @@ -95,6 +88,10 @@ class SpeedLimiter
double min_acceleration_;
double max_acceleration_;

// Deceleration limits:
double min_deceleration_;
double max_deceleration_;

// Jerk limits:
double min_jerk_;
double max_jerk_;
Expand Down
148 changes: 85 additions & 63 deletions src/speed_limiter.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 PAL Robotics S.L.
// Copyright 2022 Pixel Robotics.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -13,74 +13,95 @@
// limitations under the License.

/*
* Author: Enrique Fernández
* Author: Tony Najjar
*/

#include <algorithm>
#include <stdexcept>
#include <string>

#include "control_toolbox/speed_limiter.hpp"

namespace control_toolbox
{
SpeedLimiter::SpeedLimiter(
bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity,
double max_velocity, double min_acceleration, double max_acceleration, double min_jerk,
double max_jerk)
: has_velocity_limits_(has_velocity_limits),
has_acceleration_limits_(has_acceleration_limits),
has_jerk_limits_(has_jerk_limits),
min_velocity_(min_velocity),
double min_velocity, double max_velocity, double min_acceleration, double max_acceleration,
double min_deceleration, double max_deceleration, double min_jerk, double max_jerk)
: min_velocity_(min_velocity),
max_velocity_(max_velocity),
min_acceleration_(min_acceleration),
max_acceleration_(max_acceleration),
min_deceleration_(min_deceleration),
max_deceleration_(max_deceleration),
min_jerk_(min_jerk),
max_jerk_(max_jerk)
{
// Check if limits are valid, max must be specified, min defaults to -max if unspecified
if (has_velocity_limits_)
if (!std::isnan(min_velocity_) && std::isnan(max_velocity_))
max_velocity_ = 1000.0; // Arbitrarily big number
if (!std::isnan(max_velocity_) && std::isnan(min_velocity_)) min_velocity_ = 0.0;

if (!std::isnan(min_acceleration_) && std::isnan(max_acceleration_)) max_acceleration_ = 1000.0;
if (!std::isnan(max_acceleration_) && std::isnan(min_acceleration_)) min_acceleration_ = 0.0;

if (!std::isnan(min_deceleration_) && std::isnan(max_deceleration_)) max_deceleration_ = 1000.0;
if (!std::isnan(max_deceleration_) && std::isnan(min_deceleration_)) min_deceleration_ = 0.0;

if (!std::isnan(min_jerk_) && std::isnan(max_jerk_)) max_jerk_ = 1000.0;
if (!std::isnan(max_jerk_) && std::isnan(min_jerk_)) min_jerk_ = 0.0;

const std::string error =
" The positive limit will be applied to both directions. Setting different limits for positive "
"and negative directions is not supported. Actuators are "
"assumed to have the same constraints in both directions";
if (min_velocity_ < 0 || max_velocity_ < 0)
{
throw std::invalid_argument("Velocity cannot be negative." + error);
}

if (min_velocity_ > max_velocity_)
{
throw std::invalid_argument("Min velocity cannot be greater than max velocity.");
}

if (min_acceleration_ < 0 || max_acceleration_ < 0)
{
throw std::invalid_argument("Acceleration limits cannot be negative." + error);
}

if (min_acceleration_ > max_acceleration_)
{
throw std::invalid_argument("Min acceleration cannot be greater than max acceleration.");
}

if (min_deceleration_ < 0 || max_deceleration_ < 0)
{
throw std::invalid_argument("Deceleration limits cannot be negative." + error);
}

if (min_deceleration_ > max_deceleration_)
{
if (std::isnan(max_velocity_))
{
throw std::runtime_error("Cannot apply velocity limits if max_velocity is not specified");
}
if (std::isnan(min_velocity_))
{
min_velocity_ = -max_velocity_;
}
throw std::invalid_argument("Min deceleration cannot be greater than max deceleration.");
}
if (has_acceleration_limits_)

if (min_jerk_ < 0 || max_jerk_ < 0)
{
if (std::isnan(max_acceleration_))
{
throw std::runtime_error(
"Cannot apply acceleration limits if max_acceleration is not specified");
}
if (std::isnan(min_acceleration_))
{
min_acceleration_ = -max_acceleration_;
}
throw std::invalid_argument("Jerk limits cannot be negative." + error);
}
if (has_jerk_limits_)

if (min_jerk_ > max_jerk_)
{
if (std::isnan(max_jerk_))
{
throw std::runtime_error("Cannot apply jerk limits if max_jerk is not specified");
}
if (std::isnan(min_jerk_))
{
min_jerk_ = -max_jerk_;
}
throw std::invalid_argument("Min jerk cannot be greater than max jerk.");
}
}

double SpeedLimiter::limit(double & v, double v0, double v1, double dt)
{
const double tmp = v;

limit_jerk(v, v0, v1, dt);
limit_acceleration(v, v0, dt);
limit_velocity(v);
if (!std::isnan(min_jerk_) && !std::isnan(max_jerk_)) limit_jerk(v, v0, v1, dt);
if (!std::isnan(min_acceleration_) && !std::isnan(max_acceleration_))
limit_acceleration(v, v0, dt);
if (!std::isnan(min_velocity_) && !std::isnan(max_velocity_)) limit_velocity(v);

return tmp != 0.0 ? v / tmp : 1.0;
}
Expand All @@ -89,27 +110,31 @@ double SpeedLimiter::limit_velocity(double & v)
{
const double tmp = v;

if (has_velocity_limits_)
{
v = std::clamp(v, min_velocity_, max_velocity_);
}
v = std::clamp(std::fabs(v), min_velocity_, max_velocity_);

v *= tmp >= 0 ? 1 : -1;
return tmp != 0.0 ? v / tmp : 1.0;
}

double SpeedLimiter::limit_acceleration(double & v, double v0, double dt)
{
const double tmp = v;

if (has_acceleration_limits_)
double dv_min;
double dv_max;
if (std::fabs(v) >= std::fabs(v0))
{
const double dv_min = min_acceleration_ * dt;
const double dv_max = max_acceleration_ * dt;

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

v = v0 + dv;
dv_min = min_acceleration_ * dt;
dv_max = max_acceleration_ * dt;
}
else
{
dv_min = min_deceleration_ * dt;
dv_max = max_deceleration_ * dt;
}
double dv = std::clamp(std::fabs(v - v0), dv_min, dv_max);
dv *= (v - v0 >= 0 ? 1 : -1);
v = v0 + dv;

return tmp != 0.0 ? v / tmp : 1.0;
}
Expand All @@ -118,20 +143,17 @@ 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;
const double dv = v - v0;
const double dv0 = v0 - v1;

const double da_min = min_jerk_ * dt2;
const double da_max = max_jerk_ * dt2;
const double dt2 = 2. * dt * dt;

const double da = std::clamp(dv - dv0, da_min, da_max);
const double da_min = min_jerk_ * dt2;
const double da_max = max_jerk_ * dt2;

v = v0 + dv0 + da;
}
double da = std::clamp(std::fabs(dv - dv0), da_min, da_max);
da *= (dv - dv0 >= 0 ? 1 : -1);
v = v0 + dv0 + da;

return tmp != 0.0 ? v / tmp : 1.0;
}
Expand Down
Loading

0 comments on commit 55ee7c6

Please sign in to comment.