sr_math_utils.hpp

Go to the documentation of this file.

/*
* Copyright 2011 Shadow Robot Company Ltd.
*
* This program is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation version 2 of the License.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _SR_MATH_UTILS_HPP_
#define _SR_MATH_UTILS_HPP_

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/thread/detail/singleton.hpp>

#include <utility>

#include <ros/ros.h>

namespace sr_math_utils
{
static const double pi = 3.14159265;

static inline double to_rad(double degrees)
{
  return degrees * 0.017453292519943295;
}

static inline double to_degrees(double rad)
{
  return rad * 57.295779513082323;
}

static inline int ipow(int base, int exp)
{
  int result = 1;
  while (exp)
  {
    if (exp & 1)
    {
      result *= base;
    }
    exp >>= 1;
    base *= base;
  }

  return result;
}

static inline bool is_bit_mask_index_true(int64_t bit_mask, int index)
{
  if (bit_mask & (((int64_t) 1) << index))
  {
    return true;
  }
  else
  {
    return false;
  }
}

static inline bool is_bit_mask_index_false(int64_t bit_mask, int index)
{
  return !(is_bit_mask_index_true(bit_mask, index));
}

static inline uint64_t counter_with_overflow(uint64_t full_value, uint16_t new_value)
{
  uint16_t last_value = full_value & 0xFFFF;  // Split the full value into the lower part
  full_value &= (uint64_t) 0xFFFFFFFFFFFF0000LL;  // and the overflow part

  if (new_value < last_value)
  {  // if we overflowed
    full_value += (uint64_t) 0x0000000000010000LL;
  }  // then count the overflow

  full_value |= (uint64_t) new_value;  // replace the bottom 16 bits with their new value

  return full_value;
}

static inline double linear_interpolate_(double x,
                                         double x0, double y0,
                                         double x1, double y1)
{
  // y1 - y0
  double y = y1 - y0;
  // (y1 - y0) / (x1 - x0)
  y /= (x1 - x0);
  //  (x-x0)*((y1-y0)/(x1-x0))
  y *= (x - x0);
  //  y0 + (x-x0)*((y1-y0)/(x1-x0))
  y += y0;

  return y;
}

namespace filters
{

class LowPassFilter
{
public:
  explicit LowPassFilter(double tau = 0.05)
          : is_first(true), dt(0.0),
            timestamp_1(0.0), q_prev(0.0),
            tau(tau), value_derivative(0.0, 0.0)
  {
  };

  std::pair<double, double> compute(double q, double timestamp)
  {
    if (is_first)
    {
      q_prev = q;
      // initializing dt to 1ms
      dt = 0.001;

      is_first = false;
    }
    else
    {
      dt = timestamp - timestamp_1;
    }

    double alpha = 0.0;
    if (tau > 0.0 && dt > 0.0)
      alpha = exp(-dt / tau);

    // filtering the input
    value_derivative.first = alpha * value_derivative.first + (1 - alpha) * q;
    // filtering the derivative
    value_derivative.second = alpha * value_derivative.second + (1 - alpha) / dt * (value_derivative.first - q_prev);

    q_prev = value_derivative.first;
    timestamp_1 = timestamp;

    return value_derivative;
  };

private:
  bool is_first;
  double tau, dt, timestamp_1, q_prev;

  std::pair<double, double> value_derivative;
};

class AlphaBetaFilter
{
public:
  explicit AlphaBetaFilter(double alpha = 0.85, double beta = 0.05)
          : a(alpha), b(beta),
            xk_1(0.0), vk_1(0.0), xk(0.0), vk(0.0), rk(0.0),
            dt(0.0), timestamp_1(0.0)
  {
  };

  std::pair<double, double> compute(double xm, double timestamp)
  {
    dt = timestamp - timestamp_1;

    xk = xk_1 + (vk_1 * dt);
    vk = vk_1;

    rk = xm - xk;

    xk += a * rk;
    vk += (b * rk) / dt;

    value_derivative.first = xk;
    value_derivative.second = vk;

    xk_1 = xk;
    vk_1 = vk;
    timestamp_1 = timestamp;

    return value_derivative;
  };

protected:
  double a, b;
  double xk_1, vk_1, xk, vk, rk;
  double dt, timestamp_1;

  std::pair<double, double> value_derivative;
};
}  // namespace filters

static inline int sign(double x)
{
  return x < 0.0 ? -1 : 1;
}
//  FILTERS  //


//  RANDOM   //

class Random_
{
public:
  Random_() :
          dist(boost::uniform_real<>(0.0, 1.0))
  {
  };

  template<typename T>
  T generate(T min = static_cast<T> (0), T max = static_cast<T> (1))
  {
    return static_cast<T> (min + dist(gen) * (max - min));
  }

protected:
  boost::mt19937 gen;
  boost::uniform_real<> dist;
};

typedef boost::detail::thread::singleton<class Random_> Random;
}  // namespace sr_math_utils

/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
 */

#endif