DesignerTrajectory.cpp

Go to the documentation of this file.

#include "DesignerTrajectory.h"
#include <iostream>
using namespace std;
 
using namespace armarx;
 
VirtualRobot::RobotNodeSetPtr DesignerTrajectory::getRns()
{
    return rns;
}
 
void DesignerTrajectory::setRns(const VirtualRobot::RobotNodeSetPtr& value)
{
    if (value != nullptr)
    {
        rns = value;
    }
    else
    {
        throw InvalidArgumentException("Can not set VirtualRobot::RobotNodeSetPtr rns = nullptr");
    }
}
 
std::vector<TrajectoryPtr> DesignerTrajectory::getInterBreakpointTrajectories()
{
    return interBreakpointTrajectories;
}
 
void DesignerTrajectory::setInterBreakpointTrajectories(
    const std::vector<TrajectoryPtr>& value)
{
    if (value.size() != 0)
    {
        interBreakpointTrajectories = value;
    }
    else
    {
        throw InvalidArgumentException(
            "Can not set std::vector<TrajectoryPtr> interBreakpointTrajectories with empty vector ");
    }
}
 
 
armarx::DesignerTrajectory::DesignerTrajectory(UserWaypointPtr& firstPoint,
        VirtualRobot::RobotNodeSetPtr newRns)
{
    if (newRns != nullptr)
    {
        rns = newRns;
    }
    else
    {
        throw InvalidArgumentException("Can not construct DesignerTrajectory with VirtualRobot::RobotNodeSetPtr rns = nullptr");
    }
    if (firstPoint != nullptr)
    {
        addFirstUserWaypoint(firstPoint);
    }
    else
    {
        throw InvalidArgumentException("Can not construct DesignerTrajectory with UserWaypointPtr firstPoint = nullptr");
    }
 
    std::vector<Ice::DoubleSeq> data;
    std::vector<double> jointAngles = firstPoint->getJointAngles();
    if (jointAngles.size() != 0)
    {
        //add jointAngles to every dimension
        for (unsigned int i = 0; i < jointAngles.size(); i++)
        {
            data.push_back({jointAngles[i]});
        }
    }
    else
    {
        throw InvalidArgumentException("firstPoint of a DesignerTrajectory must have jointAngles");
    }
    //firstPoint has eveytime timestamp 0
    interBreakpointTrajectories.push_back(TrajectoryPtr(new Trajectory(data,
    {0}, rns->getNodeNames())));
}
 
DesignerTrajectory::DesignerTrajectory(const DesignerTrajectory& source) :
    rns(source.rns)
{
    std::vector<TrajectoryPtr> trajectoriesTmp;
    std::vector<UserWaypointPtr> userWaypointsTmp;
    std::vector<TransitionPtr> transitionsTmp;
 
    for (TrajectoryPtr t : source.interBreakpointTrajectories)
    {
        trajectoriesTmp.push_back(TrajectoryPtr(new Trajectory(*t)));
    }
 
 
    for (UserWaypointPtr w : source.userWaypoints)
    {
        userWaypointsTmp.push_back(UserWaypointPtr(new UserWaypoint(*w)));
    }
 
    for (unsigned int i = 0; i < userWaypointsTmp.size() - 1; i++)
    {
        UserWaypointPtr start = userWaypointsTmp[i];
        UserWaypointPtr end = userWaypointsTmp[i + 1];
        TransitionPtr trans = source.transitions[i];
 
        transitionsTmp.push_back(TransitionPtr(new Transition(*trans, start, end)));
    }
 
    userWaypoints = userWaypointsTmp;
    interBreakpointTrajectories = trajectoriesTmp;
    transitions = transitionsTmp;
}
 
void DesignerTrajectory::addFirstUserWaypoint(UserWaypointPtr& point)
{
    if (point != nullptr)
    {
        userWaypoints.insert(userWaypoints.begin(), point);
 
        if (userWaypoints.size() > 1)
        {
            transitions.insert(transitions.begin(), TransitionPtr(
                                   new Transition(userWaypoints[0], userWaypoints[1])));
 
        }
    }
    else
    {
        throw InvalidArgumentException("Can not add UserWaypoint with point = nullptr");
    }
 
}
 
void DesignerTrajectory::addLastUserWaypoint(UserWaypointPtr& point)
{
    if (point != nullptr)
    {
        userWaypoints.push_back(point);
        if (userWaypoints.size() > 1)
        {
            transitions.push_back(TransitionPtr(new Transition(
                                                    userWaypoints[userWaypoints.size() - 2],
                                                    userWaypoints[userWaypoints.size() - 1])));
        }
    }
    else
    {
        throw InvalidArgumentException("Can not add UserWaypoint with point = nullptr");
    }
}
 
void DesignerTrajectory::insertUserWaypoint(
    UserWaypointPtr& point, unsigned int index)
{
    if (point != nullptr)
    {
        if (index != 0 && index < userWaypoints.size())
        {
            userWaypoints.insert(userWaypoints.begin() + index, point);
 
            if (userWaypoints.size() > 1)
            {
                transitions.insert(transitions.begin() + index - 1,
                                   TransitionPtr(
                                       new Transition(userWaypoints[index - 1],
                                                      userWaypoints[index])));
                transitions[index]->setStart(userWaypoints[index]);
            }
        }
        else
        {
            if (index == 0)
            {
                addFirstUserWaypoint(point);
            }
            else
            {
                throw IndexOutOfBoundsException("insertUserWaypoint");
            }
        }
    }
    else
    {
        throw InvalidArgumentException("Can not add UserWaypoint with point = nullptr");
    }
 
}
 
void DesignerTrajectory::deleteUserWaypoint(unsigned int index)
{
    if (index < userWaypoints.size())
    {
        userWaypoints.erase(userWaypoints.begin() + index);
        //delete first waypoint
        if (index == 0)
        {
            transitions.erase(transitions.begin());
        }
        //delete last Waypoint
        else if (index == transitions.size())
        {
            transitions.erase(transitions.begin() + index - 1);
        }
        else
        {
            transitions.erase(transitions.begin() + index);
            transitions.erase(transitions.begin() + index - 1);
            transitions.insert(transitions.begin() + index - 1,
                               std::shared_ptr<Transition>(
                                   new Transition(userWaypoints[index - 1],
                                                  userWaypoints[index])));
        }
    }
    else
    {
        throw IndexOutOfBoundsException("deleteUserWaypoint");
    }
}
 
 
unsigned int DesignerTrajectory::getNrOfUserWaypoints() const
{
    return userWaypoints.size();
}
 
UserWaypointPtr DesignerTrajectory::getUserWaypoint(unsigned int index)
{
    if (index < userWaypoints.size())
    {
        return userWaypoints[index];
    }
    else
    {
        throw IndexOutOfBoundsException("getUserWaypoint");
    }
 
}
 
TransitionPtr DesignerTrajectory::getTransition(unsigned int index)
{
    if (index < transitions.size())
    {
        return transitions[index];
    }
    else
    {
        throw IndexOutOfBoundsException();
    }
 
}
 
TrajectoryPtr DesignerTrajectory::getTimeOptimalTrajectory()
{
    std::vector<Ice::DoubleSeq> dimensionDatas = getDimensionDatas();
    Ice::DoubleSeq timestamps = getAllTimestamps();
    TrajectoryPtr tmp = TrajectoryPtr(new Trajectory(dimensionDatas, timestamps,
                                      interBreakpointTrajectories[0]->getDimensionNames()));
    setLimitless(tmp, rns);
 
    return tmp;
}
 
TrajectoryPtr DesignerTrajectory::getTrajectorySegment(unsigned int index)
{
    if (index < interBreakpointTrajectories.size())
    {
        return interBreakpointTrajectories[index];
    }
    else
    {
        throw IndexOutOfBoundsException();
    }
 
}
 
std::vector<UserWaypointPtr> DesignerTrajectory::getAllUserWaypoints() const
{
    std::vector <UserWaypointPtr> tmp;
 
    for (unsigned int i = 0; i < userWaypoints.size(); i++)
    {
        tmp.push_back(UserWaypointPtr(new UserWaypoint(*userWaypoints[i])));
    }
    return tmp;
}
 
std::vector<UserWaypointPtr> DesignerTrajectory::getAllUserWaypoints()
{
    return userWaypoints;
}
 
TrajectoryPtr DesignerTrajectory::getFinalTrajectory() //const
{
    if (userWaypoints.size() > 1)
    {
 
        std::vector<Ice::DoubleSeq> dimensionDatas = getDimensionDatas();
        std::vector<double> timestamps = getAllTimestamps();
 
        double tmpDif = 0;
        unsigned int count = 0;
        for (unsigned int i = 0; i < transitions.size() - 1; i++)
        {
            TransitionPtr t = transitions[i];
            double stretch = t->getUserDuration() / t->getTimeOptimalDuration();
            unsigned int start = count;
            //search for the index which represents the end of the transition
            double endTime = t->getEnd()->getTimeOptimalTimestamp();
            while (count + 1 < timestamps.size() && endTime > timestamps[count + 1])
            {
                count++;
            }
            unsigned int end = count;
            for (unsigned int k = start; k < end; k++)
            {
                timestamps[k + 1] = timestamps[k + 1] + tmpDif;
                double oldDuration = timestamps[k + 1] - timestamps[k];
                double newDuration = oldDuration * stretch;
                tmpDif = tmpDif + newDuration - oldDuration;
                timestamps[k + 1] = timestamps[k] + newDuration;
            }
        }
        //last transition
        TransitionPtr t = transitions.back();
        double stretch = t->getUserDuration() / t->getTimeOptimalDuration();
        unsigned int start = count;
        unsigned int end = timestamps.size() - 1;
        for (unsigned int k = start; k < end; k++)
        {
            timestamps[k + 1] = timestamps[k + 1] + tmpDif;
            double oldDuration = timestamps[k + 1] - timestamps[k];
            double newDuration = (timestamps[k + 1] - timestamps[k]) * stretch;
            tmpDif = tmpDif + newDuration - oldDuration;
            timestamps[k + 1] = timestamps[k] + newDuration;
        }
        TrajectoryPtr traj = TrajectoryPtr(new Trajectory(dimensionDatas,
                                           timestamps,
                                           interBreakpointTrajectories[0]->getDimensionNames()));
        setLimitless(traj, rns);
        return traj;
 
    }
    else
    {
        return interBreakpointTrajectories[0];
    }
}
 
 
//////////////private/////////////////////////////////////////////////////////////////////
std::vector<std::vector<double>> DesignerTrajectory::getDimensionDatas()
{
    std::vector<Ice::DoubleSeq> dimensionDatas;
 
    if (interBreakpointTrajectories.size() != 0)
    {
        //just for the first point
        for (unsigned int i = 0; i < interBreakpointTrajectories[0]->dim(); i++)
        {
            //get the JointAngle of the first point at current dimension
            dimensionDatas.push_back({interBreakpointTrajectories[0]->getDimensionData(i)[0]});
        }
 
        //through all trajectories
        for (const TrajectoryPtr& t : interBreakpointTrajectories)
        {
            //through all dimensions
            unsigned int dimension = t->dim();
            for (unsigned int i = 0; i < dimension; i++)
            {
                //get all jointAngles
                std::vector<double> newDatas = t->getDimensionData(i);
                dimensionDatas[i].insert(dimensionDatas[i].end(),
                                         newDatas.begin() + 1,
                                         newDatas.end());
            }
        }
        return dimensionDatas;
    }
    else
    {
        throw LogicError("No inter breakpoint trajectories");
    }
}
 
 
std::vector<double> DesignerTrajectory::getAllTimestamps()
{
    std::vector<double> timestamps;
 
    if (interBreakpointTrajectories.size() != 0)
    {
        //first timestamp
        timestamps.push_back(interBreakpointTrajectories[0]->getTimestamps()[0]);
        //through all trajectories
        for (const TrajectoryPtr& t : interBreakpointTrajectories)
        {
            double d = timestamps.back();
            std::vector<double> newTimestamps = t->getTimestamps();
            for (unsigned int i = 1; i < newTimestamps.size(); i++)
            {
                timestamps.push_back(newTimestamps[i] + d);
            }
        }
        return timestamps;
    }
    else
    {
        throw LogicError("No inter breakpoint trajectories");
    }
}
 
void DesignerTrajectory::setLimitless(TrajectoryPtr traj, VirtualRobot::RobotNodeSetPtr rns)
{
    //set Limitless state for smooth interpolation
    LimitlessStateSeq states;
    for (VirtualRobot::RobotNodePtr node : rns->getAllRobotNodes())
    {
        LimitlessState state;
        state.enabled = node->isLimitless();
        state.limitLo = node->getJointLimitLow();
        state.limitHi = node->getJointLimitHigh();
        states.push_back(state);
    }
    traj->setLimitless(states);
}