constraint.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <algorithm>

#include <se2ez/core/log.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/state.h>

#include <se2ez/plan/space.h>
#include <se2ez/plan/constraint.h>

#include <ompl/base/spaces/constraint/ConstrainedStateSpace.h>

const int debugJ = 0;
const int debugF = 0;
const int debugP = 0;
const int debugL = 0;

using namespace se2ez;

plan::FrameConstraint::FrameConstraint(const std::string &name, const RobotPtr &robot,
                                       const std::string &frame, const std::string &base,
                                       unsigned int dimension, unsigned int leaf)
  : ompl::base::Foliation(robot->getNumJoints(), dimension, leaf)
  , name_(name)
  , robot_(robot)
  , frame_(frame)
  , base_(base)
  , state_(std::make_shared<State>(robot))
{
    // setTolerance(1e-3);
    setMaxIterations(15);

    if (!robot->getFrame(frame_))
    {
        SE2EZ_ERROR("%1% is not a frame in the robot!", frame_);
        throw std::invalid_argument(log::format("%1% is not a frame in the robot!", frame_));
    }

    if (!robot->getFrame(base_))
    {
        SE2EZ_ERROR("%1% is not a frame in the robot!", base_);
        throw std::invalid_argument(log::format("%1% is not a frame in the robot!", base_));
    }
}

const std::pair<Eigen::Isometry2d, Eigen::Isometry2d>
plan::FrameConstraint::getPose(const ompl::base::State *state) const
{
    const auto &as = state->as<ompl::base::ConstrainedStateSpace::StateType>()
                         ->getState()
                         ->as<plan::StateSpace::StateType>()
                         ->state;

    // as->fk(robot_);
    return std::make_pair(as->getPose(robot_, frame_), as->getPose(robot_, base_));
}

const std::pair<Eigen::Isometry2d, Eigen::Isometry2d>
plan::FrameConstraint::getPose(const Eigen::Ref<const Eigen::VectorXd> &x) const
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    state_->data = x;
    state_->dirty = true;
    // state_->fk(robot_);

    return std::make_pair(state_->getPose(robot_, frame_), state_->getPose(robot_, base_));
}

void plan::FrameConstraint::function(const ompl::base::State *state, Eigen::Ref<Eigen::VectorXd> out) const
{
    Eigen::Isometry2d frame, base;
    std::tie(frame, base) = getPose(state);
    functionInternal(frame, base, out);

    if (debugF)
    {
        const auto &as = state->as<ompl::base::ConstrainedStateSpace::StateType>()
                             ->getState()
                             ->as<plan::StateSpace::StateType>()
                             ->state;

        SE2EZ_DEBUG("CONSTRAINT FUNCTION DEBUG");
        SE2EZ_DEBUG("STATE\n %1%", tf::printVector(as->data));
        SE2EZ_DEBUG("BASE\n %1%", tf::printFrame(base));
        SE2EZ_DEBUG("POSE\n %1%", tf::printFrame(frame));
        SE2EZ_DEBUG("VALUE\n %1%\n", tf::printVector(out));
    }
}

void plan::FrameConstraint::function(const Eigen::Ref<const Eigen::VectorXd> &x,
                                     Eigen::Ref<Eigen::VectorXd> out) const
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    Eigen::Isometry2d frame, base;
    std::tie(frame, base) = getPose(x);
    functionInternal(frame, base, out);
}

void plan::FrameConstraint::jacobian(const ompl::base::State *state, Eigen::Ref<Eigen::MatrixXd> out) const
{
    const auto &as = state->as<ompl::base::ConstrainedStateSpace::StateType>()
                         ->getState()
                         ->as<plan::StateSpace::StateType>()
                         ->state;

    Eigen::Isometry2d frame, base;
    std::tie(frame, base) = getPose(state);

    jacobianInternal(frame, base, as->getJacobian(robot_, frame_), out);

    if (debugJ)
    {
        SE2EZ_DEBUG("CONSTRAINT JACOBIAN DEBUG");
        SE2EZ_DEBUG("ANALYTIC\n %1%", tf::printMatrix(out));
        Eigen::MatrixXd copy = out;

        ompl::base::Constraint::jacobian(state, out);
        SE2EZ_DEBUG("NUMERIC\n %1%", tf::printMatrix(out));

        Eigen::MatrixXd diff = copy - out;
        SE2EZ_DEBUG("DIFFERENCE\n %1%", tf::printMatrix(diff));
        SE2EZ_DEBUG("NORM\n %1%\n", diff.norm());
    }
}

void plan::FrameConstraint::jacobian(const Eigen::Ref<const Eigen::VectorXd> &x,
                                     Eigen::Ref<Eigen::MatrixXd> out) const
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    Eigen::Isometry2d frame, base;
    std::tie(frame, base) = getPose(x);

    jacobianInternal(frame, base, state_->getJacobian(robot_, frame_), out);

    if (debugJ)
    {
        SE2EZ_DEBUG("CONSTRAINT JACOBIAN DEBUG");
        SE2EZ_DEBUG("ANALYTIC\n %1%", tf::printMatrix(out));
        Eigen::MatrixXd copy = out;

        ompl::base::Constraint::jacobian(x, out);
        SE2EZ_DEBUG("NUMERIC\n %1%", tf::printMatrix(out));

        Eigen::MatrixXd diff = copy - out;
        SE2EZ_DEBUG("DIFFERENCE\n %1%", tf::printMatrix(diff));
        SE2EZ_DEBUG("NORM\n %1%\n", diff.norm());
    }
}

bool plan::FrameConstraint::project(ompl::base::State *state) const
{
    const auto &as = state->as<ompl::base::ConstrainedStateSpace::StateType>()
                         ->getState()
                         ->as<plan::StateSpace::StateType>()
                         ->state;

    unsigned int iter = 0;
    double norm = 0;
    Eigen::VectorXd f(getCoDimension());
    Eigen::MatrixXd j(getCoDimension(), n_);

    // const double squaredTolerance = tolerance_ * tolerance_;
    const double squaredTolerance = tolerance_;

    function(state, f);
    while ((norm = f.lpNorm<Eigen::Infinity>()) > squaredTolerance && iter++ < maxIterations_)
    {
        jacobian(state, j);
        as->data -= j.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(f);
        // as->data -= j.colPivHouseholderQr().solve(f);
        as->dirty = true;

        function(state, f);
    }

    return norm < squaredTolerance;
}

bool plan::FrameConstraint::project(Eigen::Ref<Eigen::VectorXd> x) const
{
    std::lock_guard<std::recursive_mutex> lock(mutex_);

    // Newton's method
    unsigned int iter = 0;
    double norm = 0;

    Eigen::VectorXd f(getCoDimension());
    Eigen::MatrixXd j(getCoDimension(), n_);

    const double squaredTolerance = tolerance_;

    function(x, f);
    while ((norm = f.lpNorm<Eigen::Infinity>()) > squaredTolerance && iter++ < maxIterations_)
    {
        jacobian(x, j);
        x -= j.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(f);
        // x -= j.colPivHouseholderQr().solve(f);
        function(x, f);
    }

    return norm < squaredTolerance;
}

double plan::FrameConstraint::distance(const ompl::base::State *state) const
{
    Eigen::VectorXd f(getCoDimension());
    function(state, f);
    return f.norm();
}

bool plan::FrameConstraint::isSatisfied(const ompl::base::State *state) const
{
    Eigen::VectorXd f(getCoDimension());
    function(state, f);
    return f.allFinite() && f.squaredNorm() <= tolerance_ * tolerance_;
}

void plan::FrameConstraint::setDrawOffset(const Eigen::Ref<const Eigen::Vector2d> &v)
{
    draw_ = v;
}

const Eigen::Vector2d &plan::FrameConstraint::getDrawOffset() const
{
    return draw_;
}

///
/// LineConstraint
///

plan::LineConstraint::LineConstraint(const std::string &name, const RobotPtr &robot, const std::string &frame,
                                     const std::string &base, const Eigen::Vector2d &a,
                                     const Eigen::Vector2d &b)
  : FrameConstraint(name, robot, frame, base)
  , a_(a)
  , b_(b)
  , v_((b_ - a_).normalized())
  , abn_((b_ - a_).norm())
{
}

ompl::base::FoliationPtr plan::LineConstraint::copy() const
{
    return std::make_shared<LineConstraint>(name_, robot_, frame_, base_, a_, b_);
}

Eigen::VectorXd plan::LineConstraint::getTransversal(const Eigen::Ref<const Eigen::VectorXd> &state) const
{
    Eigen::Isometry2d frame, base;
    std::tie(frame, base) = getPose(state);

    Result r;
    getPlace(frame, base, r);

    if (debugL)
    {
        SE2EZ_DEBUG("CONSTRAINT %1%", name_);
        SE2EZ_DEBUG("STATE\n %1%", tf::printVector(state));
        SE2EZ_DEBUG("BASE\n %1%: %2%", base_, tf::printFrame(base));
        SE2EZ_DEBUG("POSE\n %1%: %2%", frame_, tf::printFrame(frame));

        std::string p = "LINE";
        if (r.place == A)
            p = "A";
        if (r.place == B)
            p = "B";

        SE2EZ_INFORM(" A: %1%, AT: %2%", tf::printVector(a_), tf::printVector(r.at));
        SE2EZ_INFORM(" B: %1%, BT: %2%", tf::printVector(b_), tf::printVector(r.bt));
        SE2EZ_INFORM(" V: %1%, VT: %2%", tf::printVector(v_), tf::printVector(r.vt));
        SE2EZ_INFORM("CA: %1%", tf::printVector(r.ca));
        SE2EZ_INFORM("CV: %1%", r.cv);
        SE2EZ_INFORM(" P: %1%", p);
    }

    return math::toVector(math::clamp(r.cv / abn_, 0., 1.));
}

ompl::base::ConstraintPtr
plan::LineConstraint::getLeaf(const Eigen::Ref<const Eigen::VectorXd> &transversal) const
{
    double cv = transversal[0] * abn_;

    Eigen::Vector2d point;
    if (cv < 0)
        point = a_;
    else if (cv > abn_)
        point = b_;
    else
        point = a_ + cv * v_;

    if (debugL)
    {
        SE2EZ_INFORM("TRANSV: %1%", tf::printVector(transversal));
        SE2EZ_INFORM("LEAF: %1%", tf::printVector(point));
    }

    return std::make_shared<PointConstraint>(name_ + log::format("_leaf[%1%]", cv), robot_, frame_, base_,
                                             point);
}

ompl::base::FoliationPtr plan::LineConstraint::getFrond(const Eigen::Ref<const Eigen::VectorXd> &a,
                                                        const Eigen::Ref<const Eigen::VectorXd> &b) const
{
    double acv = a[0] * abn_, bcv = b[0] * abn_;

    Eigen::Vector2d pointA, pointB;
    if (acv < 0)
        pointA = a_;
    else if (acv > abn_)
        pointA = b_;
    else
        pointA = a_ + acv * v_;

    if (bcv < 0)
        pointB = a_;
    else if (bcv > abn_)
        pointB = b_;
    else
        pointB = a_ + bcv * v_;

    if (debugL)
    {
        SE2EZ_INFORM("TRANSVA: %1%", tf::printVector(a));
        SE2EZ_INFORM("TRANSVB: %1%", tf::printVector(b));
        SE2EZ_INFORM("LEAFA: %1%", tf::printVector(pointA));
        SE2EZ_INFORM("LEAFB: %1%", tf::printVector(pointB));
    }

    return std::make_shared<LineConstraint>(name_ + log::format("_frond[%1%,%2%]", acv, bcv), robot_, frame_,
                                            base_, pointA, pointB);
}

void plan::LineConstraint::functionInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                            Eigen::Ref<Eigen::VectorXd> out) const
{
    Result r;
    getPlace(pose, base, r);

    switch (r.place)
    {
        case A:
            out[0] = r.ca.squaredNorm();
            break;
        case B:
            out[0] = r.cb.squaredNorm();
            break;
        case LINE:
            out[0] = r.ca.squaredNorm() - pow(r.cv, 2);
            break;
    }
}

void plan::LineConstraint::jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                            const Eigen::MatrixXd &jac, Eigen::Ref<Eigen::MatrixXd> out) const
{
    Result r;
    getPlace(pose, base, r);

    switch (r.place)
    {
        case A:
            out = 2 * r.ca.transpose() * jac.block(0, 0, 2, n_);
            break;
        case B:
            out = 2 * r.cb.transpose() * jac.block(0, 0, 2, n_);
            break;
        case LINE:
            out = 2 * (r.ca - r.cv * r.vt).transpose() * jac.block(0, 0, 2, n_);
            break;
    }
}

void plan::LineConstraint::getPlace(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                    Result &r) const
{
    r.at = base * a_;
    r.bt = base * b_;
    r.vt = base.rotation() * v_;

    r.ca = pose.translation() - r.at;
    r.cb = pose.translation() - r.bt;
    r.cv = r.ca.dot(r.vt);
    if (r.cv < 0)
        r.place = A;
    else if (r.cv > abn_)
        r.place = B;
    else
        r.place = LINE;

    if (debugP)
    {
        std::string p = "LINE";
        if (r.place == A)
            p = "A";
        if (r.place == B)
            p = "B";

        SE2EZ_INFORM(" A: %1% %2%", tf::printVector(a_), tf::printVector(r.at));
        SE2EZ_INFORM(" B: %1% %2%", tf::printVector(b_), tf::printVector(r.bt));
        SE2EZ_INFORM(" V: %1% %2%", tf::printVector(v_), tf::printVector(r.vt));
        SE2EZ_INFORM("CA: %1%", tf::printVector(r.ca));
        SE2EZ_INFORM("CV: %1%", r.cv);
        SE2EZ_INFORM(" P: %1%", p);
    }
}

void plan::LineConstraint::getEndpoints(Eigen::Ref<Eigen::VectorXd> a, Eigen::Ref<Eigen::VectorXd> b,
                                        const StatePtr &state) const
{
    auto base = state->getPose(robot_, base_);

    a = base * a_;
    b = base * b_;
}

///
/// PointConstraint
///

plan::PointConstraint::PointConstraint(const std::string &name, const RobotPtr &robot,
                                       const std::string &frame, const std::string &base,
                                       const Eigen::Vector2d &p)
  : FrameConstraint(name, robot, frame, base), p_(p)
{
}

ompl::base::FoliationPtr plan::PointConstraint::copy() const
{
    return std::make_shared<PointConstraint>(name_, robot_, frame_, base_, p_);
}

Eigen::VectorXd plan::PointConstraint::getTransversal(const Eigen::Ref<const Eigen::VectorXd> &) const
{
    return math::toVector(0);
}

ompl::base::ConstraintPtr plan::PointConstraint::getLeaf(const Eigen::Ref<const Eigen::VectorXd> &) const
{
    // copy baby!
    return copy();
}

ompl::base::FoliationPtr plan::PointConstraint::getFrond(const Eigen::Ref<const Eigen::VectorXd> &,
                                                         const Eigen::Ref<const Eigen::VectorXd> &) const
{
    // copy baby!
    return copy();
}

void plan::PointConstraint::functionInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                             Eigen::Ref<Eigen::VectorXd> out) const
{
    const auto &p = base * p_;

    const auto &ca = pose.translation() - p;
    out[0] = ca.squaredNorm();
}

void plan::PointConstraint::jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                             const Eigen::MatrixXd &jac,
                                             Eigen::Ref<Eigen::MatrixXd> out) const
{
    const auto &p = base * p_;

    const auto &ca = pose.translation() - p;
    out = 2 * ca.transpose() * jac.block(0, 0, 2, n_);
}

void plan::PointConstraint::getPoint(Eigen::Ref<Eigen::VectorXd> a, const StatePtr &state) const
{
    auto base = state->getPose(robot_, base_);
    a = base * p_;
}

///
/// CircleConstraint
///

plan::CircleConstraint::CircleConstraint(const std::string &name, const RobotPtr &robot,
                                         const std::string &frame, const std::string &base,
                                         const Eigen::Vector2d &p, const Eigen::Vector2d &r,
                                         unsigned int exponent)
  : FrameConstraint(name, robot, frame, base), p_(p), r_(r), exp_(exponent)
{
    SE2EZ_ERROR("CircleConstraint is currently under development! Use at your own risk.");
}

double plan::CircleConstraint::getAngle(const Eigen::Isometry2d &frame, const Eigen::Isometry2d &base) const
{
    auto p = base * p_;
    double theta = std::atan2(frame.translation()[1] / r_[1] - p[1], frame.translation()[0] / r_[0] - p[0]);
    return math::angleNormalize(theta - tf::getRotation(base));
}

ompl::base::FoliationPtr plan::CircleConstraint::copy() const
{
    return std::make_shared<CircleConstraint>(name_, robot_, frame_, base_, p_, r_, exp_);
}

Eigen::VectorXd plan::CircleConstraint::getTransversal(const Eigen::Ref<const Eigen::VectorXd> &x) const
{
    Eigen::Isometry2d frame, base;
    std::tie(frame, base) = getPose(x);

    double v = getAngle(frame, base);
    return math::toVector(math::remap(-math::pi, math::pi, v, 0, 1));
}

ompl::base::ConstraintPtr plan::CircleConstraint::getLeaf(const Eigen::Ref<const Eigen::VectorXd> &x) const
{
    Eigen::Isometry2d frame, base;
    std::tie(frame, base) = getPose(x);
    auto p = frame.translation() - base.translation();

    return std::make_shared<PointConstraint>(name_, robot_, frame_, base_, p);
}

ompl::base::FoliationPtr plan::CircleConstraint::getFrond(
    const Eigen::Ref<const Eigen::VectorXd> & /*a*/, const Eigen::Ref<const Eigen::VectorXd> & /*b*/) const
{
    // copy baby!
    return copy();
}

void plan::CircleConstraint::functionInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                              Eigen::Ref<Eigen::VectorXd> out) const
{
    const auto &p = base * p_;

    Eigen::Vector2d ca = (pose.translation() - p).cwiseQuotient(r_).cwiseAbs();
    for (unsigned int i = 1; i < exp_; ++i)
        ca = ca.cwiseProduct(ca);
    out[0] = ca.sum() - 1;
}

void plan::CircleConstraint::jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::Isometry2d &base,
                                              const Eigen::MatrixXd &jac,
                                              Eigen::Ref<Eigen::MatrixXd> out) const
{
    const auto &p = base * p_;

    Eigen::Vector2d ca = (pose.translation() - p).cwiseQuotient(r_).cwiseAbs();
    for (unsigned int i = 1; i < exp_ - 1; ++i)
        ca = ca.cwiseProduct(ca);
    out = exp_ * ca.transpose() * jac.block(0, 0, 2, n_);
}

void plan::CircleConstraint::getCircle(Eigen::Ref<Eigen::VectorXd> p, Eigen::Ref<Eigen::VectorXd> r,
                                       double &v, const StatePtr &state) const
{
    auto base = state->getPose(robot_, base_);
    p = base * p_;
    r = r_;
    v = tf::getRotation(base);
}

///
/// OrientationConstraint
///

// plan::OrientationConstraint::OrientationConstraint(const RobotPtr &robot, const std::string &frame,
//                                                    const double v)
//   : FrameConstraint(robot, frame), v_(v)
// {
// }

// void plan::OrientationConstraint::functionInternal(const Eigen::Isometry2d &pose,
//                                                    Eigen::Ref<Eigen::VectorXd> out) const
// {
//     out[0] = math::angleDistance(tf::getRotation(pose), v_);
// }

// void plan::OrientationConstraint::jacobianInternal(const Eigen::Isometry2d &pose, const Eigen::MatrixXd
// &jac,
//                                                    Eigen::Ref<Eigen::MatrixXd> out) const
// {
//     // const auto &ca = pose.translation() - p_;
//     // out = 2 * ca.transpose() * jac.block(0, 0, 2, n_);
// }