KBMComponent.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    MemoryX::ArmarXObjects::KBMComponent
 * @author     Jonas Rauber ( kit at jonasrauber dot de )
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
#include <MemoryX/interface/components/KBMComponentInterface.h>
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
 
/*
 * using these #define's here isn't nice, but it works
 */
//#define KBM_NO_ARMAR_X
//#define KBM_USE_SINGLE_PRECISION
#include <cfloat>
 
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
#include <MemoryX/libraries/helpers/KinematicBezierMaps/kbm.h>
 
namespace armarx
{
    /**
     * @class KBMComponentPropertyDefinitions
     * @brief
     */
    class KBMComponentPropertyDefinitions : public ComponentPropertyDefinitions
    {
    public:
        KBMComponentPropertyDefinitions(std::string prefix) : ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "LongtermMemoryName",
                "LongtermMemory",
                "Name of the LongtermMemory component that should be used");
            defineOptionalProperty<std::string>(
                "RobotStateComponentName",
                "RobotStateComponent",
                "Name of the robot state component that should be used");
        }
    };
} // namespace armarx
 
namespace armarx::KBMDifferentialIK
{
    /**
     * @brief calculateJointDeltas is in internal function called by solve
     */
    memoryx::KBM::Vector calculateJointDeltas(const memoryx::KBM::Models::KBM& kbm,
                                              const memoryx::KBM::Vector& target,
                                              const memoryx::KBM::Vector& position,
                                              const memoryx::KBM::Vector& proprioception,
                                              float stepSizeFactor,
                                              float maxStepSize);
 
    /**
     * @brief randomFloat creates a random float between LO and HI
     */
    float randomFloat(float LO, float HI);
 
    /**
     * @brief Creates a vector of random values between the limits
     */
    memoryx::KBM::Vector
    randomProprioception(const memoryx::KBM::Vector& lowerProprioceptionLimits,
                         const memoryx::KBM::Vector& upperProprioceptionLimits);
 
    /**
     * @brief check if solution is within the limits
     */
    bool checkProprioceptionLimits(const memoryx::KBM::Vector& solution,
                                   const memoryx::KBM::Vector& lowerProprioceptionLimits,
                                   const memoryx::KBM::Vector& upperProprioceptionLimits);
 
    /**
     * @brief apply limits to solution and return true,
     * if solution has been changed = solution was not in limits
     */
    bool applyProprioceptionLimits(Eigen::Map<memoryx::KBM::Vector>& solution,
                                   const memoryx::KBM::Vector& lowerProprioceptionLimits,
                                   const memoryx::KBM::Vector& upperProprioceptionLimits);
 
    /**
     * @brief KBMDifferentialIK::solveMany runs solve many times
     * @param kbm
     * @param targetPosition
     * @param currentProprioception
     * @param lowerProprioceptionLimits
     * @param upperProprioceptionLimits
     * @param solution
     * @param maxSolves maximum number of times solve() should be called
     * @param positionTolerance
     * @param minimumDelta
     * @param requireImprovment
     * @param maxSteps
     * @param stepSizeFactor
     * @param maxStepSize
     * @return
     */
    bool solveMany(const memoryx::KBM::Models::KBM& kbm,
                   const memoryx::KBM::Vector& targetPosition,
                   const memoryx::KBM::Vector& currentPosition,
                   const memoryx::KBM::Vector& currentProprioception,
                   const memoryx::KBM::Vector& lowerProprioceptionLimits,
                   const memoryx::KBM::Vector& upperProprioceptionLimits,
                   Eigen::Map<memoryx::KBM::Vector>& solution,
                   bool applyLimits = true,
                   int maxSolves = 25,
                   float positionTolerance = 5.0f,
                   float minimumDelta = 0.0f,
                   bool requireImprovment = false,
                   int maxSteps = 50,
                   float stepSizeFactor = 0.2f,
                   float maxStepSize = FLT_MAX);
 
    /**
     * @brief solves the inverse kinematics
     * @param kbm: the learned KBM
     * @param targetPosition: the target position
     * @param currentPosition: the current position
     * @param currentProprioception: the current proprioception
     * @param solution: will contain the joint values that solve the inverse kinematics
     * @return true, if a good approximation has been found, false otherwise
     */
    bool solve(const memoryx::KBM::Models::KBM& kbm,
               const memoryx::KBM::Vector& targetPosition,
               const memoryx::KBM::Vector& currentPostion,
               const memoryx::KBM::Vector& currentProprioception,
               Eigen::Map<memoryx::KBM::Vector>& solution,
               float positionTolerance = 5.0f,
               float minimumDelta = 0.0f,
               bool requireImprovment = false,
               int maxSteps = 50,
               float stepSizeFactor = 0.2f,
               float maxStepSize = FLT_MAX);
} // namespace armarx::KBMDifferentialIK
 
namespace armarx
{
    /**
     * @class KBMComponent
     * @ingroup Memoryx-Components
     * @brief Wrapper for the KBM class
     */
    class KBMComponent : virtual public armarx::Component, virtual public KBMComponentInterface
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "KBMComponent";
        }
 
        static std::string GetDefaultName();
 
        /**
         * @brief createKBM creates a new KBM and makes it available under the given name
         * @param name the name of the KBM
         * @param nDoF degrees of freedom
         * @param dim dimensions
         * @param spreadAngle the common spreadAngle for joints
         */
        void createKBM(const std::string& name,
                       Ice::Int nDoF,
                       Ice::Int dim,
                       Ice::Float spreadAngle,
                       const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief createArmar3KBM creates a KBM from a VirtualRobot model
         * @param name the KBM will be available under this name
         * @param activeJoints the names of the joints that are used
         * @param tcpName name of the joint that corresponds best to the tool center point
         * @param useOrientation use only the position (3 dimensions) or position and orientation (9 dimensions in total)
         */
        void createArmar3KBM(const std::string& name,
                             const Ice::StringSeq& robotNodeNames,
                             const std::vector<memoryx::KBM::Real>& jointValueAverages,
                             const std::vector<memoryx::KBM::Real>& spreadAnglesSequence,
                             const std::string& tcpName,
                             bool useOrientation,
                             const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief batch learning of n training samples
         * @param name the name of the KBM
         * @param proprioception length must be DoF * n
         * @param position length must be Dim * n
         * @param method will be mapped to the Enum in the KBM class, currently 0 = STANDARD, 1 = PLS
         * @param threshold should equal the expected mean positioning error
         */
        void batch(const std::string& name,
                   const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                   const std::vector<memoryx::KBM::Real>& positionSequence,
                   Ice::Int method,
                   Ice::Float threshold,
                   const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief online learning of n training samples
         * @param name the name of the KBM
         * @param proprioception length must be DoF * n
         * @param position length must be Dim * n
         * @param learnRate learning rate between 0 and 1
         */
        void online(const std::string& name,
                    const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                    const std::vector<memoryx::KBM::Real>& positionSequence,
                    Ice::Float learningRate,
                    const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief same as online, but evaluates after each learning iteration
         * @param name the name of the KBM
         * @param proprioception length must be DoF * n
         * @param position length must be Dim * n
         * @param learnRate learning rate between 0 and 1
         */
        void onlineVerbose(const std::string& name,
                           const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                           const std::vector<memoryx::KBM::Real>& positionSequence,
                           Ice::Float learningRate,
                           const std::vector<memoryx::KBM::Real>& evaluationProprioceptionSequence,
                           const std::vector<memoryx::KBM::Real>& evaluationPositionSequence,
                           const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief predict the position for n samples of proprioception (forward kinematics)
         * @param name the name of the KBM
         * @param proprioception length must be DoF * n
         * @return the predicted position with length Dim * n
         */
        std::vector<memoryx::KBM::Real>
        predict(const std::string& name,
                const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief calls getErrors on the KBM and prints the result
         * @param name the name of the KBM
         * @param proprioception the proprioception to pass to getErrors
         * @param position the position to pass to getErrors
         */
        void printErrors(const std::string& name,
                         const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
                         const std::vector<memoryx::KBM::Real>& positionSequence,
                         const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief printAccumulatorStatistics prints information about the values in the accumulators
         */
        void printAccumulatorStatistics(int nDim,
                                        int nDoF,
                                        const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief accumulate adds the given position to the accumulator to use it later
         * @param position length must be Dim * n
         */
        void accumulatePositions(const std::vector<memoryx::KBM::Real>& positionSequence,
                                 const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief accumulates the given position in the evaluation accumulator
         * @param position length must be Dim * n
         */
        void accumulateEvaluationPositions(const std::vector<memoryx::KBM::Real>& positionSequence,
                                           const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief getRawJointValuesAverages calculates the average raw joint values for each joint
         * from the data stored in the raw joint values accumulator.
         */
        Ice::DoubleSeq
        getRawJointValuesAverages(int nDoF, const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief getSpreadAngles returns the spreadAngles calculated using the data in the proprioceptionAccumulator
         */
        Ice::DoubleSeq getSpreadAngles(int nDoF,
                                       const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief accumualteProprioceptions accumulates proprioceptions
         */
        void accumulateProprioceptions(const std::vector<memoryx::KBM::Real>& proprioceptions,
                                       const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief accumualteProprioceptions accumulates proprioceptions in the evaluate accumulator
         */
        void
        accumulateEvaluationProprioceptions(const std::vector<memoryx::KBM::Real>& proprioceptions,
                                            const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief accumualteRawJointValues accumulates raw joint values that will be transformed to proprioceptions later
         */
        void accumulateRawJointValues(const std::vector<memoryx::KBM::Real>& rawJointValuesSequence,
                                      const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief setProprioceptionAccumulatorFromRawJointAccumulator updates the proprioception accumulator
         */
        void setProprioceptionAccumulatorFromRawJointAccumulator(
            const std::vector<memoryx::KBM::Real>& rawJointValuesAverages,
            const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief setPositionLimits sets the position limits using the data in the position accumulator
         */
        void setPositionLimits(int nDim, const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief onlineAccumulator calls online after transforming the raw joint accumulator to proprioceptions
         */
        void onlineAccumulator(const std::string& name,
                               Ice::Float learningRate,
                               const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief onlineAccumulatorVerbose same as onlineAccumulator, but evaluates after each learning step
         */
        void onlineAccumulatorVerbose(const std::string& name,
                                      Ice::Float learningRate,
                                      const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief the same as batch but with the data from the accumulator
         */
        void batchAccumulator(const std::string& name,
                              Ice::Int method,
                              Ice::Float threshold,
                              const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief the same as printErrors but with the data from the accumulator
         */
        void printErrorsAccumulator(const std::string& name,
                                    const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief the same as predict but with the data from the accumulator, however the results are currently just printed and not returned
         */
        void predictAccumulator(const std::string& name,
                                const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief solveGlobalIK solves the global inverse kinematics for the given position
         * @param name the name of the KBM
         * @param targetPosition
         * @return the proprioception
         */
        std::vector<memoryx::KBM::Real>
        solveGlobalIK(const std::string& name,
                      const std::vector<memoryx::KBM::Real>& targetPosition,
                      const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @brief solveDifferentialIK solves the differential inverse kinematics for the given position
         * @param name the name of the KBM
         * @param targetPosition
         * @param currentPosition
         * @param currentProprioception
         * @return the proprioception
         */
        std::vector<memoryx::KBM::Real> solveDifferentialIK(
            const std::string& name,
            const std::vector<memoryx::KBM::Real>& targetPosition,
            const std::vector<memoryx::KBM::Real>& currentPosition,
            const std::vector<memoryx::KBM::Real>& proprioceptionSequence,
            const std::vector<memoryx::KBM::Real>& lowerProprioceptionLimitsSequence,
            const std::vector<memoryx::KBM::Real>& upperProprioceptionLimitsSequence,
            const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * Returns the names of the existing KBMs
         */
        Ice::StringSeq kbmNames(const Ice::Current& c) override;
 
        /**
         * Clears the accumulators
         */
        void clearAccumulators(const Ice::Current& c) override;
 
        void storeToMemory(const std::string&,
                           const std::string&,
                           const std::string&,
                           const std::string&,
                           const Ice::Current&) override;
        void restoreFromMemory(const std::string&,
                               const std::string&,
                               const std::string&,
                               const std::string&,
                               const Ice::Current&) override;
        bool isInMemory(const std::string& nodeSetName,
                        const std::string& referenceFrameName,
                        const std::string& robotName,
                        const Ice::Current& c) override;
 
 
    protected:
        /**
         * @see armarx::ManagedIceObject::onInitComponent()
         */
        void onInitComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onConnectComponent()
         */
        void onConnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onDisconnectComponent()
         */
        void onDisconnectComponent() override;
 
        /**
         * @see armarx::ManagedIceObject::onExitComponent()
         */
        void onExitComponent() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        /**
         * @brief mapPosition converts the position sequence into an Eigen::Map of a Vector
         * @param kbm the dimension of this KBM will be used
         * @param position the position sequence
         * @return the map
         */
        Eigen::Map<const memoryx::KBM::Vector>
        mapPosition(const memoryx::KBM::Models::KBM& kbm,
                    const std::vector<memoryx::KBM::Real>& position);
 
        /**
         * @brief mapProprioception converts the proprioception sequence into an Eigen::Map of a Vector
         * @param kbm the DoF of this KBM will be used
         * @param proprioception the proprioception sequence
         * @return the map
         */
        Eigen::Map<const memoryx::KBM::Vector>
        mapProprioception(const memoryx::KBM::Models::KBM& kbm,
                          const std::vector<memoryx::KBM::Real>& proprioception);
 
        /**
         * @brief mapPositions converts the position sequence into an Eigen::Map of a Matrix
         * @param kbm the dimension of this KBM will be used
         * @param position the position sequence
         * @return the map
         */
        Eigen::Map<const memoryx::KBM::Matrix>
        mapPositions(const memoryx::KBM::Models::KBM& kbm,
                     const std::vector<memoryx::KBM::Real>& position);
 
        /**
         * maps a sequence to an Eigen::Map
         */
        Eigen::Map<const memoryx::KBM::Matrix>
        mapMatrix(int rows, const std::vector<memoryx::KBM::Real>& data);
 
        /**
         * @brief mapProprioceptions converts the proprioception sequence into an Eigen::Map of a Matrix
         * @param kbm the DoF of this KBM will be used
         * @param proprioception the proprioception sequence
         * @return the map
         */
        Eigen::Map<const memoryx::KBM::Matrix>
        mapProprioceptions(const memoryx::KBM::Models::KBM& kbm,
                           const std::vector<memoryx::KBM::Real>& proprioception);
 
        /**
         * @brief returns a pointer to the KBM with the given name if it exists or a nullptr if it doesn't
         * @param name the name of the KBM
         * @return pointer to the KBM or nullptr
         */
        memoryx::KBM::Models::KBM_ptr getKBM(const std::string& name);
 
    private:
        /**
         * @brief kbms maps KBM names to KBMs
         */
        std::map<std::string, memoryx::KBM::Models::KBM_ptr> kbms;
 
        /**
         * @brief createdFromVirtualRobot maps KBM names to a bool indicating whether they have been created from a VirtualRobot or not
         */
        std::map<std::string, bool> createdFromVirtualRobot;
 
        /**
         * @brief proprioceptionAccumulator is used to accumulate proprioceptions
         */
        std::vector<memoryx::KBM::Real> proprioceptionAccumulator;
 
        /**
         * @brief positionAccumulator is used to accumulate positions
         */
        std::vector<memoryx::KBM::Real> positionAccumulator;
 
        /**
         * @brief rawJointValuesAccumulator is used to accumulate raw joint values
         */
        std::vector<memoryx::KBM::Real> rawJointValuesAccumulator;
 
        /**
         * @brief evaluationProprioceptionAccumulator is used to accumulate proprioceptions for evaluation
         */
        std::vector<memoryx::KBM::Real> evaluationProprioceptionAccumulator;
 
        /**
         * @brief evaluationPositionAccumulator is used to accumulate positions for evaluation
         */
        std::vector<memoryx::KBM::Real> evaluationPositionAccumulator;
 
        memoryx::KBM::Vector minima;
        memoryx::KBM::Vector maxima;
 
        memoryx::LongtermMemoryInterfacePrx longtermMemoryPrx;
        armarx::RobotStateComponentInterfacePrx robotStateComponentPrx;
    };
} // namespace armarx