DesignerTrajectoryHolder.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    ArmarXGuiPlugins::RobotTrajectoryDesigner::Manager
 * @author     Luca Quaer
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DesignerTrajectoryHolder.h"
 
#include <VirtualRobot/Robot.h>
 
using namespace armarx;
 
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Constructor(s)
////////////////////////////////////////////////////////////////////////////////////////////////////
///
 
DesignerTrajectoryHolder::DesignerTrajectoryHolder(EnvironmentPtr environment) :
    environment(environment)
{
}
 
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Public functions
////////////////////////////////////////////////////////////////////////////////////////////////////
///
bool
DesignerTrajectoryHolder::createDesignerTrajectoryManager(std::string rnsName)
{
    // check, if the RobotNodeSet-name already exists as key
    //if (designerTrajectories.find(rnsName) == designerTrajectories.end())
    if (!rnsExists(rnsName) && !rnsIsPartOfExistingRns(rnsName))
    {
        // RobotNodeSet-name does not exist as key
        designerTrajectories.insert(std::make_pair(
            rnsName,
            DesignerTrajectoryManagerPtr(new DesignerTrajectoryManager(rnsName, environment))));
        //    environment->getRobot()->getRobotNodeSet(rnsName)))));
        return true;
    }
    else
    {
        return false;
    }
}
 
bool
DesignerTrajectoryHolder::deleteDesignerTrajectoryManager(std::string rnsName)
{
    // check, if the RobotNodeSet-name already exists as key
    //if (designerTrajectories.find(rnsName) != designerTrajectories.end())
    if (rnsExists(rnsName))
    {
        // RobotNodeSetPtr does exist as key
        int numberOfErasedElements = designerTrajectories.erase(rnsName);
 
        if (numberOfErasedElements > 0)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        return false;
    }
}
 
DesignerTrajectoryManagerPtr
DesignerTrajectoryHolder::getTrajectoryManager(std::string rnsName)
{
    if (rnsExists(rnsName))
    {
        return designerTrajectories.at(rnsName);
    }
    else
    {
        throw InvalidArgumentException("RNS name does not exist in map");
    }
}
 
bool
DesignerTrajectoryHolder::import(DesignerTrajectoryPtr designerTrajectory, bool force = true)
{
    VirtualRobot::RobotNodeSetPtr tmpRns = designerTrajectory->getRns();
 
    DesignerTrajectoryManagerPtr tmpDTM(
        new DesignerTrajectoryManager(tmpRns->getName(), environment));
    tmpDTM.get()->import(designerTrajectory);
 
    // check, if the the RobotNodeSet-name already exists as key
    if (!rnsExists(tmpRns->getName()))
    {
        // RobotNodeSet-name does not exist as key
        designerTrajectories.insert(std::make_pair(tmpRns->getName(), tmpDTM));
 
        return true; // RobotNodeSet-name inserted
    }
    else if (force)
    {
        designerTrajectories[tmpRns->getName()] = tmpDTM;
        return true; // RobotNodeSet-name inserted because forced
    }
    else
    {
        return false; // RobotNodeSet-name not inserted because it already exists
    }
}
 
bool
DesignerTrajectoryHolder::rnsExists(std::string rnsName)
{
    // check, if the RobotNodeSet-name already exists as key
    if (designerTrajectories.find(rnsName) != designerTrajectories.end())
    {
        // rns found
        return true;
    }
    else
    {
        // rns not found
        return false;
    }
}
 
bool
DesignerTrajectoryHolder::rnsIsPartOfExistingRns(std::string rnsName)
{
    // save all existing RobotNodeSets as vector<vector<string>>
    // one RobotNodeSet is representet as vector of its RobotNodes-name
    std::vector<std::vector<std::string>> rnssNodeNames;
 
    for (std::map<std::string, DesignerTrajectoryManagerPtr>::iterator it =
             designerTrajectories.begin();
         it != designerTrajectories.end();
         ++it)
    {
        VirtualRobot::RobotNodeSetPtr rnsptr = environment->getRobot()->getRobotNodeSet(it->first);
        std::vector<std::string> tmpRnsNodesNames = rnsptr->getNodeNames();
        rnssNodeNames.push_back(tmpRnsNodesNames);
    }
 
    // get nodes of given rns
    std::vector<std::string> givenRnsNodeNames =
        environment->getRobot()->getRobotNodeSet(rnsName)->getNodeNames();
 
    // check if givenRnsNodeNames is a subset of any rnsNodesNames
    for (std::vector<std::string> rnsNodeNames : rnssNodeNames)
    {
        std::vector<std::string> intersection;
 
        sort(rnsNodeNames.begin(), rnsNodeNames.end());
        sort(givenRnsNodeNames.begin(), givenRnsNodeNames.end());
 
        std::set_intersection(rnsNodeNames.begin(),
                              rnsNodeNames.end(),
                              givenRnsNodeNames.begin(),
                              givenRnsNodeNames.end(),
                              std::back_inserter(intersection));
 
        if (intersection.size() != 0)
        {
            return true;
        }
    }
 
    return false;
}
 
bool
DesignerTrajectoryHolder::isInCollision(std::string activeColModelName,
                                        std::vector<std::string> bodyColModelsNames,
                                        unsigned int fps)
{
    TrajectoryPtr t = mergeTrajectories(fps);
    VirtualRobot::RobotPtr cdRob = environment->getCDRobot();
 
    ////////////////////////////////////////////////////////////////////////////////////////////////
    // Setup CDManager
    // create new CDManager
    VirtualRobot::CDManagerPtr cdm(new VirtualRobot::CDManager());
 
    // create sosActiveColModel from activeColModelName
    if (cdRob->getRobotNodeSet(activeColModelName) == nullptr)
    {
        return false; // cannot find collision model
    }
 
    VirtualRobot::SceneObjectSetPtr sosActiveColModel(new VirtualRobot::SceneObjectSet());
    sosActiveColModel->addSceneObjects(cdRob->getRobotNodeSet(activeColModelName));
 
    // add sosActiveColModel to CDManager
    cdm->addCollisionModel(sosActiveColModel);
 
    // create bodyColModels
    for (std::string bodyColModelName : bodyColModelsNames)
    {
        // create sosBodyColModel from bodyColModelName
        if (cdRob->getRobotNodeSet(bodyColModelName) == nullptr)
        {
            continue; // cannot find collision model
        }
 
        VirtualRobot::SceneObjectSetPtr sosBodyColModel(new VirtualRobot::SceneObjectSet());
        sosBodyColModel->addSceneObjects(cdRob->getRobotNodeSet(bodyColModelName));
 
        // add sosBodyColModel to CDManager
        cdm->addCollisionModel(sosBodyColModel);
    }
 
    ////////////////////////////////////////////////////////////////////////////////////////////////
    // Collision checks
 
    // increment for timesteps
    double increment = 1.0 / fps;
    double time = 0.0;
 
    while (t->dataExists(time))
    {
        ////////////////////////////////////////////////////////////////////////////////////////////
        // Create RobotConfig
        std::vector<std::string> dimNames = t->getDimensionNames();
        std::vector<double> jointValuesDouble = t->getStates(time, 0);
        std::vector<float> jointValues(jointValuesDouble.begin(), jointValuesDouble.end());
 
        VirtualRobot::RobotConfigPtr robConf(new VirtualRobot::RobotConfig(
            cdRob, "config at t=" + std::to_string(time), dimNames, jointValues));
 
        ////////////////////////////////////////////////////////////////////////////////////////////
        // Set RobotConfig to CDRobot
        cdRob->setConfig(robConf);
 
        ////////////////////////////////////////////////////////////////////////////////////////////
        // Check for collision
        if (cdm->isInCollision())
        {
            return true;
        }
 
        ////////////////////////////////////////////////////////////////////////////////////////////
        // increment loop variable
        time = time + increment;
    }
 
    return false;
}
 
TrajectoryPtr
DesignerTrajectoryHolder::mergeTrajectories(unsigned int fps)
{
    std::vector<TrajectoryPtr> trajectories;
    std::vector<double> endTimes;
    double maxEndTime = 0;
    std::vector<double> timestamps;
    TrajectoryPtr finalTrajectory(new Trajectory());
 
    for (std::map<std::string, DesignerTrajectoryManagerPtr>::iterator it =
             designerTrajectories.begin();
         it != designerTrajectories.end();
         ++it)
    {
        if (!it->second->getDesignerTrajectory())
        {
            continue;
        }
        TrajectoryPtr traj = it->second->getDesignerTrajectory()->getFinalTrajectory();
 
        trajectories.push_back(traj);
        double t = traj->getTimeLength();
        if (t > maxEndTime)
        {
            maxEndTime = t;
        }
        endTimes.push_back(t);
    }
 
    double inc = 1.0 / fps;
    for (double d = 0.0; d < maxEndTime; d = d + inc)
    {
        timestamps.push_back(d);
    }
 
    timestamps.push_back(maxEndTime);
 
    //get dimensionData of every single dimension of every Trajectory
    for (unsigned int t = 0; t < trajectories.size(); t++)
    {
        for (unsigned int dim = 0; dim < trajectories[t]->dim(); dim++)
        {
            std::vector<double> dimData;
            for (double time : timestamps)
            {
                if (time < endTimes[t])
                {
                    dimData.push_back(trajectories[t]->getState(time, dim));
                }
                else
                {
                    dimData.push_back(trajectories[t]->getState(endTimes[t], dim));
                }
            }
 
            finalTrajectory->addDimension(
                dimData, timestamps, trajectories[t]->getDimensionName(dim));
        }
    }
    return finalTrajectory;
}