DesignerTrajectoryCalculator.cpp

Go to the documentation of this file.

#include "DesignerTrajectoryCalculator.h"
#include "TimedTrajectoryFactory.h"
#include "PathFactory.h"
#include <VirtualRobot/TimeOptimalTrajectory/Path.h>
#include <RobotAPI/libraries/core/TrajectoryController.h>
 
#define TRAJECTORYCALCULATION_TIMESTEP 0.001
armarx::DesignerTrajectoryCalculator::DesignerTrajectoryCalculator(EnvironmentPtr environment) : environment(environment)
{
 
}
 
armarx::TimedTrajectory armarx::DesignerTrajectoryCalculator::calculateTimeOptimalTrajectory(std::vector<std::vector<double> >& trajectory, std::vector<std::vector<double> >& userPoints,  VirtualRobot::RobotNodeSetPtr rns, double maxDeviation, double accuracyFactor)
{
    //ARMARX_INFO << "Entering calculation";
    //ARMARX_INFO << trajectory.size() << " jointangles in trajectory";
    //Check if the Robot has the used RobotNodeSet
    if (!environment->getRobot()->hasRobotNodeSet(rns))
    {
        throw InvalidArgumentException("Robot does not have the supplied robotnodeset!");
    }
    //Create a Simox VirtualRobot Path. This is later used to calculate the TimeOptimalTrajectory
    VirtualRobot::Path path = armarx::PathFactory::createPath(trajectory, maxDeviation);
    //If all points are equal, create a trajectory with minimal duration and the respective start and end points
    //ARMARX_INFO << "Path length is " << path.getLength();
    if (path.getLength() == 0)
    {
        //If the path length is 0, create a pseudo trajectory containing the start position once for every userPoint
        std::vector<double> timestamps;
        //ARMARX_INFO << "Filling timestamps";
        for (unsigned int i = 0; i < userPoints.size(); i ++)
        {
            timestamps.push_back(0.01 * i);
        }
 
        //add first point
        std::vector<std::vector<double>> nodeData;
        for (unsigned int i = 0; i < userPoints[0].size(); i++)
        {
            nodeData.push_back({userPoints[0][i]});
 
        }
        for (unsigned int i = 1; i < userPoints.size(); i++)
        {
            for (unsigned int j = 0; j < userPoints[0].size(); j++)
            {
                nodeData[j].push_back(userPoints[i][j]);
            }
        }
 
        Ice::StringSeq dimensionNames = rns->getNodeNames();
        TrajectoryPtr traj = new Trajectory(nodeData, timestamps, dimensionNames);
        return TimedTrajectory(traj, timestamps);
    }
    //Get the maxVelocity and maxAcceleration for every joint
    //If the path length is 0, create a pseudo trajectory containing the start position once for every userPoint
 
 
    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /// CREATE TRAJECTORY TO UNFOLD
    ///
    ///
    std::vector<double> timestamps = std::vector<double>();
    for (unsigned int j = 0; j < trajectory.size(); j++)
    {
        timestamps.push_back(j * 0.01);
    }
    std::vector<std::vector<double>> nodeData = std::vector<std::vector<double>>();
    for (unsigned int i = 0; i < rns->getAllRobotNodes().size(); i++)
    {
        std::vector<double> node = std::vector<double>();
        for (unsigned int j = 0; j < trajectory.size(); j++)
        {
            node.push_back(trajectory[j][i]);
        }
        nodeData.push_back(node);
    }
    //ARMARX_INFO << trajectory;
    Ice::StringSeq dimensionNames = rns->getNodeNames();
    TrajectoryPtr traj = new Trajectory(nodeData, timestamps, dimensionNames);
    //ARMARX_INFO << traj;
    //ARMARX_INFO << std::to_string(traj->getLength());
    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);
    }
    traj->setLimitless(state);
    TrajectoryController::UnfoldLimitlessJointPositions(traj);
    //ARMARX_INFO << "UNFOLDED";
    //TURN TRAJECTORY BACK TO VECTOR
    std::vector<std::vector<double>> newPathVector = std::vector<std::vector<double>>();
    //ARMARX_INFO << std::to_string(traj->getLength());
    //ARMARX_ERROR << traj->toEigen();
    Eigen::MatrixXd trajMatrix = traj->toEigen();
    for (int i = 0; i < trajMatrix.rows(); i++)
    {
        //ARMARX_INFO << "Trying to get state at " + std::to_string(i * 0.01);
        std::vector<double> jA = std::vector<double>();
        for (int j = 0; j < trajMatrix.cols(); j++)
        {
            jA.push_back(trajMatrix(i, j));
        }
        //ARMARX_INFO << jA;
        newPathVector.push_back(jA);
 
    }
    //ARMARX_INFO << newPathVector;
    VirtualRobot::Path unfoldedPath = armarx::PathFactory::createPath(newPathVector, maxDeviation);
    //ARMARX_INFO << "FINISHED";
    /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    std::vector<double> mVelocity;
    std::vector<double> mAcceleration;
    for (VirtualRobot::RobotNodePtr rn : rns->getAllRobotNodes())
    {
        if (rn->getMaxVelocity() == -1.0)
        {
            mVelocity.push_back(10);
        }
        else
        {
            mVelocity.push_back(rn->getMaxVelocity());
        }
        if (rn->getMaxAcceleration() == -1.0)
        {
            mAcceleration.push_back(10);
        }
        else
        {
            mAcceleration.push_back(rn->getMaxAcceleration());
        }
    }
    const Eigen::VectorXd maxVelocity = Eigen::VectorXd::Map(&mVelocity[0], mVelocity.size());
    const Eigen::VectorXd maxAcceleration = Eigen::VectorXd::Map(&mAcceleration[0], mAcceleration.size());
    //Set the timestep to the trajectorycalculation timestep constant for now
    double timeStep = TRAJECTORYCALCULATION_TIMESTEP;
 
    //Calculate the TimeOptimalTrajectory using Simox, and bring the accuracyFactor into account
    //ARMARX_INFO << "Calling Simox TimeOptimalTrajectory";
    VirtualRobot::TimeOptimalTrajectory timeOptimalTrajectory = VirtualRobot::TimeOptimalTrajectory(unfoldedPath, maxVelocity, maxAcceleration, timeStep / accuracyFactor);
    //If simox failed finding a trajectory, signal this
    if (timeOptimalTrajectory.isValid() == 0)
    {
        //ARMARX_INFO << "Invalid trajectory";
        throw InvalidArgumentException("Input trajectory can not generate a continuous path");
    }
    //As simox sometimes returns valid on trajectories with nonreal or negative duration, check this too
    if (timeOptimalTrajectory.getDuration() <= 0 || std::isnan(timeOptimalTrajectory.getDuration()) || timeOptimalTrajectory.getDuration() >= HUGE_VAL)
    {
        //ARMARX_INFO << "Invalid trajectory duration";
        throw InvalidArgumentException("Input trajectory can not generate a continuous path with positive real duration");
    }
    //ARMARX_INFO << "TimeOptimalTrajectory has duration of " << timeOptimalTrajectory.getDuration();
    //Return the result
    return armarx::TimedTrajectoryFactory::createTimedTrajectory(timeOptimalTrajectory, userPoints, rns, maxDeviation);
}