InterpolationSegmentFactory.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    ArmarXGuiPlugins::RobotTrajectoryDesigner::Interpolation
 * @author     Timo Birr
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "InterpolationSegmentFactory.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "LinearInterpolation.h"
#include "SplineInterpolation.h"
 
///EXCEPTIONS
#include "../exceptions/ControlPointsInterpolationMatchException.h"
#include "../exceptions/InterpolationNotDefinedException.h"
#include "../exceptions/NoInterpolationPossibleException.h"
 
using namespace armarx;
using namespace VirtualRobot;
 
std::vector<AbstractInterpolationPtr>
InterpolationSegmentFactory::produceInterpolationSegments(
    std::vector<PoseBasePtr> controlPoints,
    std::vector<InterpolationType> interpolations)
{
    if (controlPoints.size() != interpolations.size() + 1)
    {
        throw new exceptions::local::ControlPointsInterpolationMatchException(
            controlPoints.size(), interpolations.size());
    }
 
    std::vector<AbstractInterpolationPtr> producedInterpolations =
        std::vector<AbstractInterpolationPtr>();
 
    std::vector<int> interPolationChangePoints = std::vector<int>();
    interPolationChangePoints.push_back(0);
 
    if (interpolations.size() == 1 && interpolations.at(0) == eSplineInterpolation)
    {
        interpolations.assign(0, eLinearInterpolation);
    }
 
    //mark parts of the Interpolaion where it changes
    int splineCounter = 0;
    InterpolationType current = interpolations.at(0);
    if (eSplineInterpolation == interpolations.at(0))
    {
        splineCounter++;
    }
    for (unsigned int i = 1; i < interpolations.size(); i++)
    {
        if (eSplineInterpolation == interpolations.at(i))
        {
            splineCounter++;
        }
        if (current != interpolations.at(i))
        {
            if (current == eSplineInterpolation && interpolations.at(i) != eSplineInterpolation)
            {
                if (splineCounter < 2)
                {
                    interpolations[i - 1] = eLinearInterpolation;
                }
                else
                {
                    interPolationChangePoints.push_back(i);
                }
                splineCounter = 0;
            }
            else
            {
                interPolationChangePoints.push_back(i);
            }
            current = interpolations[i];
        }
    }
    if (interpolations.size() >= 2 &&
        interpolations.at(interpolations.size() - 1) == eSplineInterpolation &&
        interpolations.at(interpolations.size() - 2) != eSplineInterpolation)
    {
        interpolations.pop_back();
        interpolations.push_back(eLinearInterpolation);
    }
    //mark parts of the Interpolaion where it changes
    /*InterpolationType current = interpolations.at(0);
    for (unsigned int i = 1; i < interpolations.size(); i++)
    {
        if (current != interpolations.at(i))
        {
            int prev = interPolationChangePoints.back();
            //A splineInterpolation needs 3 Points or more to be a valid spline Interpolation
            if (i - prev < 2 && interpolations.at(prev + 1) == InterpolationType::eSplineInterpolation && interpolations.at(i) == InterpolationType::eSplineInterpolation)
            {
                throw new exceptions::local::NoInterpolationPossibleException(3, 2);
            }
            interPolationChangePoints.push_back(i);
            current = interpolations[i];
        }
    }*/
    //Produce the InterpolationSegments and add them to producedInterpolationsVector
    for (unsigned int j = 1; j < interPolationChangePoints.size(); j++)
    {
        std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
        for (int i = interPolationChangePoints.at(j - 1); i <= interPolationChangePoints.at(j); i++)
        {
            currentCP.push_back(controlPoints.at(i));
        }
        std::vector<AbstractInterpolationPtr> temp;
        switch (interpolations.at(interPolationChangePoints.at(j - 1)))
        {
            case (InterpolationType::eLinearInterpolation):
                temp = produceLinearInterpolationSegments(currentCP);
                break;
            case (InterpolationType::eSplineInterpolation):
                temp = produceSplineInterpolationSegments(currentCP);
                break;
        }
        producedInterpolations.insert(producedInterpolations.end(), temp.begin(), temp.end());
    }
 
    std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
    for (unsigned int i = interPolationChangePoints.at(interPolationChangePoints.size() - 1);
         i < controlPoints.size();
         i++)
    {
        currentCP.push_back(controlPoints.at(i));
    }
    std::vector<AbstractInterpolationPtr> temp;
    switch (interpolations.at(interpolations.size() - 1))
    {
        case (InterpolationType::eLinearInterpolation):
            temp = produceLinearInterpolationSegments(currentCP);
            break;
        case (InterpolationType::eSplineInterpolation):
            temp = produceSplineInterpolationSegments(currentCP);
            break;
    }
    producedInterpolations.insert(producedInterpolations.end(), temp.begin(), temp.end());
    return producedInterpolations;
}
 
std::vector<AbstractInterpolationPtr>
InterpolationSegmentFactory::produceLinearInterpolationSegments(
    std::vector<PoseBasePtr> controlPoints)
{
    std::vector<AbstractInterpolationPtr> producedInterpolations =
        std::vector<AbstractInterpolationPtr>();
    for (unsigned int i = 0; i < controlPoints.size() - 1; i++)
    {
        std::vector<PoseBasePtr> currentCP = std::vector<PoseBasePtr>();
        currentCP.push_back(controlPoints.at(i));
        currentCP.push_back(controlPoints.at(i + 1));
        AbstractInterpolationPtr interpolation =
            std::shared_ptr<AbstractInterpolation>(new LinearInterpolation(currentCP));
        producedInterpolations.push_back(interpolation);
    }
    return producedInterpolations;
}
 
std::vector<AbstractInterpolationPtr>
InterpolationSegmentFactory::produceSplineInterpolationSegments(
    std::vector<PoseBasePtr> controlPoints)
{
    std::vector<AbstractInterpolationPtr> producedInterpolations =
        std::vector<AbstractInterpolationPtr>();
    SplineInterpolationPtr parent = *new std::shared_ptr<SplineInterpolation>(
        new SplineInterpolation(controlPoints)); //das hier muss genauso aussehen
 
    for (unsigned int i = 0; i < controlPoints.size() - 1; i++)
    {
        AbstractInterpolationPtr interpolation =
            std::shared_ptr<AbstractInterpolation>(parent->getInterPolationSegment(i));
        producedInterpolations.push_back(interpolation);
    }
    return producedInterpolations;
}
 
AbstractInterpolationPtr
InterpolationSegmentFactory::produceSplineInterpolationSegment(
    std::vector<PoseBasePtr> controlPoints,
    PoseBasePtr segmentStart)
{
    SplineInterpolationPtr parent = *new std::shared_ptr<SplineInterpolation>(
        new SplineInterpolation(controlPoints)); //das hier muss genauso aussehen
 
    AbstractInterpolationPtr interpolation =
        std::shared_ptr<AbstractInterpolation>(parent->getInterPolationSegment(segmentStart));
 
    return interpolation;
}
 
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// \brief InterpolationSegmentFactory::optimizeControlPoints
/// \param controlPoints
/// \param selections
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
InterpolationSegmentFactory::optimizeControlPoints(
    std::vector<PoseBasePtr>& controlPoints,
    std::vector<IKSolver::CartesianSelection>& selections)
{
    /*for(PoseBasePtr pose: controlPoints) {
        ARMARX_WARNING << Pose(pose->position, pose->orientation).toEigen();
    }
    for(IKSolver::CartesianSelection sel: selections) {
        ARMARX_WARNING << std::to_string(sel);
    }*/
 
    for (unsigned int i = 0; i < selections.size() - 1; i++)
    {
        //ARMARX_INFO << "OPTIMIZE";
        if (InterpolationSegmentFactory::isDominantOver(selections[i + 1], selections[i]))
        {
            std::vector<IKSolver::CartesianSelection> followingDominations =
                std::vector<IKSolver::CartesianSelection>();
            followingDominations.push_back(selections[i + 1]);
            int followingDominationsStart = i;
            bool isBreaking;
            do
            {
                //look left until an equal or fully dominating cartesian selection appears
                for (int j = i; j >= 0; j--)
                {
                    if ((InterpolationSegmentFactory::isDominantOver(selections[j],
                                                                     selections[i + 1]) &&
                         !InterpolationSegmentFactory::isDominantOver(selections[i + 1],
                                                                      selections[j])) ||
                        selections[i + 1] == selections[j])
                    {
 
                        //ARMARX_INFO << std::to_string(selections[j]) + " is dominant over " + std::to_string(selections[i + 1]);
                        followingDominationsStart = j + 1;
 
                        int dominanceIntervalSize = i + 1 - j;
                        PoseBasePtr dominanceIntervalStart = controlPoints[j];
                        PoseBasePtr dominanceIntervalEnd = controlPoints[i + 1];
                        std::vector<AbstractInterpolationPtr> li =
                            produceLinearInterpolationSegments(
                                {dominanceIntervalStart, dominanceIntervalEnd});
                        int counter = 1;
                        for (unsigned int k = j + 1; k < i + 1; k++)
                        {
                            controlPoints[k] = optimizePose(
                                controlPoints[k],
                                li[0]->getPoseAt(double(1.0 / dominanceIntervalSize) * counter),
                                selections[k],
                                selections[i + 1]);
                            counter++;
                            //ARMARX_INFO << std::to_string(double(1.0 / dominanceIntervalSize) * k) + " SET POINT AT------" + std::to_string(k) + " with " + std::to_string(selections[i + 1]);
                        }
                        break;
                    }
                    followingDominations.push_back(selections[j]);
                }
 
                if (i + 2 < selections.size() &&
                    isDominantOver(selections[i + 2], selections[i + 1]))
                {
                    isBreaking = false;
                    i++;
                    followingDominations.push_back(selections[i + 1]);
                }
                else
                {
                    isBreaking = true;
                }
 
            } while (!isBreaking);
            /*ARMARX_WARNING << "FOLLOWING DOMINATIONS START ";
            for(IKSolver::CartesianSelection sel: followingDominations) {
                ARMARX_WARNING << std::to_string(sel);
            }
                              ARMARX_WARNING << "FOLLOWING DOMINATIONS END ";*/
            IKSolver::CartesianSelection dominatingSelection =
                getSmallestDominating(followingDominations);
            for (unsigned int m = followingDominationsStart; m <= i + 1; m++)
            {
                //ARMARX_WARNING << "SET SELECTION " + std::to_string(m) + " with " + std::to_string(dominatingSelection);
                selections[m] = dominatingSelection;
                //selections[m] = IKSolver::CartesianSelection::All;
            }
        }
        //ARMARX_INFO << "Optimizing done";
    }
    ARMARX_INFO << "Optimizing done";
 
    /*ARMARX_WARNING << "RESULT         XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX";
    for(PoseBasePtr pose: controlPoints) {
        ARMARX_WARNING << Pose(pose->position, pose->orientation).toEigen();
    }
    for(IKSolver::CartesianSelection sel: selections) {
        ARMARX_WARNING << std::to_string(sel);
    }*/
}
 
bool
InterpolationSegmentFactory::needsOptimizing(std::vector<IKSolver::CartesianSelection>& selections)
{
    for (unsigned int i = 0; i < selections.size() - 1; i++)
    {
        //ARMARX_INFO << "NEEDS OPTIMIZING";
        if (InterpolationSegmentFactory::isDominantOver(selections[i + 1], selections[i]))
        {
            return true;
        }
    }
    return false;
}
 
bool
InterpolationSegmentFactory::isDominantOver(VirtualRobot::IKSolver::CartesianSelection current,
                                            VirtualRobot::IKSolver::CartesianSelection other)
{
    if (other == IKSolver::CartesianSelection::X || other == IKSolver::CartesianSelection::Y ||
        other == IKSolver::CartesianSelection::Z ||
        other == IKSolver::CartesianSelection::Orientation)
    {
        return current != other;
    }
    else if (other == IKSolver::CartesianSelection::Position)
    {
        return (current == IKSolver::CartesianSelection::Orientation ||
                current == IKSolver::CartesianSelection::All);
    }
    return false;
}
 
IKSolver::CartesianSelection
InterpolationSegmentFactory::getSmallestDominating(
    std::vector<VirtualRobot::IKSolver::CartesianSelection> selections)
{
    bool isInhomogenous = false;
    ARMARX_INFO << "Vector start";
    int max = 0;
    for (IKSolver::CartesianSelection current : selections)
    {
        if (current > max)
        {
            ARMARX_INFO << "In Vector" + std::to_string(current);
            max = current;
        }
        if (max != 0 && current != max)
        {
            isInhomogenous = true;
        }
    }
    if (max <= 4 && isInhomogenous)
    {
        ARMARX_INFO << "SMALLEST DOMINATING" +
                           std::to_string(IKSolver::CartesianSelection::Position);
        return IKSolver::CartesianSelection::Position;
    }
    else if (max == IKSolver::CartesianSelection::Orientation)
    {
        for (IKSolver::CartesianSelection currentSelection : selections)
        {
            if (currentSelection != IKSolver::CartesianSelection::Orientation)
            {
                ARMARX_INFO << "SMALLEST DOMINATING" +
                                   std::to_string(IKSolver::CartesianSelection::All);
                return IKSolver::CartesianSelection::All;
            }
        }
        ARMARX_INFO << "SMALLEST DOMINATING" +
                           std::to_string(IKSolver::CartesianSelection::Orientation);
        return IKSolver::CartesianSelection::Orientation;
    }
    else
    {
        ARMARX_INFO << "SMALLEST DOMINATING" + std::to_string(max);
        return static_cast<IKSolver::CartesianSelection>(max);
    }
}
 
PoseBasePtr
InterpolationSegmentFactory::optimizePose(PoseBasePtr original,
                                          PoseBasePtr corrected,
                                          IKSolver::CartesianSelection selectionOriginal,
                                          IKSolver::CartesianSelection selectionCorrecting)
{
    //ARMARX_WARNING << Pose(original->position, original->orientation).toEigen();
    //ARMARX_WARNING << Pose(corrected->position, corrected->orientation).toEigen();
    PoseBasePtr firstStep;
    switch (selectionCorrecting)
    {
        case (IKSolver::CartesianSelection::X):
            firstStep = PoseBasePtr(
                new Pose(Vector3BasePtr(new Vector3(
                             corrected->position->x, original->position->y, original->position->z)),
                         original->orientation));
        case (IKSolver::CartesianSelection::Y):
            firstStep = PoseBasePtr(
                new Pose(Vector3BasePtr(new Vector3(
                             original->position->x, corrected->position->y, original->position->z)),
                         original->orientation));
        case (IKSolver::CartesianSelection::Z):
            firstStep = PoseBasePtr(
                new Pose(Vector3BasePtr(new Vector3(
                             original->position->x, original->position->y, corrected->position->z)),
                         original->orientation));
        case (IKSolver::CartesianSelection::Position):
            firstStep = PoseBasePtr(new Pose(corrected->position, original->orientation));
        case (IKSolver::CartesianSelection::Orientation):
            firstStep = PoseBasePtr(new Pose(original->position, corrected->orientation));
        case (IKSolver::CartesianSelection::All):
            //ARMARX_WARNING << "CORRECTED ALL";
            firstStep = corrected;
    }
    switch (selectionOriginal)
    {
        case (IKSolver::CartesianSelection::X):
            return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(original->position->x,
                                                                   firstStep->position->y,
                                                                   firstStep->position->z)),
                                        firstStep->orientation));
        case (IKSolver::CartesianSelection::Y):
            return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(firstStep->position->x,
                                                                   original->position->y,
                                                                   firstStep->position->z)),
                                        firstStep->orientation));
        case (IKSolver::CartesianSelection::Z):
            return PoseBasePtr(new Pose(Vector3BasePtr(new Vector3(firstStep->position->x,
                                                                   firstStep->position->y,
                                                                   original->position->z)),
                                        firstStep->orientation));
        case (IKSolver::CartesianSelection::Position):
            return PoseBasePtr(new Pose(original->position, firstStep->orientation));
        case (IKSolver::CartesianSelection::Orientation):
            return PoseBasePtr(new Pose(firstStep->position, original->orientation));
        case (IKSolver::CartesianSelection::All):
            return original;
        default:
            return corrected;
    }
}