diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index c33e816f5fc871710c8c45c44742da32a55a14c6..bcbf667a05a4bf246cc074c2f253ed2f08f56810 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -111,7 +111,9 @@ set(LIB_HEADERS
JointControllers/JointController.h
NJointControllers/NJointController.h
- NJointControllers/NJointController.ipp
+ NJointControllers/NJointControllerBase.h
+ NJointControllers/NJointControllerRegistry.h
+ NJointControllers/NJointControllerWithTripleBuffer.h
NJointControllers/NJointTrajectoryController.h
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 28bc1e4819946d6734110fa3831e33f4bc1009b0..fd692bfe221cace409b6999790ac094c1f016c64 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -4,6 +4,8 @@
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <VirtualRobot/math/Helpers.h>
@@ -59,7 +61,7 @@ namespace armarx
}
NJointCartesianNaturalPositionController::NJointCartesianNaturalPositionController(
- RobotUnitPtr,
+ RobotUnit*,
const NJointCartesianNaturalPositionControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
index 2a59d6d7dd6481df81fe6299e7ab2d9d598f9e7c..41276169749fe1156623f14bf4a8589c500b2266 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -1,13 +1,13 @@
#pragma once
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
-
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/libraries/core/CartesianNaturalPositionController.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.h>
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+
namespace armarx
{
@@ -24,7 +24,7 @@ namespace armarx
public:
using ConfigPtrT = NJointCartesianNaturalPositionControllerConfigPtr;
NJointCartesianNaturalPositionController(
- RobotUnitPtr robotUnit,
+ RobotUnit* robotUnit,
const NJointCartesianNaturalPositionControllerConfigPtr& config,
const VirtualRobot::RobotPtr&);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
index cb488380e67ad03f0245ea270061870528ceb086..78f0447f752a1cfef771d9ed7ff8ad42ff98c1be 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp
@@ -22,9 +22,13 @@
* GNU General Public License
*/
#include "NJointCartesianTorqueController.h"
-#include <VirtualRobot/RobotNodeSet.h>
#include "../RobotUnit.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
+
#define DEFAULT_TCP_STRING "default TCP"
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
index 2615b64cd005a228a351200c9b1d4e7e43749a41..0f5dcaef0ac6bce390cdd5160040edc9d943d710 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.h
@@ -24,7 +24,7 @@
#pragma once
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
#include <VirtualRobot/Robot.h>
#include "../RobotUnit.h"
#include "../ControlTargets/ControlTarget1DoFActuator.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index eede99223f70e52f7d1a8ab1ed4e9b101901caa1..88ac3ec398a3ad691f21890369f07aa9448e4d14 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
@@ -22,9 +22,12 @@
* GNU General Public License
*/
#include "NJointCartesianVelocityController.h"
-#include <VirtualRobot/RobotNodeSet.h>
#include "../RobotUnit.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
#define DEFAULT_TCP_STRING "default TCP"
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index 7afd5493e7dcf8f0d11cb898e29c22d09293e490..470be6138ac240deb5182981b3b595fea08d1b9f 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
@@ -24,7 +24,7 @@
#pragma once
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
#include <VirtualRobot/Robot.h>
#include "../RobotUnit.h"
#include "../ControlTargets/ControlTarget1DoFActuator.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 24cafe472b7c53b0548c8221f03206b13534f597..321bea4a97384d1d8aa83c34051b8a92bb68e8ba 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -22,9 +22,13 @@
* GNU General Public License
*/
#include "NJointCartesianVelocityControllerWithRamp.h"
-#include <VirtualRobot/RobotNodeSet.h>
+
#include "../RobotUnit.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
#define DEFAULT_TCP_STRING "default TCP"
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
index 7b9994929a379228956f72a0f6de7f8efbdcf474..d85cad7d1faa6ad1a67037d48fe7cbaed41ab8de 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.h
@@ -24,7 +24,7 @@
#pragma once
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
#include <VirtualRobot/Robot.h>
#include "../RobotUnit.h"
#include "../ControlTargets/ControlTarget1DoFActuator.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index bd02cd045bb37da2a51156a60fec1b35b61a2dbf..5e80b3642507feb335287f2437b238f186cd5e13 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -4,6 +4,9 @@
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
+#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include "NJointCartesianWaypointController.h"
@@ -43,7 +46,7 @@ namespace armarx
}
NJointCartesianWaypointController::NJointCartesianWaypointController(
- RobotUnitPtr,
+ RobotUnit*,
const NJointCartesianWaypointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 22440abdc4123b86e5ec28e07bfb396b6102dfe2..affada4d1467a0c313a1d32b708469a5c703ca50 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -1,13 +1,14 @@
#pragma once
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotNodeSet.h>
-
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/libraries/core/CartesianWaypointController.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+
namespace armarx
{
@@ -24,7 +25,7 @@ namespace armarx
public:
using ConfigPtrT = NJointCartesianWaypointControllerConfigPtr;
NJointCartesianWaypointController(
- RobotUnitPtr robotUnit,
+ RobotUnit* robotUnit,
const NJointCartesianWaypointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
index 4a603b481ecee7da16744165688a26ce346f99fb..5d34536c610128cb23086e8c782249fe9a41e706 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp
@@ -20,7 +20,8 @@
* GNU General Public License
*/
-#include "NJointController.h"
+#include "NJointControllerBase.h"
+#include "NJointControllerRegistry.h"
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/application/Application.h>
@@ -29,6 +30,14 @@
#include <atomic>
+namespace armarx
+{
+ RobotUnit* getRobotUnit(RobotUnitModule::ControllerManagement* cmngr)
+ {
+ return cmngr->_modulePtr<RobotUnit>();
+ }
+}
+
namespace armarx::RobotUnitModule
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
index 057e069963abd69c62f7da0a54c86c7628b34654..f00b35bdbcabc38ece77e4e3514ad62a640d80c7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h
@@ -22,943 +22,6 @@
#pragma once
-#include <map>
-#include <atomic>
-#include <mutex>
-#include <functional>
-#include <unordered_set>
-
-#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 <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
-#include <ArmarXCore/core/services/tasks/ThreadPool.h>
-
-#include <ArmarXGui/interface/WidgetDescription.h>
-
-#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
-#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-
-#include "../ControlTargets/ControlTargetBase.h"
-
-//units
-#include "../Devices/ControlDevice.h"
-#include "../Devices/SensorDevice.h"
-
-#include "../util.h"
-#include "../util/JointAndNJointControllers.h"
-#include "../util/ControlThreadOutputBuffer.h"
-
-#include <VirtualRobot/VirtualRobot.h>
-#include <optional>
-
-namespace armarx::RobotUnitModule
-{
- class NJointControllerAttorneyForPublisher;
- class NJointControllerAttorneyForControlThread;
- class NJointControllerAttorneyForControllerManagement;
-}
-
-namespace armarx::detail
-{
- template<class> class NJointControllerRegistryEntryHelper;
-}
-
-namespace armarx
-{
- 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/RobotUnit.h>
- #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.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 "NJointPositionPassThroughController.h"
- #include <RobotAPI/libraries/core/Pose.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);
-
- /**
- * @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;
-
- /**
- * @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;
- /// @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;
- bool isControllerRequested(const Ice::Current& = Ice::emptyCurrent) const final override;
- bool isDeletable(const Ice::Current& = Ice::emptyCurrent) const final override;
- bool hasControllerError(const Ice::Current& = Ice::emptyCurrent) const final override;
-
- std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override = 0;
- std::string getInstanceName(const Ice::Current& = Ice::emptyCurrent) const final override;
-
- 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) final override;
- void deactivateController(const Ice::Current& = Ice::emptyCurrent) final 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;
- 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;
- /**
- * @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;
-
- /**
- * @brief Sets the error state to true. This will deactivate this controller!
- */
- bool rtGetErrorState() const;
-
- /**
- * @brief Returns the number of used \ref ControlDevice "ControlDevices"
- * @return The number of used \ref ControlDevice "ControlDevices"
- */
- std::size_t rtGetNumberOfUsedControlDevices() const;
-
- /**
- * @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;
-
- /**
- * @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;
-
- 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();
- 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;
- const std::vector<char>& getControlDeviceUsedBitmap() const;
- const std::vector<std::size_t>& getControlDeviceUsedIndices() const;
-
- const std::map<std::string, const JointController*>& getControlDevicesUsedJointController();
-
- //check for conflict
- std::optional<std::vector<char>> isNotInConflictWith(const NJointControllerBasePtr& other) const;
- 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);
- // //////////////////////////////////////////////////////////////////////////////////////// //
- // ////////////////////////////////////// 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);
- };
- 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;
-}
-
-#include "NJointController.ipp"
+#include "NJointControllerBase.h"
+#include "NJointControllerRegistry.h"
+#include "NJointControllerWithTripleBuffer.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
deleted file mode 100644
index 4a147cf86218c9c5035ca27474472bca1bf23c54..0000000000000000000000000000000000000000
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp
+++ /dev/null
@@ -1,519 +0,0 @@
-/*
- * 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
- */
-
-#ifndef _ARMARX_LIB_RobotAPI_NJointController_HPP
-#define _ARMARX_LIB_RobotAPI_NJointController_HPP
-
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
-
-#include "../RobotUnit.h"
-#include "NJointController.h"
-
-namespace armarx::RobotUnitModule
-{
- class ControllerManagement;
-}
-namespace armarx
-{
- class NJointControllerRegistryEntry
- {
- private:
- friend class RobotUnitModule::ControllerManagement;
- virtual NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
- const NJointControllerConfigPtr&,
- const VirtualRobot::RobotPtr&,
- bool deletable,
- bool internal,
- const std::string& instanceName) const = 0;
- virtual WidgetDescription::WidgetPtr
- GenerateConfigDescription(const VirtualRobot::RobotPtr&,
- const std::map<std::string, ConstControlDevicePtr>&,
- const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
- virtual NJointControllerConfigPtr
- GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
- virtual bool hasRemoteConfiguration() const = 0;
-
- protected:
- static thread_local bool ConstructorIsRunning_;
-
- public:
- virtual ~NJointControllerRegistryEntry() = default;
- static bool
- ConstructorIsRunning()
- {
- return ConstructorIsRunning_;
- }
- };
- using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
-
- template <class ControllerType>
- struct NJointControllerRegistration;
-
- template <typename ControlDataStruct>
- class NJointControllerWithTripleBuffer : public SynchronousNJointController
- {
- public:
- using MutexType = std::recursive_mutex;
- using LockGuardType = std::lock_guard<std::recursive_mutex>;
-
- NJointControllerWithTripleBuffer(
- const ControlDataStruct& initialCommands = ControlDataStruct()) :
- controlDataTripleBuffer{initialCommands}
- {
- }
-
- void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration) override;
-
- protected:
- const ControlDataStruct& rtGetControlStruct() const;
- bool rtUpdateControlStruct();
-
- void writeControlStruct();
- ControlDataStruct& getWriterControlStruct();
-
- void setControlStruct(const ControlDataStruct& newStruct);
-
- void reinitTripleBuffer(const ControlDataStruct& initial);
-
- mutable MutexType controlDataMutex;
-
- private:
- WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
- };
-
- template <typename ControlDataStruct>
- using NJointControllerWithTripleBufferPtr =
- IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
-} // namespace armarx
-
-//inline functions
-namespace armarx
-{
- template <class T>
- inline T*
- NJointControllerBase::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 ptr ? ptr->asA<T>() : nullptr;
- }
- template <class T>
- inline const T*
- NJointControllerBase::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 ptr ? ptr->asA<T>() : nullptr;
- }
-
- inline void
- SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
- {
- rtRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
-
- inline bool
- NJointControllerBase::rtUsesControlDevice(std::size_t deviceIndex) const
- {
- return controlDeviceUsedBitmap.at(deviceIndex);
- }
-
- inline StringStringDictionary
- NJointControllerBase::getControlDeviceUsedControlModeMap(const Ice::Current&) const
- {
- return controlDeviceControlModeMap;
- }
-
- inline const std::vector<char>&
- NJointControllerBase::getControlDeviceUsedBitmap() const
- {
- return controlDeviceUsedBitmap;
- }
-
- inline const std::vector<std::size_t>&
- NJointControllerBase::rtGetControlDeviceUsedIndices() const
- {
- return controlDeviceUsedIndices;
- }
-
- inline const std::vector<std::size_t>&
- NJointControllerBase::getControlDeviceUsedIndices() const
- {
- return controlDeviceUsedIndices;
- }
-
- inline const std::map<std::string, const JointController*>&
- NJointControllerBase::getControlDevicesUsedJointController()
- {
- return controlDeviceUsedJointController;
- }
-
- inline std::optional<std::vector<char>>
- NJointControllerBase::isNotInConflictWith(const NJointControllerBasePtr& other) const
- {
- return isNotInConflictWith(other->getControlDeviceUsedBitmap());
- }
-
- inline std::string
- NJointControllerBase::getDefaultName() const
- {
- return getClassName();
- }
-
- inline bool
- NJointControllerBase::isControllerActive(const Ice::Current&) const
- {
- return isActive;
- }
-
- inline bool
- NJointControllerBase::isControllerRequested(const Ice::Current&) const
- {
- return isRequested;
- }
-
- inline bool
- NJointControllerBase::isDeletable(const Ice::Current&) const
- {
- return deletable;
- }
-
- inline bool
- NJointControllerBase::hasControllerError(const Ice::Current&) const
- {
- return deactivatedBecauseOfError;
- }
-
- inline std::string
- NJointControllerBase::getInstanceName(const Ice::Current&) const
- {
- return instanceName_;
- }
-
- inline WidgetDescription::StringWidgetDictionary
- NJointControllerBase::getFunctionDescriptions(const Ice::Current&) const
- {
- return {};
- }
-
- inline void
- NJointControllerBase::callDescribedFunction(const std::string&,
- const StringVariantBaseMap&,
- const Ice::Current&)
- {
- }
-
- inline void
- NJointControllerBase::rtSetErrorState()
- {
- errorState.store(true);
- }
-
- inline bool
- NJointControllerBase::rtGetErrorState() const
- {
- return errorState;
- }
-
- inline std::size_t
- NJointControllerBase::rtGetNumberOfUsedControlDevices() const
- {
- return controlDeviceUsedIndices.size();
- }
-
- inline const std::string&
- NJointControllerBase::rtGetClassName() const
- {
- return rtClassName_;
- }
- inline const std::string&
- NJointControllerBase::rtGetInstanceName() const
- {
- return instanceName_;
- }
-
- //NJointControllerWithTripleBuffer<ControlDataStruct>
- template <typename ControlDataStruct>
- inline void
- NJointControllerWithTripleBuffer<ControlDataStruct>::rtSwapBufferAndRun(
- const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time& timeSinceLastIteration)
- {
- rtUpdateControlStruct();
- rtRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
-
- template <typename ControlDataStruct>
- inline const ControlDataStruct&
- NJointControllerWithTripleBuffer<ControlDataStruct>::rtGetControlStruct() const
- {
- return controlDataTripleBuffer.getReadBuffer();
- }
-
- template <typename ControlDataStruct>
- inline bool
- NJointControllerWithTripleBuffer<ControlDataStruct>::rtUpdateControlStruct()
- {
- return controlDataTripleBuffer.updateReadBuffer();
- }
-
- template <typename ControlDataStruct>
- inline void
- NJointControllerWithTripleBuffer<ControlDataStruct>::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)
- LockGuardType lock(controlDataMutex);
- controlDataTripleBuffer.commitWrite();
- }
-
- template <typename ControlDataStruct>
- inline ControlDataStruct&
- NJointControllerWithTripleBuffer<ControlDataStruct>::getWriterControlStruct()
- {
- return controlDataTripleBuffer.getWriteBuffer();
- }
-
- template <typename ControlDataStruct>
- inline void
- NJointControllerWithTripleBuffer<ControlDataStruct>::setControlStruct(
- const ControlDataStruct& newStruct)
- {
- LockGuardType lock{controlDataMutex};
- getWriterControlStruct() = newStruct;
- writeControlStruct();
- }
-
- template <typename ControlDataStruct>
- inline void
- NJointControllerWithTripleBuffer<ControlDataStruct>::reinitTripleBuffer(
- const ControlDataStruct& initial)
- {
- controlDataTripleBuffer.reinitAllBuffers(initial);
- }
-} // namespace armarx
-
-namespace armarx
-{
- template <class ItT>
- inline std::optional<std::vector<char>>
- NJointControllerBase::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;
- }
-} // namespace armarx
-namespace armarx::detail
-{
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
- hasGenerateConfigDescription,
- GenerateConfigDescription,
- NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
- ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
- hasGenerateConfigFromVariants,
- GenerateConfigFromVariants,
- NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
-
- template <class NJointControllerT>
- class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
- {
- static_assert(hasGenerateConfigDescription<NJointControllerT>::value ==
- hasGenerateConfigFromVariants<NJointControllerT>::value,
- "Either overload both GenerateConfigDescription and "
- "GenerateConfigFromVariants, or none!");
- static constexpr bool hasRemoteConfiguration_ =
- hasGenerateConfigDescription<NJointControllerT>::value;
-
- NJointControllerBasePtr
- create(RobotUnitModule::ControllerManagement* cmngr,
- const NJointControllerConfigPtr& config,
- const VirtualRobot::RobotPtr& rob,
- bool deletable,
- bool internal,
- const std::string& instanceName) const final override
- {
- ARMARX_CHECK_EXPRESSION(cmngr) << "ControllerManagement module is NULL!";
-
- using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
- ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(cfg)
- << "The configuration is of the wrong type! it has to be an instance of: "
- << GetTypeString<ConfigPtrT>();
-
- ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning())
- << "Two NJointControllers are created at the same time";
- NJointControllerBasePtr ptr;
- {
- ConstructorIsRunning_ = true;
- ARMARX_ON_SCOPE_EXIT
- {
- ConstructorIsRunning_ = false;
- };
- ptr = new NJointControllerT(cmngr->_modulePtr<RobotUnit>(), cfg, rob);
- }
- ptr->deletable = deletable;
- ptr->internal = internal;
- ptr->rtClassName_ = ptr->getClassName(Ice::emptyCurrent);
- ptr->instanceName_ = instanceName;
- return ptr;
- }
-
- WidgetDescription::WidgetPtr
- GenerateConfigDescription(
- const VirtualRobot::RobotPtr& robot,
- const std::map<std::string, ConstControlDevicePtr>& controlDevices,
- const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
- {
- if constexpr (hasRemoteConfiguration_)
- {
- try
- {
- return NJointControllerT::GenerateConfigDescription(
- robot, controlDevices, sensorDevices);
- }
- catch (Ice::UserException& e)
- {
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
- << "::GenerateConfigDescription'"
- << "\n---- file = " << e.ice_file()
- << "\n---- line = " << e.ice_line()
- << "\n---- id = " << e.ice_id() << "\n---- what:\n"
- << e.what() << "\n---- stacktrace:\n"
- << e.ice_stackTrace();
- throw;
- }
- catch (std::exception& e)
- {
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
- << "::GenerateConfigDescription'"
- << "\n---- what:\n"
- << e.what();
- throw;
- }
- catch (...)
- {
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
- << "::GenerateConfigDescription'";
- throw;
- }
- }
- else
- {
- ARMARX_CHECK_EXPRESSION(!"This function should never be called");
- }
- }
-
- NJointControllerConfigPtr
- GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
- {
- if constexpr (hasRemoteConfiguration_)
- {
- try
- {
- return NJointControllerT::GenerateConfigFromVariants(variants);
- }
- catch (Ice::UserException& e)
- {
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
- << "::GenerateConfigFromVariants'"
- << "\n---- file = " << e.ice_file()
- << "\n---- line = " << e.ice_line()
- << "\n---- id = " << e.ice_id() << "\n---- what:\n"
- << e.what() << "\n---- stacktrace:\n"
- << e.ice_stackTrace();
- throw;
- }
- catch (std::exception& e)
- {
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
- << "::GenerateConfigFromVariants'"
- << "\n---- what:\n"
- << e.what();
- throw;
- }
- catch (...)
- {
- ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
- << "::GenerateConfigFromVariants'";
- throw;
- }
- }
- else
- {
- ARMARX_CHECK_EXPRESSION(!"This function should never be called");
- }
- }
-
- bool
- hasRemoteConfiguration() const final override
- {
- return hasRemoteConfiguration_;
- }
- };
-} // namespace armarx::detail
-namespace armarx
-{
- using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
- template <class ControllerType>
- struct NJointControllerRegistration
- {
- NJointControllerRegistration(const std::string& name)
- {
- NJointControllerRegistry::registerElement(
- name,
- std::unique_ptr<NJointControllerRegistryEntry>(
- new detail::NJointControllerRegistryEntryHelper<ControllerType>));
- }
- };
-} // namespace armarx
-#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T) \
- static_assert( \
- ::armarx::detail::hasGenerateConfigDescription<T>::value, \
- #T " does not offer a construction gui (missing: static GenerateConfigDescription)"); \
- static_assert( \
- ::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
- #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
-
-
-#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..cad19193568ecff7c8e97766b89036d0b88b8e26
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
@@ -0,0 +1,1061 @@
+/*
+ * 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 <RobotAPI/interface/units/RobotUnit/NJointController.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h> // for ARMARX_CHECK
+#include <ArmarXCore/interface/core/ManagedIceObjectDefinitions.h>
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <functional>
+#include <optional>
+#include <atomic>
+#include <mutex>
+#include <map>
+
+namespace IceProxy::armarx
+{
+ class DebugDrawerInterface;
+ class RobotUnitInterface;
+}
+
+namespace armarx
+{
+ namespace RobotUnitModule
+ {
+ class NJointControllerAttorneyForPublisher;
+ class NJointControllerAttorneyForControlThread;
+ class NJointControllerAttorneyForControllerManagement;
+ }
+ namespace detail
+ {
+ template<class> class NJointControllerRegistryEntryHelper;
+ class ControlThreadOutputBufferEntry;
+ }
+ namespace WidgetDescription
+ {
+ class Widget;
+ typedef ::IceInternal::Handle< ::armarx::WidgetDescription::Widget> WidgetPtr;
+ typedef ::std::map< ::std::string, ::armarx::WidgetDescription::WidgetPtr> StringWidgetDictionary;
+ }
+ 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::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/RobotUnit.h>
+ #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.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 "NJointPositionPassThroughController.h"
+ #include <RobotAPI/libraries/core/Pose.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) final override;
+ void deactivateController(const Ice::Current& = Ice::emptyCurrent) final 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;
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
new file mode 100644
index 0000000000000000000000000000000000000000..9a5454ee5b50e911395703c407a8139878c86f86
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h
@@ -0,0 +1,245 @@
+/*
+ * 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 "NJointControllerBase.h"
+
+#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
+#include <ArmarXCore/core/util/Registrar.h>
+
+namespace armarx::RobotUnitModule
+{
+ class ControllerManagement;
+}
+
+namespace armarx
+{
+ RobotUnit* getRobotUnit(RobotUnitModule::ControllerManagement* cmgr);
+
+ class NJointControllerRegistryEntry
+ {
+ private:
+ friend class RobotUnitModule::ControllerManagement;
+ virtual NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr&,
+ const VirtualRobot::RobotPtr&,
+ bool deletable,
+ bool internal,
+ const std::string& instanceName) const = 0;
+ virtual WidgetDescription::WidgetPtr
+ GenerateConfigDescription(const VirtualRobot::RobotPtr&,
+ const std::map<std::string, ConstControlDevicePtr>&,
+ const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
+ virtual NJointControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
+ virtual bool hasRemoteConfiguration() const = 0;
+
+ protected:
+ static thread_local bool ConstructorIsRunning_;
+
+ public:
+ virtual ~NJointControllerRegistryEntry() = default;
+ static bool
+ ConstructorIsRunning()
+ {
+ return ConstructorIsRunning_;
+ }
+ };
+ using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
+
+ template <class ControllerType>
+ struct NJointControllerRegistration;
+
+ namespace detail
+ {
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+ hasGenerateConfigDescription,
+ GenerateConfigDescription,
+ NJointControllerBase::GenerateConfigDescriptionFunctionSignature);
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(
+ hasGenerateConfigFromVariants,
+ GenerateConfigFromVariants,
+ NJointControllerBase::GenerateConfigFromVariantsFunctionSignature<typename T::ConfigPtrT>);
+
+ template <class NJointControllerT>
+ class NJointControllerRegistryEntryHelper : public NJointControllerRegistryEntry
+ {
+ static_assert(hasGenerateConfigDescription<NJointControllerT>::value ==
+ hasGenerateConfigFromVariants<NJointControllerT>::value,
+ "Either overload both GenerateConfigDescription and "
+ "GenerateConfigFromVariants, or none!");
+ static constexpr bool hasRemoteConfiguration_ =
+ hasGenerateConfigDescription<NJointControllerT>::value;
+
+ NJointControllerBasePtr
+ create(RobotUnitModule::ControllerManagement* cmngr,
+ const NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& rob,
+ bool deletable,
+ bool internal,
+ const std::string& instanceName) const final override
+ {
+ ARMARX_CHECK_EXPRESSION(cmngr) << "ControllerManagement module is NULL!";
+
+ using ConfigPtrT = typename NJointControllerT::ConfigPtrT;
+ ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(cfg)
+ << "The configuration is of the wrong type! it has to be an instance of: "
+ << GetTypeString<ConfigPtrT>();
+
+ ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning())
+ << "Two NJointControllers are created at the same time";
+ NJointControllerBasePtr ptr;
+ {
+ ConstructorIsRunning_ = true;
+ ARMARX_ON_SCOPE_EXIT
+ {
+ ConstructorIsRunning_ = false;
+ };
+ ptr = new NJointControllerT(getRobotUnit(cmngr), cfg, rob);
+ }
+ ptr->deletable = deletable;
+ ptr->internal = internal;
+ ptr->rtClassName_ = ptr->getClassName(Ice::emptyCurrent);
+ ptr->instanceName_ = instanceName;
+ return ptr;
+ }
+
+ WidgetDescription::WidgetPtr
+ GenerateConfigDescription(
+ const VirtualRobot::RobotPtr& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices) const final override
+ {
+ if constexpr (hasRemoteConfiguration_)
+ {
+ try
+ {
+ return NJointControllerT::GenerateConfigDescription(
+ robot, controlDevices, sensorDevices);
+ }
+ catch (Ice::UserException& e)
+ {
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'"
+ << "\n---- file = " << e.ice_file()
+ << "\n---- line = " << e.ice_line()
+ << "\n---- id = " << e.ice_id() << "\n---- what:\n"
+ << e.what() << "\n---- stacktrace:\n"
+ << e.ice_stackTrace();
+ throw;
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'"
+ << "\n---- what:\n"
+ << e.what();
+ throw;
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigDescription'";
+ throw;
+ }
+ }
+ else
+ {
+ ARMARX_CHECK_EXPRESSION(!"This function should never be called");
+ }
+ }
+
+ NJointControllerConfigPtr
+ GenerateConfigFromVariants(const StringVariantBaseMap& variants) const final override
+ {
+ if constexpr (hasRemoteConfiguration_)
+ {
+ try
+ {
+ return NJointControllerT::GenerateConfigFromVariants(variants);
+ }
+ catch (Ice::UserException& e)
+ {
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'"
+ << "\n---- file = " << e.ice_file()
+ << "\n---- line = " << e.ice_line()
+ << "\n---- id = " << e.ice_id() << "\n---- what:\n"
+ << e.what() << "\n---- stacktrace:\n"
+ << e.ice_stackTrace();
+ throw;
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'"
+ << "\n---- what:\n"
+ << e.what();
+ throw;
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Exception calling '" << GetTypeString<NJointControllerT>()
+ << "::GenerateConfigFromVariants'";
+ throw;
+ }
+ }
+ else
+ {
+ ARMARX_CHECK_EXPRESSION(!"This function should never be called");
+ }
+ }
+
+ bool
+ hasRemoteConfiguration() const final override
+ {
+ return hasRemoteConfiguration_;
+ }
+ };
+ } // namespace detail
+
+ using NJointControllerRegistry = Registrar<std::unique_ptr<NJointControllerRegistryEntry>>;
+ template <class ControllerType>
+ struct NJointControllerRegistration
+ {
+ NJointControllerRegistration(const std::string& name)
+ {
+ NJointControllerRegistry::registerElement(
+ name,
+ std::unique_ptr<NJointControllerRegistryEntry>(
+ new detail::NJointControllerRegistryEntryHelper<ControllerType>));
+ }
+ };
+} // namespace armarx
+
+#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T) \
+ static_assert( \
+ ::armarx::detail::hasGenerateConfigDescription<T>::value, \
+ #T " does not offer a construction gui (missing: static GenerateConfigDescription)"); \
+ static_assert( \
+ ::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
+ #T " does not offer a construction gui (missing: static GenerateConfigFromVariants)")
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
new file mode 100644
index 0000000000000000000000000000000000000000..cd6befc54b45193149ce7106be84fb644599a8cf
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#include "NJointControllerBase.h"
+
+#include <ArmarXCore/util/CPPUtility/TripleBuffer.h>
+
+
+namespace armarx
+{
+ template <typename ControlDataStruct>
+ class NJointControllerWithTripleBuffer : public SynchronousNJointController
+ {
+ public:
+ using MutexType = std::recursive_mutex;
+ using LockGuardType = std::lock_guard<std::recursive_mutex>;
+
+ NJointControllerWithTripleBuffer(
+ const ControlDataStruct& initialCommands = ControlDataStruct()) :
+ controlDataTripleBuffer{initialCommands}
+ {
+ }
+
+ 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)
+ LockGuardType lock(controlDataMutex);
+ controlDataTripleBuffer.commitWrite();
+ }
+ ControlDataStruct& getWriterControlStruct()
+ {
+ return controlDataTripleBuffer.getWriteBuffer();
+ }
+
+ void setControlStruct(const ControlDataStruct& newStruct)
+ {
+ LockGuardType lock{controlDataMutex};
+ getWriterControlStruct() = newStruct;
+ writeControlStruct();
+ }
+
+ void reinitTripleBuffer(const ControlDataStruct& initial)
+ {
+ controlDataTripleBuffer.reinitAllBuffers(initial);
+ }
+
+ mutable MutexType controlDataMutex;
+
+ private:
+ WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
+ };
+
+ template <typename ControlDataStruct>
+ using NJointControllerWithTripleBufferPtr =
+ IceInternal::Handle<NJointControllerWithTripleBuffer<ControlDataStruct>>;
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 06513b41621db1af829262b95c79c6c4634276ec..ee21ebea9ae3ba743bbea9e25f5725c4c7959042 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -23,9 +23,10 @@
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
-#include <cmath>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
+#include <cmath>
namespace armarx
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index bf4d338d0e642a870c91fe86772d64371e65f1ce..d8f6cc66e62d2287b3de297f80bb2b5b2067a04d 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -30,7 +30,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
#include "../RobotUnit.h"
#include "../SensorValues/SensorValueHolonomicPlatform.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index f5ca656188153d623bd57dee0640c6f842b03e84..7324764d1666cb0210b26deb3a2d4c6141df3c70 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -23,6 +23,8 @@
*/
#include "NJointHolonomicPlatformRelativePositionController.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformRelativePositionController>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index abbd25d999ae85f7ba500e49f13479d0a4688700..54f2a8e7f560efe74dfe53bfdb98625be9217e91 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -30,7 +30,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
#include "../RobotUnit.h"
#include "../SensorValues/SensorValueHolonomicPlatform.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index 35fd7551d0f4dfd9b33de0b2cac71ee427ef5f35..fa4b4ad337ed17d35b464a6485ecf45c81f63960 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -22,12 +22,14 @@
#include "NJointHolonomicPlatformUnitVelocityPassThroughController.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx
{
NJointHolonomicPlatformUnitVelocityPassThroughController::NJointHolonomicPlatformUnitVelocityPassThroughController(
- RobotUnitPtr prov,
+ RobotUnit* prov,
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr
cfg, const VirtualRobot::RobotPtr&) :
maxCommandDelay(IceUtil::Time::milliSeconds(500))
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index 3c793d040a30b463b4419a3f168d389fa03b2f83..e2c4a3aa4146a573c7fe4e92239272fd02d0372c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -24,7 +24,7 @@
#include <VirtualRobot/Robot.h>
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
#include "../util.h"
@@ -63,7 +63,7 @@ namespace armarx
using ConfigPtrT = NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr;
NJointHolonomicPlatformUnitVelocityPassThroughController(
- RobotUnitPtr prov,
+ RobotUnit* prov,
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config,
const VirtualRobot::RobotPtr&);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
index fab7cb647930e4ea2fa71c6ad14c52927e43139c..f85806eda4386d35940f9f850055241c7dd4bab3 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.cpp
@@ -21,6 +21,9 @@
*/
#include "NJointKinematicUnitPassThroughController.h"
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
namespace armarx
{
NJointControllerRegistration<NJointKinematicUnitPassThroughController> registrationControllerNJointKinematicUnitPassThroughController("NJointKinematicUnitPassThroughController");
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
index a0fe164c4803ceb322c451b464c0c6acdc48bebf..43f520fae9d83e81ff82172ba7b66af956bbb446 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKinematicUnitPassThroughController.h
@@ -28,7 +28,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include "NJointController.h"
+#include "NJointControllerBase.h"
#include "../RobotUnit.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index c67b41d1985b7bcfc2f9404a09878424302da9a3..2f8fdd4341220145814a5af671061e59175e83e1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -22,9 +22,13 @@
* GNU General Public License
*/
#include "NJointTCPController.h"
-#include <VirtualRobot/RobotNodeSet.h>
#include "../RobotUnit.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <VirtualRobot/RobotNodeSet.h>
+
+
#define DEFAULT_TCP_STRING "default TCP"
namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
index 57d2f88812ca75e364ae18f3360bda90063e93c4..43f4246c5a0280e8aa4f5f29d7be1572bab080c2 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.h
@@ -24,7 +24,7 @@
#pragma once
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
#include <VirtualRobot/Robot.h>
#include "../RobotUnit.h"
#include "../ControlTargets/ControlTarget1DoFActuator.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
index d2d10aee0d547b4341c343d368e9c518de86b501..124d66adbc8b24f176cdb7d6acf5df4d3815eccc 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp
@@ -22,6 +22,8 @@
#include "NJointTaskSpaceImpedanceController.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
#include <SimoxUtility/math/convert/mat4f_to_pos.h>
#include <SimoxUtility/math/convert/mat4f_to_quat.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
index f23c8a04945858b9efdb071f1c5215b719073be0..06d725f856583029d4c99a781b4832a3ee4f3c80 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.h
@@ -22,7 +22,7 @@
#pragma once
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerWithTripleBuffer.h>
#include <VirtualRobot/Robot.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index fa0c743d77a225270c44e767c7e1b0843c7601e3..297841f70b8f67c56b7bb86cd54486301d30b23c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -1,13 +1,18 @@
#include "NJointTrajectoryController.h"
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
#include <ArmarXCore/core/time/TimeUtil.h>
+
#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/components/units/RobotUnit/util/RtLogging.h>
+
namespace armarx
{
NJointControllerRegistration<NJointTrajectoryController> registrationControllerNJointTrajectoryController("NJointTrajectoryController");
- NJointTrajectoryController::NJointTrajectoryController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
+ NJointTrajectoryController::NJointTrajectoryController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot)
{
cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointTrajectoryControllerConfigPtr";
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
index 560f33100464eb9e2994069059f0262925579e88..060c918b8238d94c89315f0176ee84d6ceef8c22 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.h
@@ -1,6 +1,6 @@
#pragma once
-#include "NJointController.h"
+#include "NJointControllerWithTripleBuffer.h"
#include <VirtualRobot/Robot.h>
#include <RobotAPI/interface/units/RobotUnit/NJointTrajectoryController.h>
#include <RobotAPI/libraries/core/TrajectoryController.h>
@@ -28,7 +28,7 @@ namespace armarx
public NJointTrajectoryControllerInterface
{
public:
- NJointTrajectoryController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+ NJointTrajectoryController(RobotUnit* prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
// NJointControllerInterface interface
std::string getClassName(const Ice::Current&) const override;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
index dc96e422a047fcdfcc5ebe97d14885191e28b857..6024c0dbbf0a48509afc455922a1dab9ec8103d0 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -23,7 +23,7 @@
#pragma once
#include "RobotUnitModules/RobotUnitModules.h"
-#include "NJointControllers/NJointController.h"
+//#include "NJointControllers/NJointControllerBase.h"
/**
* @defgroup Library-RobotUnit RobotUnit
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
index 557f293e3ccb818d63e4fa1918664306c60ac61f..90910c9f28b246abac25b5b15f0c609bdf33fe3e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp
@@ -26,7 +26,6 @@
#include <ArmarXCore/core/util/Preprocessor.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
-#include "../NJointControllers/NJointController.h"
#include "RobotUnitModuleBase.h"
#include "RobotUnitModules.h"
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 6b251b9cbaa0db977e102580deeb17b4b0d2165e..a3269b4d0e11cb792f639e4c477218dcb58183e8 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -29,9 +29,15 @@
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleManagement.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include "../Devices/RTThreadTimingsSensorDevice.h"
-#include "../NJointControllers/NJointController.h"
namespace armarx::RobotUnitModule
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
index f03a527d7a2b87c5c5752e2f16cdaa0baaee13c4..871d8427eb59e673ec1070c7aafda9fdcad2c2b9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp
@@ -20,11 +20,20 @@
* GNU General Public License
*/
-#include <ArmarXCore/core/util/OnScopeExit.h>
-#include <ArmarXCore/util/CPPUtility/trace.h>
#include "RobotUnitModuleControllerManagement.h"
-#include "../NJointControllers/NJointController.h"
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.h>
+
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
namespace armarx::RobotUnitModule
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index 4c6de078b00ae810a846c68e829a3da136a6ed79..4e38d6ab344958c06fa6775f2ac24843ceb55a6a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -22,8 +22,14 @@
#include "RobotUnitModulePublisher.h"
-#include "../NJointControllers/NJointController.h"
-#include "../Units/RobotUnitSubUnit.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
index a9d2e8de69625147071a2e2aaa650d43de40cdb9..09b19d38a9ede50f8e7f167aad5aee2681bf30e3 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp
@@ -26,8 +26,12 @@
#include "RobotUnitModuleSelfCollisionChecker.h"
#include "RobotUnitModuleRobotData.h"
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h>
-#include "../NJointControllers/NJointController.h"
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
#include <SimoxUtility/algorithm/string/string_tools.h>