joint.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <iostream>

#include <se2ez/core/log.h>
#include <se2ez/core/math.h>
#include <se2ez/core/joint.h>

using namespace se2ez;

const std::string Joint::TYPE_STRINGS[] = {"fixed",     "float",    "flying",    "translate",
                                           "prismatic", "revolute", "continuous"};

Joint::Type Joint::stringToType(const std::string &joint)
{
    for (unsigned int i = 0; i <= TYPE_MAX; ++i)
        if (joint == TYPE_STRINGS[i])
            return static_cast<Type>(i);

    throw std::invalid_argument("Not a valid joint type");
}

const std::string &Joint::typeToString(const Joint::Type &type)
{
    if (type <= TYPE_MAX && type >= 0)
        return TYPE_STRINGS[type];

    throw std::invalid_argument("Not a valid joint type");
}

Joint::Joint(const std::string &name, Type type, const Eigen::VectorXd &lower, const Eigen::VectorXd &upper)
  : type(type)
  , joints([&]() {
      std::vector<KDL::Joint> joints;

      // Create active joints for each joint type, suffix type of joint to name.
      switch (type)
      {
          case FLOAT:
          case FLYING:
              joints.emplace_back(name + ":x", KDL::Joint::TransX);
              joints.emplace_back(name + ":y", KDL::Joint::TransY);
              joints.emplace_back(name + ":t", KDL::Joint::RotZ);
              break;
          case TRANSLATE:
              joints.emplace_back(name + ":x", KDL::Joint::TransX);
              joints.emplace_back(name + ":y", KDL::Joint::TransY);
              break;
          case PRISMATIC:
              joints.emplace_back(name + ":p", KDL::Joint::TransX);
              break;
          case REVOLUTE:
          case CONTINUOUS:
              joints.emplace_back(name + ":r", KDL::Joint::RotZ);
              break;
          case FIXED:
          default:
              break;
      };

      // Add reference fixed joint so this joint's indices can be found easily.
      joints.emplace_back(name, KDL::Joint::None);
      return joints;
  }())
  , continuous(type == FLYING || type == CONTINUOUS)
  , fixed(type == FIXED)
  , n(joints.size() - 1)
  , lower(lower)
  , upper(upper)
  , top_((continuous) ? 1 : 0)
{
    if (upper.size() != lower.size())
        throw std::invalid_argument("Upper and lower limits must be of same dimension.");

    if (continuous && upper.size() != (n - 1))
        throw std::invalid_argument("Continuous joint limits must be of same dimension of joint!");

    else if (not continuous && upper.size() != n)
        throw std::invalid_argument("Joint limits must be of same dimension of joint!");
}

void Joint::enforceBounds(Eigen::Ref<Eigen::VectorXd> q) const
{
    for (unsigned int i = 0; i < n - top_; ++i)
        q[i] = (q[i] > upper[i]) ? upper[i] : ((q[i] < lower[i]) ? lower[i] : q[i]);

    if (continuous)
    {
        double v = fmod(q[n - 1], math::pipi);
        v += (v < -math::pi) ? math::pipi : ((v >= math::pi) ? -math::pipi : 0);

        q[n - 1] = v;
    }
}

void Joint::setRandom(Eigen::Ref<Eigen::VectorXd> q) const
{
    const auto &bounds = getBounds();
    for (unsigned int i = 0; i < n; i++)
        q[i] = math::uniformReal(bounds.first[i], bounds.second[i]);
}

bool Joint::inBounds(const Eigen::Ref<const Eigen::VectorXd> &q) const
{
    bool ret = true;
    for (unsigned int i = 0; i < n - top_ && ret; ++i)
        ret &= (q[i] <= upper[i]) && (q[i] >= lower[i]);

    if (continuous)
        ret &= q[n - 1] < math::pi && q[n - 1] >= -math::pi;

    return ret;
}

double Joint::distance(const Eigen::Ref<const Eigen::VectorXd> &a,
                       const Eigen::Ref<const Eigen::VectorXd> &b) const
{
    double d = (a.head(n - top_) - b.head(n - top_)).norm();
    if (continuous)
        d += 0.5 * math::angleDistance(a[n - 1], b[n - 1]);

    return d;
}

void Joint::interpolate(const Eigen::Ref<const Eigen::VectorXd> &from,
                        const Eigen::Ref<const Eigen::VectorXd> &to, double t,
                        Eigen::Ref<Eigen::VectorXd> state) const
{
    for (unsigned int i = 0; i < n - top_; ++i)
        state[i] = from[i] + (to[i] - from[i]) * t;

    if (continuous)
    {
        const unsigned int i = n - 1;
        double diff = to[i] - from[i];
        if (fabs(diff) <= math::pi)
            state[i] = from[i] + diff * t;
        else
        {
            double &v = state[i];
            diff = ((diff > 0.0) ? math::pipi : -math::pipi) - diff;

            v = from[i] - diff * t;
            v += (v > math::pi) ? -math::pipi : ((v < -math::pi) ? math::pipi : 0);
        }
    }
}

std::pair<Eigen::VectorXd, Eigen::VectorXd> Joint::getBounds() const
{
    if (continuous)
    {
        Eigen::VectorXd nlower(n);
        Eigen::VectorXd nupper(n);

        nlower.head(n - 1) = lower;
        nupper.head(n - 1) = upper;

        nlower[n - 1] = -math::pi;
        nupper[n - 1] = math::pi;

        return std::make_pair(nlower, nupper);
    }

    return std::make_pair(lower, upper);
}

std::string Joint::printDebug() const
{
    std::stringstream ss;
    const auto &bounds = getBounds();
    ss << log::format("J(%1%", typeToString(type));

    if (type != FIXED)
        ss << log::format(" %1% - %2%)", tf::printVector(bounds.first), tf::printVector(bounds.second));
    else
        ss << ")";

    return ss.str();
}