TrajectoryController.cpp

Go to the documentation of this file.

#include "TrajectoryController.h"
 
#include <iostream>
 
#include <QMessageBox>
 
#include "../Util/OrientationConversion.h"
using namespace std;
 
namespace armarx
{
    void
    TrajectoryController::onInitComponent()
    {
        ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on init";
    }
 
    void
    TrajectoryController::onConnectComponent()
    {
        ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on connect";
    }
 
    void
    TrajectoryController::onDisconnectComponent()
    {
        ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on disconnect";
    }
 
    void
    TrajectoryController::onExitComponent()
    {
        ARMARX_INFO << "RobotTrajectoryDesigner: TrajectoryController on exit";
    }
 
    //=============================================================================================
 
    TrajectoryController::TrajectoryController()
    {
        onInitComponent();
        onConnectComponent();
    }
 
    void
    TrajectoryController::updateTCP(QString tcp)
    {
        if (environment->getRobot() == NULL || holder == NULL)
        {
            ARMARX_INFO << "Robot or holder is NULL";
            return;
        }
        VirtualRobot::RobotNodeSetPtr helpRns =
            environment->getRobot()->getRobotNodeSet(tcp.toStdString());
        if (manager != NULL && !manager->getIsInitialized())
        {
            holder->deleteDesignerTrajectoryManager(rns->getName());
            emit removeTransitionWaypointGui();
            emit removeTrajectory(QString::fromStdString(rns->getName()));
        }
        if (manager == NULL)
        {
            rns = helpRns;
            helpCreateDesignerTrajectoryManager();
        }
        else if (rns == helpRns)
        {
            return;
        }
        else if (holder->rnsIsPartOfExistingRns(helpRns->getName()))
        {
            if (holder->rnsExists(helpRns->getName()))
            {
                emit removeTransitionWaypointGui();
                manager = holder->getTrajectoryManager(helpRns->getName());
                rns = helpRns;
                emit rnsChanged(rns);
                emit showTrajectory(manager->getDesignerTrajectory());
                addTransitionWaypointGui(manager->getDesignerTrajectory());
                emit updateSelectedTCP(QString::fromStdString(rns->getName()));
            }
            else
            {
                QMessageBox* rnsIsPartOfExistingRns = new QMessageBox;
                rnsIsPartOfExistingRns->setWindowTitle(QString::fromStdString("Error Message"));
                rnsIsPartOfExistingRns->setText(
                    QString::fromStdString("Selected TCP is part of an existing TCP."));
                rnsIsPartOfExistingRns->exec();
                emit updateSelectedTCP(QString::fromStdString(rns->getName()));
            }
        }
        else
        {
            emit showTrajectory(NULL);
            emit removeTransitionWaypointGui();
            rns = helpRns;
            helpCreateDesignerTrajectoryManager();
        }
        helpEnableButtons();
        helpEnableUndoRedo();
    }
 
    void
    TrajectoryController::updateTransition(int transition, int it)
    {
        if (manager)
        {
            try
            {
                manager->setTransitionInterpolation(transition, (InterpolationType)it);
                emit showTrajectory(manager->getDesignerTrajectory());
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpEnableUndoRedo();
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
                TransitionPtr transitionPtr =
                    manager->getDesignerTrajectory()->getTransition(transition);
                helpChangeTransitionGui(transitionPtr, transition);
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
                TransitionPtr transitionPtr =
                    manager->getDesignerTrajectory()->getTransition(transition);
                helpChangeTransitionGui(transitionPtr, transition);
            }
        }
    }
 
    void
    TrajectoryController::updateTransition(int transition, double duration)
    {
        if (manager)
        {
            try
            {
                manager->setTransitionUserDuration(transition, duration);
                emit showTrajectory(manager->getDesignerTrajectory());
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpEnableUndoRedo();
                helpCollision();
            }
            catch (InvalidArgumentException& e)
            {
                TransitionPtr transitionPtr =
                    manager->getDesignerTrajectory()->getTransition(transition);
                helpChangeTransitionGui(transitionPtr, transition);
            }
        }
    }
 
    void
    TrajectoryController::updateWaypoint(int waypoint, std::vector<double> values)
    {
        if (manager)
        {
            try
            {
                manager->editWaypointPoseBase(waypoint, parseToPoseBasePtr(values));
                emit showTrajectory(manager->getDesignerTrajectory());
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpEnableUndoRedo();
                helpCollision();
            }
            catch (LocalException& e)
            {
                UserWaypointPtr waypointPtr =
                    manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
                helpChangeWaypointGui(waypointPtr, waypoint);
            }
            catch (InvalidArgumentException& e)
            {
                UserWaypointPtr waypointPtr =
                    manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
                helpChangeWaypointGui(waypointPtr, waypoint);
            }
        }
    }
 
    void
    TrajectoryController::updateWaypoint(int waypoint)
    {
        if (manager)
        {
            try
            {
                manager->editWaypointPoseBase(
                    waypoint,
                    *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(
                        rns->getTCP()->getGlobalPose()))));
                emit showTrajectory(manager->getDesignerTrajectory());
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpEnableUndoRedo();
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
            }
        }
    }
 
    void
    TrajectoryController::updateWaypoint(int waypoint, PoseBasePtr newPoseBase)
    {
        if (manager)
        {
            try
            {
                manager->editWaypointPoseBase(waypoint, newPoseBase);
                emit showTrajectory(manager->getDesignerTrajectory());
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpEnableUndoRedo();
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
                UserWaypointPtr waypointPtr =
                    manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
                helpChangeWaypointGui(waypointPtr, waypoint);
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
                UserWaypointPtr waypointPtr =
                    manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
                helpChangeWaypointGui(waypointPtr, waypoint);
            }
        }
    }
 
    void
    TrajectoryController::updateWaypoint(int waypoint, int cartesianSelection)
    {
        if (manager)
        {
            try
            {
                manager->editWaypointIKSelection(
                    waypoint, (VirtualRobot::IKSolver::CartesianSelection)cartesianSelection);
                emit showTrajectory(manager->getDesignerTrajectory());
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                emit cartesianSelectionChanged(
                    manager->getDesignerTrajectory()->getUserWaypoint(waypoint)->getIKSelection());
                helpEnableUndoRedo();
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
                UserWaypointPtr waypointPtr =
                    manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
                helpChangeWaypointGui(waypointPtr, waypoint);
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
                UserWaypointPtr waypointPtr =
                    manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
                helpChangeWaypointGui(waypointPtr, waypoint);
            }
        }
    }
 
    void
    TrajectoryController::updateWaypoint(int waypoint, bool isBreakpoint)
    {
        if (manager == NULL)
        {
            ARMARX_INFO << "Manager is NULL";
            return;
        }
        try
        {
            manager->setWaypointAsBreakpoint(waypoint, isBreakpoint);
            emit showTrajectory(manager->getDesignerTrajectory());
            changeTransitionWaypointGui(manager->getDesignerTrajectory());
            helpEnableUndoRedo();
        }
        catch (LocalException& e)
        {
            helpExceptionMessageBox(e.getReason());
            UserWaypointPtr waypointPtr =
                manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
            helpChangeWaypointGui(waypointPtr, waypoint);
        }
        catch (InvalidArgumentException& e)
        {
            helpExceptionMessageBox(e.reason);
            UserWaypointPtr waypointPtr =
                manager->getDesignerTrajectory()->getUserWaypoint(waypoint);
            helpChangeWaypointGui(waypointPtr, waypoint);
        }
    }
 
    void
    TrajectoryController::addWaypoint(int waypoint, bool insertAfter)
    {
        if (manager == NULL)
        {
            ARMARX_WARNING << "Manager is NULL";
            return;
        }
        unsigned int helpWaypoint = static_cast<unsigned>(waypoint);
        if (!manager->getIsInitialized())
        {
            std::vector<double> jointValues;
            for (double newValue : rns->getJointValues())
            {
                jointValues.push_back((double)newValue);
            }
            manager->initializeDesignerTrajectory(jointValues);
            emit showTrajectory(manager->getDesignerTrajectory());
            helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint),
                               waypoint);
            emit newTrajectory(QString::fromStdString(rns->getName()));
            emit updateSelectedTCP(QString::fromStdString(rns->getName()));
        }
        else if (helpWaypoint == manager->getDesignerTrajectory()->getNrOfUserWaypoints() -
                                     static_cast<unsigned>(1) &&
                 insertAfter)
        {
            try
            {
                manager->addWaypoint(
                    *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(
                        rns->getTCP()->getGlobalPose()))));
                emit showTrajectory(manager->getDesignerTrajectory());
                helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint + 1),
                                   waypoint + 1);
                helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint),
                                     waypoint);
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
            }
        }
        else if (helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() -
                                    static_cast<unsigned>(1) &&
                 waypoint >= 0 && insertAfter)
        {
            try
            {
                manager->insertWaypoint(
                    waypoint + 1,
                    *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(
                        rns->getTCP()->getGlobalPose()))));
                emit showTrajectory(manager->getDesignerTrajectory());
                helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint + 1),
                                   waypoint + 1);
                helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint + 1),
                                     waypoint + 1);
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
            }
        }
        else if (helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() &&
                 waypoint >= 0 && !insertAfter)
        {
            try
            {
                manager->insertWaypoint(
                    waypoint,
                    *new PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(
                        rns->getTCP()->getGlobalPose()))));
                emit showTrajectory(manager->getDesignerTrajectory());
                helpAddWaypointGui(manager->getDesignerTrajectory()->getUserWaypoint(waypoint),
                                   waypoint);
                helpAddTransitionGui(manager->getDesignerTrajectory()->getTransition(waypoint),
                                     waypoint);
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
            }
        }
        helpEnableButtons();
        helpEnableUndoRedo();
    }
 
    void
    TrajectoryController::deleteWaypoint(int waypoint)
    {
        if (manager == NULL)
        {
            ARMARX_WARNING << "Manager is NULL";
            return;
        }
        if (!manager->getIsInitialized())
        {
            ARMARX_WARNING << "There is no designer trajectory";
            return;
        }
        unsigned int helpWaypoint = static_cast<unsigned>(waypoint);
        if (waypoint == 0 && manager->getDesignerTrajectory()->getNrOfUserWaypoints() == 1)
        {
            try
            {
                manager->deleteWaypoint(waypoint);
                emit removeTransitionWaypointGui();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
            }
        }
        else if (helpWaypoint == manager->getDesignerTrajectory()->getNrOfUserWaypoints() -
                                     static_cast<unsigned>(1))
        {
            try
            {
                manager->deleteWaypoint(waypoint);
                emit showTrajectory(manager->getDesignerTrajectory());
                emit removeWaypointGui(waypoint);
                emit removeTransitionGui(waypoint - 1);
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
            }
        }
        else if (waypoint >= 0 &&
                 helpWaypoint < manager->getDesignerTrajectory()->getNrOfUserWaypoints() -
                                    static_cast<unsigned>(1))
        {
            try
            {
                manager->deleteWaypoint(waypoint);
                emit showTrajectory(manager->getDesignerTrajectory());
                emit removeWaypointGui(waypoint);
                emit removeTransitionGui(waypoint);
                changeTransitionWaypointGui(manager->getDesignerTrajectory());
                helpCollision();
            }
            catch (LocalException& e)
            {
                helpExceptionMessageBox(e.getReason());
            }
            catch (InvalidArgumentException& e)
            {
                helpExceptionMessageBox(e.reason);
            }
        }
        helpEnableUndoRedo();
        helpEnableButtons();
    }
 
    void
    TrajectoryController::import(DesignerTrajectoryPtr designerTrajectory)
    {
        if (holder)
        {
            if (holder->rnsIsPartOfExistingRns(designerTrajectory->getRns()->getName()))
            {
                QMessageBox::StandardButton reply;
                reply = QMessageBox::question(0,
                                              "Import",
                                              "Do you want to override the existing Trajectory?",
                                              QMessageBox::Yes | QMessageBox::No);
                if (reply == QMessageBox::Yes)
                {
                    emit removeTrajectory(QString::fromStdString(rns->getName()));
                    helpImportDesignerTrajectory(designerTrajectory);
                }
                else
                {
                    QMessageBox* importFailed = new QMessageBox;
                    importFailed->setWindowTitle(QString::fromStdString("Error Message"));
                    importFailed->setText(QString::fromStdString("Import failed."));
                    importFailed->exec();
                }
            }
            else
            {
                helpImportDesignerTrajectory(designerTrajectory);
            }
            //emit showTrajectory(manager->getDesignerTrajectory());
            helpEnableButtons();
            helpEnableUndoRedo();
            helpCollision();
        }
    }
 
    void
    TrajectoryController::exportTrajectory(int fps)
    {
        if (manager)
        {
            std::vector<DesignerTrajectoryPtr> designerTrajectories;
            for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
            {
                if (holder->rnsExists(s->getName()) &&
                    holder->getTrajectoryManager(s->getName())->getIsInitialized())
                {
                    //holder->getTrajectoryManager(s)->setFPS(fps);
                    designerTrajectories.push_back(
                        holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
                }
            }
            emit exportTrajectory(designerTrajectories);
        }
    }
 
    void
    TrajectoryController::exportTrajectory()
    {
        if (manager)
        {
            std::vector<DesignerTrajectoryPtr> designerTrajectories;
            for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
            {
                if (holder->rnsExists(s->getName()) &&
                    holder->getTrajectoryManager(s->getName())->getIsInitialized())
                {
                    designerTrajectories.push_back(
                        holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
                }
            }
            emit exportTrajectory(designerTrajectories);
        }
    }
 
    void
    TrajectoryController::undo()
    {
        if (manager)
        {
            if (manager->undo())
            {
                emit removeTransitionWaypointGui();
                emit showTrajectory(manager->getDesignerTrajectory());
                addTransitionWaypointGui(manager->getDesignerTrajectory());
            }
        }
        helpEnableUndoRedo();
        helpEnableButtons();
        helpCollision();
    }
 
    void
    TrajectoryController::redo()
    {
        if (manager)
        {
            if (manager->redo())
            {
                emit removeTransitionWaypointGui();
                emit showTrajectory(manager->getDesignerTrajectory());
                addTransitionWaypointGui(manager->getDesignerTrajectory());
            }
        }
        helpEnableUndoRedo();
        helpEnableButtons();
        helpCollision();
    }
 
    void
    TrajectoryController::playTrajectories()
    {
        std::vector<DesignerTrajectoryPtr> designerTrajectories;
        for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
        {
            if (holder->rnsExists(s->getName()))
            {
                designerTrajectories.push_back(
                    holder->getTrajectoryManager(s->getName())->getDesignerTrajectory());
            }
        }
        emit playTrajectories(designerTrajectories);
    }
 
    void
    TrajectoryController::environmentChanged(EnvironmentPtr environment)
    {
        this->environment = environment;
        holder = std::make_shared<DesignerTrajectoryHolder>(environment);
        manager = NULL;
        rns = NULL;
        emit removeTransitionWaypointGui();
        emit removeAllTrajectories();
        helpEnableUndoRedo();
        helpEnableButtons();
    }
 
    void
    TrajectoryController::setActiveColModelName(QString activeColModelName)
    {
        this->activeColModelName = activeColModelName.toStdString();
        helpCollision();
    }
 
    void
    TrajectoryController::setBodyColModelsNames(QStringList bodyColModelsNames)
    {
        this->bodyColModelsNames.clear();
        for (QString name : bodyColModelsNames)
        {
            this->bodyColModelsNames.push_back(name.toStdString());
        }
        helpCollision();
    }
 
    //=============================================================================================
    //private methods
    //=============================================================================================
 
    std::vector<double>
    TrajectoryController::parseToCoordinate(PoseBasePtr pose)
    {
        float x = pose->position->x;
        float y = pose->position->y;
        float z = pose->position->z;
        QuaternionBasePtr orientation = *new QuaternionBasePtr(new FramedOrientation());
        orientation->qw = pose->orientation->qw;
        orientation->qx = pose->orientation->qx;
        orientation->qy = pose->orientation->qy;
        orientation->qz = pose->orientation->qz;
        std::vector<double> eulerAngle = OrientationConversion::toEulerAngle(orientation);
        return {x, y, z, eulerAngle[0], eulerAngle[1], eulerAngle[2]};
    }
 
    PoseBasePtr
    TrajectoryController::parseToPoseBasePtr(std::vector<double> values)
    {
        Vector3Ptr position = new Vector3(values[0], values[1], values[2]);
        QuaternionBasePtr orientation =
            OrientationConversion::toQuaternion(values[3], values[4], values[5]);
        return new FramedPose(position, orientation, "", "");
    }
 
    //=============================================================================================
 
    void
    TrajectoryController::helpAddWaypointGui(UserWaypointPtr waypoint, int waypointIndex)
    {
        std::vector<double> values = parseToCoordinate(waypoint->getPose());
        bool isBreakpoint = waypoint->getIsTimeOptimalBreakpoint();
        emit addWaypointGui(waypointIndex, values, waypoint->getIKSelection(), isBreakpoint);
    }
 
    void
    TrajectoryController::helpAddTransitionGui(TransitionPtr transition, int transitionIndex)
    {
        double duration = transition->getUserDuration();
        double startTime = transition->getStart()->getUserTimestamp();
        InterpolationType interpolation = transition->getInterpolationType();
        emit addTransitionGui(transitionIndex, duration, startTime, interpolation);
    }
 
    void
    TrajectoryController::addTransitionWaypointGui(DesignerTrajectoryPtr trajectory)
    {
        if (trajectory)
        {
            for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints(); i++)
            {
                UserWaypointPtr waypoint = trajectory->getUserWaypoint(i);
                helpAddWaypointGui(waypoint, i);
            }
 
            for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints() - 1; i++)
            {
                TransitionPtr transition = trajectory->getTransition(i);
                helpAddTransitionGui(transition, i);
            }
        }
        else
        {
            return;
        }
    }
 
    //=============================================================================================
 
    void
    TrajectoryController::helpChangeTransitionGui(TransitionPtr transition, int transitionIndex)
    {
        double duration = transition->getUserDuration();
        double startTime = transition->getStart()->getUserTimestamp();
        InterpolationType interpolation = transition->getInterpolationType();
        emit changeTransitionGui(transitionIndex, duration, startTime, interpolation);
    }
 
    void
    TrajectoryController::helpChangeWaypointGui(UserWaypointPtr waypoint, int waypointIndex)
    {
        std::vector<double> values = parseToCoordinate(waypoint->getPose());
        bool isBreakpoint = waypoint->getIsTimeOptimalBreakpoint();
        emit changeWaypointGui(waypointIndex, values, waypoint->getIKSelection(), isBreakpoint);
    }
 
    void
    TrajectoryController::changeTransitionWaypointGui(DesignerTrajectoryPtr trajectory)
    {
        for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints(); i++)
        {
            UserWaypointPtr waypoint = trajectory->getUserWaypoint(i);
            helpChangeWaypointGui(waypoint, i);
        }
 
        for (unsigned int i = 0; i < trajectory->getNrOfUserWaypoints() - 1; i++)
        {
            TransitionPtr transition = trajectory->getTransition(i);
            helpChangeTransitionGui(transition, i);
        }
    }
 
    //=============================================================================================
 
    void
    TrajectoryController::helpImportDesignerTrajectory(DesignerTrajectoryPtr designerTrajectory)
    {
        removeTransitionWaypointGui();
        holder->import(designerTrajectory, true);
        manager = holder->getTrajectoryManager(designerTrajectory->getRns()->getName());
        rns = manager->getDesignerTrajectory()->getRns();
        emit rnsChanged(rns);
        emit showTrajectory(manager->getDesignerTrajectory());
        emit newTrajectory(QString::fromStdString(rns->getName()));
        emit updateSelectedTCP(QString::fromStdString(rns->getName()));
        addTransitionWaypointGui(manager->getDesignerTrajectory());
    }
 
    //=============================================================================================
 
    void
    TrajectoryController::helpCreateDesignerTrajectoryManager()
    {
        holder->createDesignerTrajectoryManager(rns->getName());
        manager = holder->getTrajectoryManager(rns->getName());
        emit rnsChanged(rns);
    }
 
    //=============================================================================================
 
    bool
    TrajectoryController::helpEnableExportPreviewAll()
    {
        bool help = false;
        for (VirtualRobot::RobotNodeSetPtr s : environment->getRobot()->getRobotNodeSets())
        {
            if (holder->rnsExists(s->getName()) &&
                holder->getTrajectoryManager(s->getName())->getIsInitialized())
            {
                if (holder->getTrajectoryManager(s->getName())
                        ->getDesignerTrajectory()
                        ->getNrOfUserWaypoints() > 1)
                {
                    help = true;
                }
                else
                {
                    return false;
                }
            }
        }
        return help;
    }
 
    void
    TrajectoryController::helpEnableUndoRedo()
    {
        if (manager != NULL)
        {
            emit enableRedo(manager->redoPossible());
            emit enableUndo(manager->undoPossible());
        }
        else
        {
            emit enableRedo(false);
            emit enableUndo(false);
        }
    }
 
    void
    TrajectoryController::helpEnableButtons()
    {
        emit enableExportButtons(helpEnableExportPreviewAll());
        emit enablePreviewAll(helpEnableExportPreviewAll());
        if (manager != NULL)
        {
            emit enableAdd(true);
            if (manager->getIsInitialized())
            {
                unsigned int nrWaypoints = manager->getDesignerTrajectory()->getNrOfUserWaypoints();
                emit enableIKSolutionButton(false);
                if (nrWaypoints >= 1)
                {
                    emit enableDeleteChange(true);
                    if (nrWaypoints >= 2)
                    {
                        emit enablePreview(true);
                    }
                    else
                    {
                        emit enablePreview(false);
                    }
                }
                else
                {
                    emit enableDeleteChange(false);
                    emit enablePreview(false);
                }
            }
            else
            {
                emit enableDeleteChange(false);
                emit enablePreview(false);
                emit enableIKSolutionButton(true);
            }
        }
        else
        {
            emit enableAdd(false);
            emit enablePreview(false);
            emit enableDeleteChange(false);
            emit enableIKSolutionButton(false);
        }
    }
 
    void
    TrajectoryController::helpExceptionMessageBox(std::string errorMessage)
    {
        QMessageBox* errorMessageBox = new QMessageBox;
        errorMessageBox->setWindowTitle(QString::fromStdString("Error Message"));
        errorMessageBox->setText(QString::fromStdString(errorMessage));
        errorMessageBox->exec();
    }
 
    //=============================================================================================
 
    void
    TrajectoryController::helpCollision()
    {
        if (holder == NULL)
        {
            return;
        }
 
        if (holder->isInCollision(activeColModelName, bodyColModelsNames, 100))
        {
            QMessageBox* changeInterpolationFailed = new QMessageBox;
            changeInterpolationFailed->setWindowTitle(QString::fromStdString("Information"));
            changeInterpolationFailed->setText(QString::fromStdString("A collision is detected."));
            changeInterpolationFailed->exec();
        }
    }
} // namespace armarx