ik.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/state.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/box2d/collision.h>
#include <se2ez/core/ik.h>

using namespace se2ez;

///
/// IKSolver
///

IKSolver::IKSolver(const RobotPtr &robot, unsigned int frames)
  : n_(robot->getNumJoints())
  , t_(frames)
  , robot_(robot)
  , min_(tf::EigenToKDLVec(robot->getLowerLimits()))
  , max_(tf::EigenToKDLVec(robot->getUpperLimits()))
  , weights_(KDL::MatrixXd::Identity(n_, n_))
  , tasks_(KDL::MatrixXd::Identity(6 * t_, 6 * t_))
{
    // We are doing things in 2D, can ignore unimportant components.
    for (unsigned int i = 0; i < t_; ++i)
    {
        setTask(i, POS_Z, false);
        setTask(i, ROT_X, false);
        setTask(i, ROT_Y, false);
    }
}

void IKSolver::setRestarts(unsigned int restarts)
{
    restarts_ = restarts + 1;  ///< Add one since we do at least one iteration without random restart.
}

void IKSolver::setJointWeight(const std::string &frame, const Eigen::VectorXd &weights)
{
    const auto &data = robot_->getFrameDataConst(frame);

    for (unsigned int i = 0; i < data->size; ++i)
        if (weights[i] < 0)
        {
            SE2EZ_ERROR("Tried to set joint weights with value less than 0, %1%", tf::printVector(weights));
            return;
        }

    for (unsigned int i = 0; i < data->size; ++i)
        weights_(data->index + i, data->index + i) = weights[i];
}

void IKSolver::setJointWeights(const Eigen::VectorXd &weights)
{
    for (unsigned int i = 0; i < n_; ++i)
        if (weights[i] < 0)
        {
            SE2EZ_ERROR("Tried to set joint weights with value less than 0, %1%", tf::printVector(weights));
            return;
        }

    for (unsigned int i = 0; i < n_; ++i)
        weights_(i, i) = weights[i];
}

void IKSolver::setLambda(double lambda)
{
    if (lambda < math::eps)
    {
        SE2EZ_ERROR("Tried to set lambda value of %1%, <= 0. Value must be > 0", lambda);
        return;
    }

    lambda_ = lambda;
}

void IKSolver::setRandom(KDL::JntArray &array) const
{
    double v;
    for (unsigned int i = 0; i < n_; ++i)
    {
        KDL::posrandom(v);
        array(i) = min_(i) + (max_(i) - min_(i)) * v;
    }
}

void IKSolver::setRequest(unsigned int task, Request request)
{
    // Set each component of the task with the bitmask.
    setTask(task, POS_X, (request & X_AXIS) == X_AXIS);
    setTask(task, POS_Y, (request & Y_AXIS) == Y_AXIS);
    setTask(task, ROT_Z, (request & ROTATION) == ROTATION);
}

void IKSolver::setTask(unsigned int task, Index index, bool active)
{
    // Set the diagonal entry of the task matrix.
    tasks_(task * 6 + index, task * 6 + index) = (active) ? 1.0 : 0.0;
}

///
/// TreeIK
///

TreeIK::TreeIK(const RobotPtr &robot, const std::vector<std::string> &frames, unsigned int iterations,
               double eps)
  : IKSolver(robot, frames.size())
  , frames_([frames]() {
      std::map<std::string, unsigned int> f;
      for (unsigned int i = 0; i < frames.size(); ++i)
          f.emplace(frames[i], i);

      return f;
  }())
  , tree_(robot->getTree())
  , fk_(tree_)
  , velIK_(tree_, frames)
  , posIK_(tree_, frames, min_, max_, fk_, velIK_, iterations, eps)
{
    velIK_.setWeightTS(tasks_);
    velIK_.setLambda(lambda_);
}

bool TreeIK::setTask(KDL::Frames &goals, const tf::EigenMap<std::string, Eigen::Isometry2d> &frames,
                     const std::map<std::string, Request> &request)
{
    for (const auto &frame : frames_)
        setRequest(frame.second, Request::ALL);

    for (const auto &frame : frames)
    {
        const auto &name = frame.first;
        const auto &entry = frames_.find(name);

        if (entry == frames_.end())
        {
            SE2EZ_ERROR("Frame %1% specified for solve not available!", name);
            return false;
        }

        const unsigned int index = entry->second;

        // Convert goals into KDL type.
        goals.emplace(name, tf::EigenToKDLFrame(frame.second));

        // Set active indices according to request.
        auto req = request.find(name);
        if (req == request.end())
            setRequest(index, Request::ALL);
        else
            setRequest(index, req->second);
    }

    // Set underlying task matrix.
    velIK_.setWeightTS(tasks_);
    return true;
}

bool TreeIK::doIK(KDL::JntArray &in, KDL::JntArray &out, const KDL::Frames &goals)
{
    // If d < 0, then the routine has failed. Random restart.
    double d = -1;
    for (unsigned int i = 0; i < restarts_ && d < 0; ++i)
    {
        // Set to random values on all iterations but the first.
        if (i > 0)
            setRandom(in);

        d = posIK_.CartToJnt(in, goals, out);
        if (d == -2)
        {
            SE2EZ_ERROR("Frame specified for solve not available!");
            return false;
        }
    }

    return d >= 0;
}

bool TreeIK::solve(StatePtr state, const tf::EigenMap<std::string, Eigen::Isometry2d> &frames,
                   const std::map<std::string, Request> &request)
{
    std::unique_lock<std::mutex> lock(mutex_);

    KDL::Frames goals;
    if (!setTask(goals, frames, request))
        return false;

    // Seed with current state to begin.
    KDL::JntArray in(tf::EigenToKDLVec(state->data));
    KDL::JntArray out(n_);

    if (doIK(in, out, goals))
    {
        state->data = tf::KDLToEigenVec(out);
        state->dirty = true;

        return true;
    }

    return false;
}

void TreeIK::setJointWeight(const std::string &frame, const Eigen::VectorXd &weights)
{
    IKSolver::setJointWeight(frame, weights);
    velIK_.setWeightJS(weights_);
}

void TreeIK::setJointWeights(const Eigen::VectorXd &weights)
{
    IKSolver::setJointWeights(weights);
    velIK_.setWeightJS(weights_);
}

void TreeIK::setLambda(double lambda)
{
    IKSolver::setLambda(lambda);
    velIK_.setLambda(lambda_);
}

///
/// ChainIK
///

ChainIK::ChainIK(const RobotPtr &robot, const std::string &frame, unsigned int iterations, double eps)
  : TreeIK(robot, {frame}, iterations, eps), frame_(frame)
{
}

ChainIK::~ChainIK()
{
}

bool ChainIK::solve(StatePtr state, const Eigen::Isometry2d &frame, Request request)
{
    return TreeIK::solve(state, {{frame_, frame}}, {{frame_, request}});
}

///
/// CollisionAwareTreeIK
///

CollisionAwareTreeIK::CollisionAwareTreeIK(const RobotPtr &robot, const std::vector<std::string> &frames,
                                           unsigned int iterations, double eps)
  : TreeIK(robot, frames, iterations, eps)
  , cm_(std::make_shared<Box2DCollisionManager>(robot))
  , state_(std::make_shared<State>(robot))
{
}

bool CollisionAwareTreeIK::doIK(KDL::JntArray &in, KDL::JntArray &out, const KDL::Frames &goals)
{
    // If d < 0, then the routine has failed. Random restart.
    double d = -1;

    KDL::JntArray temp(out.columns());
    for (unsigned int i = 0; i < restarts_; ++i)
    {
        // Set to random values on all iterations but the first.
        if (i > 0)
            setRandom(in);

        d = posIK_.CartToJnt(in, goals, temp);
        if (d == -2)
        {
            SE2EZ_ERROR("Frame specified for solve not available!");
            return false;
        }

        if (d >= 0)
        {
            state_->data = tf::KDLToEigenVec(temp);
            state_->dirty = true;

            if (!cm_->collide(state_))
            {
                out = temp;
                return true;
            }
        }
    }

    return false;
}

///
/// CollisionAwareChainIK
///

CollisionAwareChainIK::CollisionAwareChainIK(const RobotPtr &robot, const std::string &frame,
                                             unsigned int iterations, double eps)
  : CollisionAwareTreeIK(robot, {frame}, iterations, eps), frame_(frame)
{
}

CollisionAwareChainIK::~CollisionAwareChainIK()
{
}

bool CollisionAwareChainIK::solve(StatePtr state, const Eigen::Isometry2d &frame, Request request)
{
    return CollisionAwareTreeIK::solve(state, {{frame_, frame}}, {{frame_, request}});
}