diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
index bff6b97589e78b69e69d738de8c17a421eb85c70..8d0d3f16b7493276299653314beb44496467545f 100644
--- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h
@@ -83,8 +83,8 @@ namespace armarx
*/
virtual StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const;
- protected:
ControlDevice& rtGetParent() const;
+ protected:
//called by the owning ControlDevice
/// @brief called when this JointController is run
virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 0564980702ec735105c95a668d56cc768b2e9b36..4e5a932446f4a424fa04f72f5abb6471e0df5767 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -103,21 +103,51 @@ namespace armarx::RobotUnitModule
static const std::vector<JointController*>& GetRequestedJointControllers(const ControlThread* p)
{
+ //do NOT update here!
return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointControllers;
}
static const std::vector<NJointControllerBase*>& GetRequestedNJointControllers(const ControlThread* p)
{
+ //do NOT update here!
return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().nJointControllers;
}
static const std::vector<std::size_t>& GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
{
+ //do NOT update here!
return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
}
static bool RequestedControllersChanged(const ControlThread* p)
{
+ //only place allowed to update this buffer!
return p->_module<ControlThreadDataBuffer>().controllersRequested.updateReadBuffer();
}
+
+ /// Activate a joint controller from the rt loop (only works in switch mode RTThread)
+ static void RTSetJointController(ControlThread* p, JointController* c, std::size_t index)
+ {
+ ARMARX_CHECK_NOT_NULL(c);
+ //do NOT update here!
+ auto& readbuf = p->_module<ControlThreadDataBuffer>().controllersRequested._getNonConstReadBuffer();
+ auto& j = readbuf.jointControllers;
+ auto& assig = readbuf.jointToNJointControllerAssignement;
+ auto& nj = readbuf.nJointControllers;
+ ARMARX_CHECK_LESS(index, j.size());
+ const auto assigNJ = assig.at(index);
+ if (assigNJ != JointAndNJointControllers::Sentinel())
+ {
+ //an NJointController is activated! Deactivate it and reset all joit devs
+ nj.at(assigNJ) = nullptr;
+ for (std::size_t i = 0; i < assig.size(); ++i)
+ {
+ if (assig.at(i) == assigNJ)
+ {
+ j.at(i) = j.at(i)->rtGetParent().rtGetJointStopMovementController();
+ }
+ }
+ }
+ j.at(index) = c;
+ }
};
/**
* \brief This class allows minimal access to private members of \ref Devices in a sane fashion for \ref ControlThread.
@@ -175,21 +205,17 @@ namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- bool ControlThread::rtSwitchControllerSetup()
+ bool ControlThread::rtSwitchControllerSetup(SwitchControllerMode mode)
{
rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupStart();
ARMARX_ON_SCOPE_EXIT { rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd(); };
+ //store controllers activated before switching controllers
{
- 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);
- }
+ preSwitchSetup_ActivatedJointControllers = rtGetActivatedJointControllers();
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement =
+ rtGetActivatedJointToNJointControllerAssignement();
+ preSwitchSetup_ActivatedNJointControllers = rtGetActivatedNJointControllers();
}
ARMARX_ON_SCOPE_EXIT
@@ -198,16 +224,18 @@ namespace armarx::RobotUnitModule
const auto& assig = rtGetActivatedJointToNJointControllerAssignement();
const auto& actNJC = rtGetActivatedNJointControllers();
+ //store controllers activated after switching controllers
+ {
+ postSwitchSetup_ActivatedJointControllers = actJC;
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement = assig;
+ postSwitchSetup_ActivatedNJointControllers = actNJC;
+ }
+
std::size_t numSyncNj = 0;
std::size_t numAsyncNj = 0;
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);
-
if (actNJC.at(i) == nullptr)
{
continue;
@@ -258,11 +286,13 @@ namespace armarx::RobotUnitModule
ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!").deactivateSpam(1);
}
+ const bool rtThreadOverridesControl = mode != SwitchControllerMode::IceRequests;
+ // mode == RTThread -> control flow in hand of rt thread (ignoring estop)
// !emergencyStop && !rtIsInEmergencyStop() -> normal control flow
// !emergencyStop && rtIsInEmergencyStop() -> force switch to reactivate old ( + reset flag)
// emergencyStop && !rtIsInEmergencyStop() -> deactivate all + set flag
// emergencyStop && rtIsInEmergencyStop() -> nothing to do
- if (emergencyStop)
+ if (emergencyStop && !rtThreadOverridesControl)
{
if (rtIsInEmergencyStop())
{
@@ -302,6 +332,7 @@ namespace armarx::RobotUnitModule
}
if (
+ !rtThreadOverridesControl &&
!ControlThreadDataBufferAttorneyForControlThread::RequestedControllersChanged(this) &&
!rtIsInEmergencyStop()
)
@@ -312,6 +343,8 @@ namespace armarx::RobotUnitModule
//handle nJointCtrl
{
+ const auto& allReqNJ = rtGetRequestedNJointControllers();
+ auto& allActdNJ = rtGetActivatedNJointControllers();
//"merge"
std::size_t n = rtGetControlDevices().size();
std::size_t idxAct = 0;
@@ -319,12 +352,12 @@ namespace armarx::RobotUnitModule
for (std::size_t i = 0; i < 2 * n; ++i)
{
//skip nullptrs in act
- while (idxAct < n && !rtGetActivatedNJointControllers().at(idxAct))
+ while (idxAct < n && !allActdNJ.at(idxAct))
{
++idxAct;
}
- const NJointControllerBasePtr& req = idxReq < n ? rtGetRequestedNJointControllers().at(idxReq) : nullptr; //may be null
- const NJointControllerBasePtr& act = idxAct < n ? rtGetActivatedNJointControllers().at(idxAct) : nullptr; //may be null if it is the last
+ const NJointControllerBasePtr& req = idxReq < n ? allReqNJ.at(idxReq) : nullptr; //may be null
+ const NJointControllerBasePtr& act = idxAct < n ? allActdNJ.at(idxAct) : nullptr; //may be null if it is the last
const auto reqId = reinterpret_cast<std::uintptr_t>(req.get());
const auto actId = reinterpret_cast<std::uintptr_t>(act.get());
@@ -353,18 +386,20 @@ namespace armarx::RobotUnitModule
break;
}
}
- rtGetActivatedNJointControllers() = rtGetRequestedNJointControllers();
+ allActdNJ = allReqNJ;
}
//handle Joint Ctrl
{
- ARMARX_CHECK_EQUAL(rtGetRequestedJointControllers().size(), rtGetControlDevices().size());
+ const auto& allReqJ = rtGetRequestedJointControllers();
+ auto& allActdJ = rtGetActivatedJointControllers();
+ ARMARX_CHECK_EQUAL(allReqJ.size(), rtGetControlDevices().size());
for (std::size_t i = 0; i < rtGetControlDevices().size(); ++i)
{
auto& controlDevice = rtGetControlDevices().at(i);
- const auto requestedJointCtrl = rtGetRequestedJointControllers().at(i);
+ const auto requestedJointCtrl = allReqJ.at(i);
controlDevice->rtSetActiveJointController(requestedJointCtrl);
- rtGetActivatedJointControllers().at(i) = requestedJointCtrl;
+ allActdJ.at(i) = requestedJointCtrl;
}
}
ControlThreadDataBufferAttorneyForControlThread::AcceptRequestedJointToNJointControllerAssignement(this);
@@ -1017,4 +1052,10 @@ namespace armarx::RobotUnitModule
{
rtRobot->setGlobalPose(gp, updateRobot);
}
+
+ void ControlThread::rtSetJointController(JointController* c, std::size_t index)
+ {
+ ControlThreadDataBufferAttorneyForControlThread::RTSetJointController(this, c, index);
+
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index f1adb85fd4e0e2422ee9b06ed6a6880104742e5e..0dcf52aa47c01ddcd9cacd970373cf5e3423edf1 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -86,6 +86,16 @@ namespace armarx::RobotUnitModule
RequestInactive,
RequestNone,
};
+ protected:
+ enum class SwitchControllerMode
+ {
+ /**
+ * @brief This modus is used if the RTThread is supported to control
+ * which controllers are running. (e.g. for Calibrating)
+ */
+ RTThread,
+ IceRequests
+ };
public:
/**
* @brief Returns the singleton instance of this class
@@ -165,7 +175,7 @@ namespace armarx::RobotUnitModule
* Changes can be caused by a new set of requested controllers or emergency stop
* @return Whether active controllers were changed
*/
- bool rtSwitchControllerSetup();
+ bool rtSwitchControllerSetup(SwitchControllerMode mode = SwitchControllerMode::IceRequests);
/**
* @brief Searches for the \ref NJointControllerBase responsible for the given \ref JointController and deactivates it.
@@ -265,6 +275,9 @@ namespace armarx::RobotUnitModule
void rtSetEmergencyStopState(EmergencyStopState state);
void rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot = true);
+
+ /// Activate a joint controller from the rt loop (only works in switch mode RTThread)
+ void rtSetJointController(JointController* c, std::size_t index);
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
index 295d8fddead24eabdb583287681971faf5b4ade7..c4dd0f79ab5d8b0b953f7933c2d8e9fcceddd89f 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
@@ -549,7 +549,7 @@ void EtherCATRTUnit::controlLoopRTThread()
IceUtil::Time startTimestamp = armarx::rtNow();
- while (taskRunning)
+ for (; taskRunning; ++ _iterationCount)
{
const IceUtil::Time loopStartTime = armarx::rtNow();
rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopStart();
@@ -569,6 +569,16 @@ void EtherCATRTUnit::controlLoopRTThread()
auto cappedDeltaT = IceUtil::Time::microSeconds(boost::algorithm::clamp<long>(realDeltaT.toMicroSeconds(), 1, 2000)); // TODO: Make property
// TIMING_START(load);
// if (controllerLoaded)
+ if (rtIsCalibrating())
+ {
+ rtCalibrateActivateControlers();
+ rtSwitchControllerSetup(SwitchControllerMode::RTThread);
+ rtResetAllTargets();
+ _calibrationStatus = rtCalibrate();
+ rtHandleInvalidTargets();
+ rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
+ }
+ else
{
RT_TIMING_START(RunControllers);
RT_TIMING_START(SwitchControllers);
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
index 37a1b310014e67c708e97e5a33a8206520396688..dcda1ceb3cb6907ef3facd1ef7ce5301b6c502bf 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
@@ -146,6 +146,48 @@ namespace armarx
void controlLoopRTThread();
+ enum class CalibrationStatus
+ {
+ Calibrating, Done
+ };
+ /**
+ * @brief Allows to switch controllers while calibrating
+ *
+ * use
+ * rtSetJointController(JointController* c, std::size_t index)
+ * to switch controllers
+ */
+ virtual void rtCalibrateActivateControlers()
+ {
+ }
+ /**
+ * @brief Hook to add calibration code
+ *
+ * This function is called in the rt loop! So you should not take too long!
+ *
+ * read sensors and write targets
+ * while calibrating return CalibrationStatus::Calibrating
+ * if done return CalibrationStatus::Done
+ *
+ * @return Whether done or still calibrating
+ */
+ virtual CalibrationStatus rtCalibrate()
+ {
+ return CalibrationStatus::Done;
+ }
+ bool rtIsCalibrating() const
+ {
+ return _calibrationStatus == CalibrationStatus::Calibrating;
+ }
+ std::uintmax_t getIterationCount()
+ {
+ return _iterationCount;
+ }
+ private:
+ CalibrationStatus _calibrationStatus = CalibrationStatus::Calibrating;
+ std::atomic_uintmax_t _iterationCount = 0;
+ protected:
+
void computeInertiaTorque();
DebugDrawerInterfacePrx dd;