LVL1Controller.h

/*
 * 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    RobotAPI::ArmarXObjects::LVL1Controller
 * @author     Raphael ( ufdrv at student dot kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */

#ifndef _ARMARX_LIB_RobotAPI_LVL1Controller_H
#define _ARMARX_LIB_RobotAPI_LVL1Controller_H

#include <map>
#include <atomic>
#include <mutex>
#include <functional>

#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/util/Registrar.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>

#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.h>

#include "Targets/TargetBase.h"

//units
#include "DataUnits/ForceTorqueDataUnit.h"
#include "DataUnits/HapticDataUnit.h"
#include "DataUnits/IMUDataUnit.h"
#include "DataUnits/KinematicDataUnit.h"
#include "DataUnits/PlatformDataUnit.h"

namespace armarx
{
    class LVL1ControllerDataProviderInterface: virtual public Ice::Object
    {
    public:
        virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
        virtual const      HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
        virtual const         IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
        virtual const   KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
        virtual const    PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;

        virtual TargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) = 0;

        virtual std::string getName() const = 0;
    };
    using LVL1ControllerDataProviderInterfacePtr = IceInternal::Handle<LVL1ControllerDataProviderInterface>;

    /**
    * @defgroup Library-RobotRTControllers RobotRTControllers
    * @ingroup RobotAPI
    * A description of the library RobotRTControllers.
    */

    /**
    * @ingroup Library-RobotRTControllers
    * @brief A high level controller writing its results into targes.
    *
    * This is the abstract base class for all LVL1Controllers.
    * It implements basic management routines required by the RobotUnit and some basic ice calls.
    * LVL1Controllers are intantiated and managed by RobotUnits.
    *
    * Each LVL1Controller has to do the following in their constructor:
    * \li Request joints in exactly one control mode from the robot unit (the control mode can't be changed later)
    *     You will receive pointer targets where it can store its results.
    * \li Get pointer to the state of required joints / sensors. (you can't request other data at a later point)
    *     You will receive pointer to the values.
    *
    * Each controller has two parts:
    * \li The RT controll loop
    * \li The NonRT ice communication
    *
    * \section rtcontrollloop The RT controll loop (rtRun)
    * Here you have access to the robots current state and write your results into targets.
    * \warning This part has to satisfy realtime conditions!
    * All realtime able functions of this class have the 'rt' prefix.
    *
    * You must not:
    * \li call any blocking operation
    * \li allocate ram on the heap (since this is a blocking operation)
    * \li resize any datastructure (e.g. vector::resize) (this sometimes allocates ram on the heap)
    * \li insert new datafields into datastructures (e.g. vector::push_back) (this sometimes allocates ram on the heap)
    * \li write to any stream (e.g. ARMARX_VERBOSE, std::cout, print, filestreams) (this sometimes blocks/allocates ram on the heap)
    * \li make network calls (e.g. through ice) (this blocks/allocates ram on the heap) (using begin_... end_... version of ice calls IS NO SOLUTION)
    * \li do any expensive calculations (e.g. calculate IK, run some solver, invert big matrices)
    *
    * \section nonrtpart The NonRT ice communication
    * This part consits of any ice communication.
    * Here you could get new controll parameters or targets from other components.
    *
    * \section rtnrtcomm Communication between RT and NonRT
    * All communication between RT and NonRT has to be lockfree.
    * So you have to use constructs like atomics or TripleBuffers (See armarx::LVL1ControllerWithTripleBuffer).
    *
    * \image html LVL1ControllerGeneralDataFlow.svg "General Dataflow in a LVL1Controller"
    *
    *
    *
    *
    * \image html LVL1ControllerAtomicDataFlow.svg "Dataflow in a LVL1Controller using atomics for communication between RT and NonRT"
    *
    *
    *
    *
    * \image html LVL1ControllerTripleBufferDataFlow.svg "Dataflow in a LVL1Controller using a triple buffer for communication between RT and NonRT"
    *
    * \section expensivework How to do expensive calculations
    * You have to execute expensive calculations in a different worker thread.
    * This thread could calculate target values at its own speed (e.g. 100 Hz).
    *
    * While rtRun runs at a higher frequency (e.g. 1000 Hz) and:
    * \li reads target values
    * \li optionally passes the target values to a PID controller
    * \li writes them to the targets
    * \li sends the sensor values to the worker thread.
    *
    * If you do some additional calculation in rtRun, you maybe need to pass config parameters from NonRT to RT using a nonblocking method.
    *
    * \image html LVL1ControllerWorkerThreadDataFlow.svg "Dataflow in a LVL1Controller using a worker thread"
    *
    */
    class LVL1Controller:
        virtual public LVL1ControllerInterface,
        virtual public Component
    {
    public:
        //ice interface (must not be called in the rt thread)
        virtual bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final
        {
            return isActive;
        }
        virtual std::string getInstanceName(const Ice::Current& = GlobalIceCurrent) const final
        {
            return getName();
        }
        virtual StringStringDictionary getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const final
        {
            return jointControlModeMap;
        }
        virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override  = 0;
        //c++ interface (local calls) (must not be called in the rt thread)
        //c++ interface (local calls) (can only be called in the rt thread)
        virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
        /**
         * @brief This function is called before the controller is activated.
         * You can use it to activate a thread again (DO NOT SPAWN NEW THREADS!) e.g. via a std::atomic_bool.
         */
        virtual void rtPreActivateController() {}
        /**
         * @brief This function is called after the controller is deactivated.
         * You can use it to deactivate a thread (DO NOT JOIN THREADS!) e.g. via a std::atomic_bool.
         */
        virtual void rtPostDeactivateController() {}

        bool rtUsesJoint(std::size_t jointindex) const
        {
            return std::find(jointIndices.begin(), jointIndices.end(), jointindex) == jointIndices.end();
        }
        const std::vector<std::size_t>& rtGetJointIndices() const
        {
            return jointIndices;
        }
    private:
        friend class RobotUnit;
        void rtActivateController();
        void rtDeactivateController();

        //this data is filled by the robot unit to provide convenience functions
        std::atomic_bool isActive {false};
        JointNameToControlModeDictionary jointControlModeMap;
        std::vector<std::size_t> jointIndices;

        // ManagedIceObject interface
    protected:
        std::string getDefaultName() const
        {
            return getClassName();
        }
    };
    using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;

    using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(LVL1ControllerDataProviderInterfacePtr robotUnit, LVL1ControllerConfigPtr config)>>;
    template<class ControllerType>
    struct LVL1ControllerRegistration
    {
        LVL1ControllerRegistration(const std::string& name)
        {
            LVL1ControllerRegistry::registerElement(name, [](LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
            {
                ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider is NULL!");
                return new ControllerType(prov, config);
            });
        }
    };

    /**
    * @ingroup Library-RobotRTControllers
    * @brief Brief description of class LVL1Controller.
    *
    * Detailed description of class LVL1Controller.
    *
    * \code{cpp}
    //in ice
    module armarx
    {
        struct SomeControllerConfig extends LVL1ControllerConfig
        {
            float paramSetViaConfig;
        };

        interface SomeControllerInterface extends LVL1ControllerInterface
        {
            void setSomeParamChangedViaIce(float param);
        };
    };


    //in c++
    #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
    namespace armarx
    {
        struct SomeControlDataStruct
        {
            float parameterChangedViaIceCalls;
        };

        class SomeController : virtual public LVL1Controller<SomeControlDataStruct>,
            public SomeControllerInterface
        {
            JointVelocityTargetPtr targetJointXPtr;
            std::vector<const float*> jointValuePtrs;
            float someParamSetViaConfig;
            SomeController(RobotUnitPtr robotUnit, LVL1ControllerConfigPtr config):
                LVL1Controller<SomeControlDataStruct>(SomeControlDataStruct(1)) //initial parameter value
            {
                //cast the config to your config type
                SomeControlDataStructPtr someConfig = SomeControlDataStructPtr::dynamicCast(config);
                ARMARX_CHECK_EXPRESSION_W_HINT(someConfig,
                                               "The provided config has the wrong type! The type is " << config->ice_id()
                                               << " instead of " << SomeControllerConfig::ice_staticId());
                //read the config
                someParamSetViaConfig = someConfig->parameterSetViaIceCalls
                //make sure the used units are supported
                ARMARX_CHECK_EXPRESSION_W_HINT(robotUnit->getRTKinematicDataUnit(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
                //get pointers to sensor values from units
                jointValuePtrs = robotUnit->getRTKinematicDataUnit()->getJointAngles({"jointX", "jointY"});

                //get pointers for the results of this controller
                targetJointXPtr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jointX", ControlModes::VelocityMode));
                ARMARX_CHECK_EXPRESSION_W_HINT(targetJointXPtr, "The Joint jointX does not support the mode " + ControlModes::VelocityMode);
            }

            SomeController(JointVelocityTargetPtr targetPtr, ControllerConfigPtr config):
                targetJointXPtr{targetPtr}
            {

            }

            virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
            {
                pid.update(
                            rtGetControlStruct().parameterChangedViaIceCalls,
                            jointValuePtrs.at(0),
                            timeSinceLastIteration.toMicroSeconds());
                *targetJointXPtr = pid.getValue() + someParamSetViaConfig;
            }

            virtual void setSomeParamChangedViaIce(float param, const Ice::Current& = GlobalIceCurrent) override
            {
                std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
                getWriterControlStruct().parameterChangedViaIceCalls = param;
                writeControlStruct();
            }
        }
        //register the controller
        //this line has to appear in some cpp of your lib to do its registration (including it is enough)
        LVL1ControllerRegistration<SomeController> registrationSomeController("SomeController");
    }
    * \endcode
    */
    template <typename ControlDataStruct>
    class LVL1ControllerWithTripleBuffer: virtual public LVL1Controller
    {
    public:
        LVL1ControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
            controlDataTripleBuffer {initialCommands}
        {
        }

        virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
        {
            rtUpdateControlStruct();
            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
        }
    protected:
        const ControlDataStruct& rtGetControlStruct() const
        {
            return controlDataTripleBuffer.getReadBuffer();
        }
        bool                     rtUpdateControlStruct()
        {
            return controlDataTripleBuffer.updateReadBuffer();
        }

        void writeControlStruct()
        {
            //just lock to be save and reduce the impact of an error
            //also this allows code to unlock the mutex before calling this function
            //(can happen if some lockguard in a scope is used)
            std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
            controlDataTripleBuffer.commitWrite();
        }
        ControlDataStruct& getWriterControlStruct()
        {
            return controlDataTripleBuffer.getWriteBuffer();
        }

        mutable std::recursive_mutex controlDataMutex;
    private:
        WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
    };
    template <typename ControlDataStruct>
    using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;


    struct PlatformCartesianVelocity
    {
        float vx;
        float vy;
        float vAngle;
        PlatformCartesianVelocity() : vx(0), vy(0), vAngle(0)
        { }
    };


    class AbstractLvl1PlatformVelocityController : public virtual LVL1ControllerWithTripleBuffer<PlatformCartesianVelocity> {

    public:
        virtual void setTarget(float vx, float vy, float vAngle) = 0;
    };

    typedef boost::shared_ptr <AbstractLvl1PlatformVelocityController> AbstractLvl1PlatformVelocityControllerPtr;


}
#endif