TimedTrajectoryFactory.cpp

Go to the documentation of this file.

#include "TimedTrajectoryFactory.h"
 
#include <RobotAPI/libraries/core/TrajectoryController.h>
 
#define TIMEDTRAJECTORY_PROBEFACTOR 100
 
#define TIMEDTRAJECTORY_FPS 100
 
armarx::TimedTrajectory
armarx::TimedTrajectoryFactory::createTimedTrajectory(
    VirtualRobot::TimeOptimalTrajectory& trajectory,
    std::vector<std::vector<double>>& userPoints,
    VirtualRobot::RobotNodeSetPtr rns,
    double maxDeviation)
 
{
    //ARMARX_INFO << "Creating timedTrajectory";
    //Check if the RobotNodeSet exists
    if (rns == NULL)
    {
        //ARMARX_INFO << "Rns is null";
        throw InvalidArgumentException("Rns is null");
    }
    //Check if the RobotNodeSet has nodes
    if (rns->getAllRobotNodes().size() == 0)
    {
        //ARMARX_INFO << "Nodeset has no nodes";
        throw InvalidArgumentException("Rns is empty");
    }
 
    //Tune the timing parameter here
    double timestep = 1 / ((double)(TIMEDTRAJECTORY_FPS));
    //timestep may not be 0!
    timestep = (timestep > 0) ? timestep : 0.01;
    //If the trajectory is shorter than a second, generate as many frames as you would in a second
    //(this guarantees that the probing is within the trajectory interval (timestep could otherwise be bigger)
    if (trajectory.getDuration() < 1)
    {
        timestep = trajectory.getDuration() / TIMEDTRAJECTORY_FPS;
    }
 
    std::vector<double> timestamps;
    //std::vector<double> timestamps2;
    //Nodedata is a vector containing the vectors of ALL positions at the respective timestamps for each joint.
    //(Outer dimension : joints, Inner dimension: timestamp index)
    std::vector<std::vector<double>> nodeData;
    std::vector<std::vector<double>> nodeData2;
 
    //Initialize the vectors for the respective joints, fill in the first timestamp (0)
    timestamps.push_back(0);
    //timestamps2.push_back(0);
    for (unsigned int i = 0; i < rns->getSize(); i++)
    {
        nodeData.push_back({trajectory.getPosition(0)[i]});
        //nodeData2.push_back({trajectory.getPosition(0)[i]});
    }
    //Fill in the data for all other timestamps into the now existing joint values
    for (double t = timestep; t < trajectory.getDuration(); t += timestep)
    {
        Eigen::VectorXd positions = trajectory.getPosition(t);
        //Eigen::VectorXd positions2 = trajectory.getPosition(t);
        for (unsigned int i = 0; i < positions.size(); i++)
        {
            nodeData[i].push_back(positions[i]);
            //nodeData2[i].push_back(positions2[i]);
        }
        //ARMARX_INFO << "Time : " << t;
        timestamps.push_back(t);
        //timestamps2.push_back(t);
    }
    LimitlessStateSeq state;
    for (VirtualRobot::RobotNodePtr node : rns->getAllRobotNodes())
    {
        LimitlessState current;
        current.enabled = node->isLimitless();
        current.limitLo = node->getJointLimitLow();
        current.limitHi = node->getJointLimitHigh();
        state.push_back(current);
    }
    //Get the correct names for the dimensions
    Ice::StringSeq dimensionNames = rns->getNodeNames();
    //Ice::StringSeq dimensionNames2 = rns->getNodeNames();
    //ARMARX_WARNING << "HELLO";
    //Construct the armarx Trajectory from the given data
 
    TrajectoryPtr traj = new Trajectory(nodeData, timestamps, dimensionNames);
    traj->setLimitless(state);
    //TrajectoryPtr original = new Trajectory(nodeData2, timestamps2, dimensionNames2);
    TrajectoryController::FoldLimitlessJointPositions(traj);
    /*int markedJoint = -1;
    bool positive = false;
    double start = 0;
    for (double d : traj->getTimestamps())
    {
        Ice::DoubleSeq newStates = traj->getStates(d, 0);
        Ice::DoubleSeq oldStates = original->getStates(d, 0);
        int c = 0;
        for (double f : newStates)
        {
            if ((f > 3.1 || f < -3.1) && rns->getAllRobotNodes()[c]->isLimitless())
            {
                ARMARX_ERROR << "Difference spotted " + std::to_string(f);
 
                markedJoint = c;
                start = d;
                if (f > 0)
                {
                    positive = true;
                }
            }
            if (c == markedJoint)
            {
                if (positive && f < -3.1 || !positive && f > 3.1)
                {
                    traj = cutTrajectory(traj, start, d);
                }
            }
            c++;
        }
    }*/
    //ARMARX_ERROR << traj->toEigen();
    int pass = 1;
    //Arbitrary interrupt condition
    for (pass = 1; pass < 10; pass++)
    {
 
        std::vector<double> times;
        unsigned int i = 0;
        //Map the user Points to the respective timestamps that their position is approximately reached within
        for (double t = 0; t < trajectory.getDuration();
             t += timestep / TIMEDTRAJECTORY_PROBEFACTOR)
        {
            Eigen::VectorXd jointAngles =
                Eigen::VectorXd::Map(&userPoints[i][0], userPoints[i].size());
            /*for (int i = 0; i < jointAngles.rows(); i++)
            {
                ARMARX_INFO << "Point" << i << "jointAngles [" << i << "] : " << jointAngles[i];
            }*/
            Eigen::VectorXd pointJointAngles = trajectory.getPosition(t);
            /*
            for (int i = 0; i < pointJointAngles.rows(); i++)
 
            {
                ARMARX_INFO << "Point" << i << "pointJointAngles [" << i << "] : " << pointJointAngles[i];
            }*/
            const double distance = (jointAngles - pointJointAngles).norm();
            //ARMARX_INFO << "Distance " << distance;
            //ARMARX_INFO << "MaxDeviation " << deviation;
            //If the deviation between the userPoint and the data at the respective point is smaller than the maxDeviation, map it
            if (distance <= maxDeviation)
            {
                times.push_back(t);
                //ARMARX_INFO << std::to_string(t);
                i++;
                //ARMARX_INFO << "MAPPING USER POINT" << i;
                if (i >= userPoints.size())
                {
                    //ARMARX_INFO << "mapping last user point";
                    //For the last point, just add the final time of the trajectory as the respective timestamp
                    //this can help mitigate the resulting forward drift of timestamps caused by the inaccuracy (maxDeviation)
                    times.pop_back();
                    times.push_back(trajectory.getDuration());
                    break;
                }
            }
        }
        //ARMARX_INFO << "Userpoint mapping complete";
        //ARMARX_INFO << "TimedTrajectory complete";
        //If not all points were mapped, an output is created and an empty map is returned for the userPoints
        if (times.size() < userPoints.size())
        {
            //Try to solve with recursively decreasing accuracy
            //ARMARX_INFO << "Only " << times.size() << "of " << userPoints.size() << " points found";
            //ARMARX_INFO << "Pass " << pass << " failed to find all points.";
            maxDeviation *= 10;
            //timestep /= 2;
            continue;
        }
        //Return the result and the number of passes that were   needed for the calculation
        pass == 1 ? ARMARX_INFO << pass << " pass needed to find mapping"
                  : ARMARX_INFO << pass << " passes needed to find mapping";
        return TimedTrajectory(traj, times);
    }
    ARMARX_INFO << "More than" << pass << "passes needed for calculation";
    //ARMARX_INFO << "Canceled calculation with maxDeviation of " << maxDeviation;
    throw InvalidArgumentException("No TimedTrajectory could be created");
}