diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 7f528cde5ba4f4bce245f329179a5be86bbdef93..8c0d9c011f3b331ff0f9225807690cba6d7bc850 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -72,6 +72,19 @@ namespace armarx
{
return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
}
+ static const std::vector<JointController*>& GetActivatedJointControllers(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
+ }
+ static const std::vector<NJointController*>& GetActivatedNJointControllers(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
+ }
+ static const std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
+ }
+
static void AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
{
p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement =
@@ -88,12 +101,17 @@ namespace armarx
static const std::vector<JointController*>& GetRequestedJointControllers(const ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getWriteBuffer().jointControllers;
+ return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointControllers;
}
static const std::vector<NJointController*>& GetRequestedNJointControllers(const ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getWriteBuffer().nJointControllers;
+ return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().nJointControllers;
}
+ static const std::vector<std::size_t>& GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
+ {
+ return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
+ }
+
static bool RequestedControllersChanged(const ControlThread* p)
{
return p->_module<ControlThreadDataBuffer>().controllersRequested.updateReadBuffer();
@@ -144,6 +162,34 @@ namespace armarx
{
rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupStart();
ARMARX_ON_SCOPE_EXIT { rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd(); };
+
+ {
+ const auto& actJC = rtGetActivatedJointControllers();
+ const auto& assig = rtGetActivatedJointToNJointControllerAssignement();
+ const auto& actNJC = rtGetActivatedNJointControllers();
+ for (std::size_t i = 0; i < actJC.size(); ++i)
+ {
+ preSwitchSetup_ActivatedJointControllers.at(i) = actJC.at(i);
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement.at(i) = assig.at(i);
+ preSwitchSetup_ActivatedNJointControllers.at(i) = actNJC.at(i);
+ }
+ }
+
+ ARMARX_ON_SCOPE_EXIT
+ {
+ const auto& actJC = rtGetActivatedJointControllers();
+ const auto& assig = rtGetActivatedJointToNJointControllerAssignement();
+ const auto& actNJC = rtGetActivatedNJointControllers();
+ for (std::size_t i = 0; i < actJC.size(); ++i)
+ {
+ postSwitchSetup_ActivatedJointControllers.at(i) = actJC.at(i);
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement.at(i) = assig.at(i);
+ postSwitchSetup_ActivatedNJointControllers.at(i) = actNJC.at(i);
+ }
+ };
+
+
+ rtSwitchControllerSetupChangedControllers = false;
// !emergencyStop && !rtIsInEmergencyStop() -> normal control flow
// !emergencyStop && rtIsInEmergencyStop() -> force switch to reactivate old ( + reset flag)
// emergencyStop && !rtIsInEmergencyStop() -> deactivate all + set flag
@@ -156,7 +202,6 @@ namespace armarx
return false;
}
rtIsInEmergencyStop_ = true;
- bool updatedActive = false;
//deactivate all nJointCtrl
for (auto& nJointCtrl : rtGetActivatedNJointControllers())
{
@@ -164,7 +209,7 @@ namespace armarx
{
NJointControllerAttorneyForControlThread::RtDeactivateController(nJointCtrl);
nJointCtrl = nullptr;
- updatedActive = true;
+ rtSwitchControllerSetupChangedControllers = true;
}
}
//set all JointCtrl to emergency stop (except stop movement)
@@ -178,15 +223,15 @@ namespace armarx
{
controlDevice->rtSetActiveJointController(emergency);
rtGetActivatedJointControllers().at(i) = emergency;
- updatedActive = true;
+ rtSwitchControllerSetupChangedControllers = true;
}
}
- if (updatedActive)
+ if (rtSwitchControllerSetupChangedControllers)
{
ControlThreadDataBufferAttorneyForControlThread::ResetActivatedControllerAssignement(this);
- rtCommitActivatedControllers();
+ // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
}
- return updatedActive;
+ return rtSwitchControllerSetupChangedControllers;
}
if (
@@ -222,10 +267,12 @@ namespace armarx
rtSyncNJointControllerRobot(req.get());
NJointControllerAttorneyForControlThread::RtActivateController(req);
++idxReq;
+ rtSwitchControllerSetupChangedControllers = true;
}
else if (reqId < actId)
{
NJointControllerAttorneyForControlThread::RtDeactivateController(act);
+ rtSwitchControllerSetupChangedControllers = true;
++idxAct;
}
else //if(reqId == actId)
@@ -254,7 +301,7 @@ namespace armarx
}
}
ControlThreadDataBufferAttorneyForControlThread::AcceptRequestedJointToNJointControllerAssignement(this);
- rtCommitActivatedControllers();
+ // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
return true;
}
@@ -271,21 +318,19 @@ namespace armarx
void ControlThread::rtHandleInvalidTargets()
{
rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart();
- bool oneIsInvalid = 0;
+ numberOfInvalidTargetsInThisIteration = 0;
const auto& cdevs = rtGetControlDevices();
for (std::size_t i = 0; i < cdevs.size(); ++i)
{
if (!rtGetActivatedJointControllers().at(i)->rtIsTargetValid())
{
ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'", i, cdevs.at(i)->rtGetDeviceName().c_str());
+ ARMARX_ERROR << ">>>INVALID TARGET for JointController (idx = " << i << ") '" << cdevs.at(i)->rtGetDeviceName().c_str() << "'";
rtDeactivateAssignedNJointControllerBecauseOfError(i);
- oneIsInvalid = true;
+ ++numberOfInvalidTargetsInThisIteration;
}
}
- if (oneIsInvalid)
- {
- rtCommitActivatedControllers();
- }
+ // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsEnd();
}
@@ -313,7 +358,7 @@ namespace armarx
void ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
- bool activeControllersChanged = false;
+ // bool activeControllersChanged = false;
for (std::size_t nJointCtrlIndex = 0; nJointCtrlIndex < rtGetActivatedNJointControllers().size(); ++nJointCtrlIndex)
{
NJointController* nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
@@ -328,7 +373,7 @@ namespace armarx
nJointCtrl->rtGetInstanceName().c_str()
);
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
- activeControllersChanged = true;
+ // activeControllersChanged = true;
}
auto start = TimeUtil::GetTime(true);
@@ -342,7 +387,7 @@ namespace armarx
nJointCtrl->rtGetInstanceName().c_str()
);
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
- activeControllersChanged = true;
+ // activeControllersChanged = true;
}
if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
{
@@ -368,13 +413,10 @@ namespace armarx
<< " threw an exception and is now deactivated: "
<< GetHandledExceptionString();
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
- activeControllersChanged = true;
+ // activeControllersChanged = true;
}
}
- if (activeControllersChanged)
- {
- rtCommitActivatedControllers();
- }
+ // the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
}
@@ -386,9 +428,17 @@ namespace armarx
{
const ControlDevicePtr& controlDevice = rtGetControlDevices().at(ctrlDevIdx);
JointController* es = controlDevice->rtGetJointEmergencyStopController();
+
+ ARMARX_CHECK_EQUAL_W_HINT(
+ rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx),
+ nJointCtrlIndex,
+ VAROUT(ctrlDevIdx) << "\n"
+ << VAROUT(controlDevice->getDeviceName()) << "\n"
+ << dumpRtState()
+ );
+
controlDevice->rtSetActiveJointController(es);
rtGetActivatedJointControllers().at(ctrlDevIdx) = es;
- ARMARX_CHECK_EXPRESSION(rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) == nJointCtrlIndex);
rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx) = IndexSentinel();
}
rtGetActivatedNJointControllers().at(nJointCtrlIndex) = nullptr;
@@ -446,17 +496,8 @@ namespace armarx
<< rtGetControlDevices().at(ctrlDevIndex)->rtGetDeviceName()
<< ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
<< "This means an invariant is violated! Dumping data for debugging:\n"
- << VAROUT(ctrlDevIndex) << "\nrtGetActivatedJointControllers\n"
- << ARMARX_STREAM_PRINTER
- {
- for (std::size_t i = 0; i < rtGetActivatedJointControllers().size(); ++i)
- {
- const JointController* jctrl = rtGetActivatedJointControllers().at(i);
- out << " " << i << "\t" << jctrl << " " << jctrl->getControlMode() << "\n";
- }
- }
- << VAROUT(rtGetActivatedNJointControllers()) << "\n"
- << VAROUT(rtGetActivatedJointToNJointControllerAssignement())
+ << VAROUT(ctrlDevIndex) << "\n"
+ << dumpRtState()
);
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
@@ -465,6 +506,11 @@ namespace armarx
void ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferStart();
+ //commit all changes to activated controllers (joint, njoint, assignement)
+ {
+ ControlThreadDataBufferAttorneyForControlThread::CommitActivatedControllers(this);
+ }
+
SensorAndControl& sc = _module<ControlThreadDataBuffer>().rtGetSensorAndControlBuffer();
sc.writeTimestamp = IceUtil::Time::now(); // this has to be in real time
sc.sensorValuesTimestamp = sensorValuesTimestamp;
@@ -488,10 +534,11 @@ namespace armarx
}
_module<ControlThreadDataBuffer>().rtSensorAndControlBufferCommitWrite();
+
rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferEnd();
}
- const std::vector<ControlDevicePtr>& ControlThread::rtGetControlDevices()
+ const std::vector<ControlDevicePtr>& ControlThread::rtGetControlDevices() const
{
return DevicesAttorneyForControlThread::GetControlDevices(this);
}
@@ -522,6 +569,18 @@ namespace armarx
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = std::this_thread::get_id();
+
+ ARMARX_CHECK_EQUAL(rtGetActivatedJointControllers().size(), rtGetActivatedJointToNJointControllerAssignement().size());
+ ARMARX_CHECK_EQUAL(rtGetActivatedJointControllers().size(), rtGetActivatedNJointControllers().size());
+ //resize buffers used for error oputput
+ preSwitchSetup_ActivatedJointControllers.resize(rtGetActivatedJointControllers().size());
+ postSwitchSetup_ActivatedJointControllers.resize(rtGetActivatedJointControllers().size());
+
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement.resize(rtGetActivatedJointToNJointControllerAssignement().size());
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement.resize(rtGetActivatedJointToNJointControllerAssignement().size());
+
+ preSwitchSetup_ActivatedNJointControllers.resize(rtGetActivatedNJointControllers().size());
+ postSwitchSetup_ActivatedNJointControllers.resize(rtGetActivatedNJointControllers().size());
}
void ControlThread::_preOnInitRobotUnit()
@@ -585,6 +644,21 @@ namespace armarx
}
}
+ const std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
+ }
+
+ const std::vector<NJointController*>& ControlThread::rtGetActivatedNJointControllers() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
+ }
+
+ const std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement() const
+ {
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ }
+
std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
@@ -595,25 +669,24 @@ namespace armarx
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
}
- void ControlThread::rtCommitActivatedControllers()
+ std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement()
{
- ARMARX_RT_LOGF_DEBUG("RT: activated controllers changed");
- ControlThreadDataBufferAttorneyForControlThread::CommitActivatedControllers(this);
+ return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
}
- const std::vector<JointController*>& ControlThread::rtGetRequestedJointControllers()
+ const std::vector<JointController*>& ControlThread::rtGetRequestedJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointControllers(this);
}
- const std::vector<NJointController*>& ControlThread::rtGetRequestedNJointControllers()
+ const std::vector<NJointController*>& ControlThread::rtGetRequestedNJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetRequestedNJointControllers(this);
}
- std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement()
+ const std::vector<std::size_t>& ControlThread::rtGetRequestedJointToNJointControllerAssignement() const
{
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointToNJointControllerAssignement(this);
}
void ControlThread::rtSyncNJointControllerRobot(NJointController* njctrl)
@@ -629,6 +702,81 @@ namespace armarx
}
}
+
+ void ControlThread::dumpRtControllerSetup(
+ std::ostream& out,
+ const std::string& indent,
+ const std::vector<JointController*>& activeJCtrls,
+ const std::vector<std::size_t>& assignement,
+ const std::vector<NJointController*>& activeNJCtrls) const
+ {
+ out << indent << "JointControllers:\n";
+ {
+ const auto& cdevs = rtGetControlDevices();
+ for (std::size_t i = 0; i < cdevs.size(); ++i)
+ {
+ const JointController* jctrl = activeJCtrls.at(i);
+ out << indent << "\t(" << i << ")\t" << cdevs.at(i)->rtGetDeviceName() << ":\n"
+ << indent << "\t\t Controller: " << jctrl->getControlMode() << " (" << jctrl << ")\n"
+ << indent << "\t\t Assigned NJoint: " << assignement.at(i) << "\n";
+ }
+ }
+ out << indent << "NJointControllers:\n";
+ {
+ for (std::size_t i = 0; i < activeNJCtrls.size(); ++i)
+ {
+ const auto* njctrl = activeNJCtrls.at(i);
+ out << indent << "\t(" << i << ")\t";
+ if (njctrl)
+ {
+ out << njctrl->rtGetInstanceName() << " (" << njctrl << "):"
+ << "\t Class: " << njctrl->rtGetClassName() << "\n";
+ }
+ else
+ {
+ out << " (" << njctrl << ")\n";
+ }
+ }
+ }
+
+ }
+
+ std::string ControlThread::dumpRtState() const
+ {
+ std::stringstream str;
+ str << "state requested\n";
+ dumpRtControllerSetup(
+ str, "\t",
+ rtGetRequestedJointControllers(),
+ rtGetRequestedJointToNJointControllerAssignement(),
+ rtGetRequestedNJointControllers());
+
+ str << "state before rtSwitchControllerSetup() was called\n";
+ dumpRtControllerSetup(
+ str, "\t",
+ preSwitchSetup_ActivatedJointControllers,
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement,
+ preSwitchSetup_ActivatedNJointControllers);
+
+ str << "state after rtSwitchControllerSetup() was called\n";
+ dumpRtControllerSetup(
+ str, "\t",
+ postSwitchSetup_ActivatedJointControllers,
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement,
+ postSwitchSetup_ActivatedNJointControllers);
+
+ str << "state now\n";
+ dumpRtControllerSetup(
+ str, "\t",
+ rtGetActivatedJointControllers(),
+ rtGetActivatedJointToNJointControllerAssignement(),
+ rtGetActivatedNJointControllers());
+
+ str << VAROUT(rtSwitchControllerSetupChangedControllers) << "\n";
+ str << VAROUT(numberOfInvalidTargetsInThisIteration) << "\n";
+ return str.str();
+ }
+
void ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
{
rtRobot->setGlobalPose(gp, updateRobot);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index 042959369c60a09374e9084ef3a24372caf354e6..009c950b494b04c438ba4dac03f9cbd9ba2d1813 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -208,7 +208,7 @@ namespace armarx
* @brief Returns all \ref ControlDevice "ControlDevices"
* @return All \ref ControlDevice "ControlDevices"
*/
- const std::vector<ControlDevicePtr>& rtGetControlDevices();
+ const std::vector<ControlDevicePtr>& rtGetControlDevices() const;
/**
* @brief Returns all \ref SensorDevice "SensorDevices"
* @return All \ref SensorDevice "SensorDevices"
@@ -253,37 +253,52 @@ namespace armarx
* @return The activated \ref JointController "JointControllers"
*/
std::vector<JointController*>& rtGetActivatedJointControllers();
+ const std::vector<JointController*>& rtGetActivatedJointControllers() const;
/**
* @brief Returns the activated \ref NJointController "NJointControllers"
* @return The activated \ref NJointController "NJointControllers"
*/
std::vector<NJointController*>& rtGetActivatedNJointControllers();
+ const std::vector<NJointController*>& rtGetActivatedNJointControllers() const;
+
/**
- * @brief Writes the activated \ref NJointController "NJointControllers" and
- * \ref JointController "JointControllers" to the output buffer.
+ * @brief Returns a vector containing the index of the activated \ref NJointController
+ * for each \ref JointController
+ * @return A vector containing the index of the activated \ref NJointController
+ * for each \ref JointController
*/
- void rtCommitActivatedControllers();
+ std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement();
+ const std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement() const;
+
/**
* @brief Returns the requested \ref JointController "JointControllers"
* @return The requested \ref JointController "JointControllers"
*/
- const std::vector<JointController*>& rtGetRequestedJointControllers();
+ const std::vector<JointController*>& rtGetRequestedJointControllers() const;
/**
* @brief Returns the requested \ref NJointController "NJointControllers"
* @return The requested \ref NJointController "NJointControllers"
*/
- const std::vector<NJointController*>& rtGetRequestedNJointControllers();
+ const std::vector<NJointController*>& rtGetRequestedNJointControllers() const;
/**
- * @brief Returns a vector containing the index of the activated \ref NJointController
+ * @brief Returns a vector containing the index of the requested \ref NJointController
* for each \ref JointController
- * @return A vector containing the index of the activated \ref NJointController
+ * @return A vector containing the index of the requested \ref NJointController
* for each \ref JointController
*/
- std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement();
+ const std::vector<std::size_t>& rtGetRequestedJointToNJointControllerAssignement() const;
void rtSyncNJointControllerRobot(NJointController* njctrl);
+
+ void dumpRtControllerSetup(
+ std::ostream& out,
+ const std::string& indent,
+ const std::vector<JointController*>& activeCdevs,
+ const std::vector<std::size_t>& activeJCtrls,
+ const std::vector<NJointController*>& assignement) const;
+ std::string dumpRtState() const;
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
@@ -306,6 +321,17 @@ namespace armarx
/// @brief An Error will be printed, if the execution time per ControlDev of a NJointController exceeds this parameter
std::size_t usPerDevUntilError;
+ bool rtSwitchControllerSetupChangedControllers {false};
+ std::size_t numberOfInvalidTargetsInThisIteration {0};
+
+ std::vector<JointController*> preSwitchSetup_ActivatedJointControllers;
+ std::vector<std::size_t> preSwitchSetup_ActivatedJointToNJointControllerAssignement;
+ std::vector<NJointController*> preSwitchSetup_ActivatedNJointControllers;
+
+ std::vector<JointController*> postSwitchSetup_ActivatedJointControllers;
+ std::vector<std::size_t> postSwitchSetup_ActivatedJointToNJointControllerAssignement;
+ std::vector<NJointController*> postSwitchSetup_ActivatedNJointControllers;
+
std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest {EmergencyStopStateRequest::RequestNone};
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////////// Attorneys ////////////////////////////////////// //