clamp.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 *
 * This file is inspired by Stephane's Caron original implementation as part of
 * lipm_walking_controller <https://github.com/stephane-caron/lipm_walking_controller>
 */
 
#pragma once
 
#include <mc_rtc/logging.h>
 
#include <Eigen/Core>
#include <map>
 
namespace mc_filter
{
 
namespace utils
{
 
inline double clamp(double value, double lower, double upper)
{
  return std::max(lower, std::min(value, upper));
}
 
inline void clampInPlace(double & value, double lower, double upper)
{
  value = clamp(value, lower, upper);
}
 
inline double clampAndWarn(double value, double lower, double upper, const std::string & label)
{
  if(value > upper)
  {
    mc_rtc::log::warning("{} clamped to {}", label, upper);
    ;
    return upper;
  }
  else if(value < lower)
  {
    mc_rtc::log::warning("{} clamped to {}", label, lower);
    return lower;
  }
  else { return value; }
}
 
inline void clampInPlaceAndWarn(double & value, double lower, double upper, const std::string & label)
{
  value = clampAndWarn(value, lower, upper, label);
}
 
template<typename VectorT>
inline VectorT clamp(const VectorT & v, double lower, double upper)
{
  VectorT result(v.size());
  for(unsigned i = 0; i < v.size(); i++) { result(i) = clamp(v(i), lower, upper); }
  return result;
}
 
template<typename VectorT>
inline VectorT clamp(const VectorT & v, const VectorT & lower, const VectorT & upper)
{
  VectorT result(v.size());
  for(unsigned i = 0; i < v.size(); i++) { result(i) = clamp(v(i), lower(i), upper(i)); }
  return result;
}
 
template<typename VectorT>
inline VectorT clampAndWarn(const VectorT & v, const VectorT & lower, const VectorT & upper, const std::string & label)
{
  VectorT result(v.size());
  for(unsigned i = 0; i < v.size(); i++)
  {
    result(i) = clampAndWarn(v(i), lower(i), upper(i), label + " (" + std::to_string(i) + ")");
  }
  return result;
}
 
template<typename VectorT>
inline void clampInPlace(VectorT & v, const VectorT & lower, const VectorT & upper)
{
  for(unsigned i = 0; i < v.size(); i++) { v(i) = clamp(v(i), lower(i), upper(i)); }
}
 
template<typename VectorT>
inline void clampInPlace(VectorT & v, double lower, double upper)
{
  for(unsigned i = 0; i < v.size(); i++) { v(i) = clamp(v(i), lower, upper); }
}
 
template<typename VectorT>
inline void clampInPlaceAndWarn(VectorT & vector, double lower, double upper, const std::string & label)
{
  for(unsigned i = 0; i < vector.size(); i++)
  {
    clampInPlaceAndWarn(vector(i), lower, upper, label + " (" + std::to_string(i) + ")");
  }
}
 
template<typename VectorT>
inline void clampInPlaceAndWarn(VectorT & vector,
                                const VectorT & lower,
                                const VectorT & upper,
                                const std::string & label)
{
  for(unsigned i = 0; i < vector.size(); i++)
  {
    clampInPlaceAndWarn(vector(i), lower(i), upper(i), label + " (" + std::to_string(i) + ")");
  }
}
 
} // namespace utils
} // namespace mc_filter