PathPlanner.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    RobotComponents::PathPlanner
* @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
* @date       2015 Humanoids Group, H2T, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "PathPlanner.h"
 
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
#include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 
armarx::PathPlanner::PathPlanner():
    safetyMargin {50.f},
    objects {}
{
}
 
void armarx::PathPlanner::onInitComponent()
{
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    //    offeringTopic(getProperty<std::string>("DebugDrawerName").getValue());
}
 
void armarx::PathPlanner::onConnectComponent()
{
    workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
    commonStoragePrx = workingMemoryPrx->getCommonStorage();
    fileManager.reset(new memoryx::GridFileManager(commonStoragePrx));
 
    //    debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerName").getValue());
}
 
void armarx::PathPlanner::setCollisionObjects(const ::armarx::ObjectPositionBaseList& list, const ::Ice::Current&)
{
    clearCollisionObjects();
    addCollisionObjects(list);
}
 
void armarx::PathPlanner::addCollisionObjects(const ::armarx::ObjectPositionBaseList& list, const ::Ice::Current&)
{
    ARMARX_VERBOSE << "Adding " << list.size() << " collision objects";
 
    //strong except guarantee
    std::vector<CollisionObjectData> newObjects;
 
    for (const auto& elem : list)
    {
        ARMARX_VERBOSE << "Object: " << newObjects.size();
 
        CollisionObjectData newObject;
        newObject.object = elem.objectClassBase;
 
        ARMARX_VERBOSE << "Object: name " << newObject.object->getName() << ", id " << newObject.object->getId();
 
        memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(elem.objectClassBase);
 
        if (!objectClass)
        {
            std::stringstream s;
            s << "Can't use object class with ice id " << elem.objectClassBase->ice_id();
            ARMARX_ERROR_S << s.str();
            throw armarx::InvalidArgumentException {s.str()};
        }
 
        ARMARX_VERBOSE << "Adding SimoxObjectWrapper";
        memoryx::EntityWrappers::SimoxObjectWrapperPtr sw = objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
        ARMARX_VERBOSE << "Getting ManipulationObject name and object";
        VirtualRobot::ManipulationObjectPtr orgMo = sw->getManipulationObject();
        std::string moName = orgMo->getName();
        ARMARX_VERBOSE << "Cloning ManipulationObject";
        VirtualRobot::ManipulationObjectPtr mo = orgMo->clone(moName);
 
        ARMARX_VERBOSE << "Moving object";
        //move the object to the given position
        const auto objectPose = armarx::PosePtr::dynamicCast(elem.objectPose);
 
        if (!objectPose)
        {
            std::stringstream s;
            s << "Can't convert pose of " << objectClass->getName() << " to armarx::Pose.";
            ARMARX_ERROR_S << s.str();
            throw armarx::InvalidArgumentException {s.str()};
        }
 
        mo->setGlobalPose(objectPose->toEigen());
 
        ARMARX_VERBOSE << "Get collision object";
        VirtualRobot::CollisionModelPtr colModel = mo->getCollisionModel();
 
        newObject.colModel = colModel;
        newObjects.push_back(newObject);
        ARMARX_VERBOSE << "Object: " << newObjects.size() - 1 << " done.";
    }
 
    //copy
    std::copy(newObjects.begin(), newObjects.end(), std::back_inserter(objects));
 
    ARMARX_VERBOSE << "Added " << newObjects.size() << " collision objects. (Total: " << objects.size() << ")";
}
 
void armarx::PathPlanner::clearCollisionObjects(const ::Ice::Current&)
{
    ARMARX_VERBOSE << "Cleared " << objects.size() << " objects.";
    objects.clear();
}
 
void armarx::PathPlanner::setAgent(const ::memoryx::AgentInstanceBasePtr& newAgent, const std::string& agentColModelName, const ::Ice::Current&)
{
    ARMARX_VERBOSE << "Setting agent '" << newAgent->getName() << "' with id " << newAgent->getId();
    //get agent
    std::string agentFilePath = newAgent->getAgentFilePath();
 
    if (!ArmarXDataPath::getAbsolutePath(agentFilePath, agentFilePath))
    {
        ARMARX_ERROR << "Could not find robot file " << agentFilePath << std::flush;
 
        ARMARX_VERBOSE << "Searched in " << ArmarXDataPath::getDataPaths().size() << " additional paths";
 
        for (const auto& path : ArmarXDataPath::getDataPaths())
        {
            ARMARX_VERBOSE << "\t" << path;
        }
 
        return;
    }
 
    //strong except guarantee
    ARMARX_VERBOSE << "Loading agent: " << agentFilePath;
    auto agent2 = VirtualRobot::RobotIO::loadRobot(agentFilePath, VirtualRobot::RobotIO::eFull);
    ARMARX_VERBOSE << "Agent loaded.";
 
    if (!agent2)
    {
        std::stringstream s;
        s << "Can't load agent from: " << agentFilePath;
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException {s.str()};
    }
 
    //get collision model
    if (!agent2->hasRobotNode(agentColModelName) || !agent2->getRobotNode(agentColModelName)->getCollisionModel())
    {
        std::stringstream s;
        s << "Agent has no collision model with name " << agentColModelName;
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException {s.str()};
    }
 
    agent = agent2;
    agentCollisionModel = agent->getRobotNode(agentColModelName)->getCollisionModel();//->clone();
    agentZCoord = agent->getGlobalPose()(2, 3);
    ARMARX_VERBOSE << "Setting agent...done";
}
 
void armarx::PathPlanner::setSafetyMargin(::Ice::Float margin, const ::Ice::Current&)
{
    if (margin < 0)
    {
        std::stringstream s;
        s << "Invalid margin: " << margin << " < 0.0";
        ARMARX_ERROR_S << s.str();
        throw armarx::InvalidArgumentException {s.str()};
    }
 
    safetyMargin = margin;
}
 
bool armarx::PathPlanner::isPositionValid(armarx::Vector3 position) const
{
    if (!agentCollisionModel)
    {
        throw std::logic_error {"no agent collision model"};
    }
 
    if (objects.empty())
    {
        return true;
    }
 
    //set pose
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
    pose.block<3, 3>(0, 0) = VirtualRobot::MathTools::axisangle2eigen3f(Eigen::Vector3f {0.f, 0.f, 1.f}, position.z);
    pose(0, 3) = position.x;
    pose(1, 3) = position.y;
    pose(3, 3) = agentZCoord;
    agent->setGlobalPose(pose);
 
    if (safetyMargin > 0.01f)
    {
        for (const auto& object : objects)
        {
            float dist = VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
                             agentCollisionModel,
                             object.colModel);
 
            if (dist <= safetyMargin)
            {
                return false;
            }
        }
    }
    else
    {
        //the distance is small => only check collision
        for (const auto& object : objects)
        {
            bool col = VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
                           agentCollisionModel,
                           object.colModel
                       );
 
            if (col)
            {
                return false;
            }
        }
    }
 
    return true;
}