utils.h

Go to the documentation of this file.

 
#ifndef TESSERACT_KINEMATICS_UTILS_H
#define TESSERACT_KINEMATICS_UTILS_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <console_bridge/console.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/utils.h>
#include <tesseract_common/kinematic_limits.h>
 
namespace tesseract_kinematics
{
template <typename FloatType>
using VectorX = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>;
 
class JointGroup;
class ForwardKinematics;
 
void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
                       const Eigen::Isometry3d& change_base,
                       const tesseract_kinematics::ForwardKinematics& kin,
                       const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                       const std::string& link_name,
                       const Eigen::Ref<const Eigen::Vector3d>& link_point);
 
void numericalJacobian(Eigen::Ref<Eigen::MatrixXd> jacobian,
                       const Eigen::Isometry3d& change_base,
                       const JointGroup& joint_group,
                       const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                       const std::string& link_name,
                       const Eigen::Ref<const Eigen::Vector3d>& link_point);
 
bool solvePInv(const Eigen::Ref<const Eigen::MatrixXd>& A,
               const Eigen::Ref<const Eigen::VectorXd>& b,
               Eigen::Ref<Eigen::VectorXd> x);
 
bool dampedPInv(const Eigen::Ref<const Eigen::MatrixXd>& A,
                Eigen::Ref<Eigen::MatrixXd> P,
                double eps = 0.011,
                double lambda = 0.01);
 
bool isNearSingularity(const Eigen::Ref<const Eigen::MatrixXd>& jacobian, double threshold = 0.01);
 
struct ManipulabilityEllipsoid
{
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  Eigen::VectorXd eigen_values;
 
  double measure{ 0 };
 
  double condition{ 0 };
 
  double volume{ 0 };
};
 
struct Manipulability
{
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  ManipulabilityEllipsoid m;
 
  ManipulabilityEllipsoid m_linear;
 
  ManipulabilityEllipsoid m_angular;
 
  ManipulabilityEllipsoid f;
 
  ManipulabilityEllipsoid f_linear;
 
  ManipulabilityEllipsoid f_angular;
};
 
Manipulability calcManipulability(const Eigen::Ref<const Eigen::MatrixXd>& jacobian);
 
template <typename FloatType>
inline void getRedundantSolutionsHelper(std::vector<VectorX<FloatType>>& redundant_sols,
                                        const Eigen::Ref<const Eigen::VectorXd>& sol,
                                        const Eigen::MatrixX2d& limits,
                                        std::vector<Eigen::Index>::const_iterator current_index,
                                        std::vector<Eigen::Index>::const_iterator end_index)
{
  double val{ 0 };
  for (; current_index != end_index; ++current_index)
  {
    if (std::isinf(limits(*current_index, 0)))
    {
      std::stringstream ss;
      ss << "Lower limit of joint " << *current_index << " is infinite; no redundant solutions will be generated\n";
      CONSOLE_BRIDGE_logWarn(ss.str().c_str());
    }
    else
    {
      val = sol[*current_index];
      while ((val -= (2.0 * M_PI)) > limits(*current_index, 0) ||
             tesseract_common::almostEqualRelativeAndAbs(val, limits(*current_index, 0)))
      {
        // It not guaranteed that the provided solution is within limits so this check is needed
        if (val < limits(*current_index, 1) ||
            tesseract_common::almostEqualRelativeAndAbs(val, limits(*current_index, 1)))
        {
          Eigen::VectorXd new_sol = sol;
          new_sol[*current_index] = val;
 
          if (tesseract_common::satisfiesLimits<double>(new_sol, limits))
          {
            tesseract_common::enforceLimits<double>(new_sol, limits);
            redundant_sols.push_back(new_sol.template cast<FloatType>());
          }
 
          getRedundantSolutionsHelper<FloatType>(redundant_sols, new_sol, limits, current_index + 1, end_index);
        }
      }
    }
 
    if (std::isinf(limits(*current_index, 1)))
    {
      std::stringstream ss;
      ss << "Upper limit of joint " << *current_index << " is infinite; no redundant solutions will be generated\n";
      CONSOLE_BRIDGE_logWarn(ss.str().c_str());
    }
    else
    {
      val = sol[*current_index];
      while ((val += (2.0 * M_PI)) < limits(*current_index, 1) ||
             tesseract_common::almostEqualRelativeAndAbs(val, limits(*current_index, 1)))
      {
        // It not guaranteed that the provided solution is within limits so this check is needed
        if (val > limits(*current_index, 0) ||
            tesseract_common::almostEqualRelativeAndAbs(val, limits(*current_index, 0)))
        {
          Eigen::VectorXd new_sol = sol;
          new_sol[*current_index] = val;
 
          if (tesseract_common::satisfiesLimits<double>(new_sol, limits))
          {
            tesseract_common::enforceLimits<double>(new_sol, limits);
            redundant_sols.push_back(new_sol.template cast<FloatType>());
          }
 
          getRedundantSolutionsHelper<FloatType>(redundant_sols, new_sol, limits, current_index + 1, end_index);
        }
      }
    }
  }
}
 
template <typename FloatType>
inline std::vector<VectorX<FloatType>> getRedundantSolutions(const Eigen::Ref<const VectorX<FloatType>>& sol,
                                                             const Eigen::MatrixX2d& limits,
                                                             const std::vector<Eigen::Index>& redundancy_capable_joints)
{
  if (redundancy_capable_joints.empty())
    return {};
 
  for (const Eigen::Index& idx : redundancy_capable_joints)
  {
    if (idx >= sol.size())
    {
      std::stringstream ss;
      ss << "Redundant joint index " << idx << " is greater than or equal to the joint state size (" << sol.size()
         << ")";
      throw std::runtime_error(ss.str());
    }
  }
 
  std::vector<VectorX<FloatType>> redundant_sols;
  getRedundantSolutionsHelper<FloatType>(redundant_sols,
                                         sol.template cast<double>(),
                                         limits,
                                         redundancy_capable_joints.begin(),
                                         redundancy_capable_joints.end());
  return redundant_sols;
}
 
template <typename FloatType>
inline bool isValid(const std::array<FloatType, 6>& qs)
{
  for (const auto& q : qs)
    if (!std::isfinite(q))
      return false;
 
  return true;
}
 
template <typename FloatType>
inline void harmonizeTowardZero(Eigen::Ref<VectorX<FloatType>> qs,
                                const std::vector<Eigen::Index>& redundancy_capable_joints)
{
  const static auto pi = FloatType(M_PI);
  const static auto two_pi = FloatType(2.0 * M_PI);
 
  for (const auto& i : redundancy_capable_joints)
  {
    const auto diff = std::fmod(qs[i] + pi, two_pi);
    qs[i] = (diff < 0) ? (diff + pi) : (diff - pi);
  }
}
 
template <typename FloatType>
inline void harmonizeTowardMedian(Eigen::Ref<VectorX<FloatType>> qs,
                                  const std::vector<Eigen::Index>& redundancy_capable_joints,
                                  const Eigen::Ref<const Eigen::Matrix<FloatType, Eigen::Dynamic, 2>>& position_limits)
{
  const static auto pi = FloatType(M_PI);
  const static auto two_pi = FloatType(2.0 * M_PI);
 
  for (const auto& i : redundancy_capable_joints)
  {
    const auto mean = (position_limits(i, 0) + position_limits(i, 1)) / 2.0;
    const auto diff = std::fmod((qs[i] - mean) + pi, two_pi);
    const auto vv = (diff < 0) ? (diff + pi) : (diff - pi);
    qs[i] = (vv + mean);
  }
}
 
}  // namespace tesseract_kinematics
#endif  // TESSERACT_KINEMATICS_UTILS_H