diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
index 3be94c7a50619cfe95ff85046d544afd032e9888..e6d317984ddc31b1c9d9bb92947ada0d700b2a4a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp
@@ -109,17 +109,19 @@ namespace armarx
if (!robotProjectName.empty())
{
CMakePackageFinder finder(robotProjectName);
- Ice::StringSeq projectIncludePaths;
auto pathsString = finder.getDataDir();
- boost::split(projectIncludePaths,
+ boost::split(includePaths,
pathsString,
boost::is_any_of(";,"),
boost::token_compress_on);
- includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ ArmarXDataPath::addDataPaths(includePaths);
}
if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
{
- throw UserException("Could not find robot file " + robotFileName);
+ std::stringstream str;
+ str << "Could not find robot file " + robotFileName
+ << "\nCollected paths from RobotFileNameProject '" << robotProjectName << "':" << includePaths;
+ throw UserException(str.str());
}
// read the robot
try
diff --git a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
index 5f202a8f8da633a10380e0bc2f4e0f77d4ca179f..23101df79925c5bcca01d070d043f91bcf2e1a43 100644
--- a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
@@ -283,7 +283,7 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationAndPositionBoundsTe
auto distPosHi = std::uniform_real_distribution<double> { M_PI_4, M_PI};
s.posLo = distPosLo(gen);
s.posHi = distPosHi(gen);
- double p = 20.5;
+ // double p = 20.5;
VelocityControllerWithRampedAccelerationAndPositionBounds ctrl;
auto testTick = [&]
@@ -372,7 +372,7 @@ BOOST_AUTO_TEST_CASE(VelocityControllerWithRampedAccelerationTest)
s.posLo = 0;
- double p = 20.5;
+ // double p = 20.5;
VelocityControllerWithRampedAcceleration ctrl;
auto testTick = [&]
@@ -657,7 +657,7 @@ BOOST_AUTO_TEST_CASE(MinJerkPositionControllerTest)
s.posLo = 0;
- double p = 20.5;
+ // double p = 20.5;
MinJerkPositionController ctrl;
auto testTick = [&]
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.cpp
index e3ad0afa99de791e1c07b0b2ba829d7944dc5b98..11e79d6828515061d56c4e0b01d65fbaa4c4e5e7 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/ControlDevicesWidget.cpp
@@ -78,7 +78,10 @@ namespace armarx
if (statusUpdates[status.deviceName].timestampUSec < status.timestampUSec)
{
statusUpdates[status.deviceName] = status;
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
}
}
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
index b0bb952eab4ac87d74aa687e47d7c66ce33ce6a9..81a9f43b71dae04963fea51e9f914155125c2ca0 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllerClassesWidget.cpp
@@ -100,7 +100,10 @@ namespace armarx
auto data = ru->getNJointControllerClassDescription(name);
guard.lock();
nJointControllerClassDescriptions[data.className] = std::move(data);
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
}
void NJointControllerClassesWidget::updateDefaultNameOnControllerCreated(QString createdName, bool force)
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
index 3e4f830f183182c1c5a024f26e36403ce47a6730..157900996537b82ecfe8e2c1c38ada8009d1a777 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/NJointControllersWidget.cpp
@@ -97,7 +97,10 @@ namespace armarx
if (statusUpdates[status.instanceName].timestampUSec < status.timestampUSec)
{
statusUpdates[status.instanceName] = status;
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
}
}
@@ -149,7 +152,10 @@ namespace armarx
if (!entries.count(name))
{
controllersCreated[name] = std::move(data);
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
}
}
@@ -157,7 +163,10 @@ namespace armarx
{
std::unique_lock<std::recursive_timed_mutex> guard {mutex};
controllersDeleted.emplace(name);
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
}
void NJointControllersWidget::loadSettings(QSettings* settings)
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
index cec5e3d551aee103ae032d23dc52acb7b249531d..74d4b18071191eada75541c17ee49e721bfed3f0 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.cpp
@@ -59,6 +59,16 @@ namespace armarx
QMetaObject::invokeMethod(this, "doReset", Qt::QueuedConnection);
}
+ void RobotUnitWidgetBase::setVisible(bool visible)
+ {
+ doMetaCall = visible;
+ if (visible)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
+ QWidget::setVisible(visible);
+ }
+
void RobotUnitWidgetBase::doReset()
{
std::unique_lock<std::recursive_timed_mutex> guard {mutex};
@@ -84,7 +94,10 @@ namespace armarx
<< armarx::GetHandledExceptionString();
}
gotResetData = true;
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
ARMARX_INFO << "Resetting of widget '" << objectName().toStdString() << "' done!";
}
} .detach();
@@ -92,7 +105,10 @@ namespace armarx
else
{
gotResetData = true;
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
}
}
@@ -106,7 +122,10 @@ namespace armarx
{
killTimer(resetTimerId);
resetTimerId = 0;
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
}
}
}
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.h
index d0fe28f26295a239f68e9420ec3d74a0efecab0a..c797891f04c9ff1d5adc846d94deef90b82623a7 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/RobotUnitWidgetBase.h
@@ -53,6 +53,7 @@ namespace armarx
virtual void loadSettings(QSettings*) {}
virtual void saveSettings(QSettings*) {}
+ void setVisible(bool visible) override;
protected slots:
void updateContent();
private slots:
@@ -80,6 +81,7 @@ namespace armarx
int resetTimerId {0};
int resetCount {0};
std::atomic_bool isResetting {false};
+ std::atomic_bool doMetaCall {false};
};
template<class UI>
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.cpp
index 070f0401948ed8b9639ee66aa061aac08abcf51c..5afd04ce7c522f7da9fa62632f276f1e5716606a 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/SensorDevicesWidget.cpp
@@ -62,7 +62,10 @@ namespace armarx
if (statusUpdates[status.deviceName].timestampUSec < status.timestampUSec)
{
statusUpdates[status.deviceName] = status;
- QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ if (doMetaCall)
+ {
+ QMetaObject::invokeMethod(this, "updateContent", Qt::QueuedConnection);
+ }
}
}
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
index 8fdc20d0a779d19dd37c410d025c24f6f7aa03e6..7a0b8ca72571a5b351b8382d39a5c3fcc81aaad4 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
@@ -46,6 +46,7 @@ RobotUnitPluginWidgetController::RobotUnitPluginWidgetController()
widget.groupBoxCDev->layout()->addWidget(controlDevices);
widget.groupBoxCDev->setVisible(false);
controlDevices->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Expanding);
+ controlDevices->setVisible(false);
}
//add subwidget SensorDevices
{
@@ -53,6 +54,7 @@ RobotUnitPluginWidgetController::RobotUnitPluginWidgetController()
widget.groupBoxSDev->layout()->addWidget(sensorDevices);
widget.groupBoxSDev->setVisible(false);
sensorDevices->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Expanding);
+ sensorDevices->setVisible(false);
}
//add subwidget NJointControllers
{
@@ -60,6 +62,7 @@ RobotUnitPluginWidgetController::RobotUnitPluginWidgetController()
widget.groupBoxNJointCtrl->layout()->addWidget(nJointControllers);
widget.groupBoxNJointCtrl->setVisible(true);
nJointControllers->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Expanding);
+ nJointControllers->setVisible(true);
}
//add subwidget NJointControllerClasses
{
@@ -67,6 +70,7 @@ RobotUnitPluginWidgetController::RobotUnitPluginWidgetController()
widget.groupBoxNJointCtrlClasses->layout()->addWidget(nJointControllerClasses);
widget.groupBoxNJointCtrlClasses->setVisible(false);
nJointControllerClasses->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Expanding);
+ nJointControllerClasses->setVisible(false);
}
updateToolBarActionCheckedState();
}
@@ -157,6 +161,7 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
showCDevs->setCheckable(true);
showCDevs->setToolTip("Hide/Show the list of Control Devices");
connect(showCDevs, SIGNAL(toggled(bool)), widget.groupBoxCDev, SLOT(setVisible(bool)));
+ connect(showCDevs, SIGNAL(toggled(bool)), controlDevices, SLOT(setVisible(bool)));
customToolbar->addAction(showCDevs);
customToolbar->addSeparator();
@@ -166,6 +171,7 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
showSDevs->setCheckable(true);
showSDevs->setToolTip("Hide/Show the list of Sensor Devices");
connect(showSDevs, SIGNAL(toggled(bool)), widget.groupBoxSDev, SLOT(setVisible(bool)));
+ connect(showSDevs, SIGNAL(toggled(bool)), sensorDevices, SLOT(setVisible(bool)));
customToolbar->addAction(showSDevs);
customToolbar->addSeparator();
@@ -175,6 +181,7 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
showNJoint->setCheckable(true);
showNJoint->setToolTip("Hide/Show the list of NJointControllers");
connect(showNJoint, SIGNAL(toggled(bool)), widget.groupBoxNJointCtrl, SLOT(setVisible(bool)));
+ connect(showNJoint, SIGNAL(toggled(bool)), nJointControllers, SLOT(setVisible(bool)));
customToolbar->addAction(showNJoint);
customToolbar->addSeparator();
@@ -184,6 +191,7 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
showNJointClasses->setCheckable(true);
showNJointClasses->setToolTip("Hide/Show the list of NJointControllers Classes");
connect(showNJointClasses, SIGNAL(toggled(bool)), widget.groupBoxNJointCtrlClasses, SLOT(setVisible(bool)));
+ connect(showNJointClasses, SIGNAL(toggled(bool)), nJointControllerClasses, SLOT(setVisible(bool)));
customToolbar->addAction(showNJointClasses);
customToolbar->addSeparator();
@@ -265,7 +273,6 @@ void RobotUnitPluginWidgetController::updateToolBarActionCheckedState()
}
}
-
void armarx::RobotUnitPluginWidgetController::timerEvent(QTimerEvent*)
{
if (!robotUnitPrx)
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index e80c3cf92487806e92f7ea6fa90fc64f4b96e15a..4b4211d8a5a8449e4a0fba74ffeeaeceae991f0c 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -565,6 +565,7 @@ module armarx
double getCanVal();
Ice::FloatSeq getAnomalyInput();
+ Ice::FloatSeq getAnomalyOutput();
};
};
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
index eb096aae4f45f5a013826e3b4bbd71229f099d98..b6a8ab41f0cf0943086196084bb05f0b04505a67 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.cpp
@@ -448,7 +448,7 @@ MultiNodeRapidXMLReader EtherCATRTUnit::ReadHardwareConfigFile(std::string busCo
{
throw LocalException("could not find BusConfigFilePath: ") << busConfigFilePath;
}
- ARMARX_DEBUG_S << "read the config";
+ ARMARX_INFO_S << "read the hw config from " << busConfigFilePath;
//reading the config for the Bus and create all the robot objects in the robot container
auto busConfigFilePathDir = std::filesystem::path(busConfigFilePath).parent_path();
diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
index 1619c7c1708301607ebf7588a99363bca23f1d35..21a7d6a2c5058e39f7e4ddc9d2ecf6278928bc9c 100644
--- a/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
+++ b/source/RobotAPI/libraries/ArmarXEtherCAT/EtherCATRTUnit.h
@@ -77,7 +77,7 @@ namespace armarx
/**
* @defgroup Component-EtherCATRTUnit EtherCATRTUnit
- * @ingroup Armar6RT-Components
+ * @ingroup RobotAPI-Components
* A description of the component EtherCATRTUnit.
*
* @class EtherCATRTUnit
diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
index 249b951e8fc383b76d616b78f6f4255b62b24655..8ddc9ca8c49ee0541df464981349ee91db15591d 100644
--- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
+++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp
@@ -339,8 +339,8 @@ void DSRTBimanualController::controllerRun()
Eigen::Vector3f both_arm_force_ave = - 0.5 * (left_force + right_force); // negative sign for the direction
- float left_force_z = left_force(2);
- float right_force_z = right_force(2);
+ // float left_force_z = left_force(2);
+ // float right_force_z = right_force(2);
Eigen::Vector3f left_currentTCPPositionInMeter;
Eigen::Vector3f right_currentTCPPositionInMeter;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
index efb6befa7bbd81921ec4738d05f71d23bbae2a6f..c7fe811d8358c894f2d5ac1175537cba6ff53d4d 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp
@@ -286,12 +286,6 @@ namespace armarx
velocityHorizonList.pop_front();
}
- rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
- rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head(3);
- rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
-
- rt2UserData.commitWrite();
-
Eigen::VectorXf targetVel(6);
Eigen::Vector3f axis;
Eigen::Vector3f forceInToolFrame;
@@ -573,6 +567,14 @@ namespace armarx
// targetFTInRootFrame.tail(3) = currentToolOri * targetFTInToolFrame.tail(3);
}
+
+ rt2UserData.getWriteBuffer().currentTcpPose = currentPose;
+ rt2UserData.getWriteBuffer().tcpTranslVel = currentTwist.head(3);
+ rt2UserData.getWriteBuffer().forceOutput = forceInToolFrame;
+ rt2UserData.getWriteBuffer().waitTimeForCalibration += deltaT;
+ rt2UserData.commitWrite();
+
+
/* -------------------------- VMP Phase Stop --------------------------------- */
bool isPhaseStop = false;
@@ -795,6 +797,13 @@ namespace armarx
return tvelvec;
}
+ std::vector<float> NJointAnomalyDetectionAdaptiveWipingController::getAnomalyOutput(const Ice::Current&)
+ {
+ Eigen::Vector3f force = rt2UserData.getUpToDateReadBuffer().forceOutput;
+ std::vector<float> forceVec = {force(0), force(1), force(2)};
+ return forceVec;
+ }
+
void NJointAnomalyDetectionAdaptiveWipingController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx& debugDrawer, const DebugObserverInterfacePrx& debugObs)
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
index 8940f3b0476e6d178ee4736351818373391bff31..154b2c8ce1af2afdb6caf09af548a24d3660e02e 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.h
@@ -82,6 +82,8 @@ namespace armarx
}
std::vector<float> getAnomalyInput(const Ice::Current&);
+ std::vector<float> getAnomalyOutput(const Ice::Current&);
+
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
@@ -151,6 +153,7 @@ namespace armarx
{
Eigen::Matrix4f currentTcpPose;
Eigen::Vector3f tcpTranslVel;
+ Eigen::Vector3f forceOutput;
float waitTimeForCalibration;
};
TripleBuffer<RTToUserData> rt2UserData;