From 0899eece4bb5e3175935f5f7e910d1d7569ae8fb Mon Sep 17 00:00:00 2001
From: Raphael <ufdrv@student.kit.edu>
Date: Wed, 1 Feb 2017 20:11:37 +0100
Subject: [PATCH] implemented robot unit added registration to lvl1 controllers
---
source/RobotAPI/interface/CMakeLists.txt | 2 +-
.../libraries/Controllers/LVL1Controller.ice | 3 +-
.../Controllers}/RobotUnitInterface.ice | 35 +-
.../libraries/Controllers/LVL1Controller.h | 155 ++++----
.../libraries/Controllers/RobotUnit.cpp | 292 ++++++++++++++-
.../libraries/Controllers/RobotUnit.h | 346 +++++-------------
6 files changed, 484 insertions(+), 349 deletions(-)
rename source/RobotAPI/interface/{units => libraries/Controllers}/RobotUnitInterface.ice (60%)
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index bb2e60ceb..448c67665 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -30,7 +30,6 @@ set(SLICE_FILES
units/KinematicUnitInterface.ice
units/PlatformUnitInterface.ice
units/RobotPoseUnitInterface.ice
- units/RobotUnitInterface.ice
units/TCPControlUnit.ice
units/TCPMoverUnitInterface.ice
units/UnitInterface.ice
@@ -41,6 +40,7 @@ set(SLICE_FILES
visualization/DebugDrawerInterface.ice
libraries/Controllers/LVL1Controller.ice
+ libraries/Controllers/RobotUnitInterface.ice
)
#core/RobotIK.ice
diff --git a/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice b/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
index 2804d5d29..a229edc6e 100644
--- a/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
+++ b/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
@@ -27,6 +27,7 @@
module armarx
{
+ dictionary<string, string> JointNameToControlModeDictionary;
class LVL1ControllerConfig
{
};
@@ -35,7 +36,7 @@ module armarx
{
["cpp:const"] idempotent string getClassName();
["cpp:const"] idempotent string getInstanceName();
- ["cpp:const"] idempotent StringStringDictionary getJointControlModeMap();
+ ["cpp:const"] idempotent JointNameToControlModeDictionary getJointControlModeMap();
["cpp:const"] idempotent bool isControllerActive();
};
diff --git a/source/RobotAPI/interface/units/RobotUnitInterface.ice b/source/RobotAPI/interface/libraries/Controllers/RobotUnitInterface.ice
similarity index 60%
rename from source/RobotAPI/interface/units/RobotUnitInterface.ice
rename to source/RobotAPI/interface/libraries/Controllers/RobotUnitInterface.ice
index 1dbee2d22..c30c99739 100644
--- a/source/RobotAPI/interface/units/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/libraries/Controllers/RobotUnitInterface.ice
@@ -23,6 +23,8 @@
#ifndef _ARMARX_ROBOTAPI_UNITS_RobotUnit_SLICE_
#define _ARMARX_ROBOTAPI_UNITS_RobotUnit_SLICE_
+#include <ArmarXCore/interface/core/UserException.ice>
+
#include <RobotAPI/interface/libraries/Controllers/LVL1Controller.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
@@ -32,22 +34,37 @@
module armarx
{
+ dictionary<string, Ice::StringSeq> JointNameToControlModesDictionary;
+
+ dictionary<string, string> JointNameToLVL1Dictionary;
+
interface RobotUnitInterface
{
- //robot
- ["cpp:const"] idempotent NameControlModeMap getJointControlModeMap();
-
//controllers
- ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getAllControllers();
- ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getActiveControllers();
+ //names
+ ["cpp:const"] idempotent Ice::StringSeq getControllersKnown();
+ ["cpp:const"] idempotent Ice::StringSeq getControllerNamesLoaded();
+ ["cpp:const"] idempotent Ice::StringSeq getControllerNamesRequested();
+ ["cpp:const"] idempotent Ice::StringSeq getControllerNamesActivated();
+ //proxy
+ ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersLoaded();
+ ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersRequested();
+ ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersActivated();
+ //assignement
+ ["cpp:const"] idempotent JointNameToLVL1Dictionary getControllerJointAssignment();
+
+ //modes
+ ["cpp:const"] idempotent JointNameToControlModeDictionary getJointControlModesRequested();
+ ["cpp:const"] idempotent JointNameToControlModeDictionary getJointControlModesActivated();
+ ["cpp:const"] idempotent JointNameToControlModesDictionary getJointControlModesSupported();
- bool switchSetup(Ice::StringSeq controllerInstanceNames);
- LVL1ControllerInterface* loadController(string className, string instanceName, Ice::StringSeq joints, LVL1ControllerConfig config);
+ //loading
+ void switchSetup(Ice::StringSeq controllerInstanceNames) throws InvalidArgumentException;
+ LVL1ControllerInterface* loadController(string className, string instanceName, LVL1ControllerConfig config) throws InvalidArgumentException;
["cpp:const"] idempotent bool loadLibFromPath(string path);
- ["cpp:const"] idempotent bool loadLibFromPackage(string name, string package);
+ ["cpp:const"] idempotent bool loadLibFromPackage(string package, string lib);
- ["cpp:const"] idempotent ControlModeBoolMap supportedControlModes();
//units
["cpp:const"] idempotent KinematicUnitInterface* getKinematicUnit();
diff --git a/source/RobotAPI/libraries/Controllers/LVL1Controller.h b/source/RobotAPI/libraries/Controllers/LVL1Controller.h
index 3d3aca6c3..e894ca8e2 100644
--- a/source/RobotAPI/libraries/Controllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/Controllers/LVL1Controller.h
@@ -26,18 +26,18 @@
#include <map>
#include <atomic>
#include <mutex>
+#include <functional>
#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/util/TrippleBuffer.h>
+#include <ArmarXCore/core/util/Registrar.h>
+#include <ArmarXCore/core/util/TripleBuffer.h>
#include <RobotAPI/interface/libraries/Controllers/LVL1Controller.h>
-#include "RobotUnit.h"
#include "Targets/JointTargetBase.h"
namespace armarx
{
-
class LVL1Controller:
virtual public LVL1ControllerInterface,
virtual public Component
@@ -50,7 +50,7 @@ namespace armarx
}
virtual std::string getInstanceName(const Ice::Current& = GlobalIceCurrent) const final
{
- return instanceName;
+ return getName();
}
virtual StringStringDictionary getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const final
{
@@ -64,12 +64,25 @@ namespace armarx
private:
friend class RobotUnit;
//this data is filled by the robot unit to provide convenience functions
- std::string instanceName;
std::atomic_bool isActive {false};
- StringStringDictionary jointControlModeMap;
+ JointNameToControlModeDictionary jointControlModeMap;
};
using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
+ class RobotUnit;
+
+ using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)>>;
+ template<class ControllerType>
+ struct LVL1ControllerRegistration
+ {
+ LVL1ControllerRegistration(const std::string& name)
+ {
+ LVL1ControllerRegistry::registerElement(name, [](IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)
+ {
+ return new ControllerType(robotUnit, config);
+ });
+ }
+ };
/**
* @defgroup Library-Controllers Controllers
@@ -86,15 +99,15 @@ namespace armarx
//in ice
module armarx
{
- struct SomeControllerConfig extends LVL1ControllerConfig
- {
- float paramSetViaConfig;
- };
+ struct SomeControllerConfig extends LVL1ControllerConfig
+ {
+ float paramSetViaConfig;
+ };
- interface SomeControllerInterface extends LVL1ControllerInterface
- {
- void setSomeParamChangedViaIce(float param);
- };
+ interface SomeControllerInterface extends LVL1ControllerInterface
+ {
+ void setSomeParamChangedViaIce(float param);
+ };
};
@@ -102,59 +115,61 @@ namespace armarx
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx
{
- struct SomeControlDataStruct
- {
- float parameterChangedViaIceCalls;
- };
-
- class SomeController : virtual public LVL1Controller<SomeControlDataStruct>,
- public SomeControllerInterface
- {
- JointVelocityTargetPtr targetJointXPtr;
- std::vector<const float*> jointValuePtrs;
- float someParamSetViaConfig;
- ExampleLvl1Controller(RobotUnitPtr robotUnit, LVL1ControllerConfigPtr config):
- LVL1Controller<SomeControlDataStruct>(SomeControlDataStruct(1)) //initial parameter value
+ struct SomeControlDataStruct
{
- //cast the config to your config type
- SomeControlDataStructPtr someConfig = SomeControlDataStructPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION_W_HINT(someConfig,
- "The provided config has the wrong type! The type is " << config->ice_id()
- << " instead of " << SomeControllerConfig::ice_staticId());
- //read the config
- someParamSetViaConfig = someConfig->parameterSetViaIceCalls
- //make sure the used units are supported
- ARMARX_CHECK_EXPRESSION_W_HINT(robotUnit->getKinematicUnitData(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
- //get pointers to sensor values from units
- jointValuePtrs = robotUnit->getKinematicUnitData()->getJointAngles({"jointX", "jointY"});
-
- //get pointers for the results of this controller
- targetJointXPtr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jointX", ControlModes::VelocityMode));
- ARMARX_CHECK_EXPRESSION_W_HINT(targetJointXPtr, "The Joint jointX does not support the mode " + ControlModes::VelocityMode);
- }
+ float parameterChangedViaIceCalls;
+ };
- ExampleLvl1Controller(JointVelocityTargetPtr targetPtr, ControllerConfigPtr config):
- targetJointXPtr{targetPtr}
+ class SomeController : virtual public LVL1Controller<SomeControlDataStruct>,
+ public SomeControllerInterface
{
-
+ JointVelocityTargetPtr targetJointXPtr;
+ std::vector<const float*> jointValuePtrs;
+ float someParamSetViaConfig;
+ SomeController(RobotUnitPtr robotUnit, LVL1ControllerConfigPtr config):
+ LVL1Controller<SomeControlDataStruct>(SomeControlDataStruct(1)) //initial parameter value
+ {
+ //cast the config to your config type
+ SomeControlDataStructPtr someConfig = SomeControlDataStructPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(someConfig,
+ "The provided config has the wrong type! The type is " << config->ice_id()
+ << " instead of " << SomeControllerConfig::ice_staticId());
+ //read the config
+ someParamSetViaConfig = someConfig->parameterSetViaIceCalls
+ //make sure the used units are supported
+ ARMARX_CHECK_EXPRESSION_W_HINT(robotUnit->getKinematicDataUnit(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
+ //get pointers to sensor values from units
+ jointValuePtrs = robotUnit->getKinematicDataUnit()->getJointAngles({"jointX", "jointY"});
+
+ //get pointers for the results of this controller
+ targetJointXPtr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jointX", ControlModes::VelocityMode));
+ ARMARX_CHECK_EXPRESSION_W_HINT(targetJointXPtr, "The Joint jointX does not support the mode " + ControlModes::VelocityMode);
+ }
+
+ SomeController(JointVelocityTargetPtr targetPtr, ControllerConfigPtr config):
+ targetJointXPtr{targetPtr}
+ {
+
+ }
+
+ virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
+ {
+ pid.update(
+ rtGetControlStruct().parameterChangedViaIceCalls,
+ jointValuePtrs.at(0),
+ timeSinceLastIteration.toMicroSeconds());
+ *targetJointXPtr = pid.getValue() + someParamSetViaConfig;
+ }
+
+ virtual void setSomeParamChangedViaIce(float param, const Ice::Current& = GlobalIceCurrent) override
+ {
+ std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
+ getWriterControlStruct().parameterChangedViaIceCalls = param;
+ writeControlStruct();
+ }
}
-
- virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
- {
- pid.update(
- rtGetControlStruct().parameterChangedViaIceCalls,
- jointValuePtrs.at(0),
- timeSinceLastIteration.toMicroSeconds());
- *targetJointXPtr = pid.getValue() + someParamSetViaConfig;
- }
-
- virtual void setSomeParamChangedViaIce(float param, const Ice::Current& = GlobalIceCurrent) override
- {
- std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
- getWriterControlStruct().parameterChangedViaIceCalls = param;
- writeControlStruct();
- }
- }
+ //register the controller
+ LVL1ControllerRegistration<SomeController> registrationSomeController("SomeController");
}
* \endcode
*/
@@ -163,18 +178,17 @@ namespace armarx
{
public:
LVL1ControllerTemplate(const ControlDataStruct& initialCommands = ControlDataStruct()):
- controlData {initialCommands},
- controlDataTrippleBuffer {initialCommands}
+ controlDataTripleBuffer {initialCommands}
{
}
protected:
const ControlDataStruct& rtGetControlStruct() const
{
- return controlDataTrippleBuffer.getReadBuffer();
+ return controlDataTripleBuffer.getReadBuffer();
}
bool rtUpdateControlStruct()
{
- return controlDataTrippleBuffer.getUpToDateData();
+ return controlDataTripleBuffer.getUpToDateData();
}
virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
{
@@ -188,17 +202,16 @@ namespace armarx
//also this allows code to unlock the mutex before calling this function
//(can happen if some lockguard in a scope is used)
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
- controlDataTrippleBuffer.getWriteBuffer() = controlData;
+ controlDataTripleBuffer.commitWrite();
}
ControlDataStruct& getWriterControlStruct()
{
- return controlData;
+ return controlDataTripleBuffer.getWriteBuffer();
}
mutable std::recursive_mutex controlDataMutex;
private:
- TripleBuffer<ControlDataStruct> controlDataTrippleBuffer;
- ControlDataStruct controlData;
+ WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
};
template <typename ControlDataStruct>
using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerTemplate<ControlDataStruct>>;
diff --git a/source/RobotAPI/libraries/Controllers/RobotUnit.cpp b/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
index 20f7b7b2d..2a070fd77 100644
--- a/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
@@ -22,36 +22,292 @@
#include "RobotUnit.h"
+armarx::PropertyDefinitionsPtr armarx::RobotUnit::createPropertyDefinitions()
+{
+ return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
+}
-//using namespace armarx;
+Ice::StringSeq armarx::RobotUnit::getControllersKnown(const Ice::Current&) const
+{
+ return LVL1ControllerRegistry::getKeys();
+}
+Ice::StringSeq armarx::RobotUnit::getControllerNamesLoaded(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ Ice::StringSeq result;
+ result.reserve(lvl1Controllers.size());
+ for (const auto& lvl1 : lvl1Controllers)
+ {
+ result.emplace_back(lvl1.first);
+ }
+ return result;
+}
+Ice::StringSeq armarx::RobotUnit::getControllerNamesRequested(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ Ice::StringSeq result;
+ result.reserve(lvl1Controllers.size());
+ for (const auto& lvl1 : controllersRequested.getWriteBuffer().lvl1Controllers)
+ {
+ result.emplace_back(lvl1->getName());
+ }
+ return result;
+}
+Ice::StringSeq armarx::RobotUnit::getControllerNamesActivated(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ Ice::StringSeq result;
+ result.reserve(lvl1Controllers.size());
+ for (const auto& lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ {
+ result.emplace_back(lvl1->getName());
+ }
+ return result;
+}
-//void RobotUnit::onInitComponent()
-//{
+armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersLoaded(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ StringLVL1ControllerPrxDictionary result;
+ for (const auto& lvl1 : lvl1Controllers)
+ {
+ result[lvl1.first] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1.second->getProxy());
+ }
+ return result;
+}
+armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersRequested(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ StringLVL1ControllerPrxDictionary result;
+ for (const auto& lvl1 : controllersRequested.getWriteBuffer().lvl1Controllers)
+ {
+ result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
+ }
+ return result;
+}
+armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersActivated(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ StringLVL1ControllerPrxDictionary result;
+ for (const auto& lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ {
+ result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
+ }
+ return result;
+}
-//}
+armarx::JointNameToLVL1Dictionary armarx::RobotUnit::getControllerJointAssignment(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ JointNameToLVL1Dictionary result;
+ for (const auto& lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ {
+ for (const auto& jointMode : lvl1->jointControlModeMap)
+ {
+ result[jointMode.first] = lvl1->getName();
+ }
+ }
+ return result;
+}
+armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModesRequested(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ JointNameToControlModeDictionary result;
+ const auto& requestedModes = controllersRequested.getWriteBuffer().lvl0Controllers;
+ ARMARX_CHECK_AND_THROW(jointNames.size() == requestedModes.size(), std::logic_error);
+ for (std::size_t i = 0; i < jointNames.size(); ++i)
+ {
+ result[jointNames.at(i)] = requestedModes.at(i)->getControlMode();
+ }
+ return result;
+}
+armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModesActivated(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ JointNameToControlModeDictionary result;
+ const auto& activatedModes = controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
+ ARMARX_CHECK_AND_THROW(jointNames.size() == activatedModes.size(), std::logic_error);
+ for (std::size_t i = 0; i < jointNames.size(); ++i)
+ {
+ result[jointNames.at(i)] = activatedModes.at(i)->getControlMode();
+ }
+ return result;
+}
+armarx::JointNameToControlModesDictionary armarx::RobotUnit::getJointControlModesSupported(const Ice::Current&) const
+{
+ GuardType guard {dataMutex};
+ JointNameToControlModesDictionary result;
+ for (const auto& joint : lvl0Controllers)
+ {
+ std::vector<std::string> modes;
+ modes.reserve(joint.second.size());
+ for (const auto& mode : joint.second)
+ {
+ modes.emplace_back(mode.first);
+ }
+ result[joint.first] = std::move(modes);
+ }
+ return result;
+}
-//void RobotUnit::onConnectComponent()
-//{
+void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current&)
+{
+ GuardType guard {dataMutex};
+ //check if all of these controllers exist
+ for (const auto& lvl1 : controllerRequestedNames)
+ {
+ if (!hasLVL1Controller(lvl1))
+ {
+ ARMARX_ERROR << "No controler of the name '" << lvl1 << "' is loaded!";
+ throw InvalidArgumentException {"No controler of the name '" + lvl1 + "' is loaded!"};
+ }
+ }
+ //check controllers (is there a collision/which controllers need to be deactivated)
+ const auto currentActiveLVL1Controllers = getControllerNamesActivated();
+ auto lvl1ControllerAssignement = getControllerJointAssignment();
-//}
+ ARMARX_INFO << "current active lvl1 controllers:\n" << currentActiveLVL1Controllers;
+ std::set<std::string> controllersToActivate {currentActiveLVL1Controllers.begin(), currentActiveLVL1Controllers.end()}; //these controllers will be set as active controllers
-//void RobotUnit::onDisconnectComponent()
-//{
+ for (const auto& toActivate : controllerRequestedNames)
+ {
+ if (controllersToActivate.count(toActivate))
+ {
+ ARMARX_INFO << "controller already active: " << toActivate;
+ continue;
+ }
+ controllersToActivate.emplace(toActivate);
+ ARMARX_INFO << "activate '" << toActivate << "'";
+ const auto& lvl1 = lvl1Controllers.at(toActivate);
+ //check and update the assignement map
+ for (const auto& jointControlMode : lvl1->jointControlModeMap)
+ {
+ const auto& joint = jointControlMode.first;
+ const auto& currentAssignedLVL1 = lvl1ControllerAssignement.at(joint);
+ if (toActivate == currentAssignedLVL1)
+ {
+ continue;
+ }
+ if (currentAssignedLVL1.empty())
+ {
+ lvl1ControllerAssignement[joint] = toActivate;
+ continue;
+ }
+ //deactivate other controller
+ for (auto& assignement : lvl1ControllerAssignement)
+ {
+ if (assignement.second == currentAssignedLVL1)
+ {
+ assignement.second.clear();
+ }
+ }
+ controllersToActivate.erase(currentAssignedLVL1);
+ ARMARX_INFO << "deactivated '" << currentAssignedLVL1 << "' (caused by activation of '" << toActivate << "')";
+ lvl1ControllerAssignement[joint] = toActivate;
+ }
+ }
+ //verify (exception for collision of requested lvl1)
+ for (const auto& toActivate : controllerRequestedNames)
+ {
+ if (!controllersToActivate.count(toActivate))
+ {
+ switchSetupError = true;
+ ARMARX_ERROR << "The given set of controllers is in conflict and could not be activated! (conflicting is: " + toActivate + ")";
+ throw InvalidArgumentException {"The given set of controllers is in conflict and could not be activated! (conflicting is: " + toActivate + ")"};
+ }
+ }
+ //verify (warn for orphant joints) + populate controllersRequested.lvl0Controllers
+ controllersRequested.getWriteBuffer().lvl0Controllers.resize(jointNames.size());
+ for (std::size_t i = 0; i < jointNames.size(); ++i)
+ {
+ const auto& joint = jointNames.at(i);
+ if (lvl1ControllerAssignement[joint].empty())
+ {
+ controllersRequested.getWriteBuffer().lvl0Controllers.at(i) = lvl0Controllers.at(joint).at(ControlModes::EmergencyStopMode);
+ ARMARX_WARNING << "Joint '" << joint << "' has no lvl1 controller assigned!";
+ continue;
+ }
+ const auto& lvl0Mode = lvl1Controllers.at(lvl1ControllerAssignement[joint])->jointControlModeMap.at(joint);
+ controllersRequested.getWriteBuffer().lvl0Controllers.at(i) = lvl0Controllers.at(joint).at(lvl0Mode);
+ }
+ //populate controllersRequested.lvl1Controllers
+ controllersRequested.getWriteBuffer().lvl1Controllers.clear();
+ controllersRequested.getWriteBuffer().lvl1Controllers.reserve(controllersToActivate.size());
+ for (const auto& lvl1 : controllersToActivate)
+ {
+ controllersRequested.getWriteBuffer().lvl1Controllers.emplace_back(lvl1Controllers.at(lvl1));
+ }
-//}
+ //now change the assignement
+ controllersRequested.commitWrite();
+ ARMARX_INFO << "requested lvl1 controllers:\n" << getControllerNamesRequested();
+}
+armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::string& className, const std::string& instanceName, const armarx::LVL1ControllerConfigPtr& config, const Ice::Current&)
+{
+ if (instanceName.empty())
+ {
+ ARMARX_ERROR << "The instance name is empty!";
+ throw InvalidArgumentException {"The instance name is empty!"};
+ }
+ //check if we would be able to create the class
+ if (!LVL1ControllerRegistry::has(className))
+ {
+ std::stringstream ss;
+ ss << "Requested controller class '" << className << "' unknown! Known classes:" << LVL1ControllerRegistry::getKeys();
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ auto factory = LVL1ControllerRegistry::get(className);
-//void RobotUnit::onExitComponent()
-//{
+ GuardType guard {dataMutex};
+ //check if the instance name is already in use
+ if (hasLVL1Controller(instanceName))
+ {
+ std::stringstream ss;
+ ss << "There already is a controller instance with the name '" << instanceName << "'";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ //create the controler
+ jointsUsedByLVL1Controler.clear();
+ LVL1ControllerPtr lvl1 = factory(this, config);
+ lvl1->jointControlModeMap = jointsUsedByLVL1Controler;
+ getArmarXManager()->addObject(lvl1, instanceName);
+ const auto prx = lvl1->getProxy(-1);
+ lvl1Controllers[instanceName] = lvl1;
+ return LVL1ControllerInterfacePrx::uncheckedCast(prx);
+}
-//}
+armarx::JointTargetBase* armarx::RobotUnit::getJointTarget(const std::string& jointName, const std::string& controlMode)
+{
+ GuardType guard {dataMutex};
+ if (!hasLVL0Controller(jointName, controlMode))
+ {
+ return nullptr;
+ }
+ const auto& lvl0 = lvl0Controllers.at(jointName).at(controlMode);
+ if (!lvl0)
+ {
+ return nullptr;
+ }
+ ARMARX_CHECK_EXPRESSION(lvl0->getControlMode() == controlMode);
+ jointsUsedByLVL1Controler[jointName] = controlMode;
+ return lvl0->getTarget();
+}
-//armarx::PropertyDefinitionsPtr RobotUnit::createPropertyDefinitions()
-//{
-// return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(
-// getConfigIdentifier()));
-//}
+bool armarx::RobotUnit::hasLVL1Controller(const std::string& name) const
+{
+ GuardType guard {dataMutex};
+ return lvl1Controllers.end() != lvl1Controllers.find(name);
+}
+bool armarx::RobotUnit::hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const
+{
+ GuardType guard {dataMutex};
+ return lvl0Controllers.end() != lvl0Controllers.find(jointName) &&
+ lvl0Controllers.at(jointName).end() != lvl0Controllers.at(jointName).find(controlMode);
+}
diff --git a/source/RobotAPI/libraries/Controllers/RobotUnit.h b/source/RobotAPI/libraries/Controllers/RobotUnit.h
index c4659b752..d8c971d56 100644
--- a/source/RobotAPI/libraries/Controllers/RobotUnit.h
+++ b/source/RobotAPI/libraries/Controllers/RobotUnit.h
@@ -30,11 +30,20 @@
#include <unordered_map>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/algorithm.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <RobotAPI/interface/units/RobotUnitInterface.h>
-#include <RobotAPI/libraries/Controllers/LVL0Controller.h>
+#include <RobotAPI/interface/libraries/Controllers/RobotUnitInterface.h>
+#include "LVL0Controller.h"
+#include "LVL1Controller.h"
+
+#include "DataUnits/ForceTorqueDataUnit.h"
+#include "DataUnits/HapticDataUnit.h"
+#include "DataUnits/IMUDataUnit.h"
+#include "DataUnits/KinematicDataUnit.h"
+#include "DataUnits/PlatformDataUnit.h"
namespace armarx
{
@@ -70,255 +79,94 @@ namespace armarx
virtual public armarx::RobotUnitInterface
{
public:
- // /**
- // * @see armarx::ManagedIceObject::getDefaultName()
- // */
- // virtual std::string getDefaultName() const
- // {
- // return "RobotUnit";
- // }
-
- // // RobotUnitInterface interface
- // //robot
- // virtual NameControlModeMap getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const override;
-
- // //controllers
- // virtual StringLVL1ControllerPrxDictionary getAllControllers(const Ice::Current& = GlobalIceCurrent) const override
- // {
- // std::lock_guard<std::mutex> guard {slowDataMutex};
- // StringLVL1ControllerPrxDictionary result;
- // std::transform(
- // loadedLVL1Controllers.begin(), loadedLVL1Controllers.end(), std::back_inserter(result),
- // [](const LVL1ControllerPtr & lvl1)
- // {
- // return std::make_pair(lvl1->getName(), lvl1->getProxy());
- // }
- // );
- // return result;
- // }
- // virtual StringLVL1ControllerPrxDictionary getActiveControllers(const Ice::Current& = GlobalIceCurrent) const override
- // {
- // std::lock_guard<std::mutex> guard {slowDataMutex};
- // StringLVL1ControllerPrxDictionary result;
- // std::transform(
- // activeLVL1Controllers.begin(), activeLVL1Controllers.end(), std::back_inserter(result),
- // [](const LVL1ControllerPtr & lvl1)
- // {
- // return std::make_pair(lvl1->getName(), lvl1->getProxy());
- // }
- // );
- // return result;
- // }
-
- // virtual bool switchSetup(const Ice::StringSeq&, const Ice::Current& = GlobalIceCurrent) override;
- // virtual LVL1ControllerInterfacePrx loadController(
- // const std::string& className, const std::string& instanceName, const Ice::StringSeq& joints, const LVL1ControllerConfigPtr&,
- // const Ice::Current& = GlobalIceCurrent) override;
-
-
- // virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) const override
- // {
- // throw "NYI"; ///TODO mirko
- // }
- // virtual bool loadLibFromPackage(const std::string& package, const std::string& name, const Ice::Current& = GlobalIceCurrent) const override
- // {
- // throw "NYI"; ///TODO mirko
- // }
- // //units
- // virtual KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const override
- // {
- // return kinematicUnitPrx;
- // }
- // virtual ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const override
- // {
- // return forceTorqueUnitPrx;
- // }
- // virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const override
- // {
- // return inertialMeasurementUnitPrx;
- // }
- // virtual PlatformUnitInterfacePrx getPlatformUnitInterface(const Ice::Current& = GlobalIceCurrent) const override
- // {
- // return platformUnitPrx;
- // }
-
- // //functions subclasses need to implement
- // virtual ControlModeBoolMap supportedControlModes(const Ice::Current& = GlobalIceCurrent) = 0;
- // virtual void initializeLVL0Controllers() = 0;
- // virtual void initializeBus() = 0;
- // virtual void copyControlBufferOut() = 0;
- // /**
- // * @brief send and receive data on the bus
- // */
- // virtual void sendReceiveData() = 0;
- // virtual void shutdownBus() = 0;
- // virtual void emergencyStop() = 0;
- // // virtual void stopRobot() = 0; // may be implemented as {emergencyStop();}
-
- // virtual void controlLoopIteration()
- // {
- // ///how bridge loop while controllers are switched? (need some halt mode)
- // for (auto& lvl1 : activeLVL1Controllers)
- // {
- // lvl1->run();
- // }
- // for (auto& lvl0 : activeLVL0Controllers)
- // {
- // lvl0->run();
- // }
- // copyControlBufferOut();
- // sendReceiveData();
- // }
-
- // protected:
- // /**
- // * @see armarx::ManagedIceObject::onInitComponent()
- // */
- // virtual void onInitComponent() override
- // {
-
- // jointIndices = toIndexMap(jointNames);
- // rtControlThread = std::thread {[this]{rtControlThread();}};
- // }
-
- // /**
- // * @see armarx::ManagedIceObject::onConnectComponent()
- // */
- // virtual void onConnectComponent() override;
-
- // /**
- // * @see armarx::ManagedIceObject::onDisconnectComponent()
- // */
- // virtual void onDisconnectComponent() override;
-
- // /**
- // * @see armarx::ManagedIceObject::onExitComponent()
- // */
- // virtual void onExitComponent() override;
-
- // /**
- // * @see PropertyUser::createPropertyDefinitions()
- // */
- // virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- // //slow data (data changed in non rt threads)
- // mutable std::mutex slowDataMutex;
- // std::vector<LVL1ControllerPtr> loadedLVL1Controllers;
-
-
- // //slow data: running data (this data changes only in switchSetup)
- // NameControlModeMap jointControlModes;
- // std::vector<LVL1ControllerPtr> activeLVL1Controllers;
- // std::vector<LVL0ControllerBase*> activeLVL0Controllers;
-
- // // //slow data: units
- // // KinematicUnitInterfacePtr kinematicUnit;
- // // KinematicUnitInterfacePrx kinematicUnitPrx;
- // // ForceTorqueUnitInterfacePtr forceTorqueUnit;
- // // ForceTorqueUnitInterfacePrx forceTorqueUnitPrx;
- // // InertialMeasurementUnitInterfacePtr inertialMeasurementUnit;
- // // InertialMeasurementUnitInterfacePrx inertialMeasurementUnitPrx;
- // // PlatformUnitInterfacePtr platformUnit;
- // // PlatformUnitInterfacePrx platformUnitPrx;
-
- // //constant data (this data does not change after onInitComponent)
- // std::vector<std::string> jointNames;
- // std::unordered_map<std::string, std::size_t> jointIndices;
- // std::chrono::nanoseconds controlLoopPeriod;
- // std::chrono::nanoseconds controlLoopEmergencyStopPeriod;
- // std::map<std::string, std::vector<LVL0ControllerBase*>> lvl0ControllerMap;
-
- // private:
- // enum class ControlThreadState
- // {
- // Unborn,
- // Uninitialized,
- // ControllerLoop,
- // /**
- // * @brief The LVL1 controllers are switched.
- // * The controller loop now bridges this period.
- // */
- // BridgingControllerSwitch,
- // Exception,
- // Dead
- // };
-
- // //fast data (used in rt threads)
- // std::thread rtControlThread;
- // std::atomic_bool stopControlLoop {false};
- // std::atomic<ControlThreadState> controlThreadState {ControlThreadState::Unborn};
- // std::atomic_bool requestBridging {true};
- // void rtControlThreadFunction()
- // {
- // controlThreadState = ControlThreadState::Uninitialized;
- // initializeBus();
- // ARMARX_ON_SCOPE_EXIT
- // {
- // shutdownBus();
- // controlThreadState = ControlThreadState::Dead;
- // };
- // try
- // {
- // initializeLVL0Controllers();
- // //validate
- // for (const auto& mode : supportedControlModes)
- // {
- // switch (mode.first)
- // {
- //#define ERROR_HEAD "Wrong number of LVL0Controllers for"
- //#define ERROR_TAIL ". (If supported, each joint needs one controller. If unsupported no controllers must be initialized!"
- // case ePositionControl:
- // if (lvl0PositionControlController.size() != mode.second * jointNames.size())
- // {
- // throw std::invalid_argument {ERROR_HEAD "ePositionControl" ERROR_TAIL};
- // }
- // break;
- // case eVelocityControl:
- // if (lvl0VelocityControlController.size() != mode.second * jointNames.size())
- // {
- // throw std::invalid_argument {ERROR_HEAD "eVelocityControl" ERROR_TAIL};
- // }
- // break;
- // case eTorqueControl:
- // if (lvl0TorqueControlController.size() != mode.second * jointNames.size())
- // {
- // throw std::invalid_argument {ERROR_HEAD "eTorqueControl" ERROR_TAIL};
- // }
- // break;
- // case ePositionVelocityControl:
- // if (lvl0PositionVelocityControlController.size() != mode.second * jointNames.size())
- // {
- // throw std::invalid_argument {ERROR_HEAD "ePositionVelocityControl" ERROR_TAIL};
- // }
- // break;
- //#undef ERROR_HEAD
- //#undef ERROR_TAIL
- // default:
- // if (mode.second)
- // {
- // throw std::invalid_argument {"Unknown ControlMode supported! mode = " + std::to_string(mode.first)};
- // }
- // }
- // }
- // controlThreadState = ControlThreadState::ControllerLoop;///TODO bridging
- // //loop
- // while (!stopControlLoop)
- // {
- // auto iterationStartTime = std::chrono::high_resolution_clock::now();
- // controlLoopIteration();
- // std::this_thread::sleep_until(iterationStartTime + controlLoopPeriod);
- // }
+ using MutexType = std::recursive_mutex;
+ using GuardType = std::lock_guard<MutexType>;
+ //controllers
+ virtual Ice::StringSeq getControllersKnown(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual Ice::StringSeq getControllerNamesLoaded(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual Ice::StringSeq getControllerNamesRequested(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual Ice::StringSeq getControllerNamesActivated(const Ice::Current& = GlobalIceCurrent) const override;
+
+ virtual StringLVL1ControllerPrxDictionary getControllersLoaded(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual StringLVL1ControllerPrxDictionary getControllersRequested(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual StringLVL1ControllerPrxDictionary getControllersActivated(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual JointNameToLVL1Dictionary getControllerJointAssignment(const Ice::Current& = GlobalIceCurrent) const override;
+ //modes
+ virtual JointNameToControlModeDictionary getJointControlModesRequested(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual JointNameToControlModeDictionary getJointControlModesActivated(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual JointNameToControlModesDictionary getJointControlModesSupported(const Ice::Current& = GlobalIceCurrent) const override;
+ //loading and switching
+ virtual void switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current& = GlobalIceCurrent) override;
+ virtual LVL1ControllerInterfacePrx loadController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current& = GlobalIceCurrent) override;
+ virtual bool loadLibFromPath(const std::string& path , const Ice::Current& = GlobalIceCurrent) const override
+ {
+ throw "NYI";/*TODO mirko*/
+ }
+ virtual bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) const override
+ {
+ throw "NYI";/*TODO mirko*/
+ }
- // }
- // catch (...)
- // {
- // controlThreadState = ControlThreadState::Exception;
- // emergencyStop();
- // //TODO HANDLING
- // }
- // }
+ //units (ice)
+ virtual KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
+ virtual ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
+ virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
+ virtual PlatformUnitInterfacePrx getPlatformUnitInterface(const Ice::Current& = GlobalIceCurrent) const = 0;
+ //units (rt)
+ virtual const ForceTorqueDataUnitInterface& getForceTorqueDataUnit() const = 0;
+ virtual const HapticDataUnitInterface& getHapticDataUnit() const = 0;
+ virtual const IMUDataUnitInterface& getIMUDataUnit() const = 0;
+ virtual const KinematicDataUnitInterface& getKinematicDataUnit() const = 0;
+ virtual const PlatformDataUnitInterface& getPlatformDataUnit() const = 0;
+ //targets
+ JointTargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode);
+
+ protected:
+ /**
+ * @see PropertyUser::createPropertyDefinitions()
+ */
+ virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ bool hasLVL1Controller(const std::string& name) const;
+ bool hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
+
+ static void setLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool isActive = 1)
+ {
+ lvl1->isActive = isActive;
+ }
+ struct LVL0AndLVL1ControllerList
+ {
+ std::vector<LVL0ControllerBase*> lvl0Controllers;
+ std::vector<LVL1ControllerPtr > lvl1Controllers;
+ };
+ mutable MutexType dataMutex;
+ //data accessible in RT and nonRT
+ const std::vector<std::string> jointNames;
+ std::atomic_bool switchSetupError {false};
+ //used to communicate with rt
+ /// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT)
+ TripleBuffer<LVL0AndLVL1ControllerList> controllersRequested;
+ /// @brief Controllers the RT thread is currently running (direction RT -> nonRT)
+ TripleBuffer<LVL0AndLVL1ControllerList> controllersActivated;
+ //non rt data
+ /**
+ * @brief Holds pointer to all lvl0 controllers. (index: [jointname][controlmode])
+ * Is initialized by derived classes.
+ * May not be accessed when the controll loop is running
+ */
+ std::map<std::string, std::map<std::string, LVL0ControllerBase*>> lvl0Controllers;
+ /**
+ * @brief Holds all currently loaded LVL1 controllers (index: [instancename])
+ * May not be accessed in rt.
+ */
+ std::map<std::string, LVL1ControllerPtr> lvl1Controllers;
+
+ private:
+ JointNameToControlModeDictionary jointsUsedByLVL1Controler;
};
+
+ using RobotUnitPtr = IceInternal::Handle<RobotUnit>;
}
#endif
--
GitLab