kinematic_limits.h

Go to the documentation of this file.

 
#ifndef TESSERACT_COMMON_KINEMATIC_LIMITS_H
#define TESSERACT_COMMON_KINEMATIC_LIMITS_H
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/serialization/export.hpp>
#include <boost/serialization/base_object.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
namespace tesseract_common
{
struct Serialization;
 
struct KinematicLimits
{
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  Eigen::MatrixX2d joint_limits;
 
  Eigen::MatrixX2d velocity_limits;
 
  Eigen::MatrixX2d acceleration_limits;
 
  Eigen::MatrixX2d jerk_limits;
 
  void resize(Eigen::Index size);
 
  bool operator==(const KinematicLimits& rhs) const;
  bool operator!=(const KinematicLimits& rhs) const;
 
private:
  friend class boost::serialization::access;
  friend struct tesseract_common::Serialization;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int version);  // NOLINT
};
 
template <typename FloatType>
bool isWithinLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& values,
                    const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits)
{
  auto p = values.array();
  auto l0 = limits.col(0).array();
  auto l1 = limits.col(1).array();
  return (!(p > l1).any() && !(p < l0).any());
}
 
template <typename FloatType>
bool satisfiesLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& values,
                     const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits,
                     const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& max_diff,
                     const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& max_rel_diff)
{
  auto p = values.array();
  auto l0 = limits.col(0).array();
  auto l1 = limits.col(1).array();
  auto md = max_diff.array();
  auto mrd = max_rel_diff.array();
 
  auto lower_diff_abs = (p - l0).abs();
  auto lower_diff = (lower_diff_abs <= md);
  auto lower_relative_diff = (lower_diff_abs <= mrd * (p.abs().max)(l0.abs()));
  auto lower_check = p > l0 || lower_diff || lower_relative_diff;
 
  auto upper_diff_abs = (p - l1).abs();
  auto upper_diff = (upper_diff_abs <= md);
  auto upper_relative_diff = (upper_diff_abs <= mrd * (p.abs().max)(l1.abs()));
  auto upper_check = p < l1 || upper_diff || upper_relative_diff;
 
  return (lower_check.all() && upper_check.all());
}
 
template <typename FloatType>
bool satisfiesLimits(const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 1>>& values,
                     const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits,
                     FloatType max_diff = static_cast<FloatType>(1e-6),
                     FloatType max_rel_diff = std::numeric_limits<FloatType>::epsilon())
{
  const auto eigen_max_diff = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>::Constant(values.size(), max_diff);
  const auto eigen_max_rel_diff = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>::Constant(values.size(), max_rel_diff);
  // NOLINTNEXTLINE(clang-analyzer-core.uninitialized.UndefReturn)
  return satisfiesLimits<FloatType>(values, limits, eigen_max_diff, eigen_max_rel_diff);
}
 
template <typename FloatType>
void enforceLimits(Eigen::Ref<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> values,
                   const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& limits)
{
  values = ((values.array().min)(limits.col(1).array()).max)(limits.col(0).array());
}
}  // namespace tesseract_common
 
BOOST_CLASS_EXPORT_KEY(tesseract_common::KinematicLimits)
 
#endif  // TESSERACT_COMMON_KINEMATIC_LIMITS_H