inverse.cpp

Go to the documentation of this file.

/* *********************
 * @file inverse.cpp
 *
 * @author Stefan Ulbrich
 * @date 2013-2014
 *
 * @brief This file contains the implementation the methods of the KBM::Models::KBM class related to subdivision and the global IK algorithms.
 * *********************/
 
#include "kbm.h"
#include <cmath>
#include <assert.h>
#include <queue>
#include <boost/multi_array.hpp>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
namespace memoryx::KBM::Models
{
 
    /* Removec can be evaluated with subdivide(center,Eigen3::VectorXf::Constant(x,y)
     * void KBM::subdivide(const Vector &center, Real newSpreadAngle){
        Vector newSpreadAngles = Vector::Constant(this->nDoF,newSpreadAngle);
        return this->subdivide(center,newSpreadAngles);
    }*/
 
    void KBM::subdivide(const Vector& center, const Vector& newSpreadAngles)
    {
        this->getSubdivisedNet(center, newSpreadAngles, this->controlNet);
        this->spreadAngles = newSpreadAngles;
        this->center = center;
    }
 
    void KBM::getSubdivisedNet(unsigned int subdividedDoF, Real newCenter, Real newSpreadAngle, Matrix& resultingControlNet) const
    {
        Matrix SLE = Matrix::Constant(nControlPoints, nControlPoints, 1.0);
        using CacheType = boost::multi_array<Real, 3>;
        CacheType cache(boost::extents[this->nDoF][3][3]);
 
        for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
        {
            for (int idx = 0; idx < 3; idx++)
                for (int idx2 = 0; idx2 < 3; idx2++)
                {
                    assert(iDoF >= 0);
                    if (static_cast<unsigned int>(iDoF) != subdividedDoF)
                    {
                        if (idx != idx2)
                        {
                            cache[iDoF][idx][idx2] = 0.0f;
                        }
                        else
                        {
                            cache[iDoF][idx][idx2] = 1.0f;
                        }
                    }
                    else
                    {
                        Real a1, a2;
 
                        switch (idx2)
                        {
                            case 0:
                                a1 = (newCenter - this->center(iDoF)) - newSpreadAngle;
                                a2 = (newCenter - this->center(iDoF)) - newSpreadAngle;
                                break;
 
                            case 1:
                                a1 = (newCenter - this->center(iDoF)) - newSpreadAngle;
                                a2 = (newCenter - this->center(iDoF)) + newSpreadAngle;
                                break;
 
                            case 2:
                                a1 = (newCenter - this->center(iDoF)) + newSpreadAngle;
                                a2 = (newCenter - this->center(iDoF)) + newSpreadAngle;
                                break;
 
                        }
 
                        Real t1 = std::tan(a1 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
                        Real t2 = std::tan(a2 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
 
                        //ARMARX_DEBUG_S << idx2 << "," << iDoF<< ": " << a1 << "," << a2 << "," << t1 << ","<< t2<<std::endl;
                        switch (idx)
                        {
                            case 0:
                                cache[iDoF][idx][idx2] = (1 - t1) * (1 - t2);
                                break;
 
                            case 1:
                                cache[iDoF][idx][idx2] = ((1 - t1) * t2 + (1 - t2) * t1) * std::cos(this->spreadAngles[iDoF]);
                                break;
 
                            case 2:
                                cache[iDoF][idx][idx2]  = t1 * t2;
                                break;
                        }
 
                    }
 
                    //ARMARX_DEBUG_S << iDoF << "," << idx << "," << idx2 << "," << cache[iDoF][idx][idx2] <<std::endl;
                }
 
        }
 
        for (int iSamples = 0; iSamples < nControlPoints; iSamples++)
        {
            Real weight = 0;
 
            for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
            {
                // SLE(iControlPoints,iSamples)=1.0;
                // ARMARX_DEBUG_S << SLE;
                for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
                {
                    int idx = this->index(iControlPoints, iDoF);
                    int idx2 = this->index(iSamples, iDoF);
 
                    SLE(iControlPoints, iSamples) *= cache[iDoF][idx][idx2];
                }
 
                //            ARMARX_DEBUG_S << SLE << std::endl;
                weight += SLE(iControlPoints, iSamples);
            }
 
            for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
            {
                SLE(iControlPoints, iSamples) /= weight;
            }
        }
 
        //    ARMARX_DEBUG_S << std::endl << SLE <<std::endl;
        resultingControlNet = this->controlNet * SLE;
        //    ARMARX_DEBUG_S << this->controlNet << std::endl;
    }
 
    void KBM::getSubdivisedNet(const Vector newCenter, Vector newSpreadAngles, Matrix& resultingControlNet) const
    {
 
        Matrix SLE = Matrix::Constant(nControlPoints, nControlPoints, 1.0);
 
        using CacheType = boost::multi_array<Real, 3>;
        CacheType cache(boost::extents[this->nDoF][3][3]);
 
        for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
        {
            for (int idx = 0; idx < 3; idx++)
                for (int idx2 = 0; idx2 < 3; idx2++)
                {
                    Real a1, a2;
 
                    switch (idx2)
                    {
                        case 0:
                            a1 = (newCenter(iDoF) - this->center(iDoF)) - newSpreadAngles[iDoF];
                            a2 = (newCenter(iDoF) - this->center(iDoF)) - newSpreadAngles[iDoF];
                            break;
 
                        case 1:
                            a1 = (newCenter(iDoF) - this->center(iDoF)) - newSpreadAngles[iDoF];
                            a2 = (newCenter(iDoF) - this->center(iDoF)) + newSpreadAngles[iDoF];
                            break;
 
                        case 2:
                            a1 = (newCenter(iDoF) - this->center(iDoF)) + newSpreadAngles[iDoF];
                            a2 = (newCenter(iDoF) - this->center(iDoF)) + newSpreadAngles[iDoF];
                            break;
 
                    }
 
                    Real t1 = std::tan(a1 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
                    Real t2 = std::tan(a2 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
 
                    //ARMARX_DEBUG_S << idx2 << "," << iDoF<< ": " << a1 << "," << a2 << "," << t1 << ","<< t2<<std::endl;
                    switch (idx)
                    {
                        case 0:
                            cache[iDoF][idx][idx2] = (1 - t1) * (1 - t2);
                            break;
 
                        case 1:
                            cache[iDoF][idx][idx2] = ((1 - t1) * t2 + (1 - t2) * t1) * std::cos(this->spreadAngles[iDoF]);
                            break;
 
                        case 2:
                            cache[iDoF][idx][idx2]  = t1 * t2;
                            break;
                    }
                }
 
        }
 
 
        for (int iSamples = 0; iSamples < nControlPoints; iSamples++)
        {
            Real weight = 0;
 
            for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
            {
                // SLE(iControlPoints,iSamples)=1.0;
                // ARMARX_DEBUG_S << SLE;
                for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
                {
                    int idx = this->index(iControlPoints, iDoF);
                    int idx2 = this->index(iSamples, iDoF);
                    SLE(iControlPoints, iSamples) *= cache[iDoF][idx][idx2];
                }
 
                //ARMARX_DEBUG_S << SLE;
                weight += SLE(iControlPoints, iSamples);
            }
 
            for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
            {
                SLE(iControlPoints, iSamples) /= weight;
            }
        }
 
        //    ARMARX_DEBUG_S << std::endl << SLE <<std::endl;
        resultingControlNet = this->controlNet * SLE;
        //    ARMARX_DEBUG_S << this->controlNet << std::endl;
 
    }
    /*Removed -- can be emulated by checkBBox(point,point)
     * bool KBM::checkBBox(const Vector &point) const
    {
        return KBM::checkBBoxes(point,this->controlNet);
    }*/
 
    KBM::BBCheckType KBM::checkBBox(const Vector& lower, const Vector& upper, Real tolerance) const
    {
        return KBM::checkBBoxes(lower, upper, this->controlNet, tolerance);
    }
 
    KBM::BBCheckType KBM::checkBBoxes(const Vector& lower, const Vector& upper, const Matrix& controlNet, Real tolerance)
    {
        Vector BBlower, BBupper;
        KBM::getBBox(BBlower, BBupper, controlNet);
        //ARMARX_DEBUG_S << "lower: " << BBlower.transpose() << ", upper: " << BBupper.transpose() << std::endl << controlNet  << std::endl
        //      << "Target lower: " << lower.transpose() << ", uppder: " << upper.transpose() << std::endl;
        bool covered = true;
        bool included = true;
 
        for (int i = 0; i < lower.size(); i++)
        {
            if ((BBlower(i) > (upper(i)) + tolerance) || BBupper(i) <= (lower(i) - tolerance))
            {
                return KBM::DISJOINT;
            }
 
            if ((BBlower(i) < lower(i)) || (BBupper(i) > upper(i)))
            {
                covered = false;
            }
 
            if ((BBlower(i) > lower(i)) || (BBupper(i) < upper(i)))
            {
                included = false;
            }
 
        }
 
        if (covered)
        {
            return KBM::COVERED;
        }
 
        if (included)
        {
            return KBM::INCLUDED;
        }
 
        return KBM::OVERLAPPING;
    }
 
    /*Removed -- can be emulated by checkBBoxes(point,point)
    bool KBM::checkBBoxes(const Vector &point, const Matrix &controlNet)
    {
        return KBM::checkBBoxes(point,point,controlNet);
    }*/
 
    void KBM::getBBox(Vector& lower, Vector& upper, const Matrix& controlNet)
    {
        lower = controlNet.rowwise().minCoeff();
        upper = controlNet.rowwise().maxCoeff();
    }
 
    void KBM::getBBox(Vector& lower, Vector& upper)
    {
        getBBox(lower, upper, this->controlNet);
    }
 
    Real KBM::getVolume()
    {
        Vector lower, upper;
        this->getBBox(lower, upper);
        Vector size = upper - lower;
        return size.prod();
    }
 
    Real KBM::getOverlappingVolumeRatio(const Vector& lower, const Vector& upper, Real targetVolume)
    {
        Real volume = this->getOverlappingVolume(lower, upper);
        return volume / targetVolume;
    }
 
    Real KBM::getOverlappingVolume(const Vector& lower, const Vector& upper)
    {
        Vector BBlower, BBupper;
        this->getBBox(BBlower, BBupper);
        Real volume = 1.0;
 
        for (int i = 0; i < lower.size(); i++)
        {
            volume *= std::min(upper(i), BBupper(i)) - std::max(lower(i), BBlower(i));
 
            if (volume <= 0.0f)
            {
                return 0.0f;
            }
        }
 
        return volume;
    }
 
}
//int KBM::predictInverse(const Vector &shape, int nSubdivisions, unsigned int dim)
//{
//    KBM childA(*this), childB(*this);
//    // TODO: Check if solution lies in control net (this->controlNet.[min|max]Coefficient) [>|<] shape
//    // return 0 if not
//    // Todo compute the new centers and spread angles for the children
//    Vector centerA,centerB;
//    std::vector<Real> spreadAnglesA, spreadAnglesB;
//    childA.subdivide(centerA,spreadAnglesA);
//    childB.subdivide(centerB,spreadAnglesB);
//    return childA.predictInverse(shape,nSubdivisions,dim+1) + childB.predictInverse(shape,nSubdivisions,dim+1);
//}
 
namespace memoryx::KBM::Inverse
{
    // Recursive search
    void solveGlobalIK(unsigned int recursion, int side,
                       SolutionSet& solutionSet,
                       Models::KBM_ptr kbm,
                       const Vector& lower,
                       const Vector& upper,
                       Real resolution,
                       Vector spreadAngles,
                       Vector center)
    {
 
        // 1. Check if target is in BBox. Else stop
        Matrix controlNet;
        kbm->getSubdivisedNet(center, spreadAngles, controlNet);
 
        KBM::Models::KBM::BBCheckType check = Models::KBM::checkBBoxes(lower, upper, controlNet);
 
        // <for ARMARX_DEBUG_S>
        Vector BBlower, BBupper;
        Models::KBM::getBBox(BBlower, BBupper, controlNet);
        // <for ARMARX_DEBUG_S/>
        Solution solution(center, spreadAngles, BBupper, BBlower, check);
 
        // if completely disjoint stop search in this interval.
        if (check == KBM::Models::KBM::DISJOINT) //The tolerance should be implemented by upper lower.
        {
            solutionSet.push_back(solution);
            ARMARX_DEBUG_S << "Disjoint:  " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl;/*
              << "lower: " << lower.transpose() << ", upper: " << upper.transpose() << std::endl
              << "BB-lower: " << BBlower.transpose() << ", BB-upper: " << BBupper.transpose() << std::endl;*/
            return;
        }
 
 
        // if the interval covers the bounding box, a solution has been found.
        if (check == KBM::Models::KBM::COVERED)
        {
            solutionSet.push_back(solution);
            ARMARX_DEBUG_S << "Covered:   " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl; /* << "center: " << center.transpose() << ", max. angle: " << spreadAngles.maxCoeff()
              << ", lower: " << lower.transpose() << ", upper: " << upper.transpose()
              << ", BB-lower: " << BBlower << ", BB-upper: " << BBupper << std::endl;*/
            return;
        }
 
        // 2. Now check if resolution has been reached.
        if (spreadAngles.maxCoeff() <= resolution)
        {
            // Include those solutions as well?
            if (check == KBM::Models::KBM::INCLUDED)
            {
                ARMARX_DEBUG_S << "Max Res:   " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl << "center: " << center.transpose() << ", max. angle: " << spreadAngles.maxCoeff()
                               << ", lower:     " << lower.transpose() << ", upper: " << upper.transpose()
                               << ", BB-lower: " << BBlower.transpose() << ", BB-upper: " << BBupper.transpose() << std::endl;
            }
 
 
            solutionSet.push_back(solution);
 
            //ARMARX_DEBUG_Sging stuff
            ARMARX_DEBUG_S << "Max Res:   " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl; /* <<"center: " << center.transpose() << ", max. angle: " << spreadAngles.maxCoeff()
            << ", lower:     " << lower.transpose() << ", upper: " << upper.transpose()
             << ", BB-lower: " << BBlower.transpose() << ", BB-upper: " << BBupper.transpose() << std::endl;
        */
        }
        // subdivide the input interval
        else
        {
 
            ARMARX_DEBUG_S << "Subdivide: " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl;
            Vector center1 = center;
            Vector center2 = center;
 
            unsigned int dof = recursion % kbm->getNDoF();
            center1(dof) = center(dof) + spreadAngles[dof] / 2.0f;
            center2(dof) = center(dof) - spreadAngles[dof] / 2.0f;
            spreadAngles[dof] /= 2.0f;
            solveGlobalIK(recursion + 1, 1, solutionSet, kbm, lower, upper, resolution, spreadAngles, center1);
            solveGlobalIK(recursion + 1, 2, solutionSet, kbm, lower, upper, resolution, spreadAngles, center2);
        }
    }
 
    SolutionSet solveGlobalIK(Models::KBM_ptr kbm, const Vector& lower, const Vector& upper, Real resolution)
    {
        SolutionSet solution;
        solveGlobalIK(0, 0, solution, kbm, lower, upper, resolution, kbm->getSpreadAngles(), kbm->getCenter());
        return solution;
    }
 
    SolutionSet solveGlobalIK(Models::KBM_ptr kbm, const Vector& target, Real resolution)
    {
        SolutionSet solution;
        solveGlobalIK(0, 0, solution, kbm, target, target, resolution, kbm->getSpreadAngles(), kbm->getCenter());
        return solution;
    }
 
 
    void GlobalIKBase::solve(Models::KBM_ptr kbm, const Vector lower, const Vector upper, Real resolution)
    {
        this->targetLower = lower;
        this->targetUpper = upper;
        this->resolution = resolution;
        this->recurse(0, kbm);
    }
 
    Models::KBM_ptr GlobalIKBase::subdivideKBM(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm)
    {
        Vector center;
        Vector spreadAngles;
        this->subdivideAngles(side, recursion, kbm, center, spreadAngles);
        Matrix controlNet;
#if 1
        unsigned int dof = recursion % kbm->getNDoF();
        kbm->getSubdivisedNet(dof, center[dof], spreadAngles[dof], controlNet);
#else
        Matrix controlNet2;
        kbm->getSubdivisedNet(center, spreadAngles, controlNet2);
        ARMARX_DEBUG_S << "Similarity of Dof " << dof << ": " <<  controlNet.isApprox(controlNet2) << std::endl;
#endif
 
 
        return Models::KBM::createKBM(kbm->getNDoF(), kbm->getNDim(), center, spreadAngles, controlNet);
    }
 
    void GlobalIKBase::subdivideAngles(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm, Vector& center, Vector& spreadAngles)
    {
        unsigned int dof = recursion % kbm->getNDoF();
        center = kbm->getCenter();
        spreadAngles = kbm->getSpreadAngles();
 
        if (side == GlobalIKBase::LOWER)
        {
            center(dof) = center(dof) - spreadAngles[dof] / 2.0f;
        }
        else
        {
            center(dof) = center(dof) + spreadAngles[dof] / 2.0f;
        }
 
        spreadAngles[dof] /= 2.0f;
    }
 
    unsigned int GlobalIKExtensive::recurse(unsigned int level, Models::KBM_ptr kbm)
    {
        KBM::Models::KBM::BBCheckType check = kbm->checkBBox(this->targetLower, this->targetUpper);
        // <for ARMARX_DEBUG_S/>
        Vector BBupper, BBlower;
        kbm->getBBox(BBupper, BBlower);
        Solution solution(kbm->getCenter(), kbm->getSpreadAngles(), BBupper, BBlower, check);
 
        // 1. Definitively a Solution or no definitively solution
        if (check == KBM::Models::KBM::DISJOINT)
        {
            solutions.push_back(solution);
            return 0;
        }
 
        if (check == KBM::Models::KBM::COVERED)
        {
            solutions.push_back(solution);
            return 1;
        }
 
        // 2. Now check if resolution has been reached.
        if (kbm->getSpreadAngles().maxCoeff() <= resolution)
        {
            solutions.push_back(solution);
            return 0;
        }
        else
        {
            //solutions.push_back(solution);
 
            Models::KBM_ptr sub = this->subdivideKBM(GlobalIKBase::LOWER, level, kbm);
            int n = this->recurse(level + 1, sub);
            sub = this->subdivideKBM(GlobalIKBase::UPPER, level, kbm);
            kbm.reset();
            n += this->recurse(level + 1, sub);
            return n;
        }
 
        return 0;
    }
 
    unsigned int GlobalIKSemiBreadth::recurse(unsigned int level, Models::KBM_ptr kbm)
    {
        this->targetVolume = (this->targetUpper - this->targetLower).prod();
        Real overlap = kbm->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, this->targetVolume);
 
        KBM::Models::KBM::BBCheckType check = kbm->checkBBox(this->targetLower, this->targetUpper);
        // <for ARMARX_DEBUG_S/>
        Solution solution(kbm, check);
        bool breadth = kbm->getSpreadAngles().maxCoeff() > this->semiBreadthRecursion ;
 
        if (breadth)
        {
            ARMARX_DEBUG_S << "breadth " << std::string(level * 2, '-') << level << std::endl;
        }
        else
        {
            ARMARX_DEBUG_S << "depth   " << std::string(level * 2, '-') << level << ":  " << kbm->getSpreadAngles().maxCoeff() / M_PI * 180.0f << std::endl;
        }
 
        // 1. Definitively a Solution or no definitively solution
        if (check == KBM::Models::KBM::DISJOINT)
        {
            if (breadth && (check & solutionSelect))
            {
                solutions.push_back(solution);
            }
 
            ARMARX_DEBUG_S << "Dead End: " << overlap << std::endl;
            assert(overlap == 0.0f);
            return 0;
        }
 
        if (check == KBM::Models::KBM::COVERED)
        {
            ARMARX_DEBUG_S << "Solution found: " << overlap << std::endl;
            solutions.push_back(solution);
            Real overlap = kbm->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, kbm->getVolume());
            ARMARX_CHECK_EXPRESSION(overlap == 1.0f);
            return 1;
        }
 
        // 2. Now check if resolution has been reached.
        if (kbm->getSpreadAngles().maxCoeff() <= resolution)
        {
            ARMARX_DEBUG_S << "Max recursion, target overlap " << overlap * 100.f << ", " << kbm->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, kbm->getVolume()) << "%"
                           << std::endl;
            misses++;
            return 0;
        }
        else
        {
            if (!breadth && (check & solutionSelect))
 
            {
                solutions.push_back(solution);
            }
 
 
            if (!breadth)
            {
                Real ratio = kbm->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, kbm->getVolume());
                GraphNode initial(kbm, ratio, level, kbm->getVolume());
                SolutionSet s = this->runDijkstra(initial);
                solutions.insert(solutions.end(), s.begin(), s.end());
                return s.size();
 
            }
            else
            {
                Models::KBM_ptr sub1 = this->subdivideKBM(GlobalIKBase::LOWER, level, kbm);
                Models::KBM_ptr sub2 = this->subdivideKBM(GlobalIKBase::UPPER, level, kbm);
                Real ratio1 = sub1->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, this->targetVolume);
                Real ratio2 = sub2->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, this->targetVolume);
                //Real ratio1 = sub1->getOverlappingVolumeRatio(this->targetLower,this->targetUpper, kbm->getVolume());
                //Real ratio2 = sub2->getOverlappingVolumeRatio(this->targetLower,this->targetUpper, kbm->getVolume());
                ARMARX_DEBUG_S << "Reduction by subdivision: " <<  100.f* sub1->getVolume() / kbm->getVolume() << "%, " << sub2->getVolume() / kbm->getVolume() * 100.f << "%" << std::endl;
                ARMARX_DEBUG_S << "Target overlapping: " << ratio1 * 100.f << "%, " << ratio2 * 100.f << "%, " << this->targetVolume << std::endl;
                kbm.reset();
 
                //if (std::max(ratio1,ratio2)<0.75){
                //    ARMARX_DEBUG_S << "Not enough overlap\n";
                //    return 0;
                // }
                int n;
 
                if (ratio1 > ratio2)
                {
                    n = this->recurse(level + 1, sub1);
                }
                else
                {
                    n = this->recurse(level + 1, sub2);
                }
 
                if (breadth || n == 0)
                {
                    //sub = this->subdivideKBM(GlobalIKBase::UPPER,level,kbm);
                    //kbm.reset();
                    if (ratio1 > ratio2)
                    {
                        n += this->recurse(level + 1, sub2);
                    }
                    else
                    {
                        n += this->recurse(level + 1, sub1);
                    }
                }
 
                return n;
            }
        }
 
        assert(0);
        return 0;
 
    }
 
    // ReachabilityTree Klasse anlegen!
 
    // Reuse GraphNode for extensive search too
 
    bool operator<(const GraphNode& left, const GraphNode& right)
    {
        if (left.ratio != right.ratio)
        {
            return left.ratio < right.ratio;
        }
 
        return false;
    }
 
 
    GlobalIKBase::SolutionSet GlobalIKSemiBreadth::runDijkstra(GraphNode initial)
    {
        SolutionSet s;
        std::priority_queue<GraphNode> priority_queue;
        //priority_queue.push(GraphNode(model,this->targetLower,this->targetUpper,this->targetVolume));
        priority_queue.push(initial);
        int counter = 0;
 
        while (!priority_queue.empty())
        {
            GraphNode topNode = priority_queue.top();
            ARMARX_DEBUG_S <<  topNode.ratio << ", " ;
            priority_queue.pop();
 
            if (topNode.ratio == 1.0)
            {
                s.push_back(Solution(topNode.model, Models::KBM::COVERED));
                return s;
            }
 
            if (topNode.model->getSpreadAngles().maxCoeff() >= resolution)
                for (unsigned int side = GlobalIKBase::LOWER; side != GlobalIKBase::END; side++)
                {
                    Models::KBM_ptr child = this->subdivideKBM((GlobalIKBase::Side) side, topNode.level + 1, topNode.model);
                    Real volume = child->getVolume();
                    Real ratio = child->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, volume);
 
                    if (ratio != 0.0f)
                    {
                        counter++;
                        priority_queue.push(GraphNode(child, ratio, topNode.level + 1, volume));
                        std::cout << side + 1 << ". Ratio: " << ratio << " (" << ratio / topNode.ratio * 100.0f << "%), Vol.:  " << volume / topNode.volume * 100.f << "%, ";
                    }
                    else
                    {
                        std::cout << "0 ";
                    }
                }
            else
            {
                std::cout << "Resolution: " << topNode.model->getSpreadAngles().maxCoeff()  ;
            }
 
            std::cout << ", " << topNode.model->getSpreadAngles().maxCoeff() / M_PI * 180.0f << "(" << resolution / M_PI * 180.0f << ")" << ", Level: " << topNode.level << std::endl;
        }
 
        ARMARX_DEBUG_S << counter << std::endl;
        return s;
    }
 
    GlobalIKSemiBreadth::GlobalIKSemiBreadth(Real _semiBreadthRecursion, int _solutionSelect) : semiBreadthRecursion(_semiBreadthRecursion), solutionSelect(_solutionSelect)
    {
        this->misses = 0;
    }
}