motionbuilder.cpp

Go to the documentation of this file.

/* Author: Constantinos Chamzas */

#include <iostream>

#include <se2ez/core/log.h>
#include <se2ez/core/math.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/state.h>
#include <se2ez/core/box2d/collision.h>
#include <se2ez/core/scene.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/ik.h>

#include <se2ez/core/motionbuilder.h>

using namespace se2ez;

///
/// MotionBuilder
///

MotionBuilder::MotionBuilder(const RobotPtr &robot, const std::string &ee,
                             const std::vector<std::string> &frames)
  : robot_(robot), cm_(std::make_shared<Box2DCollisionManager>(robot)), ee_(ee)
{
    ik_ = std::make_shared<CollisionAwareChainIK>(robot_, ee, 100);
    for (const auto &fname : frames)
        ees_.emplace(fname);
}

MotionBuilder::MotionBuilder(const RobotPtr &robot, const std::string &ee, const std::string &prefix)
  : MotionBuilder(robot, ee, [&]() {
      std::vector<std::string> frames;

      for (const auto &fname : robot->getFrameNames())
      {
          if (fname.find(prefix) != std::string::npos)
              frames.emplace_back(fname);
      }
      return frames;
  }())

{
}

const std::map<std::string, StatePtr> &MotionBuilder::genGoals(QueryType type)
{
    goals_.clear();
    switch (type)
    {
        case FRAME:
            genFrameGoals();
            break;
        case NAMED:
            genNamedGoals();
            break;
        case ALL:
            genAllGoals();
    }

    return goals_;
}

const std::set<std::pair<StatePtr, StatePtr>> &MotionBuilder::getQueries(QueryType type)
{
    queries_.clear();
    genGoals(type);

    for (auto it1 = goals_.begin(); it1 != goals_.end(); ++it1)
        for (auto it2 = std::next(it1); it2 != goals_.end(); ++it2)
            queries_.emplace(it1->second, it2->second);

    if (!queries_.size())
        SE2EZ_WARN("Queries list is empty");

    return queries_;
}
void MotionBuilder::genFrameGoals()
{
    for (const auto &ee : ees_)
    {
        const auto &eepose = robot_->getFrame(ee);
        auto goal = std::make_shared<State>(robot_);

        if (ik_->solve(goal, eepose->getTip()))
            goals_.emplace(eepose->getName(), goal);
        else
            SE2EZ_WARN("IK solution for: %s with EE:%s was not found. \n. Did you Choose the correct EE?",
                       eepose->getName(), ee_);
    }
}

void MotionBuilder::genNamedGoals()
{
    for (auto pair : robot_->getNamedStatesMap())
        if (!cm_->collide(pair.second))
            goals_.emplace(pair.first, pair.second);
        else
            SE2EZ_WARN("Named state: %s is in collision", pair.second);
}

void MotionBuilder::genAllGoals()
{
    genFrameGoals();
    genNamedGoals();
}