NJointControllerBase.h

Go to the documentation of this file.

/*
 * 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::NJointController
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <atomic>
#include <map>
#include <mutex>
#include <optional>
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h> // for ARMARX_CHECK
#include <ArmarXCore/core/services/tasks/ThreadPool.h>
#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
 
// #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
namespace IceProxy::armarx
{
    class DebugDrawerInterface;
    class DebugObserverInterface;
    class RobotUnitInterface;
} // namespace IceProxy::armarx
 
namespace armarx
{
    namespace RobotUnitModule
    {
        class NJointControllerAttorneyForPublisher;
        class NJointControllerAttorneyForControlThread;
        class NJointControllerAttorneyForControllerManagement;
    } // namespace RobotUnitModule
 
    namespace detail
    {
        template <class>
        class NJointControllerRegistryEntryHelper;
        class ControlThreadOutputBufferEntry;
    } // namespace detail
 
    namespace WidgetDescription
    {
        class Widget;
        typedef ::IceInternal::Handle<::armarx::WidgetDescription::Widget> WidgetPtr;
        typedef ::std::map<::std::string, ::armarx::WidgetDescription::WidgetPtr>
            StringWidgetDictionary;
    } // namespace WidgetDescription
 
    using SensorAndControl = detail::ControlThreadOutputBufferEntry;
    class JointController;
    class SensorValueBase;
    class ControlTargetBase;
    using ConstControlDevicePtr = std::shared_ptr<const class ControlDevice>;
    using ConstSensorDevicePtr = std::shared_ptr<const class SensorDevice>;
 
    typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface>
        DebugDrawerInterfacePrx;
    typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface>
        DebugObserverInterfacePrx;
    typedef ::IceInternal::ProxyHandle<::IceProxy::armarx::RobotUnitInterface>
        RobotUnitInterfacePrx;
 
    using ThreadPoolPtr = std::shared_ptr<class ThreadPool>;
 
    TYPEDEF_PTRS_HANDLE(NJointControllerBase);
    TYPEDEF_PTRS_HANDLE(SynchronousNJointController);
    TYPEDEF_PTRS_HANDLE(AsynchronousNJointController);
 
    TYPEDEF_PTRS_HANDLE(RobotUnit);
 
    /**
     * @defgroup Library-RobotUnit-NJointControllers NJointControllers
     * @ingroup Library-RobotUnit
     */
 
    /**
    * @ingroup Library-RobotUnit-NJointControllers
    * @brief A high level controller writing its results into \ref ControlTargetBase "ControlTargets".
    *
    * This is the abstract base class for all NJointControllers.
    * It implements basic management routines required by the RobotUnit and some basic ice calls.
    * \ref NJointControllerBase "NJointControllers" are instantiated and managed by the \ref RobotUnit.
    *
    * A \ref NJointControllerBase calculates \ref ControlTargetBase "ControlTargets" for a set of
    * \ref ControlDevice "ControlDevices" in a specific ControlMode.
    * This ControlMode is defined for each \ref ControlDevice during construction.
    *
    * \section nj-state Requested and Active
    * A \ref NJointControllerBase can is requested / not requested and active / inactive.
    * All four combinations are possible.
    *
    * \subsection nj-state-req Requested / Not Requested
    * If the user wants this \ref NJointControllerBase to be executed it is in a requested state (see \ref isControllerRequested).
    * Otherwise the controler is not requested.
    *
    * Calling \ref activateController sets the state to requested.
    * Calling \ref deactivateController sets the state to not requested.
    * If the \ref NJointControllerBase causes an error or a different \ref NJointControllerBase using one or more of the same
    * \ref ControlDevice's is requested, this \ref NJointControllerBase is deactivated.
    *
    *
    * \subsection nj-state-act Active / Inactive
    * Is the \ref NJointControllerBase executed by the \ref RobotUnitModules::ControlThread "ControlThread" it is active
    * (see \ref isControllerActive).
    *
    * To be executed by the \ref RobotUnitModules::ControlThread "ControlThread", the \ref NJointControllerBase has to be
    * requested at some point in the past.
    *
    * If a controller is active, it has to write a valid \ref ControlTargetBase "ControlTarget"
    * for each of its \ref ControlDevice "ControlDevicea" (if it fails, it will be deactivated).
    *
    * \section Constructor
    * In the Constructor, a \ref NJointControllerBase has to declare all \ref ControlDevice "ControlDevices" it uses.
    *
    * The constructor takes a pointer to a configuration structure of the type \ref NJointControllerBase::ConfigPtrT.
    * If an implementation requires a special configuration structure (e.g.: SomeOtherCfg), it has to override this type by calling adding
    * \code{.cpp}
    * using ConfigPtrT = SomeOtherCfgPtr;
    * \endcode
    * to it's clas definition.
    * The type SomeOtherCfg has to derive \ref NJointControllerConfigPtr.
    *
    * There is a way to generate a small gui widget for controller construction (see this \ref nj-ctor-gui "section").
    *
    * \subsection nj-ctor-req-ctrl Using ControlTargets
    * A \ref NJointControllerBase can use \ref peekControlDevice to examine a \ref ControlDevice before using it
    * (e.g.: checking the supported \ref ControlTargetBase "ControlTargets").
    *
    * If a \ref ControlDevice should be used by this \ref NJointControllerBase, it has to call \ref useControlDevice.
    * This sets the ControlMode for the \ref ControlDevice and returns a pointer to the \ref ControlTargetBase "ControlTarget".
    * This pointer has to be used to send commands in each iteration of \ref rtRun.
    * The ControlMode can't be changed afterwards (A new \ref NJointControllerBase has to be created).
    *
    *
    * \subsection nj-ctor-req-sens Using SensorValues
    * A \ref NJointControllerBase can use \ref peekSensorDevice to examine a \ref SensorDevice before using it.
    *
    * If a \ref SensorDevice should be used by this \ref NJointControllerBase, it has to call \ref useSensorDevice.
    * This returns a pointer to the \ref SensorValueBase "SensorValue".
    *
    * \subsection nj-ctor-rob A synchronized Virtualrobot
    * If the controller needs a simox robot in \ref rtRun, it should call \ref useSynchronizedRtRobot.
    * This will provide a simoxRobot via \ref rtGetRobot.
    * This robot is synchronized with the real robots state before calling \ref rtRun
    *
    * \section nj-parts Rt and non Rt
    * Each \ref NJointControllerBase has two parts:
    * \li The RT controll loop
    * \li The NonRT ice communication
    *
    * \subsection rtcontrollloop The Rt controll loop (\ref rtRun)
    * \warning This part has to satisfy realtime conditions!
    * All realtime functions of \ref NJointControllerBase have the 'rt' prefix.
    *
    * Here the \ref NJointControllerBase has access to the robot's current state
    * and has to write results into \ref ControlTargetBase "ControlTargets".
    *
    * It 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)
    *
    * \subsection nonrtpart The NonRT ice communication
    * This part consits of any ice communication.
    * Here the \ref NJointControllerBase can 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.
    * The \ref NJointControllerBase has to use constructs like atomics or
    * \ref TripleBuffer "TripleBuffers" (See \ref armarx::NJointControllerWithTripleBuffer).
    *
    * \image html NJointControllerGeneralDataFlow.svg "General Dataflow in a NJointControllerBase"
    *
    * \image html NJointControllerAtomicDataFlow.svg "Dataflow in a NJointControllerBase using atomics for communication between RT and NonRT"
    *
    * \image html NJointControllerTripleBufferDataFlow.svg "Dataflow in a NJointControllerBase using a triple buffer for communication between RT and NonRT"
    *
    *
    * \image html NJointControllerDataFlow_Graph.svg "Dataflow in a NJointControllerBase as a Graph of the two participating domains"
    * The above image shows the two participating domains: one RT thread and multiple ICE threads.
    * If data has to flow along an arrow, you need some construct for non blocking message passing.
    *
    * \warning If you use \ref TrippleBuffer "TrippleBuffers" or \ref WriteBufferedTrippleBuffer "WriteBufferedTrippleBuffers" you need a separate one for each arrow.
    *
    * \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 NJointControllerWorkerThreadDataFlow.svg "Dataflow in a NJointControllerBase using a worker thread"
    *
    * \image html NJointControllerWorkerThreadDataFlow_Graph.svg "Dataflow in a NJointControllerBase using a worker thread as a Graph of the three participating domains"
    * The above image shows the three participating domains: one RT thread, one worker trhead and multiple ICE threads.
    * If data has to flow along an arrow, you need some construct for non blocking message passing.
    *
    * \warning If you use \ref TrippleBuffer "TrippleBuffers" or \ref WriteBufferedTrippleBuffer "WriteBufferedTrippleBuffers" you need a separate one for each arrow.
    *
    * \section nj-ctor-gui Providing a gui for controller construction.
    * By implementing these functions:
    * \code{.cpp}
     static WidgetDescription::WidgetPtr GenerateConfigDescription
     (
        const VirtualRobot::RobotPtr& robot,
        const std::map<std::string, ConstControlDevicePtr>& controlDevices,
        const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
     ); //describes how the widget is supposed to look
     static ConfigPtrT GenerateConfigFromVariants(const StringVariantBaseMap&); // turns the resulting variants into a config
    * \endcode
    *
    * The \ref RobotUnitGui will provide a widget to configure and construct a \ref NJointControllerBase of this type.
    *
    * \section Examples
    *
    * More examples can be found in the Tutorials Package
    *
    * \subsection nj-example-1 A simple pass Position controller
    * \note The code can be found in the Tutorial package
    *
    * \subsection nj-example-1-h Header
    * Include headers
    * \code{.cpp}
        #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
        #include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
        #include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
    * \endcode
    *
    * Open the namespace
    * \code{.cpp}
        namespace armarx
        {
    * \endcode
    *
    * Typedef some pointers
    * \code{.cpp}
            TYPEDEF_PTRS_HANDLE(NJointPositionPassThroughController);
            TYPEDEF_PTRS_HANDLE(NJointPositionPassThroughControllerConfig);
    * \endcode
    *
    * The config has to inherit \ref NJointControllerConfig and consists only of the joint name
    * \code{.cpp}
            class NJointPositionPassThroughControllerConfig : virtual public NJointControllerConfig
            {
            public:
                NJointPositionPassThroughControllerConfig(const std::string& name): deviceName {name} {}
                std::string deviceName;
            };
    * \endcode
    *
    * The controller class has to inherit \ref NJointControllerBase
    * \code{.cpp}
            class NJointPositionPassThroughController: public NJointControllerBase
            {
            public:
    * \endcode
    *
    * The \ref NJointControllerBase provides a config widget for the \ref RobotUnitGuiPlugin
    * \code{.cpp}
                static WidgetDescription::WidgetPtr GenerateConfigDescription
                (
                    const VirtualRobot::RobotPtr&,
                    const std::map<std::string, ConstControlDevicePtr>& controlDevices,
                    const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
                );
                static NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
    * \endcode
    *
    * The ctor receives a pointer to the \ref RobotUnit, a pointer to the config and a pointer to a VirtualRobot model.
    * \code{.cpp}
                NJointPositionPassThroughController(
                    const RobotUnitPtr& prov,
                    NJointControllerConfigPtr config,
                    const VirtualRobot::RobotPtr& r
                );
    * \endcode
    *
    * The function to provide the class name.
    * \code{.cpp}
                std::string getClassName(const Ice::Current&) const override;
    * \endcode
    *
    * This controller provides widgets for function calls.
    * \code{.cpp}
                WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current&) const override;
                void callDescribedFunction(const std::string& name, const StringVariantBaseMap& value, const Ice::Current&) override;
    * \endcode
    *
    * The run function executed to calculate new targets
    * \code{.cpp}
                void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
    * \endcode
    *
    * Hooks for this \ref NJointControllerBase to execute code during publishing.
    * \code{.cpp}
                void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
                void onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
                void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&) override;
    * \endcode
    *
    * This \ref NJointControllerBase uses one position \ref SensorValue1DoFActuatorPosition "SensorValue", calculates one
    * position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and stores the current position target and position value in atomics.
    * \code{.cpp}
                const SensorValue1DoFActuatorPosition* sensor;
                ControlTarget1DoFActuatorPosition* target;
                std::atomic<float> targetPos {0};
                std::atomic<float> currentPos {0};
    * \endcode
    *
    * Close the class and the namespace
    * \code{.cpp}
            };
        }
    * \endcode
    *
    * \subsection nj-example-1-c Source file
    *
    * Include the required headers.
    * \code{.cpp}
        #include <RobotAPI/libraries/core/Pose.h>
 
        #include "NJointPositionPassThroughController.h"
    * \endcode
    *
    * Open the namespace
    * \code{.cpp}
        namespace armarx
        {
    * \endcode
    *
    * This generates a config widget used to display a config gui in \ref RobotUnitGui.
    * It consist out of a \ref HBoxLayout containing a \ref Label and a \ref StringComboBox
    * \code{.cpp}
            WidgetDescription::WidgetPtr NJointPositionPassThroughController::GenerateConfigDescription(const VirtualRobot::RobotPtr&, const std::map<std::string, ConstControlDevicePtr>& controlDevices, const std::map<std::string, ConstSensorDevicePtr>& sensorDevices)
            {
                using namespace armarx::WidgetDescription;
                HBoxLayoutPtr layout = new HBoxLayout;
                LabelPtr label = new Label;
                label->text = "control device name";
                layout->children.emplace_back(label);
                StringComboBoxPtr box = new StringComboBox;
                box->defaultIndex = 0;
    * \endcode
    *
    * The \ref StringComboBox only contains the names of \ref ControlDevice "ControlDevices" accepting
    * a position \ref ControlTarget1DoFActuatorPosition "ControlTarget" and providing a position
    * \ref SensorValue1DoFActuatorPosition "SensorValue"
    * \code{.cpp}
                //filter control devices
                for (const auto& name2dev : controlDevices)
                {
                    const ConstControlDevicePtr& dev = name2dev.second;
                    const auto& name = name2dev.first;
                    if (
                        dev->hasJointController(ControlModes::Position1DoF) &&
                        dev->getJointController(ControlModes::Position1DoF)->getControlTarget()->isA<ControlTarget1DoFActuatorPosition>() &&
                        sensorDevices.count(name) &&
                        sensorDevices.at(name)->getSensorValue()->isA<SensorValue1DoFActuatorPosition>()
                    )
                    {
                        box->options.emplace_back(name);
                    }
                }
                box->name = "name";
                layout->children.emplace_back(box);
                return layout;
            }
    * \endcode
    *
    * This turns a map of variants into the config required by this \ref NJointControllerBase.
    * The \ref Variant for the key 'name' defines the \ref ControlDevice.
    * \code{.cpp}
            NJointControllerConfigPtr NJointPositionPassThroughController::GenerateConfigFromVariants(const StringVariantBaseMap& values)
            {
                //you should check here for types null additional params etc
                return new NJointPositionPassThroughControllerConfig {values.at("name")->getString()};
            }
    * \endcode
    *
    * The constructors implementation.
    * \code{.cpp}
            NJointPositionPassThroughController::NJointPositionPassThroughController(
                const RobotUnitPtr& ru,
                NJointControllerConfigPtr config,
                const VirtualRobot::RobotPtr&
            )
            {
                ARMARX_CHECK_EXPRESSION(ru);
    * \endcode
    *
    * It checks whether the provided config has the correct type.
    * \code{.cpp}
                NJointPositionPassThroughControllerConfigPtr cfg = NJointPositionPassThroughControllerConfigPtr::dynamicCast(config);
                ARMARX_CHECK_EXPRESSION(cfg) << "The provided config has the wrong type! The type is " << config->ice_id();
    * \endcode
    *
    * Then it retrieves the \ref SensorValue1DoFActuatorPosition "SensorValue" and
    * \ref ControlTarget1DoFActuatorPosition "ControlTarget", casts then to the required types and stores them to the member variables.
    * \code{.cpp}
                const SensorValueBase* sv = useSensorValue(cfg->deviceName);
                ControlTargetBase* ct = useControlTarget(cfg->deviceName, ControlModes::Position1DoF);
                ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
                ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorPosition>());
                sensor = sv->asA<SensorValue1DoFActuatorPosition>();
                target = ct->asA<ControlTarget1DoFActuatorPosition>();
            }
    * \endcode
    *
    * The class name has to be unique, hence the c++ class name should be used.
    * \code{.cpp}
            std::string NJointPositionPassThroughController::getClassName(const Ice::Current&) const
            {
                return "NJointPositionPassThroughController";
            }
 
    * \endcode
    *
    * This \ref NJointControllerBase provides two widgets for function calls.
    * \code{.cpp}
            WidgetDescription::StringWidgetDictionary NJointPositionPassThroughController::getFunctionDescriptions(const Ice::Current&) const
            {
                using namespace armarx::WidgetDescription;
    * \endcode
    *
    * The first widget has a \ref Label and a \ref FloatSpinBox.
    * \code{.cpp}
                HBoxLayoutPtr layoutSetPos = new HBoxLayout;
                {
                    LabelPtr label = new Label;
                    label->text = "positiontarget";
                    layoutSetPos->children.emplace_back(label);
                    FloatSpinBoxPtr spin = new FloatSpinBox;
                    spin->defaultValue = 0;
                    spin->max = 1;
                    spin->min = -1;
    * \endcode
    *
    * The \ref FloatSpinBox creates a variant called 'spinmearound'
    * \code{.cpp}
                    spin->name = "spinmearound";
                    layoutSetPos->children.emplace_back(spin);
                }
    * \endcode
    *
    * The second widget also has a \ref Label and a \ref FloatSpinBox.
    * \code{.cpp}
                VBoxLayoutPtr layoutSetHalfPos = new VBoxLayout;
                {
                    LabelPtr label = new Label;
                    label->text = "positiontarget / 2";
                    layoutSetHalfPos->children.emplace_back(label);
                    FloatSpinBoxPtr spin = new FloatSpinBox;
                    spin->defaultValue = 0;
                    spin->max = 0.5;
                    spin->min = -0.5;
                    spin->name = "spinmehalfaround";
                    layoutSetHalfPos->children.emplace_back(spin);
                }
    * \endcode
    *
    * This returns a map of boths widgets.
    * The keys will be used to identify the called function.
    * \code{.cpp}
                return {{"SetPosition", layoutSetPos}, {"SetPositionHalf", layoutSetHalfPos}};
            }
    * \endcode
    *
    * This function is called from the \ref RobotUnitGuiPlugin when the prior defined functions are called.
    * Both functions set the target position.
    * \code{.cpp}
            void NJointPositionPassThroughController::callDescribedFunction(const std::string& name, const StringVariantBaseMap& value, const Ice::Current&)
            {
                if (name == "SetPosition")
                {
                    //you should check here for types null additional params etc
                    targetPos = value.at("spinmearound")->getFloat();
                }
                else if (name == "SetPositionHalf")
                {
                    //you should check here for types null additional params etc
                    targetPos = value.at("spinmehalfaround")->getFloat() * 2;
                }
                else
                {
                    ARMARX_ERROR << "CALLED UNKNOWN REMOTE FUNCTION: " << name;
                }
            }
    * \endcode
    *
    * The \ref rtRun function sets the \ref ControlTarget1DoFActuatorPosition "ControlTarget" to the
    * target value set via the atomic and stores the current
    * \ref SensorValue1DoFActuatorPosition "SensorValue" to the other atomic.
    * \code{.cpp}
            void NJointPositionPassThroughController::rtRun(const IceUtil::Time& t, const IceUtil::Time&)
            {
                ARMARX_RT_LOGF_ERROR("A MESSAGE PARAMETER %f", t.toSecondsDouble()).deactivateSpam(1);
                ARMARX_RT_LOGF_IMPORTANT("A MESSAGE WITHOUT PARAMETERS").deactivateSpam(1);
                target->position = targetPos;
                currentPos = sensor->position;
            }
    * \endcode
    *
    * The publish activation \ref onPublishActivation "hook" does nothing, but could be used to call
    * \ref DebugDraver::setRobotVisu.
    * \code{.cpp}
            void NJointPositionPassThroughController::onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
            {
                //we could do some setup for the drawer here. e.g. add a robot
            }
 
    * \endcode
    *
    * The publish deactivation \ref onPublishDeactivation "hook" clears the \ref DebugDrawer layer
    * \code{.cpp}
            void NJointPositionPassThroughController::onPublishDeactivation(const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&)
            {
                drawer->removeLayer("Layer_" + getInstanceName());
            }
 
    * \endcode
    *
    * The publish \ref onPublish "hook" draws a sphere with radius of the position stored in the atomic times 2000 to the \ref DebugDrawer.
    * \code{.cpp}
            void NJointPositionPassThroughController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& drawer, const DebugObserverInterfacePrx&)
            {
                drawer->setSphereVisu("Layer_" + getInstanceName(), "positionball", new Vector3, armarx::DrawColor {1, 1, 1, 1}, currentPos * 2000);
            }
    * \endcode
    *
    * This registers a factory for this \ref NJointControllerBase.
    * This factory is used by the \ref RobotUnit to create the \ref NJointControllerBase.
    * The passed name has to match the name returned by \ref getClassName.
    * \code{.cpp}
            NJointControllerRegistration<NJointPositionPassThroughController> registrationControllerNJointPositionPassThroughController("NJointPositionPassThroughController");
    * \endcode
    *
    * This statically asserts a factory for the type NJointPositionPassThroughController is present.
    * \code{.cpp}
            ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(NJointPositionPassThroughController);
    * \endcode
    * Close the namespace
    * \code{.cpp}
        }
    * \endcode
    */
    class NJointControllerBase :
        virtual public NJointControllerInterface,
        virtual public ManagedIceObject
    {
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////////// typedefs /////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        using ConfigPtrT = NJointControllerConfigPtr;
        using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr (*)(
            const VirtualRobot::RobotPtr&,
            const std::map<std::string, ConstControlDevicePtr>& controlDevices,
            const std::map<std::string, ConstSensorDevicePtr>& sensorDevices);
        template <class ConfigPrtType>
        using GenerateConfigFromVariantsFunctionSignature =
            ConfigPrtType (*)(const StringVariantBaseMap&);
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////// constructor setup functions ////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        /**
         * @brief Get a const ptr to the given \ref SensorDevice
         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
         * @param deviceName The \ref SensorDevice's name
         * @return A const ptr to the given \ref SensorDevice
         */
        ConstSensorDevicePtr peekSensorDevice(const std::string& deviceName) const;
        /**
         * @brief Get a const ptr to the given \ref ControlDevice
         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
         * @param deviceName The \ref ControlDevice's name
         * @return A const ptr to the given \ref ControlDevice
         */
        ConstControlDevicePtr peekControlDevice(const std::string& deviceName) const;
 
        /**
         * @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
         * for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
         * @param deviceName The \ref ControlDevice's name
         * @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
         * @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
         */
        ControlTargetBase* useControlTarget(const std::string& deviceName,
                                            const std::string& controlMode);
 
        /**
         * @brief Declares to calculate the \ref ControlTargetBase "ControlTarget"
         * for the given \ref ControlDevice in the given ControlMode when \ref rtRun is called
         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
         * @param deviceName The \ref ControlDevice's name
         * @param controlMode The \ref ControlTargetBase "ControlTarget's" ControlMode
         * @return A ptr to the given \ref ControlDevice's \ref ControlTargetBase "ControlTarget"in the given ControlMode
         */
        template <class T>
        T*
        useControlTarget(const std::string& deviceName, const std::string& controlMode)
        {
            static_assert(std::is_base_of<ControlTargetBase, T>::value,
                          "The given type does not derive ControlTargetBase");
            ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode);
            return dynamic_cast<T*>(ptr);
        }
 
        /**
         * @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
         * @param deviceName The \ref SensorDevice's name
         * @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
         */
        const SensorValueBase* useSensorValue(const std::string& sensorDeviceName) const;
 
        /**
         * @brief Get a const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
         * @param deviceName The \ref SensorDevice's name
         * @return A const ptr to the given \ref SensorDevice's \ref SensorValueBase "SensorValue"
         */
        template <class T>
        const T*
        useSensorValue(const std::string& deviceName) const
        {
            static_assert(std::is_base_of<SensorValueBase, T>::value,
                          "The given type does not derive SensorValueBase");
            const SensorValueBase* const ptr = useSensorValue(deviceName);
            return dynamic_cast<const T*>(ptr);
        }
 
        /**
         * @brief Requests a VirtualRobot for use in \ref rtRun
         *          *
         * \warning This function can only be called in a \ref NJointControllerBase's ctor (or functions called in it)
         *
         * The robot is updated before \ref rtRun is called and can be accessed via \rtGetRobot
         * @param updateCollisionModel Whether the robot's collision model should be updated
         * @see rtGetRobot
         */
        const VirtualRobot::RobotPtr& useSynchronizedRtRobot(bool updateCollisionModel = false);
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ////////////////////////////////// Component interface ///////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        /// @see Component::getDefaultName
        std::string
        getDefaultName() const override
        {
            return getClassName();
        }
 
        /// @see Component::onInitComponent
        void onInitComponent() final;
        /// @see Component::onConnectComponent
        void onConnectComponent() final;
        /// @see Component::onDisconnectComponent
        void onDisconnectComponent() final;
        /// @see Component::onExitComponent
        void onExitComponent() final;
 
        virtual void
        onInitNJointController()
        {
        }
 
        virtual void
        onConnectNJointController()
        {
        }
 
        virtual void
        onDisconnectNJointController()
        {
        }
 
        virtual void
        onExitNJointController()
        {
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ////////////////////////////////// ThreadPool functionality///////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        ThreadPoolPtr getThreadPool() const;
 
        /**
         * @brief Executes a given task in a separate thread from the Application ThreadPool.
         * @param taskName Descriptive name of this task to identify it on errors.
         * @param task std::function object (or lambda) that is to be executed.
         * @note This task will be joined in onExitComponent of the NJointControllerBase. So make sure it terminates, when the
         * controller is deactivated or removed!
         *
         * @code{.cpp}
         *  runTask("PlotterTask", [&]
                {
                    CycleUtil c(30);
                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
                    while (getState() == eManagedIceObjectStarted)
                    {
                        if (isControllerActive())
                        {
                            StringVariantBaseMap errors;
                            for (size_t i = 0; i < sensors.size(); i++)
                            {
                                errors[cfg->jointNames.at(i)] = new Variant(currentError(i));
                            }
                            dbgObs->setDebugChannel("TrajectoryController", errors);
                        }
                        c.waitForCycleDuration();
                    }
                });
         * @endcode
         */
        template <typename Task>
        void
        runTask(const std::string& taskName, Task&& task)
        {
            std::unique_lock lock(threadHandlesMutex);
            ARMARX_CHECK_EXPRESSION(!taskName.empty());
            ARMARX_CHECK_EXPRESSION(!threadHandles.count(taskName));
            ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
            ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName
                           << "' - current available thread count: "
                           << getThreadPool()->getAvailableTaskCount();
            auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
            ARMARX_CHECK_EXPRESSION(handlePtr->isValid())
                << "Could not add task (" << taskName << " - " << GetTypeString(task)
                << " ) - available threads: " << getThreadPool()->getAvailableTaskCount();
            threadHandles[taskName] = handlePtr;
        }
 
        std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;
        std::mutex threadHandlesMutex;
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// ice interface //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        bool
        isControllerActive(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return isActive;
        }
 
        bool
        isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return isRequested;
        }
 
        bool
        isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return deletable;
        }
 
        bool
        hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return deactivatedBecauseOfError;
        }
 
        std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override = 0;
 
        std::string
        getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return instanceName_;
        }
 
        NJointControllerDescription
        getControllerDescription(const Ice::Current& = Ice::emptyCurrent) const final override;
        NJointControllerStatus
        getControllerStatus(const Ice::Current& = Ice::emptyCurrent) const final override;
        NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(
            const Ice::Current& = Ice::emptyCurrent) const final override;
        //RobotUnitInterfacePrx getRobotUnit(const Ice::Current& = Ice::emptyCurrent) const final override;
 
        void activateController(const Ice::Current& = Ice::emptyCurrent) override;
        void deactivateController(const Ice::Current& = Ice::emptyCurrent) override;
        void deleteController(const Ice::Current& = Ice::emptyCurrent) final override;
        void deactivateAndDeleteController(const Ice::Current& = Ice::emptyCurrent) final override;
 
        WidgetDescription::StringWidgetDictionary
        getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override
        {
            return {};
        }
 
        void
        callDescribedFunction(const std::string&,
                              const StringVariantBaseMap&,
                              const Ice::Current& = Ice::emptyCurrent) override
        {
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// rt interface ///////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public: ///TODO make protected and use attorneys
        /**
         * @brief Returns the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
         * @return The virtual robot used by this \ref NJointControllerBase in the \ref rtRun
         * @see useSynchronizedRtRobot
         * @see rtGetRobotNodes
         */
        const VirtualRobot::RobotPtr&
        rtGetRobot()
        {
            return rtRobot;
        }
 
        /**
         * @brief Returns the nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
         * @return The nodes of the virtual robot used by this \ref NJointControllerBase in the \ref rtRun
         * @see useSynchronizedRtRobot
         * @see rtGetRobot
         */
        const std::vector<VirtualRobot::RobotNodePtr>&
        rtGetRobotNodes()
        {
            return rtRobotNodes;
        }
 
        /**
         * @brief Returns whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
         * for the given \ref ControlDevice
         * @param deviceIndex The \ref ControlDevice's index
         * @return Whether this \ref NJointControllerBase calculates a \ref ControlTargetBase "ControlTarget"
         * for the given \ref ControlDevice
         */
        bool
        rtUsesControlDevice(std::size_t deviceIndex) const
        {
            return controlDeviceUsedBitmap.at(deviceIndex);
        }
 
        /**
         * @brief Returns the indices of all \ref ControlDevice's this \ref NJointControllerBase
         * calculates a \ref ControlTargetBase "ControlTarget" for.
         * @return The indices of all \ref ControlDevice's this \ref NJointControllerBase
         * calculates a \ref ControlTargetBase "ControlTarget" for.
         */
        const std::vector<std::size_t>&
        rtGetControlDeviceUsedIndices() const
        {
            return controlDeviceUsedIndices;
        }
 
        /**
         * @brief Sets the error state to true. This will deactivate this controller!
         */
        bool
        rtGetErrorState() const
        {
            return errorState;
        }
 
        /**
         * @brief Returns the number of used \ref ControlDevice "ControlDevices"
         * @return The number of used \ref ControlDevice "ControlDevices"
         */
        std::size_t
        rtGetNumberOfUsedControlDevices() const
        {
            return controlDeviceUsedIndices.size();
        }
 
        /**
         * @brief Returns the class name. (the c-string may be used for rt message logging)
         * @return The class name. (the c-string may be used for rt message logging)
         */
        const std::string&
        rtGetClassName() const
        {
            return rtClassName_;
        }
 
        /**
         * @brief Returns the instance name. (the c-string may be used for rt message logging)
         * @return The instance name. (the c-string may be used for rt message logging)
         */
        const std::string&
        rtGetInstanceName() const
        {
            return instanceName_;
        }
 
    protected:
        /**
         * @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()
        {
        }
 
        /**
         * @brief Sets the error state to true. This will deactivate this controller!
         */
        void
        rtSetErrorState()
        {
            errorState.store(true);
        }
 
    private:
        /**
         * @brief Activates this controller in the \ref RobotUnitModules::ControlThread "ControlThread".
         *
         * Calls \ref rtPreActivateController
         * @see rtDeactivateController
         * @see rtDeactivateControllerBecauseOfError
         * @see rtPreActivateController
         */
        void rtActivateController();
        /**
         * @brief Deactivates this controller in the \ref RobotUnitModules::ControlThread "ControlThread".
         *
         * Calls \ref rtPostDeactivateController
         * @see rtActivateController
         * @see rtDeactivateControllerBecauseOfError
         * @see rtPostDeactivateController
         */
        void rtDeactivateController();
        /**
         * @brief Deactivates this controller in the \ref RobotUnitModules::ControlThread "ControlThread"
         * and sets the error flag.
         *
         * Calls \ref rtPostDeactivateController
         * This function is called when this \ref NJointControllerBase produces an error
         * (e.g.: calculates an invalid \ref ControlTargetBase "ControlTarget", throws an exception)
         * @see rtActivateController
         * @see rtDeactivateController
         * @see rtPostDeactivateController
         */
        void rtDeactivateControllerBecauseOfError();
 
    public:
        static const NJointControllerBasePtr NullPtr;
 
        NJointControllerBase();
 
        ~NJointControllerBase() override;
        //ice interface (must not be called in the rt thread)
 
        //c++ interface (local calls) (must be called in the rt thread)
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// used targets ///////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        //used control devices
        StringStringDictionary
        getControlDeviceUsedControlModeMap(
            const Ice::Current& = Ice::emptyCurrent) const final override
        {
            return controlDeviceControlModeMap;
        }
 
        const std::vector<char>&
        getControlDeviceUsedBitmap() const
        {
            return controlDeviceUsedBitmap;
        }
 
        const std::vector<std::size_t>&
        getControlDeviceUsedIndices() const
        {
            return controlDeviceUsedIndices;
        }
 
        const std::map<std::string, const JointController*>&
        getControlDevicesUsedJointController()
        {
            return controlDeviceUsedJointController;
        }
 
        //check for conflict
        std::optional<std::vector<char>>
        isNotInConflictWith(const NJointControllerBasePtr& other) const
        {
            return isNotInConflictWith(other->getControlDeviceUsedBitmap());
        }
 
        std::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
 
        template <class ItT>
        static std::optional<std::vector<char>>
        AreNotInConflict(ItT first, ItT last)
        {
            if (first == last)
            {
                return std::vector<char>{};
            }
            std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
            std::vector<char> inuse(n, false);
            while (first != last)
            {
                auto r = (*first)->isNotInConflictWith(inuse);
                if (!r)
                {
                    return r;
                }
                inuse = std::move(*r);
                ++first;
            }
            return inuse;
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ////////////////////////////////////// publishing ////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        //publish thread hooks
        virtual void
        onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
        {
        }
 
        virtual void
        onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&)
        {
        }
 
        virtual void
        onPublish(const SensorAndControl&,
                  const DebugDrawerInterfacePrx&,
                  const DebugObserverInterfacePrx&)
        {
        }
 
    private:
        void publish(const SensorAndControl& sac,
                     const DebugDrawerInterfacePrx& draw,
                     const DebugObserverInterfacePrx& observer);
        void deactivatePublish(const DebugDrawerInterfacePrx& draw,
                               const DebugObserverInterfacePrx& observer);
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////////// data ///////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /// @brief reference to the owning \ref RobotUnit
        RobotUnit& robotUnit;
        /// @brief Maps names of used \ref ControlDevice "ControlDevices" to the \ref JointController "JointController's" control mode
        StringStringDictionary controlDeviceControlModeMap;
        /// @brief Maps names of used \ref ControlDevice "ControlDevices" to the used \ref JointController for this \ref ControlDevice
        std::map<std::string, const JointController*> controlDeviceUsedJointController;
        /// @brief Bitmap of used \ref ControlDevice "ControlDevices" (1 == used, 0 == not used)
        std::vector<char> controlDeviceUsedBitmap;
        /// @brief Indices of used \ref ControlDevice "ControlDevices"
        std::vector<std::size_t> controlDeviceUsedIndices;
 
        std::string rtClassName_;
        std::string instanceName_;
 
        //this data is filled by the robot unit to provide convenience functions
        std::atomic_bool isActive{false};
        std::atomic_bool isRequested{false};
        std::atomic_bool deactivatedBecauseOfError{false};
        std::atomic_bool errorState{false};
        bool deletable{false};
        bool internal{false};
 
        std::atomic_bool publishActive{false};
 
        std::atomic_bool statusReportedActive{false};
        std::atomic_bool statusReportedRequested{false};
 
        VirtualRobot::RobotPtr rtRobot;
        std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////////// friends //////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /**
        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::ControllerManagement.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class RobotUnitModule::NJointControllerAttorneyForControllerManagement;
        /**
        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::ControlThread.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class RobotUnitModule::NJointControllerAttorneyForControlThread;
        /**
        * \brief This class allows minimal access to private members of \ref NJointControllerBase in a sane fashion for \ref RobotUnitModule::Publisher.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class RobotUnitModule::NJointControllerAttorneyForPublisher;
        /**
        * \brief This is required for the factory
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        template <class>
        friend class detail::NJointControllerRegistryEntryHelper;
    };
 
    class SynchronousNJointController : public NJointControllerBase
    {
    public: ///TODO make protected and use attorneys
        virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp,
                           const IceUtil::Time& timeSinceLastIteration) = 0;
 
        virtual void
        rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
                           const IceUtil::Time& timeSinceLastIteration)
        {
            rtRun(sensorValuesTimestamp, timeSinceLastIteration);
        }
    };
 
    class AsynchronousNJointController : public NJointControllerBase
    {
    public: ///TODO make protected and use attorneys
        virtual void rtRunIterationBegin(const IceUtil::Time& sensorValuesTimestamp,
                                         const IceUtil::Time& timeSinceLastIteration) = 0;
        virtual void rtRunIterationEnd(const IceUtil::Time& sensorValuesTimestamp,
                                       const IceUtil::Time& timeSinceLastIteration) = 0;
    };
 
    using NJointController = SynchronousNJointController;
    using NJointControllerPtr = SynchronousNJointControllerPtr;
} // namespace armarx