TransformTrajectoryIntoTimeOptimal.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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    RobotSkillTemplates::MotionControlGroup
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "TransformTrajectoryIntoTimeOptimal.h"
#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
 
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
using namespace armarx;
using namespace MotionControlGroup;
 
using NameFloatMap = std::map<std::string, float>;
 
// DO NOT EDIT NEXT LINE
TransformTrajectoryIntoTimeOptimal::SubClassRegistry TransformTrajectoryIntoTimeOptimal::Registry(TransformTrajectoryIntoTimeOptimal::GetName(), &TransformTrajectoryIntoTimeOptimal::CreateInstance);
 
TrajectoryPtr TransformTrajectoryIntoTimeOptimal::balanceTimestamps(TrajectoryPtr t)
{
    ARMARX_INFO << t->output();
    double length = t->getLength(0);
    if (length == 0.0f)
    {
        return t;
    }
    ARMARX_INFO << VAROUT(length);
    double timelength = t->getTimeLength();
    ARMARX_INFO << VAROUT(timelength);
    auto timestamps = t->getTimestamps();
    ARMARX_INFO << VAROUT(timestamps);
    Ice::DoubleSeq newTimestamps;
    newTimestamps.push_back(0);
    for (size_t var = 0; var < timestamps.size() - 1; ++var)
    {
        double tBefore = timestamps.at(var);
        double tAfter = (timestamps.at(var + 1));
        ARMARX_INFO << VAROUT(tBefore) << VAROUT(tAfter);
        double partLength = t->getLength(0, tBefore, tAfter);
        double lengthPortion = partLength / length;
        ARMARX_INFO << VAROUT(partLength) << VAROUT(lengthPortion);
        newTimestamps.push_back(*newTimestamps.rbegin() + timelength * lengthPortion);
    }
    ARMARX_INFO << VAROUT(newTimestamps);
    TrajectoryPtr newTraj = new Trajectory();
    for (size_t d = 0; d < t->dim(); ++d)
    {
        newTraj->addDimension(t->getDimensionData(d), newTimestamps, t->getDimensionName(d));
    }
    ARMARX_INFO << newTraj->output();
 
    return newTraj;
}
 
void TransformTrajectoryIntoTimeOptimal::onEnter()
{
 
}
 
void TransformTrajectoryIntoTimeOptimal::run()
{
    TrajectoryPtr inputTrajectory = in.getTrajectory();
 
    double maxDeviation = in.getMaxDeviation();
    double timeStep = in.getTimeStep();
    float maxJointVelocity = in.getMaxJointVelocity();
    float maxJointAcceleration = in.getMaxJointAcceleration();
 
    Ice::StringSeq jointNames = inputTrajectory->getDimensionNames();
    Eigen::VectorXd maxAccelerations(jointNames.size());
    Eigen::VectorXd maxVelocities(jointNames.size());
    for (size_t i = 0; i < jointNames.size(); i++)
    {
        maxAccelerations(i) = maxJointAcceleration;
        maxVelocities(i) = maxJointVelocity;
    }
 
    if (in.isMaxJointVelocitiesSet())
    {
        NameFloatMap maxJointVelocities = in.getMaxJointVelocities();
        NameFloatMap::iterator itv;
        int i = 0;
        for (std::string name : jointNames)
        {
            itv = maxJointVelocities.find(name);
            maxVelocities(i) = (double)(itv != maxJointVelocities.end() ? itv->second : maxJointVelocity);
            ++i;
        }
    }
 
    if (in.isMaxJointAccelerationsSet())
    {
        NameFloatMap maxJointAccelerations = in.getMaxJointAccelerations();
        NameFloatMap::iterator ita;
        int i = 0;
        for (std::string name : jointNames)
        {
            ita = maxJointAccelerations.find(name);
            maxAccelerations(i) = (double)(ita != maxJointAccelerations.end() ? ita->second : maxJointAcceleration);
            ++i;
        }
    }
    ARMARX_INFO << "Calculating time optimal trajectory: " << inputTrajectory->output();
    auto timeOptimalTraj = inputTrajectory->calculateTimeOptimalTrajectory(maxVelocities, maxAccelerations, maxDeviation, IceUtil::Time::secondsDouble(timeStep));
    ARMARX_INFO << "Done: " << timeOptimalTraj->output();
    out.setTimeOptimalTrajectory(timeOptimalTraj);
    emitSuccess();
    //    std::list<Eigen::VectorXd> waypoints;
    //    Eigen::VectorXd waypoint(jointNames.size());
 
    //    std::vector<Ice::DoubleSeq> allData(jointNames.size());
    //    for (size_t i = 0; i < jointNames.size(); i++)
    //    {
    //        allData.at(i) = inputTrajectory->getDimensionData(i);
    //    }
 
    //    Ice::DoubleSeq timeStamps = inputTrajectory->getTimestamps();
    //    for (size_t var = 0; var < timeStamps.size(); var++)
    //    {
    //        for (size_t i = 0; i < jointNames.size(); i++)
    //        {
    //            waypoint(i) = allData[i][var];
    //        }
    //        waypoints.push_back(waypoint);
    //    }
 
    //    VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(VirtualRobot::Path(waypoints, maxDeviation), maxVelocities, maxAccelerations, timeStep);
 
    //    if (!timeOptimalTraj.isValid())
    //    {
    //        ARMARX_WARNING << "Generation of time-optimal trajectory failed. The original trajectory is returned.";
    //        inputTrajectory = balanceTimestamps(inputTrajectory);
    //        out.setTimeOptimalTrajectory(inputTrajectory);
    //        emitFailure();
    //    }
    //    else
    //    {
    //        TrajectoryPtr newTraj = new Trajectory();
 
    //        Ice::DoubleSeq newTimestamps;
    //        double duration = timeOptimalTraj.getDuration();
    //        for (double t = 0.0; t < duration; t += timeStep)
    //        {
    //            newTimestamps.push_back(t);
    //        }
    //        newTimestamps.push_back(duration);
 
    //        for (size_t d = 0; d < jointNames.size(); d++)
    //        {
    //            Ice::DoubleSeq position;
    //            for (double t = 0.0; t < duration; t += timeStep)
    //            {
    //                position.push_back(timeOptimalTraj.getPosition(t)[d]);
    //            }
    //            position.push_back(timeOptimalTraj.getPosition(duration)[d]);
    //            newTraj->addDimension(position, newTimestamps, inputTrajectory->getDimensionName(d));
 
    //            Ice::DoubleSeq derivs;
    //            for (double t = 0.0; t < duration; t += timeStep)
    //            {
    //                derivs.clear();
    //                derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
    //                derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
    //                newTraj->addDerivationsToDimension(d, t, derivs);
    //            }
    //            derivs.clear();
    //            derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
    //            derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
    //            newTraj->addDerivationsToDimension(d, duration, derivs);
    //        }
 
    //        out.setTimeOptimalTrajectory(newTraj);
    //        emitSuccess();
    //    }
}
 
//void TransformTrajectoryIntoTimeOptimal::onBreak()
//{
//    // put your user code for the breaking point here
//    // execution time should be short (<100ms)
//}
 
void TransformTrajectoryIntoTimeOptimal::onExit()
{
    // put your user code for the exit point here
    // execution time should be short (<100ms)
}
 
 
// DO NOT EDIT NEXT FUNCTION
XMLStateFactoryBasePtr TransformTrajectoryIntoTimeOptimal::CreateInstance(XMLStateConstructorParams stateData)
{
    return XMLStateFactoryBasePtr(new TransformTrajectoryIntoTimeOptimal(stateData));
}