frame.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <se2ez/core/log.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/geometry.h>

using namespace se2ez;

FramePtr Frame::getRoot()
{
    static FramePtr root = std::make_shared<Frame>("root", Eigen::Isometry2d::Identity(), Joint::FIXED);
    return root;
}

Frame::Frame(const std::string &name, const Eigen::Isometry2d &tip, const std::vector<GeometryPtr> &geometry)
  : Frame(name, tip, Joint::FIXED, Eigen::VectorXd{}, Eigen::VectorXd{}, geometry)
{
}

Frame::Frame(const std::string &name, const Eigen::Vector3d &tip, const std::vector<GeometryPtr> &geometry)
  : Frame(name, tf::toIsometry(tip), geometry)
{
}

Frame::Frame(const std::string &name, const Eigen::Isometry2d &tip, const Joint::Type &type,
             const Eigen::VectorXd &lower, const Eigen::VectorXd &upper,
             const std::vector<GeometryPtr> &geometry)
  : name_(name), tip_(tip), joint_(name, type, lower, upper), geometry_(geometry), segments_([&]() {
      std::vector<KDL::Segment> segments;
      unsigned int s = joint_.joints.size();
      for (unsigned int i = 0; i < s; ++i)
          segments.emplace_back(joint_.joints[i].getName(),  // Use joint name, as last joint is frame name)
                                joint_.joints[i],
                                (i == s - 1) ? tf::EigenToKDLFrame(tip_) :  // Use tip transform only on last
                                    KDL::Frame::Identity());                // Otherwise just identity
      return segments;
  }())
{
    if (!name.size())
        throw std::invalid_argument("Name for frame cannot be empty!");
}

Frame::Frame(const std::string &name, const Eigen::Vector3d &tip, const Joint::Type &type,
             const Eigen::VectorXd &lower, const Eigen::VectorXd &upper,
             const std::vector<GeometryPtr> &geometry)
  : Frame(name, tf::toIsometry(tip), type, lower, upper, geometry)
{
}

Frame::Frame(const std::string &name, const Eigen::Isometry2d &tip, const Joint::Type &type,
             const double lower, const double upper, const std::vector<GeometryPtr> &geometry)
  : Frame(name, tip, type, math::toVector(lower), math::toVector(upper), geometry)
{
}

Frame::Frame(const std::string &name, const Eigen::Vector3d &tip, const Joint::Type &type, const double lower,
             const double upper, const std::vector<GeometryPtr> &geometry)
  : Frame(name, tf::toIsometry(tip), type, lower, upper, geometry)
{
}

const std::string &Frame::getName() const
{
    return name_;
}

const std::vector<KDL::Segment> &Frame::getSegments() const
{
    return segments_;
}

const std::vector<GeometryPtr> &Frame::getGeometry() const
{
    return geometry_;
}

const Joint &Frame::getJoint() const
{
    return joint_;
}

const Eigen::Isometry2d &Frame::getTip() const
{
    return tip_;
}

Eigen::Isometry2d Frame::getPose(const Eigen::Ref<const Eigen::VectorXd> &q) const
{
    KDL::Frame f = KDL::Frame::Identity();
    for (unsigned int i = 0; i < segments_.size() - 1; ++i)
        f = f * segments_[i].pose(q[i]);

    f = f * segments_.back().pose(0);
    return tf::KDLToEigenFrame(f);
}

void Frame::getGeometryPoses(const Eigen::Isometry2d &pose, tf::EigenVector<Eigen::Isometry2d> &poses) const
{
    poses.resize(geometry_.size());
    for (unsigned int i = 0; i < geometry_.size(); ++i)
        poses[i] = (geometry_[i]->isOffset) ? pose * geometry_[i]->offset : pose;
}

std::string Frame::printDebug() const
{
    std::stringstream ss;
    ss << log::format("[%1% %2% %3%", name_, tf::printFrame(tip_), joint_.printDebug());
    for (const auto &geometry : geometry_)
        ss << geometry->printGeometry();

    ss << "]";
    return ss.str();
}