Skip to content
Snippets Groups Projects
Commit 95a48216 authored by Raphael's avatar Raphael
Browse files

add RobotUnit::setJoint method (instead of setting the joints in the ctor) 

add some other access utiliti functions
order the members of RobotUnit
add the LVL1ControllerDataProviderInterface (this is used by LVL1Controllers to get their data)
parent e0a6a7de
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 313 additions and 224 deletions
source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
+ 27
− 27
View file @ 95a48216
......@@ -40,18 +40,18 @@ namespace armarx
template <typename T>
struct AtomicWrapper
{
std::atomic<T> val;
AtomicWrapper( ):val{ }{}
AtomicWrapper(const T& v ):val{v }{}
AtomicWrapper(const std::atomic<T> &val ):val{val.load() }{}
AtomicWrapper(const AtomicWrapper &other):val{other.val.load()}{}
AtomicWrapper &operator=(const AtomicWrapper &other)
{
val.store(other.val.load());
return *this;
}
std::atomic<T> val;
AtomicWrapper(): val { } {}
AtomicWrapper(const T& v): val {v } {}
AtomicWrapper(const std::atomic<T>& val): val {val.load() } {}
AtomicWrapper(const AtomicWrapper& other): val {other.val.load()} {}
AtomicWrapper& operator=(const AtomicWrapper& other)
{
val.store(other.val.load());
return *this;
}
};
template <typename TargetType>
......@@ -65,7 +65,7 @@ namespace armarx
public:
using Traits = PassThroughControllerTargetTypeTraits<TargetType>;
inline PassThroughController(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config);
inline PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
inline virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
......@@ -73,8 +73,8 @@ namespace armarx
//ice interface
inline virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override;
inline virtual void setJoint(const std::string& name, Ice::Float value, const Ice::Current & = GlobalIceCurrent) override;
inline virtual void setJoints(const StringFloatDictionary& values, const Ice::Current & = GlobalIceCurrent) override;
inline virtual void setJoint(const std::string& name, Ice::Float value, const Ice::Current& = GlobalIceCurrent) override;
inline virtual void setJoints(const StringFloatDictionary& values, const Ice::Current& = GlobalIceCurrent) override;
protected:
std::vector<float*> lvl0Targets;
std::vector<const float*> jointStates;
......@@ -136,7 +136,7 @@ namespace armarx
LVL1ControllerRegistration<PassThroughController<JointPositionTarget>> registrationSomeControllerPositionPassThroughController("PositionPassThroughController");
LVL1ControllerRegistration<PassThroughController<JointVelocityTarget>> registrationSomeControllerVelocityPassThroughController("VelocityPassThroughController");
LVL1ControllerRegistration<PassThroughController<JointTorqueTarget >> registrationSomeControllerJointPassThroughController ("JointPassThroughController");
LVL1ControllerRegistration<PassThroughController<JointTorqueTarget >> registrationSomeControllerJointPassThroughController("JointPassThroughController");
template <typename TargetType>
std::string PassThroughController<TargetType>::getClassName(const Ice::Current&) const
......@@ -145,7 +145,7 @@ namespace armarx
}
template <typename TargetType>
PassThroughController<TargetType>::PassThroughController(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)
PassThroughController<TargetType>::PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
{
PassThroughControllerConfigPtr cfg = PassThroughControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
......@@ -153,19 +153,19 @@ namespace armarx
<< " instead of " << PassThroughControllerConfig::ice_staticId());
//make sure the used units are supported
auto kinunit = robotUnit->getRTKinematicDataUnit();
ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
auto kinunit = prov->getRTKinematicDataUnit();
ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The DataProvider " << prov->getName() << " has no kinematic data unit");
//get pointers to sensor values from units
jointStates = Traits::getSensorValues(kinunit, cfg->jointNames) ;
//not initialized, this is done in rtPreActivateController
iceTargets.resize(cfg->jointNames.size(),0);
iceTargets.resize(cfg->jointNames.size(), 0);
//get pointers for the results of this controller
lvl0Targets.reserve(cfg->jointNames.size());
for (const auto & j : cfg->jointNames)
for (const auto& j : cfg->jointNames)
{
auto target = dynamic_cast<TargetType*>(robotUnit->getJointTarget(j, Traits::getControlMode()));
auto target = dynamic_cast<TargetType*>(prov->getJointTarget(j, Traits::getControlMode()));
ARMARX_CHECK_EXPRESSION_W_HINT(target, "The joint " << j << " has no controll mode " << Traits::getControlMode());
lvl0Targets.emplace_back(Traits::getTargetDatamember(*target));
}
......@@ -197,20 +197,20 @@ namespace armarx
}
template <typename TargetType>
void PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current &)
void PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current&)
{
auto targetIt = indices.find(name);
if(targetIt == indices.end())
if (targetIt == indices.end())
{
throw InvalidArgumentException{"The joint " + name + " is not controlled by this (" + getName() + ") controller"};
throw InvalidArgumentException {"The joint " + name + " is not controlled by this (" + getName() + ") controller"};
}
iceTargets.at(targetIt->second).val.store(value);
}
template <typename TargetType>
void PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current &)
void PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current&)
{
for(const auto& value:values)
for (const auto& value : values)
{
setJoint(value.first, value.second);
}
......
source/RobotAPI/libraries/RobotRTControllers/Constants.h
+ 3
− 0
View file @ 95a48216
......@@ -30,7 +30,10 @@ namespace armarx
{
namespace ControllerConstants
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
static const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
#pragma GCC diagnostic pop
}
}
......
source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
+ 31
− 9
View file @ 95a48216
......@@ -31,13 +31,36 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/util/Registrar.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.h>
#include "Targets/JointTargetBase.h"
//units
#include "DataUnits/ForceTorqueDataUnit.h"
#include "DataUnits/HapticDataUnit.h"
#include "DataUnits/IMUDataUnit.h"
#include "DataUnits/KinematicDataUnit.h"
#include "DataUnits/PlatformDataUnit.h"
namespace armarx
{
class LVL1ControllerDataProviderInterface: virtual public Ice::Object
{
public:
virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
virtual const HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
virtual const IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
virtual JointTargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) = 0;
virtual std::string getName() const = 0;
};
using LVL1ControllerDataProviderInterfacePtr = IceInternal::Handle<LVL1ControllerDataProviderInterface>;
/**
* @defgroup Library-RobotRTControllers RobotRTControllers
* @ingroup RobotAPI
......@@ -82,7 +105,7 @@ namespace armarx
*
* \section rtnrtcomm Communication between RT and NonRT
* All communication between RT and NonRT has to be lockfree.
* So you have to use constructs like atomics or TripleBuffers (See armarx::LVL1ControllerTemplate).
* So you have to use constructs like atomics or TripleBuffers (See armarx::LVL1ControllerWithTripleBuffer).
*
* \image html LVL1ControllerGeneralDataFlow.svg "General Dataflow in a LVL1Controller"
*
......@@ -172,17 +195,16 @@ namespace armarx
};
using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
class RobotUnit;
using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)>>;
using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(LVL1ControllerDataProviderInterfacePtr robotUnit, LVL1ControllerConfigPtr config)>>;
template<class ControllerType>
struct LVL1ControllerRegistration
{
LVL1ControllerRegistration(const std::string& name)
{
LVL1ControllerRegistry::registerElement(name, [](IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)
LVL1ControllerRegistry::registerElement(name, [](LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
{
return new ControllerType(robotUnit, config);
ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider is NULL!");
return new ControllerType(prov, config);
});
}
};
......@@ -273,10 +295,10 @@ namespace armarx
* \endcode
*/
template <typename ControlDataStruct>
class LVL1ControllerTemplate: virtual public LVL1Controller
class LVL1ControllerWithTripleBuffer: virtual public LVL1Controller
{
public:
LVL1ControllerTemplate(const ControlDataStruct& initialCommands = ControlDataStruct()):
LVL1ControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
controlDataTripleBuffer {initialCommands}
{
}
......@@ -313,6 +335,6 @@ namespace armarx
WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
};
template <typename ControlDataStruct>
using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerTemplate<ControlDataStruct>>;
using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
}
#endif
source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
+ 60
− 105
View file @ 95a48216
......@@ -35,6 +35,7 @@ void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::
{
std::string controlMode = lvl0Controller.getControlMode();
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
if (hasLVL0Controller(jointName, controlMode))
{
ARMARX_ERROR << "A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!";
......@@ -47,26 +48,20 @@ void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::
}
}
const armarx::LVL0ControllerBase& armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode) const
const armarx::LVL0ControllerBase* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode) const
{
GuardType guard {dataMutex};
return *lvl0Controllers.at(jointName).at(controlMode);
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
return lvl0Controllers.at(jointName).at(controlMode);
}
armarx::LVL0ControllerBase& armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode)
armarx::LVL0ControllerBase* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode)
{
GuardType guard {dataMutex};
return *lvl0Controllers.at(jointName).at(controlMode);
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
return lvl0Controllers.at(jointName).at(controlMode);
}
armarx::RobotUnit::RobotUnit(const std::vector<std::string>& jointNames):
jointNames {jointNames},
jointNameIndices {toIndexMap(jointNames)},
controllersRequested {jointNames.size()},
controllersActivated {jointNames.size()},
lvl0ControllersEmergencyStop {jointNames.size()}
{}
Ice::StringSeq armarx::RobotUnit::getControllersKnown(const Ice::Current&) const
{
return LVL1ControllerRegistry::getKeys();
......@@ -201,6 +196,7 @@ armarx::JointNameToControlModesDictionary armarx::RobotUnit::getJointControlMode
void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current&)
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
//check if all of these controllers exist
for (const auto& lvl1 : controllerRequestedNames)
{
......@@ -272,12 +268,12 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
const auto& joint = jointNames.at(i);
if (lvl1ControllerAssignement[joint].empty())
{
getRequestedLVL0Controllers().at(i) = &getLVL0Controller(joint, ControlModes::EmergencyStopMode);
getRequestedLVL0Controllers().at(i) = getLVL0Controller(joint, ControlModes::EmergencyStopMode);
ARMARX_WARNING << "Joint '" << joint << "' has no lvl1 controller assigned!";
continue;
}
const auto& lvl0Mode = lvl1Controllers.at(lvl1ControllerAssignement[joint])->jointControlModeMap.at(joint);
getRequestedLVL0Controllers().at(i) = &getLVL0Controller(joint, lvl0Mode);
getRequestedLVL0Controllers().at(i) = getLVL0Controller(joint, lvl0Mode);
}
//populate controllersRequested.lvl1Controllers
getRequestedLVL1Controllers().clear();
......@@ -320,7 +316,9 @@ armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::
}
//create the controller
jointsUsedByLVL1Controler.clear();
LVL1ControllerPtr lvl1 = factory(this, config);
RobotUnitInterfacePtr::dynamicCast(RobotUnitPtr {this});
LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this});
LVL1ControllerPtr lvl1 = factory(LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config);
lvl1->jointControlModeMap = jointsUsedByLVL1Controler;
lvl1->jointIndices.clear();
lvl1->jointIndices.reserve(jointsUsedByLVL1Controler.size());
......@@ -342,19 +340,31 @@ armarx::JointTargetBase* armarx::RobotUnit::getJointTarget(const std::string& jo
{
return nullptr;
}
const auto& lvl0 = getLVL0Controller(jointName, controlMode);
ARMARX_CHECK_EXPRESSION(lvl0.getControlMode() == controlMode);
const auto lvl0 = getLVL0Controller(jointName, controlMode);
ARMARX_CHECK_EXPRESSION(lvl0->getControlMode() == controlMode);
jointsUsedByLVL1Controler[jointName] = controlMode;
return lvl0.getTarget();
return lvl0->getTarget();
}
void armarx::RobotUnit::setJointNames(const std::vector<std::string>& names)
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(!areJointNamesSet(), "JointNames were already set!");
jointNames = names;
jointNameIndices = toIndexMap(jointNames);
lvl0ControllersEmergencyStop.resize(jointNames.size());
controllersRequested.reinitAllBuffers(LVL0AndLVL1ControllerList(jointNames.size()));
controllersActivated.reinitAllBuffers(LVL0AndLVL1ControllerList(jointNames.size()));
}
bool armarx::RobotUnit::hasLVL1Controller(const std::string& name) const
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
return lvl1Controllers.end() != lvl1Controllers.find(name);
}
void armarx::RobotUnit::setLVL1ControllerActive(const armarx::LVL1ControllerPtr& lvl1, bool isActive)
void armarx::RobotUnit::rtSetLVL1ControllerActive(const armarx::LVL1ControllerPtr& lvl1, bool isActive)
{
if (isActive)
{
......@@ -366,9 +376,16 @@ void armarx::RobotUnit::setLVL1ControllerActive(const armarx::LVL1ControllerPtr&
}
}
bool armarx::RobotUnit::areJointNamesSet() const
{
GuardType guard {dataMutex};
return !jointNames.empty();
}
bool armarx::RobotUnit::hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
return lvl0Controllers.end() != lvl0Controllers.find(jointName) &&
lvl0Controllers.at(jointName).end() != lvl0Controllers.at(jointName).find(controlMode) &&
lvl0Controllers.at(jointName).at(controlMode);
......@@ -452,76 +469,6 @@ bool armarx::RobotUnit::loadLibFromPackage(const std::string& package, const std
return false;
}
bool armarx::RobotUnit::rtControllersWereSwitched() const
{
return controllersRequested.updateReadBuffer();
}
std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getRequestedLVL0Controllers()
{
return controllersRequested.getWriteBuffer().lvl0Controllers;
}
std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getRequestedLVL1Controllers()
{
return controllersRequested.getWriteBuffer().lvl1Controllers;
}
const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getRequestedLVL0Controllers() const
{
return controllersRequested.getWriteBuffer().lvl0Controllers;
}
const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getRequestedLVL1Controllers() const
{
return controllersRequested.getWriteBuffer().lvl1Controllers;
}
const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::getActivatedLVL0Controllers() const
{
return controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
}
const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::getActivatedLVL1Controllers() const
{
return controllersActivated.getUpToDateReadBuffer().lvl1Controllers;
}
const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetRequestedLVL0Controllers() const
{
return controllersRequested.getReadBuffer().lvl0Controllers;
}
const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetRequestedLVL1Controllers() const
{
return controllersRequested.getReadBuffer().lvl1Controllers;
}
std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetActivatedLVL0Controllers()
{
return controllersActivated.getWriteBuffer().lvl0Controllers;
}
std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetActivatedLVL1Controllers()
{
return controllersActivated.getWriteBuffer().lvl1Controllers;
}
const std::vector<armarx::LVL0ControllerBase*>& armarx::RobotUnit::rtGetActivatedLVL0Controllers() const
{
return controllersActivated.getWriteBuffer().lvl0Controllers;
}
const std::vector<armarx::LVL1ControllerPtr>& armarx::RobotUnit::rtGetActivatedLVL1Controllers() const
{
return controllersActivated.getWriteBuffer().lvl1Controllers;
}
void armarx::RobotUnit::rtCommitActivatedLVL1Controllers()
{
controllersActivated.commitWrite();
}
bool armarx::RobotUnit::rtSwitchSetup()
{
if (!rtControllersWereSwitched())
......@@ -532,25 +479,31 @@ bool armarx::RobotUnit::rtSwitchSetup()
for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
{
//deactivate old
if (rtGetActivatedLVL1Controllers().at(i))
if (rtGetActivatedLVL1Controller(i))
{
rtGetActivatedLVL1Controllers().at(i)->rtDeactivateController();
rtGetActivatedLVL1Controller(i)->rtDeactivateController();
}
//activate new
if (rtGetRequestedLVL1Controllers().at(i))
if (rtGetRequestedLVL1Controller(i))
{
rtGetRequestedLVL1Controllers().at(i)->rtActivateController();
rtGetRequestedLVL1Controller(i)->rtActivateController();
}
//update activated
rtGetActivatedLVL1Controllers().at(i) = rtGetRequestedLVL1Controllers().at(i);
rtGetActivatedLVL1Controller(i) = rtGetRequestedLVL1Controller(i);
}
//handle lvl0
for (std::size_t i = 0; i < rtGetRequestedLVL0Controllers().size(); ++i)
{
if (rtGetRequestedLVL0Controllers().at(i) != rtGetActivatedLVL0Controllers().at(i))
if (rtGetRequestedLVL0Controller(i) != rtGetActivatedLVL0Controller(i))
{
rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controllers().at(i), rtGetRequestedLVL0Controllers().at(i));
rtGetActivatedLVL0Controllers().at(i) = rtGetRequestedLVL0Controllers().at(i);
rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetRequestedLVL0Controller(i));
rtGetActivatedLVL0Controller(i) = rtGetRequestedLVL0Controller(i);
}
if (!rtGetRequestedLVL0Controller(i))
{
//no lvl0 controller is set!
rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetEmergencyStopLVL0Controller(i));
rtGetActivatedLVL0Controller(i) = rtGetActivatedLVL0Controller(i);
}
}
return true;
......@@ -572,19 +525,19 @@ void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
{
for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
{
if (!rtGetRequestedLVL1Controllers().at(i))
if (!rtGetRequestedLVL1Controller(i))
{
continue;
}
if (rtGetRequestedLVL1Controllers().at(i)->rtUsesJoint(index))
if (rtGetRequestedLVL1Controller(i)->rtUsesJoint(index))
{
rtGetRequestedLVL1Controllers().at(i)->rtDeactivateController();
for (auto used : rtGetRequestedLVL1Controllers().at(i)->rtGetJointIndices())
rtGetRequestedLVL1Controller(i)->rtDeactivateController();
for (auto used : rtGetRequestedLVL1Controller(i)->rtGetJointIndices())
{
rtSwitchLVL0Controller(used, rtGetActivatedLVL0Controllers().at(i), lvl0ControllersEmergencyStop.at(i));
rtGetActivatedLVL0Controllers().at(i) = lvl0ControllersEmergencyStop.at(i);
rtSwitchLVL0Controller(used, rtGetActivatedLVL0Controller(i), rtGetEmergencyStopLVL0Controller(i));
rtGetActivatedLVL0Controller(i) = rtGetEmergencyStopLVL0Controller(i);
}
rtGetActivatedLVL1Controllers().at(i) = nullptr;
rtGetActivatedLVL1Controller(i) = nullptr;
return;
}
}
......@@ -598,8 +551,10 @@ void armarx::RobotUnit::rtRunLVL0Controllers()
}
}
bool armarx::RobotUnit::rtValidateLVL0ControllerSetup() const
bool armarx::RobotUnit::validateAddedLVL0Controllers() const
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
for (const auto& lvl0 : lvl0ControllersEmergencyStop)
{
if (!lvl0)
......
source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
+ 192
− 83
View file @ 95a48216
This diff is collapsed.
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment