DesignerTrajectoryManager.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 "DesignerTrajectoryManager.h"
 
using namespace armarx;
 
////////////////////////////////////////////////////////////////////////////////////////////
/// Struct-functions
////////////////////////////////////////////////////////////////////////////////////////////
 
void
DesignerTrajectoryManager::ManipulationInterval::addFirstUserWaypoint(UserWaypointPtr uw)
{
    userWaypointsList.push_back(uw);
}
 
void
DesignerTrajectoryManager::ManipulationInterval::pushFront(UserWaypointPtr uw, TransitionPtr t)
{
    userWaypointsList.push_front(uw);
    transitionsList.push_front(t);
    lowerIntervalLimit--;
}
 
void
DesignerTrajectoryManager::ManipulationInterval::pushBack(UserWaypointPtr uw, TransitionPtr t)
{
    userWaypointsList.push_back(uw);
    transitionsList.push_back(t);
    upperIntervalLimit++;
}
 
void
DesignerTrajectoryManager::ManipulationInterval::addBreakpointIndex(int index)
{
    breakpointIndicesSet.insert(index);
}
 
void
DesignerTrajectoryManager::ManipulationInterval::finishSearch()
{
    // copy userWaypointsList to userWaypoints vector
    userWaypoints.reserve(userWaypointsList.size());
    std::copy(
        userWaypointsList.begin(), userWaypointsList.end(), std::back_inserter(userWaypoints));
 
    // copy transitionsList to transitions vector
    transitions.reserve(transitionsList.size());
    std::copy(transitionsList.begin(), transitionsList.end(), std::back_inserter(transitions));
 
    // copy breakpointInicesSet to breakpointIndices vector
    breakpointIndices.reserve(breakpointIndicesSet.size());
    std::copy(breakpointIndicesSet.begin(),
              breakpointIndicesSet.end(),
              std::back_inserter(breakpointIndices));
}
 
UserWaypointPtr
DesignerTrajectoryManager::ManipulationInterval::getUserWaypointByRealIndex(unsigned int i) const
{
    if (i - lowerIntervalLimit < userWaypoints.size())
    {
        return userWaypoints[i - lowerIntervalLimit];
    }
    else
    {
        // modify Exception?
        throw OutOfRangeException();
    }
}
 
UserWaypointPtr
DesignerTrajectoryManager::ManipulationInterval::getUserWaypointByZeroBasedIndex(
    unsigned int i) const
{
    if (i < userWaypoints.size())
    {
        return userWaypoints[i];
    }
    else
    {
        // modify Exception?
        throw OutOfRangeException();
    }
}
 
TransitionPtr
DesignerTrajectoryManager::ManipulationInterval::getTransitionByRealIndex(unsigned int i) const
{
    if (i - lowerIntervalLimit < transitions.size())
    {
        return transitions[i - lowerIntervalLimit];
    }
    else
    {
        // modify Exception?
        throw OutOfRangeException();
    }
}
 
TransitionPtr
DesignerTrajectoryManager::ManipulationInterval::getTransitionByZeroBasedIndex(unsigned int i) const
{
    if (i < transitions.size())
    {
        return transitions[i];
    }
    else
    {
        // modify Exception?
        throw OutOfRangeException();
    }
}
 
std::vector<std::vector<double>>
DesignerTrajectoryManager::ManipulationInterval::getUserWaypointsIKSolutions(
    std::vector<std::vector<double>>& ikSolutions,
    unsigned int intervalStart,
    unsigned int intervalEnd)
{
    std::vector<std::vector<double>> result;
    for (unsigned int& newIndex : newIndexOfUserWaypoint)
    {
        if (newIndex < intervalStart)
        {
            continue;
        }
        if (newIndex > intervalEnd)
        {
            break;
        }
 
        result.push_back(ikSolutions[newIndex]);
    }
    return result;
}
 
void
DesignerTrajectoryManager::ManipulationInterval::applyJointAnglesOfUserWaypoints(
    std::vector<std::vector<double>> ikSolution)
{
    std::vector<std::vector<double>> jointAngles = getUserWaypointsIKSolutions(
        ikSolution, newIndexOfUserWaypoint[0], newIndexOfUserWaypoint[userWaypoints.size() - 1]);
 
    int count = 0;
    for (UserWaypointPtr w : userWaypoints)
    {
        w->setJointAngles(jointAngles[count++]);
    }
}
 
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Auxiliary functions
////////////////////////////////////////////////////////////////////////////////////////////////////
 
void
DesignerTrajectoryManager::saveState()
{
    //cut off the last mementos behind the current memento
    if (mementos.currentMemento < mementos.mementoList.size() - 1)
    {
        mementos.mementoList.resize(mementos.currentMemento + 1);
    }
 
    if (designerTrajectory)
    {
        mementos.mementoList.push_back(
            std::shared_ptr<DesignerTrajectory>(new DesignerTrajectory(*designerTrajectory)));
    }
    else
    {
        mementos.mementoList.push_back(nullptr);
    }
 
    //if the size of the list is bigger than the MEMENTO_MAX, it just delete the first
    //(oldest) memento
    if (mementos.mementoList.size() > MEMENTO_MAX)
    {
        mementos.mementoList.pop_front();
    }
    else
    {
        mementos.currentMemento++;
    }
}
 
DesignerTrajectoryManager::ManipulationInterval
DesignerTrajectoryManager::calculateManipulationInterval(unsigned int manipulationIndex)
{
    DesignerTrajectoryPtr& dt = designerTrajectory;
    ManipulationInterval mi;
 
    // set start upper- and lowerIntervalLimit to manipulationIndex
    mi.lowerIntervalLimit = manipulationIndex;
    mi.upperIntervalLimit = manipulationIndex;
 
    // add UserWaypoint with index = manipulationIndex to ManipulationInterval
    mi.addFirstUserWaypoint(dt->getUserWaypoint(manipulationIndex));
 
    // add index of first user waypoint if it is a breakepoint
    if (dt->getUserWaypoint(manipulationIndex)->getIsTimeOptimalBreakpoint())
    {
        mi.addBreakpointIndex(manipulationIndex);
    }
 
    // search upwards
    for (unsigned int i = manipulationIndex + 1; i < dt->getNrOfUserWaypoints(); i++)
    {
        UserWaypointPtr tmpUwp = dt->getUserWaypoint(i);
        TransitionPtr tmpTrans = dt->getTransition(i - 1);
        mi.pushBack(tmpUwp, tmpTrans);
 
        if (tmpUwp->getIsTimeOptimalBreakpoint())
        {
            mi.addBreakpointIndex(i);
 
            /*
            if (dt->getTransition(i - 1)->getInterpolationType()
                != dt->getTransition(i)->getInterpolationType())
            {
                break;
            }
            */
        }
    }
 
    // search downwards
    for (unsigned int i = manipulationIndex - 1; i != static_cast<unsigned>(-1); i--)
    {
        UserWaypointPtr tmpUwp = dt->getUserWaypoint(i);
        TransitionPtr tmpTrans = dt->getTransition(i);
        mi.pushFront(tmpUwp, tmpTrans);
 
        if (tmpUwp->getIsTimeOptimalBreakpoint())
        {
            mi.addBreakpointIndex(i);
 
            /*
            if (dt->getTransition(i - 1)->getInterpolationType()
                != dt->getTransition(i)->getInterpolationType())
            {
                break;
            }
            */
        }
    }
 
    // add first and last point to breakpointIndicesSet if they are not included already
    mi.addBreakpointIndex(mi.lowerIntervalLimit);
    mi.addBreakpointIndex(mi.upperIntervalLimit);
    mi.finishSearch();
 
    return mi;
}
 
std::vector<AbstractInterpolationPtr>
DesignerTrajectoryManager::getInterpolationObjects(ManipulationInterval& mi)
{
    std::vector<PoseBasePtr> poses;
    for (UserWaypointPtr& uwp : mi.userWaypoints)
    {
        poses.push_back(uwp->getPose());
    }
 
    std::vector<InterpolationType> interpolationTypes;
    for (TransitionPtr& tp : mi.transitions)
    {
        interpolationTypes.push_back(tp->getInterpolationType());
    }
 
    return InterpolationSegmentFactory::produceInterpolationSegments(poses, interpolationTypes);
}
 
void
DesignerTrajectoryManager::calculateInterpolatedPoints(
    std::vector<AbstractInterpolationPtr> interpolationObjects,
    unsigned int fps,
    ManipulationInterval& mi)
{
    // requirement: time in seconds !!!!
 
    // treat first point (with time 0.0) seperately
    ////mi.interpolatedPoses.push_back(interpolationObjects[0]->getPoseAt(0.0));
    ////mi.newIndexOfUserWaypoint.push_back(0);
 
    // calculate all timestamps and add the poses accordingly
    for (unsigned int i = 0; i < mi.transitions.size(); i++)
    {
        std::vector<double> timestamps;
 
        // check if transition is linear interpolated (no need to calculate further points between)
        // get the transition
        TransitionPtr tmpTransition = mi.getTransitionByZeroBasedIndex(i);
 
        // calculate total frame count (how many frames to insert between 2 userwaypoints)
        //unsigned int totalFrameCount = tmpTransition->getUserDuration() * fps;
        unsigned int totalFrameCount = tmpTransition->getTimeOptimalDuration() * fps;
 
        if (totalFrameCount == 0)
        {
            totalFrameCount = DEFAULT_FRAME_COUNT;
        }
 
        if (totalFrameCount > 1)
        {
            // calculate the increment value to get evenly distributed timestamps
            double increment = 1.0 / (totalFrameCount - 1);
 
            // starting at k = 1 jumps the 0.0 timestamp in every transition
            ////for (unsigned int k = 1; k < totalFrameCount; k++)
            for (unsigned int k = 0; k < totalFrameCount; k++)
            {
                timestamps.push_back(k * increment);
            }
        }
        else
        {
            timestamps.push_back(0.0);
            timestamps.push_back(1.0);
        }
 
 
        std::vector<PoseBasePtr> tmpInterpolatedPositions;
        for (double timestamp : timestamps)
        {
            ////mi.interpolatedPoses.push_back(interpolationObjects[i]->getPoseAt(timestamp));
            tmpInterpolatedPositions.push_back(interpolationObjects[i]->getPoseAt(timestamp));
        }
 
        mi.interpolatedTransitions.push_back(tmpInterpolatedPositions);
        tmpInterpolatedPositions.clear();
 
        // add the index of the last pose, which originated from an userwaypoint, to the
        // newIndexOfUserWaypoint vector
        ////mi.newIndexOfUserWaypoint.push_back(mi.interpolatedPoses.size() - 1);
    }
}
 
bool
DesignerTrajectoryManager::checkTransitionReachability(ManipulationInterval& mi,
                                                       unsigned int transitionIndex)
{
    for (PoseBasePtr pose : mi.interpolatedTransitions[transitionIndex])
    {
        continue;
        //if (!kin->isReachable(rns, pose))
 
        //provisorisch
        if (!kinSolver->isReachable(rns, pose, VirtualRobot::IKSolver::Position))
        {
            return false;
        }
    }
 
    return true;
}
 
std::vector<std::vector<double>>
DesignerTrajectoryManager::calculateIKSolutions(ManipulationInterval& mi)
{
    // IKSolutions
    std::vector<std::vector<double>> ikSolutions;
    ikSolutions.push_back(mi.userWaypoints[0]->getJointAngles());
 
    // the new index of userwaypoint 0 is 0
    mi.newIndexOfUserWaypoint.push_back(0);
 
    // motion planning insertion indices
    std::vector<InsertTransition> mpInserts;
 
    for (unsigned int transitionIndex = 0; transitionIndex < mi.interpolatedTransitions.size();
         transitionIndex++)
    {
        if (!checkTransitionReachability(mi, transitionIndex))
        {
            // this transition is not reachable
            InsertTransition mpInsert;
            mpInsert.transitionIndex = transitionIndex;
            mpInsert.insertionIndex = ikSolutions.size() - 1; //destinations.size() - 1;
            mpInserts.push_back(mpInsert);
 
            //calculate IKSolution (jump to end point of this transition)
            std::vector<double> start = {ikSolutions.back()};
 
            std::vector<PoseBasePtr> destinations = {
                mi.interpolatedTransitions[transitionIndex].back()};
 
            std::vector<VirtualRobot::IKSolver::CartesianSelection> selections = {
                mi.userWaypoints[transitionIndex + 1]->getIKSelection()};
 
            std::vector<std::vector<double>> tmpIkSolutions =
                kinSolver->solveRecursiveIKRelative(rns, start, destinations, selections);
 
            // insert tmpIkSolutions into ikSolutions
            ikSolutions.insert(ikSolutions.end(), tmpIkSolutions.begin(), tmpIkSolutions.end());
 
            // the new index of the userwaypoint transitionIndex+1 is set later
            // when inserting motion planning results into ikSolutions
            // reserve place in vector
            mi.newIndexOfUserWaypoint.push_back(-1);
 
            continue;
        }
 
        // this transition is reachable
        // calculate IKSolution of this transition
        // get start
        std::vector<double> start = {ikSolutions.back()};
 
        // get destinations: all poses from interpolatedTransition except for the first
        std::vector<PoseBasePtr> destinations(mi.interpolatedTransitions[transitionIndex].begin() +
                                                  1,
                                              mi.interpolatedTransitions[transitionIndex].end());
 
        // get selection
        std::vector<VirtualRobot::IKSolver::CartesianSelection> selections;
 
        // |destinations| - 1  times do:
        for (unsigned int i = 1; i < destinations.size(); i++)
        {
            selections.push_back(mi.userWaypoints[transitionIndex + 1]->getIKSelection());
        }
 
        // for last destination: get real IKSelection of UserWaypoint
        selections.push_back(mi.userWaypoints[transitionIndex + 1]->getIKSelection());
 
        // call ikSolver
        std::vector<std::vector<double>> tmpIkSolutions =
            kinSolver->solveRecursiveIKRelative(rns, start, destinations, selections);
        // END: calculate IKSolution of this transition
 
        // check for tmpIkSolutions for collision
 
        // this transition is reachable and has no collisions
        // insert tmpIkSolutions into ikSolutions
        ikSolutions.insert(ikSolutions.end(), tmpIkSolutions.begin(), tmpIkSolutions.end());
        // save the new index of the userwaypoint with index transitionIndex + 1
        mi.newIndexOfUserWaypoint.push_back(ikSolutions.size() - 1);
    }
    return ikSolutions;
}
 
std::vector<TimedTrajectory>
DesignerTrajectoryManager::calculateTimeOptimalTrajectories(
    std::vector<std::vector<double>> ikSolutions,
    ManipulationInterval& mi)
{
    std::vector<TimedTrajectory> result;
 
    for (unsigned int i = 0; i < mi.breakpointIndices.size() - 1; i++)
    {
        /*
         * mi.breakpointIndices are the indices before the interpolated points were inserted.
         */
        int intervalStart = mi.breakpointIndices[i];
        int intervalEnd = mi.breakpointIndices[i + 1];
        std::vector<std::vector<double>> ikSolutionsInterval(
            ikSolutions.begin() + mi.newIndexOfUserWaypoint[intervalStart],
            ikSolutions.begin() + 1 + mi.newIndexOfUserWaypoint[intervalEnd]);
 
        /*
         * Because intervalStart/intervalEnd are the breakpoint indices BEFORE interpolated
         * points were inserted,
         * their new index (retrieved with mi.newIndexOfUserWaypoint[...]) is needed as parameter
         * for the function mi.getUserWaypointsIKSolutions(..., ..., ...)
         */
        std::vector<std::vector<double>> userWaypointsIKSolution =
            mi.getUserWaypointsIKSolutions(ikSolutions,
                                           mi.newIndexOfUserWaypoint[intervalStart],
                                           mi.newIndexOfUserWaypoint[intervalEnd]);
 
        result.push_back(DesignerTrajectoryCalculator(environment)
                             .calculateTimeOptimalTrajectory(
                                 ikSolutionsInterval, userWaypointsIKSolution, rns, MAX_DEVIATION));
    }
 
    return result;
}
 
bool
DesignerTrajectoryManager::undo()
{
    if (mementos.currentMemento != 0)
    {
        if (*(std::next(mementos.mementoList.begin(), mementos.currentMemento - 1)))
        {
            designerTrajectory = DesignerTrajectoryPtr(new DesignerTrajectory(
                *(*(std::next(mementos.mementoList.begin(), mementos.currentMemento - 1)))));
            isInitialized = true;
        }
        else
        {
            isInitialized = false;
        }
        mementos.currentMemento--;
        return true;
    }
    else
    {
        return false;
    }
}
 
bool
DesignerTrajectoryManager::redo()
{
    if (mementos.currentMemento < mementos.mementoList.size() - 1)
    {
        if (*std::next(mementos.mementoList.begin(), mementos.currentMemento + 1))
        {
            designerTrajectory = DesignerTrajectoryPtr(new DesignerTrajectory(
                *(*(std::next(mementos.mementoList.begin(), mementos.currentMemento + 1)))));
        }
        else
        {
            isInitialized = false;
        }
 
        mementos.currentMemento++;
        return true;
    }
    else
    {
        return false;
    }
}
 
bool
DesignerTrajectoryManager::getIsInitialized()
{
    return isInitialized;
}
 
void
DesignerTrajectoryManager::theUniversalMethod(unsigned int index)
{
    if (designerTrajectory->getNrOfUserWaypoints() > 1)
    {
        //calculate ikSolution of mainpulation intervall//////////////////////////////////////
        ManipulationInterval mi = calculateManipulationInterval(index);
 
        // poses
        std::vector<PoseBasePtr> poses;
        std::vector<PoseBasePtr> posesBackup;
 
        // selections
        std::vector<VirtualRobot::IKSolver::CartesianSelection> selections;
        std::vector<VirtualRobot::IKSolver::CartesianSelection> selectionsBackup;
 
        for (UserWaypointPtr uwp : mi.userWaypoints)
        {
            poses.push_back(uwp->getPose());
            posesBackup.push_back(PoseBasePtr(
                new Pose(Pose(uwp->getPose()->position, uwp->getPose()->orientation).toEigen())));
            selections.push_back(uwp->getIKSelection());
            selectionsBackup.push_back(uwp->getIKSelection());
        }
 
        if (InterpolationSegmentFactory::needsOptimizing(selections))
        {
            InterpolationSegmentFactory::optimizeControlPoints(poses, selections);
        }
 
        std::vector<InterpolationType> interpolations;
        for (TransitionPtr trans : mi.transitions)
        {
            interpolations.push_back(trans->getInterpolationType());
        }
        int count = 0;
        for (UserWaypointPtr uwp : mi.userWaypoints)
        {
            uwp->setPose(poses[count]);
            uwp->setIKSelection(selections[count]);
            count++;
        }
 
        calculateInterpolatedPoints(
            InterpolationSegmentFactory::produceInterpolationSegments(poses, interpolations),
            DEFAULT_FRAME_COUNT,
            mi);
        //calculateInterpolatedPoints(getInterpolationObjects(mi), DEFAULT_FRAME_COUNT, mi);
 
        //bool motionPlanningCalled = false;
 
        std::vector<std::vector<double>> ikSolutions = calculateIKSolutions(mi);
 
        count = 0;
        for (UserWaypointPtr uwp : mi.userWaypoints)
        {
            uwp->setIKSelection(selectionsBackup[count]);
            uwp->setPose(posesBackup[count]);
            count++;
        }
 
        //Set the JointAngles of the UserWaypoints////////////////////////////////////////////
        mi.applyJointAnglesOfUserWaypoints(ikSolutions);
 
        //set interBreakpointsTrajectories////////////////////////////////////////////////////
        std::vector<TimedTrajectory> timedTrajectories =
            calculateTimeOptimalTrajectories(ikSolutions, mi);
        //search how many interBreakPointTrajectories are behind and before the new
        //interBreakpointTrajectories
        int countBefore = 0;
        int countAfter = 0;
        if (mi.lowerIntervalLimit != 0)
        {
            countBefore++;
            for (unsigned int i = mi.lowerIntervalLimit - 1; i > 0; i--)
            {
                if (designerTrajectory->getUserWaypoint(i)->getIsTimeOptimalBreakpoint())
                {
                    countBefore++;
                }
            }
        }
 
        if (mi.upperIntervalLimit != designerTrajectory->getNrOfUserWaypoints() - 1)
        {
            countAfter++;
            for (unsigned int i = mi.upperIntervalLimit + 1;
                 i < designerTrajectory->getNrOfUserWaypoints() - 1;
                 i++)
            {
                countAfter++;
            }
        }
 
        std::vector<TrajectoryPtr> InterBreakPointTrajectories =
            designerTrajectory->getInterBreakpointTrajectories();
        std::vector<TrajectoryPtr> newInterBreakPointTrajectories;
        newInterBreakPointTrajectories.insert(newInterBreakPointTrajectories.end(),
                                              InterBreakPointTrajectories.begin(),
                                              InterBreakPointTrajectories.begin() + countBefore);
 
        // set new inter breakpoint trajectories
        for (TimedTrajectory& t : timedTrajectories)
        {
            TrajectoryPtr traj = t.getTrajectory();
            newInterBreakPointTrajectories.push_back(traj);
        }
 
        newInterBreakPointTrajectories.insert(newInterBreakPointTrajectories.end(),
                                              InterBreakPointTrajectories.end() - countAfter,
                                              InterBreakPointTrajectories.end());
        designerTrajectory->setInterBreakpointTrajectories(newInterBreakPointTrajectories);
 
        //set newTimeOptimalDuration of each Transition///////////////////////////////////////
        //and timeOptimalTimestamp of each UserWaypoint///////////////////////////////////////
        //and Trajectories of all changed Transitions ////////////////////////////////////////
        TrajectoryPtr traj = designerTrajectory->getTimeOptimalTrajectory();
        std::vector<double> timestamps = traj->getTimestamps();
        unsigned int trajCount = 0;
 
        //search for the timestamp of first changed point of traj
        while (trajCount < timestamps.size() &&
               designerTrajectory->getUserWaypoint(mi.breakpointIndices[0])
                       ->getTimeOptimalTimestamp() > timestamps[trajCount])
        {
            trajCount++;
        }
 
        for (unsigned int t = 0; t < timedTrajectories.size(); t++)
        {
            //all timestamps of the userWaypoint of the timedTrajectory
            std::vector<double> timedUserWaypoints = timedTrajectories[t].getUserPoints();
 
            if (timedUserWaypoints.size() == 0)
            {
                throw InvalidArgumentException();
            }
 
            unsigned int count = 0;
 
            //set timeOptimalDuration of all changed transitions
            //set timeOptimalTimestamp of all changed userWaypoints
            for (unsigned int i = mi.breakpointIndices[t]; i < mi.breakpointIndices[t + 1]; i++)
            {
                TransitionPtr trans = designerTrajectory->getTransition(i);
                trans->setTimeOptimalDuration(timedUserWaypoints[count + 1] -
                                              timedUserWaypoints[count]);
                count++;
            }
 
 
            //set all Trajectories of changed Transitions
            for (unsigned int i = mi.breakpointIndices[t]; i < mi.breakpointIndices[t + 1]; i++)
            {
                TransitionPtr trans = designerTrajectory->getTransition(i);
                UserWaypointPtr end = trans->getEnd();
                unsigned int trajCountEnd = trajCount + 1;
                std::vector<double> newTimestamps = {0.0};
                std::vector<std::vector<double>> newTrajData;
                //add first jointAngles of first timestamp
                for (unsigned int dim = 0; dim < traj->dim(); dim++)
                {
                    newTrajData.push_back({traj->getState(timestamps[trajCount], dim)});
                }
 
                //add all jointAngles for all timestamps at transition
                while (trajCountEnd < timestamps.size() &&
                       timestamps[trajCountEnd] < end->getTimeOptimalTimestamp())
                {
                    newTimestamps.push_back(timestamps[trajCountEnd] - timestamps[trajCount]);
                    for (unsigned int dim = 0; dim < traj->dim(); dim++)
                    {
                        //jointAngles of the dimension
                        newTrajData[dim].push_back(traj->getState(timestamps[trajCountEnd], dim));
                    }
                    trajCountEnd++;
                }
                trajCount = trajCountEnd;
                TrajectoryPtr newTraj = TrajectoryPtr(
                    new Trajectory(newTrajData, newTimestamps, traj->getDimensionNames()));
                trans->setTrajectory(newTraj);
            }
        }
 
        //shift all timeOptimaltimestamps of the userWaypoints behind last BreakpointIndices
        //it goes through transitions (getNrOfUserWaypoints - 2 = NrOfTransitions - 1)
        for (unsigned int i = mi.breakpointIndices.back();
             i <= designerTrajectory->getNrOfUserWaypoints() - 2;
             i++)
        {
            //the setter of transitions updates the timeOptimalTimestamps of end- and start-
            //Userwaypoint
            double time = designerTrajectory->getTransition(i)->getTimeOptimalDuration();
            designerTrajectory->getTransition(i)->setTimeOptimalDuration(time);
        }
 
 
        //update UserTimestamps of all UserWaypoints
        for (unsigned int i = 0; i < designerTrajectory->getNrOfUserWaypoints() - 2; i++)
        {
            double time = designerTrajectory->getTransition(i)->getStart()->getUserTimestamp() +
                          designerTrajectory->getTransition(i)->getUserDuration();
            designerTrajectory->getTransition(i)->getEnd()->setUserTimestamp(time);
        }
    }
    saveState();
}
 
std::vector<double>
DesignerTrajectoryManager::getNewIkSolutionOfFirstPoint(PoseBasePtr oldStart,
                                                        PoseBasePtr newStart,
                                                        std::vector<double> jointAnglesOldStart)
{
    //Resolve recursive IK from oldStart to NewStart
    std::vector<AbstractInterpolationPtr> ip =
        InterpolationSegmentFactory::produceLinearInterpolationSegments({oldStart, newStart});
    std::vector<PoseBasePtr> poses;
    std::vector<VirtualRobot::IKSolver::CartesianSelection> selections;
 
    for (AbstractInterpolationPtr current : ip)
    {
        for (double i = 1; i < DEFAULT_FRAME_COUNT - 1; i = i + 1.0)
        {
            poses.push_back(current->getPoseAt(i / DEFAULT_FRAME_COUNT));
            selections.push_back(VirtualRobot::IKSolver::CartesianSelection::Position);
        }
    }
    poses.push_back(newStart);
    selections.push_back(VirtualRobot::IKSolver::CartesianSelection::All);
    std::vector<std::vector<double>> ikSolutions =
        kinSolver->solveRecursiveIKRelative(rns, jointAnglesOldStart, poses, selections);
 
    return ikSolutions.back();
}
 
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Constructor(s)
////////////////////////////////////////////////////////////////////////////////////////////////////
 
DesignerTrajectoryManager::DesignerTrajectoryManager(std::string rnsName,
                                                     EnvironmentPtr environment) :
    kinSolver(KinematicSolver::getInstance(environment->getScene(), environment->getRobot())),
    environment(environment)
{
    rns = environment->getRobot()->getRobotNodeSet(rnsName);
}
 
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Public Functions
////////////////////////////////////////////////////////////////////////////////////////////////////
 
void
DesignerTrajectoryManager::initializeDesignerTrajectory(std::vector<double>& jointAngles)
{
    PoseBasePtr pb = kinSolver->doForwardKinematic(rns, jointAngles);
    Pose pose = Pose(pb->position, pb->orientation);
    pb = PoseBasePtr(new Pose(rns->getKinematicRoot()->toLocalCoordinateSystem(pose.toEigen())));
 
    // create UserWaypoint with the given jointAngles
    UserWaypointPtr tmpUwp(new UserWaypoint(pb));
    tmpUwp->setJointAngles(jointAngles);
 
    designerTrajectory = DesignerTrajectoryPtr(new DesignerTrajectory(tmpUwp, rns));
 
    //mementos.currentMemento = 0;
 
    if (mementos.mementoList.size() != 0)
    {
        saveState();
    }
    else
    {
        mementos.mementoList.push_back(
            DesignerTrajectoryPtr(new DesignerTrajectory(*designerTrajectory)));
    }
 
    isInitialized = true;
}
 
void
DesignerTrajectoryManager::addWaypoint(const PoseBasePtr pb)
{
    if (isInitialized)
    {
        //get a clean copy of the designerTrajectory
        designerTrajectory = getDesignerTrajectory();
 
        UserWaypointPtr newPoint = UserWaypointPtr(new UserWaypoint(pb));
        designerTrajectory->addLastUserWaypoint(newPoint);
        theUniversalMethod(designerTrajectory->getNrOfUserWaypoints() - 1);
    }
    else
    {
        throw NotInitializedException("Manager is not intialized", "Manager");
    }
}
 
void
DesignerTrajectoryManager::insertWaypoint(unsigned int index, const PoseBasePtr pb)
{
    if (isInitialized)
    {
        //get a clean copy of the designerTrajectory
        designerTrajectory = getDesignerTrajectory();
 
        if (index != 0)
        {
            UserWaypointPtr newPoint = UserWaypointPtr(new UserWaypoint(pb));
            designerTrajectory->insertUserWaypoint(newPoint, index);
            theUniversalMethod(index);
        }
        else
        {
            PoseBasePtr oldStart = designerTrajectory->getUserWaypoint(0)->getPose();
            std::vector<double> jointAngles = getNewIkSolutionOfFirstPoint(
                oldStart, pb, designerTrajectory->getUserWaypoint(0)->getJointAngles());
            if (jointAngles.size() == 0)
            {
                return;
            }
            UserWaypointPtr newPoint = UserWaypointPtr(new UserWaypoint(pb));
            designerTrajectory->insertUserWaypoint(newPoint, index);
            designerTrajectory->getUserWaypoint(0)->setJointAngles(jointAngles);
 
            theUniversalMethod(index);
        }
    }
    else
    {
        throw NotInitializedException("Manager is not intialized", "Manager");
    }
}
 
void
DesignerTrajectoryManager::editWaypointPoseBase(unsigned int index, const PoseBasePtr pb)
{
    if (isInitialized)
    {
        //get a clean copy of the designerTrajectory
        designerTrajectory = getDesignerTrajectory();
        if (index != 0)
        {
            designerTrajectory->getUserWaypoint(index)->setPose(pb);
        }
        else
        {
            std::vector<double> newJointAngles;
            if (designerTrajectory->getNrOfUserWaypoints() > 1)
            {
                newJointAngles = getNewIkSolutionOfFirstPoint(
                    designerTrajectory->getUserWaypoint(1)->getPose(),
                    pb,
                    designerTrajectory->getUserWaypoint(1)->getJointAngles());
                if (newJointAngles.size() == 0)
                {
                    return;
                }
                designerTrajectory->getUserWaypoint(0)->setPose(pb);
                designerTrajectory->getUserWaypoint(0)->setJointAngles(newJointAngles);
            }
            else
            {
                //newJointAngles = kinSolver->solveIK(rns, pb, designerTrajectory->getUserWaypoint(0)->getIKSelection(), 50);
                newJointAngles = getNewIkSolutionOfFirstPoint(
                    designerTrajectory->getUserWaypoint(0)->getPose(),
                    pb,
                    designerTrajectory->getUserWaypoint(0)->getJointAngles());
                if (newJointAngles.size() == 0)
                {
                    return;
                }
                UserWaypointPtr uwpTmp(new UserWaypoint(pb));
                uwpTmp->setJointAngles(newJointAngles);
                uwpTmp->setIKSelection(designerTrajectory->getUserWaypoint(0)->getIKSelection());
                uwpTmp->setIsTimeOptimalBreakpoint(
                    designerTrajectory->getUserWaypoint(0)->getIsTimeOptimalBreakpoint());
                DesignerTrajectoryPtr dtTmp(new DesignerTrajectory(uwpTmp, rns));
                designerTrajectory = dtTmp;
            }
        }
        theUniversalMethod(index);
    }
    else
    {
        throw NotInitializedException("Manager is not intialized", "Manager");
    }
}
 
void
DesignerTrajectoryManager::editWaypointIKSelection(
    unsigned int index,
    VirtualRobot::IKSolver::CartesianSelection ikSelection)
{
    if (isInitialized)
    {
        //get a clean copy of the designerTrajectory
        designerTrajectory = getDesignerTrajectory();
 
        designerTrajectory->getUserWaypoint(index)->setIKSelection(ikSelection);
        theUniversalMethod(index);
    }
    else
    {
        throw NotInitializedException("Manager is not intialized", "Manager");
    }
}
 
void
DesignerTrajectoryManager::deleteWaypoint(unsigned int index)
{
    if (isInitialized)
    {
        //get a clean copy of the designerTrajectory
        designerTrajectory = getDesignerTrajectory();
 
        if (designerTrajectory->getNrOfUserWaypoints() == 1)
        {
            isInitialized = false;
            designerTrajectory = nullptr;
            saveState();
        }
        else
        {
            designerTrajectory->deleteUserWaypoint(index);
 
            //check if first userwaypoint was delted
            if (index == 0)
            {
                designerTrajectory->getUserWaypoint(0)->setTimeOptimalTimestamp(0);
                designerTrajectory->getUserWaypoint(0)->setUserTimestamp(0);
            }
 
            // check if just one waypoint is left
            if (designerTrajectory->getNrOfUserWaypoints() == 1)
            {
                saveState();
                return;
            }
 
            // check if last userwaypoint was deleted
            if (index == designerTrajectory->getNrOfUserWaypoints())
            {
                theUniversalMethod(index - 1);
            }
            else
            {
                theUniversalMethod(index);
            }
        }
    }
    else
    {
        throw NotInitializedException("Manager is not intialized", "Manager");
    }
}
 
void
DesignerTrajectoryManager::setTransitionInterpolation(unsigned int index, InterpolationType it)
{
    if (isInitialized)
    {
        //get a clean copy of the designerTrajectory
        designerTrajectory = getDesignerTrajectory();
 
        designerTrajectory->getTransition(index)->setInterpolationType(it);
        theUniversalMethod(index);
    }
    else
    {
        throw NotInitializedException("Manager is not intialized", "Manager");
    }
}
 
void
DesignerTrajectoryManager::setWaypointAsBreakpoint(unsigned int index, bool b)
{
    if (isInitialized)
    {
        //get a clean copy of the designerTrajectory
        designerTrajectory = getDesignerTrajectory();
 
        designerTrajectory->getUserWaypoint(index)->setIsTimeOptimalBreakpoint(b);
        theUniversalMethod(index);
    }
    else
    {
        throw NotInitializedException("Manager is not intialized", "Manager");
    }
}
 
void
DesignerTrajectoryManager::setTransitionUserDuration(unsigned int index, double duration)
{
    if (isInitialized)
    {
        //get a clean copy of the designerTrajectory
        designerTrajectory = getDesignerTrajectory();
 
        designerTrajectory->getTransition(index)->setUserDuration(duration);
        //shift all userTimestamps of the userwaypoints behind
        for (unsigned int i = index + 1; i < designerTrajectory->getNrOfUserWaypoints() - 1; i++)
        {
            TransitionPtr trans = designerTrajectory->getTransition(i);
            trans->setUserDuration(trans->getUserDuration());
        }
        //theUniversalMethod(index);
        saveState();
    }
    else
    {
        throw NotInitializedException("Manager is not intialized", "Manager");
    }
}
 
DesignerTrajectoryPtr
DesignerTrajectoryManager::getDesignerTrajectory() const
{
    if (isInitialized)
    {
        //return DesignerTrajectoryPtr(new DesignerTrajectory(*designerTrajectory));
        return DesignerTrajectoryPtr(new DesignerTrajectory(
            *(*std::next(mementos.mementoList.begin(), mementos.currentMemento))));
    }
    else
    {
        return nullptr;
    }
}
 
bool
DesignerTrajectoryManager::import(DesignerTrajectoryPtr newDesignerTrajectory)
{
    if (newDesignerTrajectory->getNrOfUserWaypoints() > 1 &&
        newDesignerTrajectory->getRns()->getName() == rns->getName())
    {
        TrajectoryPtr trajectory = newDesignerTrajectory->getTimeOptimalTrajectory();
 
        //set Trajectories of transitions
        for (unsigned int i = 0; i < newDesignerTrajectory->getNrOfUserWaypoints() - 1; i++)
        {
            TransitionPtr trans = newDesignerTrajectory->getTransition(i);
            double transBeginTime = trans->getStart()->getTimeOptimalTimestamp();
            double transEndTime = trans->getEnd()->getTimeOptimalTimestamp();
            TrajectoryPtr traj = trajectory->getPart(transBeginTime, transEndTime, 2);
 
 
            //shift timestamps
            std::vector<std::vector<double>> nodeData;
            for (unsigned int dim = 0; dim < traj->dim(); dim++)
            {
                nodeData.push_back(traj->getDimensionData(dim));
            }
            std::vector<double> newTimestamps = {0.0};
            std::vector<double> oldTimestamps = traj->getTimestamps();
            for (unsigned int j = 1; j < oldTimestamps.size(); j++)
            {
                if (oldTimestamps[j] - transBeginTime >= 0)
                {
                    newTimestamps.push_back(oldTimestamps[j] - transBeginTime);
                }
                else
                {
                    newTimestamps.push_back(0);
                }
            }
 
            TrajectoryPtr shiftedTraj(
                new Trajectory(nodeData, newTimestamps, traj->getDimensionNames()));
            trans->setTrajectory(shiftedTraj);
        }
        designerTrajectory = newDesignerTrajectory;
 
        if (mementos.mementoList.size() != 0)
        {
            saveState();
        }
        else
        {
            mementos.mementoList.push_back(
                DesignerTrajectoryPtr(new DesignerTrajectory(*designerTrajectory)));
        }
        isInitialized = true;
        return true;
    }
    return false;
}
 
bool
DesignerTrajectoryManager::undoPossible()
{
    return mementos.currentMemento > 0;
}
 
bool
DesignerTrajectoryManager::redoPossible()
{
    if (mementos.mementoList.size() != 0 &&
        mementos.currentMemento < mementos.mementoList.size() - 1)
    {
        return true;
    }
    else
    {
        return false;
    }
}