kbm.h

Go to the documentation of this file.

/* *********************
 * @file kbm.h
 *
 * @author Stefan Ulbrich
 * @date 2013-2014
 *
 * @brief This file contains the definition the class representing the Kinematic Bezier Maps.
 * *********************/
 
 
#pragma once
 
#include <iostream>
#include <vector>
#include <ostream>
#include <Eigen/Eigen>
#include <Eigen/QR>
#include <memory>
#include <queue>
 
#define USE_SIMOX
#ifdef USE_SIMOX
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobotException.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/KinematicChain.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#endif
 
#define KBM_USE_DOUBLE_PRECISION
 
#define KBM_IMPORT_EXPORT
namespace memoryx::KBM
{
#ifdef KBM_USE_DOUBLE_PRECISION
    using Real = double;
    using Matrix = Eigen::MatrixXd;
    using Vector = Eigen::VectorXd;
    using Matrix4 = Eigen::Matrix4d;
    using Vector3 = Eigen::Vector3d;
#else
    using Real = float;
    using Matrix = Eigen::MatrixXf;
    using Vector = Eigen::VectorXf;
    using Matrix4 = Eigen::Matrix4f;
    using Vector3 = Eigen::Vector3f;
#endif
}
namespace memoryx::KBM::PLS
{
    Matrix KBM_IMPORT_EXPORT solve(const Matrix& input, const Matrix output, Real threshold);
}
 
namespace memoryx::KBM::Models
{
 
    class KBM;
    using KBM_ptr = std::shared_ptr<KBM>;
 
    class KBM_IMPORT_EXPORT KBM
    {
    public:
 
        enum Optimization
        {
            STANDARD,
            PLS
        };
 
        enum BBCheckType
        {
            INCLUDED = 1,
            COVERED = 2,
            DISJOINT = 4,
            OVERLAPPING = 8,
            ALL = 15
        };
 
        struct ErrorValuesType
        {
            Real Mean;
            Real STD;
            Real Median;
            Real IQR;
            Real Max;
            friend KBM_IMPORT_EXPORT std::ostream& operator<<(std::ostream& os, const ErrorValuesType& et);
        };
 
        KBM(const KBM& other);
 
        KBM(int nDoF, int dim, Real spreadAngle);
 
        bool restore(std::string fileName = "");
        void store(std::string fileName = "");
        void online(const Matrix& proprioception, const Matrix& shape, Real learnRate = 1.0);
        // @param learnRate Learning rate between (0,1)
        void batch(const Matrix& proprioception, const Matrix& shape, Optimization method = KBM::STANDARD, Real threshold = 0.0);
        ErrorValuesType getErrors(const Matrix& proprioception, const Matrix& shape);
        void reset();
 
        Matrix predict(const Matrix& proprioception, int dim = 0) const;
 
        void differentiate();
 
        Vector getPartialDerivative(const Vector& proprioception, int iDoF) const;
 
        Matrix getJacobian(const Vector& proprioception) const;
 
        void /*std::shared_ptr<KBM>*/ subdivide(const Vector& center, const Vector& newSpreadAngles);
 
        void getSubdivisedNet(const Vector newCenter, Vector newSpreadAngles, Matrix& resultingControlNet) const;
 
        void getSubdivisedNet(unsigned int dof, Real center, Real newSpreadAngle, Matrix& resultingControlNet) const;
 
        BBCheckType checkBBox(const Vector& lower, const Vector& upper, Real tolerance = 0.0f) const;
 
        static BBCheckType checkBBoxes(const Vector& lower, const Vector& upper, const Matrix& controlNet, Real tolerance = 0.0f);
 
 
        static void getBBox(Vector& lower, Vector& upper, const Matrix& controlNet);
 
        void getBBox(Vector& lower, Vector& upper);
        Real getVolume();
        Real getOverlappingVolume(const Vector& lower, const Vector& upper);
        Real getOverlappingVolumeRatio(const Vector& lower, const Vector& upper, Real targetVolume);
 
        Vector getSpreadAngles() const;
 
        Vector getCenter() const;
 
        int getNDoF() const;
        int getNDim() const;
        Matrix getControlNet() const;
 
        static KBM_ptr createKBM(int nDoF, int dim, const Vector& center, const Vector& spreadAngles, const Matrix& controlNet);
        static KBM_ptr createFromVirtualRobot(VirtualRobot::KinematicChainPtr chain, VirtualRobot::SceneObjectPtr FoR, const Vector& spreadAngles, bool useOrientation = false);
    private:
        Matrix createSLE(const Matrix& proprioception, int dim = -1, Real a1 = 0.0f, Real a2 = 0.0f, bool project = true) const;
        void createIndices();
        int nDoF;
        int dim;
        //Real spreadAngle;
        //std::vector<Real> spreadAngles;
        Vector spreadAngles;
        Matrix index;
        Matrix controlNet;
        std::vector<Matrix> partialDerivatives;
        int nControlPoints;
        Vector center;
    };
 
 
}
 
namespace memoryx::KBM::Inverse
{
 
    class GlobalIKBase
    {
    public:
        struct Solution
        {
            Solution(const Vector& _center, const Vector& _spreadAngles, const Vector& _upper,
                     const Vector& _lower, const Models::KBM::BBCheckType& _type) :
                center(_center), spreadAngles(_spreadAngles), upper(_upper), lower(_lower), type(_type) {}
            Solution(Models::KBM_ptr kbm, Models::KBM::BBCheckType type) : type(type)
            {
                this->center = kbm->getCenter();
                this->spreadAngles = kbm->getSpreadAngles();
                Vector BBupper, BBlower;
                kbm->getBBox(BBupper, BBlower);
                this->lower = BBlower;
                this->upper = BBupper;
            }
 
            Vector center;
            Vector spreadAngles;
            Vector upper;
            Vector lower;
            Models::KBM::BBCheckType type;
        };
        using SolutionSet = std::vector<Solution>;
 
        enum Side
        {
            LOWER,
            UPPER,
            END
        };
 
        GlobalIKBase() {}
        void solve(Models::KBM_ptr kbm, const Vector lower, const Vector upper, Real resolution = 2.0f * M_PI / 180.0f);
 
        SolutionSet solutions;
 
    protected:
        Models::KBM_ptr subdivideKBM(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm);
        void subdivideAngles(Side side, unsigned int recursion, Models::KBM_ptr kbm, Vector& center, Vector& spreadAngles);
 
        virtual unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) = 0;
        Vector targetLower, targetUpper;
        Real resolution;
    };
 
 
    struct GraphNode
    {
        GraphNode(Models::KBM_ptr _model, Real _ratio, int _level, Real _volume) : model(_model), ratio(_ratio), level(_level), volume(_volume)
        {
        }
        Models::KBM_ptr model;
        Real ratio;
        int level;
        Real volume;
    };
 
    class GlobalIKExtensive : public GlobalIKBase
    {
    protected:
        unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override;
    };
 
 
    class GlobalIKSemiBreadth : public GlobalIKBase
    {
    public:
        GlobalIKSemiBreadth(Real _semiBreadthRecursion, int _solutionSelect = Models::KBM::COVERED);
        int misses;
        GlobalIKBase::SolutionSet runDijkstra(KBM::Inverse::GraphNode initial);
    protected:
        Real targetVolume;
        Real semiBreadthRecursion;
        int solutionSelect;
        unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override;
    };
 
 
    struct Solution
    {
        Solution(const Vector& _center, const Vector& _spreadAngles, const Vector& _upper,
                 const Vector& _lower, const Models::KBM::BBCheckType& _type) :
            center(_center), spreadAngles(_spreadAngles), upper(_upper), lower(_lower), type(_type) {}
        Vector center;
        Vector spreadAngles;
        Vector upper;
        Vector lower;
        Models::KBM::BBCheckType type;
    };
 
    using SolutionSet = std::vector<Solution>;
 
    SolutionSet solveGlobalIK(KBM::Models::KBM_ptr kbm, const Vector& lower, const Vector& upper, Real resolution = M_PI / 90.0f);
    //SolutionSet solveGlobalIK(KBM::Models::KBM_ptr kbm, const Vector &target, Real resolution=M_PI/90.0f);
 
 
}