robot.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <se2ez/core/log.h>
#include <se2ez/core/math.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/state.h>
#include <se2ez/core/collision.h>

#include <se2ez/core/robot.h>

using namespace se2ez;

///
/// FrameMap
///

FrameMap::FrameMap(const Robot & /*robot*/)
{
    // TODO: Create map before hand
}

FrameMap::FrameMap(const RobotPtr & /*robot*/)
{
    // TODO: Create map before hand
}

void FrameMap::dirty()
{
    for (auto &frame : frames)
    {
        frame.second.dirty = true;
        frame.second.jc = false;
    }
}

void FrameMap::insert(FrameMapPtr other)
{
    for (const auto &frame : frames)
        other->getEntry(frame.first) = frame.second;
}

FrameMap::Value &FrameMap::getEntry(const std::string &frame_name)
{
    auto inserted = frames.emplace(frame_name, Value{});
    return inserted.first->second;
}

const FrameMap::Value &FrameMap::getEntryConst(const std::string &frame_name) const
{
    const auto &result = frames.find(frame_name);
    if (result == frames.end())
        throw std::invalid_argument(log::format("Invalid frame name %1%!", frame_name));

    return result->second;
}

std::string FrameMap::printFrames(const RobotPtr &robot) const
{
    return robot->printTree([&](const Robot::FrameDataPtr &frame) -> std::string {
        try
        {
            const auto &entry = getEntryConst(frame->frame->getName());

            std::stringstream ss;
            ss << (entry.dirty ? "D" : "C") << tf::printFrame(entry.pose) << "(";
            for (const auto &tf : entry.geometry)
                ss << tf::printFrame(tf);
            ss << ")";

            return ss.str();
        }
        catch (...)
        {
            return "X[ 0, 0, 0 ] ()";
        }
    });
}

///
/// FrameData
///

Robot::FrameData::FrameData() : root(true), frame(Frame::getRoot())
{
}

Robot::FrameData::FrameData(FramePtr frame, FrameDataPtr parent) : frame(frame), parent(parent)
{
}

Eigen::Ref<Eigen::VectorXd> Robot::FrameData::getValues(Eigen::Ref<Eigen::VectorXd> data) const
{
    return data.segment(index, size);
}

const Eigen::Ref<const Eigen::VectorXd>
Robot::FrameData::getValuesConst(const Eigen::Ref<const Eigen::VectorXd> &data) const
{
    return data.segment(index, size);
}

///
/// Robot
///

Robot::Robot() : acm_(std::make_shared<ACM>())
{
    clear();
}

void Robot::clear()
{
    data_.clear();
    ees_.clear();
    tree_ = KDL::Tree();
    data_.emplace("root", std::make_shared<FrameData>());
    static_.reset();
}

///
/// Frame / Kinematic Tree Updating
///

void Robot::attachFrame(const FramePtr &frame)
{
    attachFrame(frame, "root");
}

void Robot::attachFrame(const FramePtr &frame, const FramePtr &parent)
{
    attachFrame(frame, parent->getName());
}
void Robot::attachFrame(const FramePtr &frame, const std::string &parent_name)
{
    if (frame->getName() == "root")
        throw std::invalid_argument("Cannot add frame named root to tree!");

    if (!inTree(parent_name))
        throw std::invalid_argument(log::format("Parent frame %1 not in tree!", parent_name));

    updateFrame(frame, parent_name);
}

void Robot::detachFrame(const FramePtr &frame)
{
    detachFrame(frame->getName());
}

void Robot::detachFrame(const std::string &frame_name)
{
    if (!inTree(frame_name))
    {
        SE2EZ_WARN("frame %1 not in tree!", frame_name);
        return;
    }

    auto data = data_[frame_name];

    if (data->movable)
    {
        SE2EZ_WARN("frame %1 is movable thus it cannot be removed", frame_name);
        return;
    }

    // removing all the children of this frame
    for (const auto &child : data->children)
        detachFrame(child->frame);

    removeChild(data->parent, frame_name);
    data_.erase(frame_name);
}

void Robot::addFixedRobot(const RobotPtr &frobot, const StatePtr &state, const std::string &scene_name)
{
    for (const auto &fname : frobot->getFrameNames())
    {
        if (fname != "root")
        {
            // The new frame i namespaced based on the scene
            auto nframe_name = scene_name + ":" + fname;
            auto nframe = std::make_shared<Frame>(nframe_name, state->getPose(frobot, fname),
                                                  frobot->getFrame(fname)->getGeometry());
            attachFrame(nframe, "root");
        }
    }
}

void Robot::addActiveRobot(const RobotPtr &arobot)
{
    for (const auto &fname : arobot->getFrameNames())
    {
        auto oframe = arobot->getFrame(fname);

        if (fname != "root")
        {
            auto nframe = std::make_shared<Frame>(fname, oframe->getTip(), oframe->getJoint().type,
                                                  oframe->getJoint().lower, oframe->getJoint().upper,
                                                  arobot->getFrame(fname)->getGeometry());

            auto parent = arobot->getFrameParent(fname);
            attachFrame(nframe, parent->getName());
        }
    }

    // Copy the allowable collision matrix
    setACM(arobot->getACM());
    // Copy the named states
    setNamedStates(arobot->getNamedStatesMap());
}

void Robot::removeChild(FrameDataPtr parent, const std::string &child)
{
    const auto &found =
        std::find_if(parent->children.begin(), parent->children.end(),
                     [child](const FrameDataPtr &f) -> bool { return f->frame->getName() == child; });

    if (found == parent->children.end())
        throw std::invalid_argument(
            log::format("Child %1% not found in parent %2%", child, parent->frame->getName()));

    parent->children.erase(found);
}

void Robot::updateFrame(const FramePtr &frame, const std::string &parent_name)
{
    tree_update_ = true;

    const auto &frame_name = frame->getName();
    const auto &new_parent = getFrameDataConst(parent_name);

    // Frame already existed, need to modify old parent
    auto inserted = data_.emplace(frame->getName(), std::make_shared<FrameData>(frame, new_parent));
    FrameDataPtr &data = inserted.first->second;

    // Emplacement failed due to element already existing
    if (!inserted.second)
    {
        // Remove this frame from old parent's child list
        const auto &old_parent = data->parent;
        removeChild(old_parent, frame_name);

        // Possibly the frame has changed as well?
        data->frame = frame;

        // Add this frame's new parent
        data->parent = new_parent;
    }

    // Update new parent's children
    new_parent->children.emplace_back(data);
}

void Robot::compileTree()
{
    // No work to do
    if (!tree_update_)
        return;

    tree_update_ = false;
    tree_ = KDL::Tree();
    names_.clear();
    ees_.clear();
    maxDepth_ = 0;

    const auto &root = getFrameData("root");
    for (auto &child : root->children)
        compileTreeRecursive("root", child);

    joints_.clear();
    lower_ = Eigen::VectorXd(getNumJoints());
    upper_ = Eigen::VectorXd(getNumJoints());

    for (const auto &name : getFrameNames())
    {
        const auto &frame = getFrameDataConst(name);
        auto &joint = frame->frame->getJoint();
        if (joint.type == Joint::FIXED)
            continue;

        joints_.emplace_back(name, frame);

        const auto &bounds = joint.getBounds();
        frame->getValues(lower_) = bounds.first;
        frame->getValues(upper_) = bounds.second;
    }

    std::sort(joints_.begin(), joints_.end(),
              [](const auto &a, const auto &b) { return a.second->index < b.second->index; });

    jac_ = std::make_shared<KDL::TreeJntToJacSolver>(tree_);

    staticFK();
    generateSignature();
}

void Robot::compileTreeRecursive(const std::string &previous, FrameDataPtr &data, bool movable)
{
    const auto &frame = data->frame;
    const auto &name = data->frame->getName();
    auto &children = data->children;

    const std::string *parent = &previous;
    const auto &segments = frame->getSegments();

    // Build up chain that represents frame
    for (const auto &segment : segments)
    {
        tree_.addSegment(segment, *parent);
        parent = &segment.getName();
    }

    const bool isFixed = frame->getJoint().type == Joint::FIXED;
    data->movable = !isFixed || movable;
    data->depth = data->parent->depth + 1;
    if (data->depth > maxDepth_)
        maxDepth_ = data->depth;

    names_.emplace_back(name);

    // Continue tree compilation for children
    for (auto &child : children)
        compileTreeRecursive(name, child, data->movable);

    // Add information about configuration index
    data->index = GetTreeElementQNr(tree_.getSegment(segments[0].getName())->second);
    data->size = segments.size() - 1;

    // If there are no children then we are an end-effector (leaf)!
    if (children.empty() && data->movable)
        ees_.emplace_back(name);
}

void Robot::staticFK()
{
    auto map = std::make_shared<FrameMap>(*this);
    auto state = std::make_shared<State>(*this);
    for (const auto &data : data_)
    {
        const auto &frame = data.second;
        if (!frame->movable)
            getPose(frame, state->data, map);
    }

    static_ = map;
}

void Robot::generateSignature()
{
    std::stringstream ss;

    for (const auto &joint : this->getJoints())
    {
        const auto &jointName = joint.first;
        const auto &jointType = Joint::typeToString(joint.second->frame->getJoint().type);
        ss << jointName << "_" << jointType << "_";
    }

    signature_ = ss.str();
}

///
/// Getters and Setters
///

unsigned int Robot::getNumJoints() const
{
    return tree_.getNrOfJoints();
}

const std::vector<std::string> &Robot::getEEs() const
{
    return ees_;
}

const std::vector<std::string> &Robot::getFrameNames() const
{
    return names_;
}

FramePtr Robot::getFrame(const std::string &name)
{
    return getFrameData(name)->frame;
}

FramePtr Robot::getFrameParent(const std::string &name)
{
    if (getFrameData(name)->root)
        SE2EZ_ERROR("Root does not have a parent");

    return getFrameData(name)->parent->frame;
}

const FramePtr &Robot::getFrameConst(const std::string &name) const
{
    return getFrameDataConst(name)->frame;
}

bool Robot::inTree(const std::string &name) const
{
    return data_.count(name) > 0;
}

Robot::FrameDataPtr Robot::getFrameData(const std::string &frame_name)
{
    const auto &data = data_.find(frame_name);
    if (data != data_.end())
        return data->second;

    throw std::invalid_argument(log::format("Frame %1% does not exist!", frame_name));
}

const Robot::FrameDataPtr &Robot::getFrameDataConst(const std::string &frame_name) const
{
    const auto &data = data_.find(frame_name);
    if (data != data_.end())
        return data->second;

    throw std::invalid_argument(log::format("Frame %1% does not exist!", frame_name));
}

const std::string &Robot::getSignature()
{
    return signature_;
}

void Robot::setNamedState(const std::string &name, const StatePtr &state)
{
    auto entry = named_.find(name);
    if (entry != named_.end())
        entry->second->copy(state);

    else
    {
        entry = named_.emplace(name, std::make_shared<State>(*this)).first;
        entry->second->copy(state);
    }
}

void Robot::getNamedState(const std::string &name, StatePtr state)
{
    const auto &entry = named_.find(name);
    if (entry == named_.end())
        throw std::invalid_argument(log::format("Named state %1% does not exist!", name));

    state->copy(entry->second);
}

std::vector<std::string> Robot::getNamedStates() const
{
    std::vector<std::string> names;
    for (const auto &named : named_)
        names.emplace_back(named.first);

    return names;
}

const std::map<std::string, StatePtr> &Robot::getNamedStatesMap()
{
    return named_;
}

void Robot::setNamedStates(const std::map<std::string, StatePtr> &named)
{
    for (const auto &e : named)
        setNamedState(e.first, e.second);
}

ACMPtr Robot::getACM()
{
    return acm_;
}

void Robot::setACM(const ACMPtr &acm)
{
    acm_ = acm;
}

const Eigen::VectorXd Robot::getUpperLimits() const
{
    return upper_;
}

const Eigen::VectorXd Robot::getLowerLimits() const
{
    return lower_;
}

const std::vector<bool> Robot::getContinuity() const
{
    std::vector<bool> conts;
    for (const auto &joint : joints_)
    {
        const auto &j = joint.second->frame->getJoint();
        if (j.continuous)
        {
            for (unsigned int i = 0; i < j.n - 1; ++i)
                conts.emplace_back(false);
            conts.emplace_back(true);
        }
        else
            for (unsigned int i = 0; i < j.n; ++i)
                conts.emplace_back(false);
    }

    return conts;
}

const std::vector<std::pair<std::string, Robot::FrameDataPtr>> &Robot::getJoints() const
{
    return joints_;
}

const KDL::Tree &Robot::getTree() const
{
    return tree_;
}

///
/// Pose / Configuration Management
///

Eigen::Ref<Eigen::VectorXd> Robot::getFrameValues(const std::string &frame_name,
                                                  Eigen::Ref<Eigen::VectorXd> q) const
{
    return getFrameDataConst(frame_name)->getValues(q);
}

const Eigen::Ref<const Eigen::VectorXd>
Robot::getFrameValuesConst(const std::string &frame_name, const Eigen::Ref<const Eigen::VectorXd> &q) const
{
    return getFrameDataConst(frame_name)->getValuesConst(q);
}

void Robot::fk(const Eigen::Ref<const Eigen::VectorXd> &q, FrameMapPtr frames) const
{
    for (const auto &ee : ees_)
        getPose(ee, q, frames);

    static_->insert(frames);
}

const FrameMap::Value &Robot::getPose(const std::string &frame_name,
                                      const Eigen::Ref<const Eigen::VectorXd> &q, FrameMapPtr frames) const
{
    const auto &entry = getPose(getFrameDataConst(frame_name), q, frames);
    return entry;
}

const FrameMap::Value &Robot::getPose(const FrameDataPtr &data, const Eigen::Ref<const Eigen::VectorXd> &q,
                                      FrameMapPtr frames) const
{
    const auto &name = data->frame->getName();
    auto &entry = frames->getEntry(name);
    if (!entry.dirty)
        return entry;

    if (!data->movable && static_)
    {
        entry = static_->getEntry(name);
        return entry;
    }

    const auto &values = data->getValuesConst(q);
    const auto &local = data->frame->getPose(values);
    const auto &parent = (data->root) ? FrameMap::Value() : getPose(data->parent, q, frames);

    entry.dirty = false;
    entry.pose = parent.pose * local;
    data->frame->getGeometryPoses(entry.pose, entry.geometry);

    return entry;
}

const FrameMap::Value &Robot::getJacobian(const std::string &frame_name,
                                          const Eigen::Ref<const Eigen::VectorXd> &q,
                                          FrameMapPtr frames) const
{
    auto &entry = frames->getEntry(frame_name);
    if (entry.jc)
        return entry;

    KDL::JntArray in(tf::EigenToKDLVec(q));
    KDL::Jacobian j(q.size());
    jac_->JntToJac(in, j, frame_name);

    entry.jc = true;

    entry.jacobian = Eigen::MatrixXd(3, q.size());
    entry.jacobian.row(0) = j.data.row(0);
    entry.jacobian.row(1) = j.data.row(1);
    entry.jacobian.row(2) = j.data.row(5);

    return entry;
}

double Robot::distance(const StatePtr &a, const StatePtr &b, bool weighted) const
{
    double d = 0;

    const double n = maxDepth_ + 1.;
    for (const auto &pair : joints_)
    {
        const auto &frame = pair.second;
        const auto &joint = frame->frame->getJoint();

        const double m = frame->depth;
        const double weight = (weighted) ? (n - m) / n : 1.;

        d += weight * joint.distance(frame->getValuesConst(a->data), frame->getValuesConst(b->data));
    }

    return d;
}

void Robot::enforceLimits(StatePtr a) const
{
    for (const auto &pair : joints_)
    {
        const auto &frame = pair.second;
        const auto &joint = frame->frame->getJoint();

        joint.enforceBounds(frame->getValues(a->data));
    }

    a->dirty = true;
}

bool Robot::inLimits(const StatePtr &a) const
{
    for (const auto &pair : joints_)
    {
        const auto &frame = pair.second;
        const auto &joint = frame->frame->getJoint();

        if (!joint.inBounds(frame->getValues(a->data)))
            return false;
    }

    return true;
}

void Robot::interpolate(const StatePtr &from, const StatePtr &to, double t, StatePtr state) const
{
    for (const auto &pair : joints_)
    {
        const auto &frame = pair.second;
        const auto &joint = frame->frame->getJoint();

        joint.interpolate(frame->getValuesConst(from->data), frame->getValuesConst(to->data), t,
                          frame->getValues(state->data));
    }

    state->dirty = true;
}

///
/// Informative
///

std::string Robot::printDebug() const
{
    std::stringstream ss;
    printTreeRecursive(ss, getFrameDataConst("root"), [&](const Robot::FrameDataPtr &frame) -> std::string {
        std::stringstream ss;

        ss << frame->frame->printDebug();

        for (unsigned int i = 0; i < frame->children.size(); ++i)
            ss << ((i > 0) ? ", " : " C[") << frame->children[i]->frame->getName();

        if (frame->parent)
            ss << log::format("] P[%1%]", frame->parent->frame->getName());
        else
            ss << "]";

        if (frame->frame->getJoint().type != Joint::FIXED)
            ss << log::format(" [%1%/%2%]", frame->index, frame->size);

        ss << log::format(" D:%1% M:%2%", frame->depth, frame->movable);

        return ss.str();
    });

    return ss.str();
}

std::string Robot::printTree(const PrintHelper &function) const
{
    std::stringstream ss;
    printTreeRecursive(ss, getFrameDataConst("root"), function);
    return ss.str();
}

void Robot::printTreeRecursive(std::stringstream &ss, const FrameDataPtr &frame, const PrintHelper &function,
                               const std::string &prefix) const
{
    ss << prefix << "\\_ " << frame->frame->getName();

    if (function)
        ss << " (" << function(frame) << ")";

    ss << std::endl;

    for (unsigned int i = 0; i < frame->children.size(); ++i)
        printTreeRecursive(ss, frame->children[i], function,
                           prefix + ((i == frame->children.size() - 1) ? " " : "|"));
}