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 <memory>
#include <ostream>
#include <queue>
#include <vector>
 
#include <Eigen/Eigen>
#include <Eigen/QR>
 
#define USE_SIMOX
#ifdef USE_SIMOX
#include <VirtualRobot/KinematicChain.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Nodes/RobotNodeRevolute.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobotException.h>
#endif
 
#define KBM_USE_DOUBLE_PRECISION
 
#define KBM_IMPORT_EXPORT
 
namespace memoryx::KBM
{
    /// Type definition of the underlying Realing point type.
#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
 
/// Namespace where the <i>Partial Least Squares (PLS-1)</i> solver is defined in.
namespace memoryx::KBM::PLS
{
    /** Solves a linear system of equations using the partial least squares algorithm.
     * @param input Input values (i.e., the rational polynomial basis)
     * @param output Output values (e.g., TCP positions/orientation)
     * @param threshold Tells the algorithm when to stop. If zero, the result is the same as the standard least mean squares (but w/o matrix inversions)
     */
    Matrix KBM_IMPORT_EXPORT solve(const Matrix& input, const Matrix output, Real threshold);
} // namespace memoryx::KBM::PLS
 
/** Where the model representation for a Body Schema (especially the Kinematic B&acute;ezier Maps) reside.
 * @todo Dynamic B&acute;ezier Maps (KBM for the inverse dynamics) are still to be implemented.
 */
namespace memoryx::KBM::Models
{
 
    class KBM;
    using KBM_ptr = std::shared_ptr<KBM>;
 
    /** @brief The Kinematic B\'ezier Maps.
     *
     * @details This class represents the Kinematic B\'ezier Map algorithm.
     * See the unit test on hints on how to use it.
     * @todo In the predict function, the center vector should be substracted from the proprioception!
     * @todo Introduce more the terms of intervals instead of spread angle and center!?
     * @todo offer a linearized version (for small angles).
     */
    class KBM_IMPORT_EXPORT KBM
    {
    public:
        /// Enum for the preferred optimization method during batch learning
        enum Optimization
        {
            STANDARD,
            PLS
        };
 
        /** Cases for the check of bounding boxes (i.e., an interval \f$ [c_{l,1},c_{u,1}]\times\ldots\times [c_{l,n},c_{u,n}] \f$)
         * and an interval \f$i\f$ (\f$ [i_{l,1},i_{u,1}]\times\ldots\times [i_{l,n},i_{u,n}] \f$)both in Cartesian coordinates.
         */
        enum BBCheckType
        {
            /// The interval is included in the bounding box \f$ \forall j: i_{l,j} > c_{l,j} \wedge c_{u,j} > i_{u,j} \f$
            INCLUDED = 1,
            /// The bounding box is covered by the interval \f$ \forall j: i_{l,j} < c_{l,j} \wedge c_{u,j} < i_{u,j} \f$ (most interesting case for inverse kinematics).
            COVERED = 2,
            /// The bounding box and interval are disjoint \f$ \exists_j: i_{u,j} < c_{l,j} \vee c_{u,j} < i_{l,j} \f$
            DISJOINT = 4,
            /// The interval and bounding box are partially overlapping (if none of the above applies).
            OVERLAPPING = 8,
            ALL = 15
        };
 
        /// Return type of the evaluation (KBM::getErrors()) function providing insight in the statistics of the prediction error.
        struct ErrorValuesType
        {
            /// Mean error
            Real Mean;
            /// Standard deviation
            Real STD;
            /// Median of the error
            Real Median;
            /// Inerquartile range (50% of all errors): \f$IQR= Q_{75\%} - Q_{25\%}\f$
            Real IQR;
            /// Maximal error
            Real Max;
            /// For debugging output
            friend KBM_IMPORT_EXPORT std::ostream& operator<<(std::ostream& os,
                                                              const ErrorValuesType& et);
        };
 
        /// Copy constructor
        KBM(const KBM& other);
 
        /** @brief Constructor
             * @param nDoF The number of the kinematic chain's degrees of freedom (number of input dimensions)
             * @param dim The number of output dimensions (usually three)
             * @param spreadAngle The input values should be centered around 0 and this angle should represent
             * their distribution in input space.
             */
        KBM(int nDoF, int dim, Real spreadAngle);
 
        /// loads a KBM with same input/output dimensions from disk (comma-separated values).
        bool restore(std::string fileName = "");
        /// stores the current KBM as a comma-separated matrix to disk.
        void store(std::string fileName = "");
        /** @brief Implements the online learning algorithm.
             * @param proprioception The input values (joint angles)
             * @param shape The output values (e.g. 3D TCP positions)
             * @param learnRate Learning rate between (0,1)
             * Can be called  (and still makes sense) after KBM::restore() and KBM::batch() and
             * prior calls to KBM::online().
             * Proprioception must have the same number of columns (i.e., number of samples)
             * while the rows must match the number of input and output dimensions respectively.
             */
        void online(const Matrix& proprioception, const Matrix& shape, Real learnRate = 1.0);
        // @param learnRate Learning rate between (0,1)
        /** @brief Implements the batch learning algorithm.
             * @param proprioception The input values (joint angles)
             * @param shape The output values (e.g. 3D TCP positions)
             * @param method Select optimization with standard linear least mean squares (KBM::STANDARD) or partial least squares (KMB::PLS)
             * @param threshold Models the noise and should equal the expected mean positioning error of the system. Only used with partial least squares (KBM::PLS)
             * Proprioception must have the same number of columns (i.e., number of samples)
             * while the rows must match the number of input and output dimensions respectively.
             */
        void batch(const Matrix& proprioception,
                   const Matrix& shape,
                   Optimization method = KBM::STANDARD,
                   Real threshold = 0.0);
        /** After learning this function gives various error measures over a given test set.online
             * Proprioception must have the same number of columns (i.e., number of samples)
             * while the rows must match the number of input and output dimensions respectively.*/
        ErrorValuesType getErrors(const Matrix& proprioception, const Matrix& shape);
        /// Forgets all previously learned samples.
        void reset();
 
        /** @brief Predicts a value of the FK given a set of joint angles.
             * @param proprioception The input values (joint angles in the columns).
             * @param dim If > 0, the prediction assumes it is the last joint in the kinematic chain and creates a coordinate frame
             * according to the partial derivative.
             */
        Matrix predict(const Matrix& proprioception, int dim = 0) const;
 
        /// Prepares the calculus of the derivatives. Has to be called alwayes after learning.
        void differentiate();
 
        /// Computes a partial derivative with respect to a configuration and the specified degree of freedom.
        Vector getPartialDerivative(const Vector& proprioception, int iDoF) const;
 
        /// Computes the partial derivative with respect to a configuration.
        Matrix getJacobian(const Vector& proprioception) const;
 
        /** @brief <b>Work in progress</b> This method subdivides the KBM representation.
         * @param center The new zero of the input values.
         * @param newSpreadAngles The new spread angle vector after subdivision. It can also be used to increase
         * the original angles. <b>Important:</b> The new spread angles must be smaller than
         * \f$90^\circ\f$ degrees and a smaller value is more suitable in general.
         * @todo This method needs to be further developed to enable the global inverse kinematics search.
         */
        void /*std::shared_ptr<KBM>*/ subdivide(const Vector& center,
                                                const Vector& newSpreadAngles);
 
        /** @brief <b>Work in progress</b> This method computes the control net of a subdivised KBM instance.
         * @param center The new zero of the input values.
         * @param newSpreadAngles The new spread angle vector after subdivision.
         * @param resultingControlNet The resulting control net of the subdivised KBM.
         *
         * This method computes only the control net (i.e., does not create a new KBM instance). It can be used, for instance,
         * in the global inverse kinematics algorithms.
         */
        void getSubdivisedNet(const Vector newCenter,
                              Vector newSpreadAngles,
                              Matrix& resultingControlNet) const;
 
        /// Makes the subdivision only for one degree of freedom
        void getSubdivisedNet(unsigned int dof,
                              Real center,
                              Real newSpreadAngle,
                              Matrix& resultingControlNet) const;
 
        /** @brief Check whether an interval overlaps the the bounding box of the control points.
         * @param lower The lower bounds of the interval.
         * @param upper The upper bounds of the interval.
         * @param tolerance Widens each bounding box by twice this skalar. You should use lower and upper to implement this behavior regularly.
         * @todo Add parameter to chose between overlapping and containment.
         */
        BBCheckType
        checkBBox(const Vector& lower, const Vector& upper, Real tolerance = 0.0f) const;
 
        /** @brief Check whether an interval overlaps the the bounding box of a given control net.
         * @param lower The lower bounds of the interval.
         * @param upper The upper bounds of the interval.
         * @param controlNet Check againt the bounding box of the points defined by the columns of this matrix.
         * @param tolerance Widens each bounding box by twice this skalar. You should use lower and upper to implement this behavior regularly.
         * @see KBM::Models::KBM::BBCheckType for details.
         * @todo Add parameter to chose between overlapping and containment. Containment seems better but more expensive (more recursions)
         *
         */
        static BBCheckType checkBBoxes(const Vector& lower,
                                       const Vector& upper,
                                       const Matrix& controlNet,
                                       Real tolerance = 0.0f);
 
 
        /// Helper function that extracts the bounding box of a control net.
        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);
 
        /// Return the spread angles.
        Vector getSpreadAngles() const;
 
        /// Return the center.
        Vector getCenter() const;
 
        /// Get the number of degrees of freedom (DoF).
        int getNDoF() const;
        int getNDim() const;
        Matrix getControlNet() const;
 
        /// Factory methods
        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:
        /** @brief Creates the system of linear equations formed by the Bernstein-polynomials and the projection.
             * @param proprioception Matrix with joint angles in its columns / (suggested) ColumnVector for blossom
             * @param dim If 0 < dim < #joint angles, then calculate the blossom for the dim-th joint.
             * @param a1 If calculating the blossom, a1 and a2 are used instead of proprioception(dim,*).
             *
             * About the blossom: In short, it creates all intermediate points constructed by DeCasteljau's algorithm.
             * It can be used to obtain the control points for the curves in the main direction or its the partial derivative.
             */
        Matrix createSLE(const Matrix& proprioception,
                         int dim = -1,
                         Real a1 = 0.0f,
                         Real a2 = 0.0f,
                         bool project = true) const;
        /// Creates an index for addressing the control vertice
        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::Models
 
/// Namespace for algorithms related to solving the inverse kinematics
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:
        /// Subdivides the kbm and creates new KBM. Calls GlobalIKBase::subdivideAngles().
        Models::KBM_ptr
        subdivideKBM(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm);
        /// Subdivides the joint angles only. Can be used to select in which direction to subdivise first.
        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;
    };
 
    /** Finds <i>all</i> solutions to the Global IK.
     *
     * Note that this may be \f$ 2^\frac{\alpha_{spread}}{\alpha_{resolution}}\f$ (i.e., <i> a lot </i>) solutions.
     */
    class GlobalIKExtensive : public GlobalIKBase
    {
    protected:
        unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override;
    };
 
    /** Expands all nets until a resolution has been reached, then search only for a single solution.
     *
     * This is the most promising approach for large kinematics.
     */
    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;
    };
 
    /// Return type of the global inverse kinematics solvers.
    using SolutionSet = std::vector<Solution>;
 
    /** @brief Algorithm to find the <b>global</b> solution to the inverse kinematics based on the KBM structure. <b>Work in progress</b>.
     * @param kbm The already learned KBM representation of a kinematics.
     * @param lower The lower bound of the interval in Cartesian space.
     * @param upper The upper bound of the interval in Cartesian space. Lower and upper bounds should be a target destination plus a desired tolerance, but can be chosen with equal values.
     * @param resolution The granularity of the found solutions (defaults to \f$2^\circ\f$).
     * @return The algorithm returns a list of joint angle intervals (the size is specified by the resolution parameters).
     *
     * This algorithm solves the inverse kinematics <b>globally</b> and <b>numerically</b> by using an interval based
     * version of the forward kinemtatics
     * \f[ f(\alpha_1, \beta_1,\ldots,\alpha_n,\beta_n) = ([a_x, \ldots, b_x],[a_y,\ldots,b_y],[a_z,\ldots,b_z]).\f]
     * which can be subdivided easily because of the linearisation emerging from the KBM model.
     * This means that if there exist more than a single solution,
     * a list of intervals that <i>probably</i> contain a solution. Specifically, this means that the target position lies within
     * the bounding box of the control net of the subdivised kbm. If a sufficient amount of steps has been performed,
     * the solution is garantueed to lie at least close to the desired target. The number of steps can be regulated
     * by the resolution parameter (i.e., \f$n_{steps}=\log_2\frac{\alpha_{spread}}{\alpha_{resolution}}\f$).
     * An interval-based version of this algorithm exists to define this distance in Cartesian space.
     *
     * \image html subdivision.png "Subdivision in angular space"
     * \image html subdivision2.png "Subdivision in angular space"
     *
     * @todo Allow for additional constraints and behaviors in the form of a functor parameter.
     * @todo Transform into class (with solution set embedded). For easier parameter settings. Further, SolutionSet should be a class with more information.
     * @todo Change the enum type in KBM::Models::KBM::BBCheckType such that they can be combined by binary operators.
     * @todo Allow depth search, also conditional after a given accuracy to avoid multiple solutions.
     */
    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);
 
 
} // namespace memoryx::KBM::Inverse