diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
index 7c466cdacab7c4a95862d2a7e9508d2d8dda450f..0e2ae813fbef99c1ce24e35c8be0be63deeeff8b 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.cpp
@@ -110,7 +110,7 @@ namespace armarx::viz::coin
<< "\nFilename: " << element.filename;
return true;
}
- IceUtil::Time time_load = IceUtil::Time::now();
+ //IceUtil::Time time_load = IceUtil::Time::now();
bool drawStyleChanged = loadedDrawStyle != element.drawStyle;
if (robotChanged || drawStyleChanged)
@@ -118,7 +118,7 @@ namespace armarx::viz::coin
recreateVisualizationNodes(element.drawStyle);
loadedDrawStyle = element.drawStyle;
}
- IceUtil::Time time_style = IceUtil::Time::now();
+ //IceUtil::Time time_style = IceUtil::Time::now();
// Set pose, configuration and so on
@@ -132,33 +132,40 @@ namespace armarx::viz::coin
// robot.setGlobalPose(pose, false);
robot.setJointValues(element.jointValues);
- IceUtil::Time time_set = IceUtil::Time::now();
+ //IceUtil::Time time_set = IceUtil::Time::now();
if (loadedDrawStyle & data::ModelDrawStyle::OVERRIDE_COLOR)
{
- int numChildren = node->getNumChildren();
- for (int i = 0; i < numChildren; i++)
+ if (loadedColor.r != element.color.r
+ || loadedColor.g != element.color.g
+ || loadedColor.b != element.color.b
+ || loadedColor.a != element.color.a)
{
- SoSeparator* nodeSep = static_cast<SoSeparator*>(node->getChild(i));
- // The first entry must be a SoMaterial (see recreateVisualizationNodes)
- SoMaterial* m = dynamic_cast<SoMaterial*>(nodeSep->getChild(0));
- if (!m)
+ int numChildren = node->getNumChildren();
+ for (int i = 0; i < numChildren; i++)
{
- ARMARX_WARNING << "Error at node with index: " << i;
- continue;
+ SoSeparator* nodeSep = static_cast<SoSeparator*>(node->getChild(i));
+ // The first entry must be a SoMaterial (see recreateVisualizationNodes)
+ SoMaterial* m = dynamic_cast<SoMaterial*>(nodeSep->getChild(0));
+ if (!m)
+ {
+ ARMARX_WARNING << "Error at node with index: " << i;
+ continue;
+ }
+
+ auto color = element.color;
+ const float conv = 1.0f / 255.0f;
+ float a = color.a * conv;
+ SbColor coinColor(conv * color.r, conv * color.g, conv * color.b);
+ m->diffuseColor = coinColor;
+ m->ambientColor = coinColor;
+ m->transparency = 1.0f - a;
+ m->setOverride(true);
}
-
- auto color = element.color;
- const float conv = 1.0f / 255.0f;
- float a = color.a * conv;
- SbColor coinColor(conv * color.r, conv * color.g, conv * color.b);
- m->diffuseColor = coinColor;
- m->ambientColor = coinColor;
- m->transparency = 1.0f - a;
- m->setOverride(true);
+ loadedColor = element.color;
}
}
- IceUtil::Time time_color = IceUtil::Time::now();
+ //IceUtil::Time time_color = IceUtil::Time::now();
// Setting the joint angles takes > 0.5 ms! This is unexpected...
// ARMARX_INFO << "Total: " << (time_color - time_start).toMilliSecondsDouble()
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
index cb6699735a40b1783bb0e09270891910514278da..63aa3f1f8cabf92029b1d1b763d808f9b2123d77 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationRobot.h
@@ -25,6 +25,7 @@ namespace armarx::viz::coin
LoadedRobot loaded;
int loadedDrawStyle = data::ModelDrawStyle::ORIGINAL;
+ armarx::viz::data::Color loadedColor{0, 0, 0, 0};
};
void clearRobotCache();
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index f11c7aa9de6672cafcda2bbcfdb2e5114eb2be69..692800bc7d85488b4af3dc7090c6efabde509911 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -143,8 +143,8 @@ namespace armarx::viz
layerIt = layers.emplace(layerID, CoinLayer(coinNode)).first;
}
- IceUtil::Time time_start1 = IceUtil::Time::now();
- IceUtil::Time time_start = IceUtil::Time::now();
+ //IceUtil::Time time_start1 = IceUtil::Time::now();
+ //IceUtil::Time time_start = IceUtil::Time::now();
// Add or update the elements in the update
CoinLayer& layer = layerIt->second;
@@ -171,10 +171,10 @@ namespace armarx::viz
continue;
}
coin::ElementVisualizer* visualizer = elementVisualizers[visuIndex].get();
- IceUtil::Time time_findVisu = IceUtil::Time::now();
+ //IceUtil::Time time_findVisu = IceUtil::Time::now();
auto oldElementIter = layer.elements.find(updatedElement.id);
- IceUtil::Time time_findElement = IceUtil::Time::now();
+ //IceUtil::Time time_findElement = IceUtil::Time::now();
if (oldElementIter != layer.elements.end())
{
// Element already exists
@@ -238,7 +238,7 @@ namespace armarx::viz
iter = layer.elements.erase(iter);
}
}
- IceUtil::Time time_remove = IceUtil::Time::now();
+ //IceUtil::Time time_remove = IceUtil::Time::now();
// if (update.name == "Example")
// {
@@ -290,7 +290,7 @@ namespace armarx::viz
{
case CoinVisualizerUpdateResult::SUCCESS:
{
- IceUtil::Time time_start = IceUtil::Time::now();
+ //IceUtil::Time time_start = IceUtil::Time::now();
// We should restart the pull for updates so it can run in parallel
data::LayerUpdates currentUpdates = pulledUpdates;
@@ -299,20 +299,20 @@ namespace armarx::viz
auto layerIDsBefore = getLayerIDs();
- IceUtil::Time time_afterPull = IceUtil::Time::now();
+ //IceUtil::Time time_afterPull = IceUtil::Time::now();
for (data::LayerUpdate const& update : currentUpdates.updates)
{
apply(update);
}
- IceUtil::Time time_afterApply = IceUtil::Time::now();
+ //IceUtil::Time time_afterApply = IceUtil::Time::now();
auto layerIDsAfter = getLayerIDs();
if (layerIDsAfter != layerIDsBefore)
{
emitLayersChanged(layerIDsAfter);
}
- IceUtil::Time time_afterLayersChanged = IceUtil::Time::now();
+ //IceUtil::Time time_afterLayersChanged = IceUtil::Time::now();
// Most of the time is spent in apply
// ARMARX_INFO << "Updates: " << pulledUpdates.updates.size()
diff --git a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
index 4a83bdedaabbabcc2f46bc4e2815a826ee661156..a1be9a1ff18a6d9c86a570b75d5b9079f9ae8b68 100644
--- a/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
+++ b/source/RobotAPI/components/DSObstacleAvoidance/DSObstacleAvoidance.cpp
@@ -549,17 +549,23 @@ armarx::DSObstacleAvoidance::run_visualization()
Layer layer_sms = arviz.layer("safety_margins");
Layer layer_rps = arviz.layer("reference_points");
Layer layer_bbs = arviz.layer("bounding_boxes");
- Color color = Color::red(255, 128);
- Color color_m = Color::red(255, 64);
-
- if (not m_doa.cfg.only_2d)
- {
- color = Color::blue(255, 128);
- color_m = Color::blue(255, 64);
- }
for (const obstacledetection::Obstacle& obstacle : getObstacles())
{
+ Color color = Color::orange(255, 128);
+ Color color_m = Color::orange(255, 64);
+
+ if (obstacle.name == "human")
+ {
+ color = Color::red(255, 128);
+ color_m = Color::red(255, 64);
+ }
+ else if (not m_doa.cfg.only_2d)
+ {
+ color = Color::blue(255, 128);
+ color_m = Color::blue(255, 64);
+ }
+
const double safetyMarginZ = m_doa.cfg.only_2d ? 1 : obstacle.safetyMarginZ;
const double posZ = m_doa.cfg.only_2d ? 1 : obstacle.posZ;
const double refPosZ = m_doa.cfg.only_2d ? 1 : obstacle.refPosZ;
@@ -574,6 +580,7 @@ armarx::DSObstacleAvoidance::run_visualization()
if (m_doa.cfg.only_2d)
{
+
layer_obs.add(Cylindroid{obstacle.name}
.pose(pose)
.axisLengths(dim.head<2>())
diff --git a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
index 46515a2c2f9a61acbaf63c186920478dab82cb1c..e712e30ccb2978fdc7caa0210b919773b7dce46b 100644
--- a/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
+++ b/source/RobotAPI/components/DynamicObstacleManager/DynamicObstacleManager.cpp
@@ -234,7 +234,9 @@ namespace armarx
void DynamicObstacleManager::wait_unitl_obstacles_are_ready(const Ice::Current&)
{
- usleep(m_min_value_for_accepting);
+ ARMARX_INFO << "Waiting for obstacles to get ready";
+ usleep(2.0 * m_min_value_for_accepting);
+ ARMARX_INFO << "Finished waiting for obstacles";
}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
index 38578c06d1ad3cc784896d377a3986bbc73c3e6a..1b2caffb7f5f818c18e1ee068b6e9d7e0745eabc 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
@@ -17,12 +17,8 @@ set(GUI_MOC_HDRS ${HEADERS})
set(GUI_UIS CartesianWaypointControlGuiWidget.ui)
set(COMPONENT_LIBS
- VirtualRobot
-
SimpleConfigDialog
-
- RobotAPIInterfaces
- RobotAPICore
+ RobotAPIComponentPlugins
)
if(ArmarXGui_FOUND)
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
index 5d2d12234ba03ac626e0579ed633b4699eae868d..5348bcaaa944c72d5b05f85afa0cd5253ca5ce9c 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
@@ -58,35 +58,27 @@ namespace armarx
void CartesianWaypointControlGuiWidgetController::loadSettings(QSettings* settings)
{
std::lock_guard g{_allMutex};
- _robotStateComponentName = settings->value("rsc", "Armar6StateComponent").toString().toStdString();
- _robotUnitName = settings->value("ru", "Armar6Unit").toString().toStdString();
+ getRobotStateComponentPlugin().setRobotStateComponentName(settings->value("rsc", "Armar6StateComponent").toString().toStdString());
+ getRobotUnitComponentPlugin().setRobotUnitName(settings->value("ru", "Armar6Unit").toString().toStdString());
}
void CartesianWaypointControlGuiWidgetController::saveSettings(QSettings* settings)
{
std::lock_guard g{_allMutex};
- settings->setValue("rsc", QString::fromStdString(_robotStateComponentName));
- settings->setValue("ru", QString::fromStdString(_robotUnitName));
+ settings->setValue("rsc", QString::fromStdString(getRobotStateComponentPlugin().getRobotStateComponentName()));
+ settings->setValue("ru", QString::fromStdString(getRobotUnitComponentPlugin().getRobotUnitName()));
}
void CartesianWaypointControlGuiWidgetController::onInitComponent()
{
- std::lock_guard g{_allMutex};
- usingProxy(_robotStateComponentName);
- usingProxy(_robotUnitName);
}
void CartesianWaypointControlGuiWidgetController::onConnectComponent()
{
std::lock_guard g{_allMutex};
- //proxies
- {
- _robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(_robotStateComponentName);
- _robotUnit = getProxy<RobotUnitInterfacePrx>(_robotUnitName);
- }
//robot
{
- _robot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent, VirtualRobot::RobotIO::eStructure);
+ _robot = addRobot("state robot", VirtualRobot::RobotIO::eStructure);
}
//fill rns combo box
{
@@ -135,7 +127,7 @@ namespace armarx
ARMARX_IMPORTANT << "Creating " << njointControllerClassName << " '"
<< _controllerName << "'";
_controller = NJointCartesianWaypointControllerInterfacePrx::checkedCast(
- _robotUnit->createNJointController(
+ getRobotUnit()->createNJointController(
njointControllerClassName,
_controllerName,
cfg));
@@ -199,8 +191,8 @@ namespace armarx
void CartesianWaypointControlGuiWidgetController::configured()
{
std::lock_guard g{_allMutex};
- _robotStateComponentName = _dialog->getProxyName("rsc");
- _robotUnitName = _dialog->getProxyName("ru");
+ getRobotStateComponentPlugin().setRobotStateComponentName(_dialog->getProxyName("rsc"));
+ getRobotUnitComponentPlugin().setRobotUnitName(_dialog->getProxyName("ru"));
}
void CartesianWaypointControlGuiWidgetController::triggerParsing()
@@ -283,11 +275,11 @@ namespace armarx
void CartesianWaypointControlGuiWidgetController::timerEvent(QTimerEvent*)
{
std::lock_guard g{_allMutex};
- if (!_robot || !_robotStateComponent)
+ if (!_robot)
{
return;
}
- RemoteRobot::synchronizeLocalClone(_robot, _robotStateComponent);
+ synchronizeLocalClone(_robot);
if (_controller)
{
ARMARX_INFO << deactivateSpam() << "setting visu gp to:\n" << _robot->getGlobalPose();
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
index ae6961634e680913480e1441c572af6f397aa15f..fc6b1936d3abe105a02c8fdf2ca72439dcf8d84f 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
@@ -34,6 +34,8 @@
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/gui-plugins/CartesianWaypointControlGui/ui_CartesianWaypointControlGuiWidget.h>
@@ -59,7 +61,9 @@ namespace armarx
*/
class ARMARXCOMPONENT_IMPORT_EXPORT
CartesianWaypointControlGuiWidgetController:
- public armarx::ArmarXComponentWidgetControllerTemplate < CartesianWaypointControlGuiWidgetController >
+ public armarx::ArmarXComponentWidgetControllerTemplate < CartesianWaypointControlGuiWidgetController >,
+ public virtual RobotUnitComponentPluginUser,
+ public virtual RobotStateComponentPluginUser
{
Q_OBJECT
@@ -105,10 +109,6 @@ namespace armarx
void timerEvent(QTimerEvent*) override;
private:
- std::string _robotStateComponentName;
- std::string _robotUnitName;
- RobotStateComponentInterfacePrx _robotStateComponent;
- RobotUnitInterfacePrx _robotUnit;
Ui::CartesianWaypointControlGuiWidget _ui;
QPointer<SimpleConfigDialog> _dialog;
NJointCartesianWaypointControllerInterfacePrx _controller;
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidget.ui b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidget.ui
index 8d4e7919a123d4fe736a68de36dc19a10f6daa84..8b4af8524ff6102e7230794f12fdaac64b07a876 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidget.ui
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidget.ui
@@ -14,12 +14,6 @@
<string>RobotUnitPluginWidget</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
<item>
<widget class="QSplitter" name="splitter">
<property name="orientation">
@@ -101,6 +95,116 @@
</property>
</layout>
</widget>
+ <widget class="QGroupBox" name="groupBoxLogging">
+ <property name="title">
+ <string>Logging</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0" colspan="2">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Filter</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEditLoggingFilter"/>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0" colspan="2">
+ <widget class="QTreeWidget" name="treeWidgetLoggingNames">
+ <column>
+ <property name="text">
+ <string/>
+ </property>
+ </column>
+ </widget>
+ </item>
+ <item row="3" column="0" colspan="2">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Save path</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="lineEditLoggingPath">
+ <property name="toolTip">
+ <string><html><head/><body><p>Accepts patterns:</p><p>{LEFT<span style=" color:#c0c0c0;"/>CURLY}<span style=" color:#c0c0c0;"/>-&gt;<span style=" color:#c0c0c0;"/>{</p><p><span style=" font-family:'monospace';">{RIGHT</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">CURLY}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">}</span></p><p><span style=" font-family:'monospace';">{year</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">/</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Year}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">year</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">(</span><span style=" font-family:'monospace'; color:#000080;">2</span><span style=" font-family:'monospace';">/</span><span style=" font-family:'monospace'; color:#000080;">4</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">digits)</span></p><p><span style=" font-family:'monospace';">{month}</span></p><p><span style=" font-family:'monospace';">{day}</span></p><p><span style=" font-family:'monospace';">{hour}</span></p><p><span style=" font-family:'monospace';">{minute}</span></p><p><span style=" font-family:'monospace';">{second}</span></p><p><span style=" font-family:'monospace';">{Date}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">YYYY-MM-DD</span></p><p><span style=" font-family:'monospace';">{Time}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">HH-MM-SS</span></p><p><span style=" font-family:'monospace';">{DateTime}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">{Date}_{Time}</span></p><p><span style=" font-family:'monospace';">{time-since-epoch}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">nanoseconds</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">since</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">epoch</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">(can</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">be</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">used</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">as</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">uuid)</span></p><p><span style=" font-family:'monospace';">{PackageDir:...}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Directory</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">of</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">the</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">given</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">package</span></p><p><span style=" font-family:'monospace';">{ScenarioDir:...}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Scenario</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Directory</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">of</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">the</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">given</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">package</span></p><p><span style=" font-family:'monospace';">{DataDir:...}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Data</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Directory</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">of</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">the</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">given</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">package</span></p><p><span style=" font-family:'monospace';">{BinaryeDir:...}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Binary</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Directory</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">of</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">the</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">given</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">package</span></p><p><span style=" font-family:'monospace';">{CMakeDir:...}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">CMake</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Directory</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">of</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">the</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">given</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">package</span></p><p><span style=" font-family:'monospace';">{BuildDir:...}</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">-&gt;</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Build</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">Directory</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">of</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">the</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">given</span><span style=" font-family:'monospace'; color:#c0c0c0;"/><span style=" font-family:'monospace';">package</span></p></body></html></string>
+ </property>
+ <property name="text">
+ <string>/tmp/logging/{DateTime}.csv</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButtonLoggingStart">
+ <property name="text">
+ <string>Start</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButtonLoggingStop">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="text">
+ <string>Stop</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="0" colspan="2">
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="0">
+ <widget class="QLineEdit" name="lineEditLoggingMark1">
+ <property name="text">
+ <string>Start</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLineEdit" name="lineEditLoggingMark2">
+ <property name="text">
+ <string>Stop</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLineEdit" name="lineEditLoggingMark3"/>
+ </item>
+ <item row="1" column="1">
+ <widget class="QPushButton" name="pushButtonLoggingMark2">
+ <property name="text">
+ <string>Set Mark</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="pushButtonLoggingMark1">
+ <property name="text">
+ <string>Set Mark</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QPushButton" name="pushButtonLoggingMark3">
+ <property name="text">
+ <string>Set Mark</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
</widget>
</item>
</layout>
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
index 076dfbbcebef9b02331fe6943fb7ba18943aa6c3..9f6229c5105a523109babbc9f2b7121ace039e42 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
@@ -24,13 +24,14 @@
#include "../QWidgets/StyleSheets.h"
#include <string>
-
+#include <regex>
#include <filesystem>
#include <QDir>
#include <QSortFilterProxyModel>
#include <QAction>
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -72,6 +73,21 @@ RobotUnitPluginWidgetController::RobotUnitPluginWidgetController()
nJointControllerClasses->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Expanding);
nJointControllerClasses->setVisible(false);
}
+ //update logging
+ {
+ widget.groupBoxLogging->setVisible(false);
+ widget.groupBoxLogging->setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::Expanding);
+
+ connect(widget.pushButtonLoggingStart, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingStart_clicked()));
+ connect(widget.pushButtonLoggingStop, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingStop_clicked()));
+ connect(widget.pushButtonLoggingMark1, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingMark1_clicked()));
+ connect(widget.pushButtonLoggingMark2, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingMark2_clicked()));
+ connect(widget.pushButtonLoggingMark3, SIGNAL(clicked()), this, SLOT(on_pushButtonLoggingMark3_clicked()));
+ connect(widget.lineEditLoggingFilter, SIGNAL(textChanged(const QString&)),
+ this, SLOT(on_lineEditLoggingFilter_textChanged(const QString&)));
+ connect(widget.treeWidgetLoggingNames, SIGNAL(itemChanged(QTreeWidgetItem*, int)),
+ this, SLOT(on_treeWidgetLoggingNames_itemChanged(QTreeWidgetItem*, int)));
+ }
updateToolBarActionCheckedState();
}
@@ -195,6 +211,14 @@ QPointer<QWidget> RobotUnitPluginWidgetController::getCustomTitlebarWidget(QWidg
customToolbar->addAction(showNJointClasses);
customToolbar->addSeparator();
+ customToolbar->addAction(QIcon(":/icons/view-refresh-7.png"), "", this, SLOT(refreshLogging()));
+ showLogging = new QAction {"Logging", customToolbar};
+ showLogging->setCheckable(true);
+ showLogging->setToolTip("Hide/Show the logging pane");
+ connect(showLogging, SIGNAL(toggled(bool)), widget.groupBoxLogging, SLOT(setVisible(bool)));
+ customToolbar->addAction(showLogging);
+ customToolbar->addSeparator();
+
customToolbar->addAction(QIcon(":/icons/document-save.svg"), "Write log", this, SLOT(writeLogClicked()))
->setToolTip("Writes the log to /tmp/");
}
@@ -285,6 +309,263 @@ void armarx::RobotUnitPluginWidgetController::timerEvent(QTimerEvent*)
refreshNJointControllerClassesClicked();
refreshControlDevicesClicked();
refreshSensorDevicesClicked();
+ refreshLogging();
killTimer(onConnectTimerId);
}
}
+
+void armarx::RobotUnitPluginWidgetController::refreshLogging()
+{
+ on_pushButtonLoggingStop_clicked();
+
+ widget.treeWidgetLoggingNames->clear();
+ loggingData.allItems.clear();
+
+ const auto allLoggingNames = robotUnitPrx->getLoggingNames();
+
+ std::map<std::string, QTreeWidgetItem*> separators;
+ auto add = [&](const std::string & name, const std::string & display, auto parent)
+ {
+ QTreeWidgetItem* i = new QTreeWidgetItem(parent);
+ i->setText(0, QString::fromStdString(display) + "*");
+ i->setData(0, Qt::ToolTipRole, QString::fromStdString(name));
+ separators[name] = i;
+ loggingData.allItems.emplace_back(i);
+ i->setCheckState(0, Qt::Unchecked);
+ };
+ add("", "", widget.treeWidgetLoggingNames);
+ loggingData.top = loggingData.allItems.front();
+
+ for (const auto& name : allLoggingNames)
+ {
+ const auto parts = Split(name, ".", true, true);
+ std::string reassembled;
+ for (const auto& p : parts)
+ {
+ ARMARX_CHECK_EXPRESSION(separators.count(reassembled));
+ auto parent = separators.at(reassembled);
+ reassembled += (reassembled.empty() ? p : "." + p) ;
+ if (!separators.count(reassembled))
+ {
+ add(reassembled, p, parent);
+ }
+ }
+ loggingData.allItems.back()->setText(0, QString::fromStdString(parts.back()));
+ }
+
+}
+
+void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStart_clicked()
+{
+ if (!robotUnitPrx || loggingData.handle)
+ {
+ return;
+ }
+ std::vector<std::string> loggingNames;
+ std::function<void(QTreeWidgetItem*)> recurseChildren = [&](auto it)
+ {
+ switch (it->checkState(0))
+ {
+ case Qt::Checked:
+ loggingNames.emplace_back(it->data(0, Qt::ToolTipRole).toString().toStdString());
+ break;
+ case Qt::Unchecked:
+ break;
+ case Qt::PartiallyChecked:
+ for (int i = 0; i < it->childCount(); ++i)
+ {
+ recurseChildren(it->child(i));
+ }
+ break;
+ }
+ };
+ recurseChildren(loggingData.top);
+ ARMARX_INFO << "start logging " << loggingNames;
+
+ loggingData.handle = robotUnitPrx->startRtLogging(
+ widget.lineEditLoggingPath->text().toStdString(),
+ loggingNames);
+ widget.pushButtonLoggingStart->setEnabled(false);
+ widget.pushButtonLoggingStop->setEnabled(true);
+ widget.pushButtonLoggingMark1->setEnabled(true);
+ widget.pushButtonLoggingMark2->setEnabled(true);
+ widget.pushButtonLoggingMark3->setEnabled(true);
+}
+
+void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingStop_clicked()
+{
+ widget.pushButtonLoggingStart->setEnabled(true);
+ widget.pushButtonLoggingStop->setEnabled(false);
+ widget.pushButtonLoggingMark1->setEnabled(false);
+ widget.pushButtonLoggingMark2->setEnabled(false);
+ widget.pushButtonLoggingMark3->setEnabled(false);
+ if (!loggingData.handle)
+ {
+ return;
+ }
+ robotUnitPrx->stopRtLogging(loggingData.handle);
+ loggingData.handle = nullptr;
+}
+
+void armarx::RobotUnitPluginWidgetController::on_lineEditLoggingFilter_textChanged(const QString& arg1)
+{
+ std::regex reg;
+ try
+ {
+ reg = std::regex{arg1.toStdString()};
+ }
+ catch (...)
+ {
+ //broken regex
+ widget.lineEditLoggingFilter->setStyleSheet("QLineEdit { background: rgb(255, 150, 150); selection-background-color: rgb(255, 0, 0); }");
+ return;
+ }
+ widget.lineEditLoggingFilter->setStyleSheet("QLineEdit {}");
+
+ std::function<void(QTreeWidgetItem* item)> setVisible = [&](auto it)
+ {
+ it->setHidden(false);
+ auto parent = it->parent();
+ //set parent state depending on its childrens state
+ if (!parent)
+ {
+ return;
+ }
+ setVisible(parent);
+ };
+ for (auto item : loggingData.allItems)
+ {
+ item->setHidden(true);
+ }
+ for (auto item : loggingData.allItems)
+ {
+ const auto str = item->data(0, Qt::ToolTipRole).toString().toStdString();
+ std::smatch match;
+ if (std::regex_search(str, match, reg))
+ {
+ setVisible(item);
+ }
+ }
+ widget.treeWidgetLoggingNames->blockSignals(true);
+ for (auto item : MakeReversedRange(loggingData.allItems))
+ {
+ loggingData.updateCheckStateUpward(item, false);
+ }
+ widget.treeWidgetLoggingNames->blockSignals(false);
+}
+
+void armarx::RobotUnitPluginWidgetController::on_treeWidgetLoggingNames_itemChanged(QTreeWidgetItem* item, int)
+{
+ widget.treeWidgetLoggingNames->blockSignals(true);
+ loggingData.updateCheckStateDownward(item, item->checkState(0), true);
+ loggingData.updateCheckStateUpward(item->parent(), true);
+ widget.treeWidgetLoggingNames->blockSignals(false);
+}
+
+void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark1_clicked()
+{
+ if (!loggingData.handle)
+ {
+ return;
+ }
+ const auto mark = widget.lineEditLoggingMark1->text().toStdString();
+ ARMARX_INFO << "set mark " << mark;
+ robotUnitPrx->addMarkerToRtLog(loggingData.handle, mark);
+}
+
+void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark2_clicked()
+{
+ if (!loggingData.handle)
+ {
+ return;
+ }
+ const auto mark = widget.lineEditLoggingMark2->text().toStdString();
+ ARMARX_INFO << "set mark " << mark;
+ robotUnitPrx->addMarkerToRtLog(loggingData.handle, mark);
+}
+
+void armarx::RobotUnitPluginWidgetController::on_pushButtonLoggingMark3_clicked()
+{
+ if (!loggingData.handle)
+ {
+ return;
+ }
+ const auto mark = widget.lineEditLoggingMark3->text().toStdString();
+ ARMARX_INFO << "set mark " << mark;
+ robotUnitPrx->addMarkerToRtLog(loggingData.handle, mark);
+}
+
+void RobotUnitPluginWidgetController::LoggingData::updateCheckStateUpward(QTreeWidgetItem* item, bool recurseParents)
+{
+ //set item state depending on its childrens state and maybe recurse parent
+ for (; item; item = recurseParents ? item->parent() : nullptr)
+ {
+ if (item->isHidden())
+ {
+ continue;
+ }
+ bool anyChecked = false;
+ bool anyUnchecked = false;
+ bool anyTri = false;
+ for (int i = 0; i < item->childCount(); ++i)
+ {
+ auto c = item->child(i);
+ if (c->isHidden())
+ {
+ continue;
+ }
+ switch (c->checkState(0))
+ {
+ case Qt::Checked:
+ anyChecked = true;
+ break;
+ case Qt::Unchecked:
+ anyUnchecked = true;
+ break;
+ case Qt::PartiallyChecked:
+ anyTri = true;
+ break;
+ }
+ if (anyChecked + anyUnchecked + anyTri > 1 || anyTri)
+ {
+ item->setCheckState(0, Qt::PartiallyChecked);
+ }
+ else if (anyChecked)
+ {
+ item->setCheckState(0, Qt::Checked);
+ }
+ else if (anyUnchecked)
+ {
+ item->setCheckState(0, Qt::Unchecked);
+ }
+ //case all false -> leaf -> do nothing
+ }
+ }
+}
+
+void RobotUnitPluginWidgetController::LoggingData::updateCheckStateDownward(
+ QTreeWidgetItem* item,
+ Qt::CheckState state,
+ bool recurseChildren)
+{
+ if (item->isHidden())
+ {
+ return;
+ }
+ item->setCheckState(0, state);
+
+ if (!recurseChildren)
+ {
+ return;
+ }
+ for (int i = 0; i < item->childCount(); ++i)
+ {
+ auto c = item->child(i);
+ if (c->isHidden())
+ {
+ continue;
+ }
+ c->setCheckState(0, state);
+ updateCheckStateDownward(c, state, true);
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
index 9b57ef2d510a9c2ce896e86a36dbab461e9e7a12..06270ecbc7e9c7280fac72f6dac959e504db26bd 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.h
@@ -132,11 +132,21 @@ namespace armarx
void refreshNJointControllerClassesClicked();
void refreshControlDevicesClicked();
void refreshSensorDevicesClicked();
+ void refreshLogging();
void writeLogClicked();
void startOnConnectTimer();
+ //logging
+ void on_lineEditLoggingFilter_textChanged(const QString& arg1);
+ void on_pushButtonLoggingStop_clicked();
+ void on_pushButtonLoggingStart_clicked();
+ void on_treeWidgetLoggingNames_itemChanged(QTreeWidgetItem* item, int column);
+ void on_pushButtonLoggingMark1_clicked();
+ void on_pushButtonLoggingMark2_clicked();
+ void on_pushButtonLoggingMark3_clicked();
+
private:
void timerEvent(QTimerEvent*) override;
void updateToolBarActionCheckedState();
@@ -162,6 +172,17 @@ namespace armarx
QPointer<QAction> showSDevs;
QPointer<QAction> showNJoint;
QPointer<QAction> showNJointClasses;
+ QPointer<QAction> showLogging;
+
+ struct LoggingData
+ {
+ QTreeWidgetItem* top;
+ std::vector<QTreeWidgetItem*> allItems;
+ RemoteReferenceCounterBasePtr handle;
+ static void updateCheckStateUpward(QTreeWidgetItem* item, bool recurseParents);
+ static void updateCheckStateDownward(QTreeWidgetItem* item, Qt::CheckState state, bool recurseChildren);
+ };
+ LoggingData loggingData;
int onConnectTimerId;
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
index 2707d344c94ae887ca725a970e1adc67257629b9..a00ef88cca841dec09a24ba64e1a3e7059ceb931 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointBimanualObjLevelController.ice
@@ -110,9 +110,17 @@ module armarx
// void setViaPoints(double canVal, Ice::DoubleSeq point);
void setGoals(Ice::DoubleSeq goals);
void setViaPoints(double u, Ice::DoubleSeq viapoint);
+ void removeAllViaPoints();
double getVirtualTime();
+ void setKpImpedance(Ice::FloatSeq value);
+ void setKdImpedance(Ice::FloatSeq value);
+ void setKmAdmittance(Ice::FloatSeq value);
+ void setKpAdmittance(Ice::FloatSeq value);
+ void setKdAdmittance(Ice::FloatSeq value);
+ Ice::FloatSeq getCurrentObjVel();
+ Ice::FloatSeq getCurrentObjForce();
};
diff --git a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
index d92ea1cf2a18c40c413699c4fae09f99b000558a..ddf9458379e26add5eb9c58ba1054306f5b5fd2e 100644
--- a/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
+++ b/source/RobotAPI/libraries/DMPController/TaskSpaceDMPController.cpp
@@ -321,7 +321,7 @@ void TaskSpaceDMPController::setViaPose(double canVal, const std::vector<double>
void TaskSpaceDMPController::removeAllViaPoints()
{
- // dmpPtr->removeViaPoints();
+ dmpPtr->removeViaPoints();
}
void TaskSpaceDMPController::prepareExecution(const Eigen::Matrix4f& currentPose, const Eigen::Matrix4f& goalPose)
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index 6567872ebd97ab3016b3af816f2048d473040464..eb2a743cc6435d7ab3d44a77dc8283a0e754138d 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -21,6 +21,8 @@ set(LIB_FILES
TrajectoryPlayerComponentPlugin.cpp
CartesianPositionControlComponentPlugin.cpp
NaturalIKComponentPlugin.cpp
+ HandUnitComponentPlugin.cpp
+ FrameTrackingComponentPlugin.cpp
)
set(LIB_HEADERS
RobotStateComponentPlugin.h
@@ -34,6 +36,8 @@ set(LIB_HEADERS
TrajectoryPlayerComponentPlugin.h
CartesianPositionControlComponentPlugin.h
NaturalIKComponentPlugin.h
+ HandUnitComponentPlugin.h
+ FrameTrackingComponentPlugin.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5ba6417ad1ee3c5a683a33209678c3465c76277
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "FrameTrackingComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void FrameTrackingComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void FrameTrackingComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_frameTracking, PROPERTY_NAME);
+ }
+
+ void FrameTrackingComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "FrameTracking",
+ "Name of the FrameTracking");
+ }
+ }
+
+ FrameTrackingInterfacePrx FrameTrackingComponentPlugin::getFrameTracking()
+ {
+ return _frameTracking;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ FrameTrackingComponentPluginUser::FrameTrackingComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ FrameTrackingInterfacePrx FrameTrackingComponentPluginUser::getFrameTracking()
+ {
+ return plugin->getFrameTracking();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..82b1b593d1c457b1b2c636a292ee0ce8aebce477
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/FrameTrackingComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/components/FrameTrackingInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class FrameTrackingComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ FrameTrackingInterfacePrx getFrameTracking();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "FrameTrackingName";
+ FrameTrackingInterfacePrx _frameTracking;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use FrameTracking.
+ */
+ class FrameTrackingComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ FrameTrackingComponentPluginUser();
+ FrameTrackingInterfacePrx getFrameTracking();
+
+ private:
+ armarx::plugins::FrameTrackingComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using FrameTrackingComponentPluginUser = armarx::FrameTrackingComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c99b6a9c2dafb560e00e86c28eee0108b650c888
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.cpp
@@ -0,0 +1,54 @@
+#include "HandUnitComponentPlugin.h"
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ void HandUnitComponentPlugin::preOnInitComponent()
+ {
+ parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
+ }
+
+ void HandUnitComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxyFromProperty(_handUnit, PROPERTY_NAME);
+ }
+
+ void HandUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
+ {
+ properties->defineOptionalProperty<std::string>(
+ PROPERTY_NAME,
+ "HandUnit",
+ "Name of the HandUnit");
+ }
+ }
+
+ HandUnitInterfacePrx HandUnitComponentPlugin::getHandUnit()
+ {
+ return _handUnit;
+ }
+
+
+ }
+}
+
+namespace armarx
+{
+
+ HandUnitComponentPluginUser::HandUnitComponentPluginUser()
+ {
+ addPlugin(plugin);
+ }
+
+ HandUnitInterfacePrx HandUnitComponentPluginUser::getHandUnit()
+ {
+ return plugin->getHandUnit();
+ }
+
+
+}
+
+
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..4cd3593cb95abd83e023e8aa5642b68e23bea3fb
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HandUnitComponentPlugin.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+
+ class HandUnitComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ void preOnInitComponent() override;
+
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ HandUnitInterfacePrx getHandUnit();
+
+ private:
+ static constexpr const char* PROPERTY_NAME = "HandUnitName";
+ HandUnitInterfacePrx _handUnit;
+ };
+
+ }
+}
+
+namespace armarx
+{
+ /**
+ * @brief Provides a ready-to-use HandUnit.
+ */
+ class HandUnitComponentPluginUser : virtual public ManagedIceObject
+ {
+ public:
+ HandUnitComponentPluginUser();
+ HandUnitInterfacePrx getHandUnit();
+
+ private:
+ armarx::plugins::HandUnitComponentPlugin* plugin = nullptr;
+ };
+
+}
+
+
+namespace armarx
+{
+ namespace plugins
+ {
+ // Legacy typedef.
+ using HandUnitComponentPluginUser = armarx::HandUnitComponentPluginUser;
+ }
+}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
index c5445458598bfcfe4ea427a4f519f6d5baacddaf..3f28f11c839e988564882fe679eaec47b74cb649 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.cpp
@@ -199,9 +199,14 @@ namespace armarx::plugins
void RobotStateComponentPlugin::preOnInitComponent()
{
+ if (!_robotStateComponent && _robotStateComponentName.empty())
+ {
+ parent<Component>().getProperty(_robotStateComponentName, makePropertyName(_propertyName));
+ }
+
if (!_robotStateComponent)
{
- parent<Component>().usingProxyFromProperty(makePropertyName(_propertyName));
+ parent<Component>().usingProxy(_robotStateComponentName);
}
}
@@ -209,7 +214,7 @@ namespace armarx::plugins
{
if (!_robotStateComponent)
{
- parent<Component>().getProxyFromProperty(_robotStateComponent, makePropertyName(_propertyName));
+ parent<Component>().getProxy(_robotStateComponent, _robotStateComponentName);
}
}
@@ -242,6 +247,18 @@ namespace armarx::plugins
ARMARX_CHECK_NOT_NULL(node) << "No tcp configured for the robot ";
return SimpleDiffIK::CalculateReachability(targets, initialJV, rns, node, params);
}
+
+ void RobotStateComponentPlugin::setRobotStateComponentName(const std::string& name)
+ {
+ ARMARX_CHECK_NOT_EMPTY(name);
+ ARMARX_CHECK_EMPTY(_robotStateComponentName);
+ _robotStateComponentName = name;
+ }
+
+ const std::string& RobotStateComponentPlugin::getRobotStateComponentName() const
+ {
+ return _robotStateComponentName;
+ }
}
namespace armarx
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
index a338957d196d166494ad031ed3280abeef47b899..25ca57fffc2155f8fc88114de62c0836b206c02e 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h
@@ -53,6 +53,8 @@ namespace armarx::plugins
struct RobotData;
using ComponentPlugin::ComponentPlugin;
+ void setRobotStateComponentName(const std::string& name);
+ const std::string& getRobotStateComponentName() const;
//get / add
public:
bool hasRobot(const std::string& id) const;
@@ -137,6 +139,7 @@ namespace armarx::plugins
//data
public:
static constexpr auto _propertyName = "RemoteStateComponentName";
+ std::string _robotStateComponentName;
RobotStateComponentInterfacePrx _robotStateComponent;
mutable std::recursive_mutex _robotsMutex;
std::map<std::string, RobotData> _robots;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 383de616a6001cd7358a2fd5604c44f7e669ecc6..1e94a395814376eb08b20e45f5d043f21fa6dbb4 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -1,53 +1,64 @@
#include "RobotUnitComponentPlugin.h"
-namespace armarx
+namespace armarx::plugins
{
- namespace plugins
+ void RobotUnitComponentPlugin::preOnInitComponent()
{
-
- void RobotUnitComponentPlugin::preOnInitComponent()
- {
- parent<Component>().usingProxyFromProperty(PROPERTY_NAME);
- }
-
- void RobotUnitComponentPlugin::preOnConnectComponent()
+ if (_robotUnitName.empty())
{
- parent<Component>().getProxyFromProperty(_robotUnit, PROPERTY_NAME);
+ parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
}
+ parent<Component>().usingProxy(_robotUnitName);
+ }
- void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
- {
- if (!properties->hasDefinition(PROPERTY_NAME))
- {
- properties->defineRequiredProperty<std::string>(
- PROPERTY_NAME,
- "Name of the RobotUnit");
- }
- }
+ void RobotUnitComponentPlugin::preOnConnectComponent()
+ {
+ parent<Component>().getProxy(_robotUnit, _robotUnitName);
+ }
- RobotUnitInterfacePrx RobotUnitComponentPlugin::getRobotUnit()
+ void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(PROPERTY_NAME))
{
- return _robotUnit;
+ properties->defineRequiredProperty<std::string>(
+ PROPERTY_NAME,
+ "Name of the RobotUnit");
}
+ }
+ RobotUnitInterfacePrx RobotUnitComponentPlugin::getRobotUnit() const
+ {
+ return _robotUnit;
+ }
+ void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
+ {
+ ARMARX_CHECK_NOT_EMPTY(name);
+ ARMARX_CHECK_EMPTY(_robotUnitName);
+ _robotUnitName = name;
+ }
+ const std::string& RobotUnitComponentPlugin::getRobotUnitName() const
+ {
+ return _robotUnitName;
}
}
namespace armarx
{
-
RobotUnitComponentPluginUser::RobotUnitComponentPluginUser()
{
addPlugin(plugin);
}
- RobotUnitInterfacePrx RobotUnitComponentPluginUser::getRobotUnit()
+ RobotUnitInterfacePrx RobotUnitComponentPluginUser::getRobotUnit() const
{
return plugin->getRobotUnit();
}
-
+ plugins::RobotUnitComponentPlugin& RobotUnitComponentPluginUser::getRobotUnitComponentPlugin()
+ {
+ return *plugin;
+ }
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 4dc53dcb9fff70332663f1dd4a0211f3fdfd916d..b3cbf9a3b1e43d262370fc3b6082d07612a9c7cc 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -20,11 +20,14 @@ namespace armarx
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
- RobotUnitInterfacePrx getRobotUnit();
+ RobotUnitInterfacePrx getRobotUnit() const;
+ void setRobotUnitName(const std::string& name);
+ const std::string& getRobotUnitName() const;
private:
static constexpr const char* PROPERTY_NAME = "RobotUnitName";
RobotUnitInterfacePrx _robotUnit;
+ std::string _robotUnitName;
};
}
@@ -39,8 +42,8 @@ namespace armarx
{
public:
RobotUnitComponentPluginUser();
- RobotUnitInterfacePrx getRobotUnit();
-
+ RobotUnitInterfacePrx getRobotUnit() const;
+ plugins::RobotUnitComponentPlugin& getRobotUnitComponentPlugin();
private:
armarx::plugins::RobotUnitComponentPlugin* plugin = nullptr;
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
index 7f5f67758fe368038d697df0beda4f96b22af98a..37cf71bfc6ab71a1eef4c148be3d16dbc60cffdb 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.cpp
@@ -105,6 +105,7 @@ namespace armarx
tcpLeft = leftRNS->getTCP();
tcpRight = rightRNS->getTCP();
+ //initialize control parameters
KpImpedance.setZero(cfg->KpImpedance.size());
ARMARX_CHECK_EQUAL(cfg->KpImpedance.size(), 12);
@@ -145,6 +146,15 @@ namespace armarx
KmAdmittance(i) = cfg->KmAdmittance.at(i);
}
+
+ Inferface2rtData initInt2rtData;
+ initInt2rtData.KpImpedance = KpImpedance;
+ initInt2rtData.KdImpedance = KdImpedance;
+ initInt2rtData.KmAdmittance = KmAdmittance;
+ initInt2rtData.KpAdmittance = KpAdmittance;
+ initInt2rtData.KdAdmittance = KdAdmittance;
+ interface2rtBuffer.reinitAllBuffers(initInt2rtData);
+
leftDesiredJointValues.resize(leftTargets.size());
ARMARX_CHECK_EQUAL(cfg->leftDesiredJointValues.size(), leftTargets.size());
@@ -192,6 +202,9 @@ namespace armarx
ControlInterfaceData initInterfaceData;
initInterfaceData.currentLeftPose = tcpLeft->getPoseInRootFrame();
initInterfaceData.currentRightPose = tcpRight->getPoseInRootFrame();
+ initInterfaceData.currentObjPose = boxInitialPose;
+ initInterfaceData.currentObjForce.setZero();
+ initInterfaceData.currentObjVel.setZero();
controlInterfaceBuffer.reinitAllBuffers(initInterfaceData);
@@ -255,6 +268,10 @@ namespace armarx
{
targetWrench(i) = cfg->targetWrench[i];
}
+
+
+
+
}
@@ -315,7 +332,6 @@ namespace armarx
controlInterfaceBuffer.getWriteBuffer().currentLeftPose = currentLeftPose;
controlInterfaceBuffer.getWriteBuffer().currentRightPose = currentRightPose;
- controlInterfaceBuffer.commitWrite();
double deltaT = timeSinceLastIteration.toSecondsDouble();
ftcalibrationTimer += deltaT;
@@ -343,6 +359,13 @@ namespace armarx
ARMARX_INFO << "modified right pose:\n " << rightPose;
}
+ // --------------------------------------------- get control parameters ---------------------------------------
+ KpImpedance = interface2rtBuffer.getUpToDateReadBuffer().KpImpedance;
+ KdImpedance = interface2rtBuffer.getUpToDateReadBuffer().KdImpedance;
+ KmAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KmAdmittance;
+ KpAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KpAdmittance;
+ KdAdmittance = interface2rtBuffer.getUpToDateReadBuffer().KdAdmittance;
+
if (ftcalibrationTimer < cfg->ftCalibrationTime)
{
ftOffset.block<3, 1>(0, 0) = 0.5 * ftOffset.block<3, 1>(0, 0) + 0.5 * leftForceTorque->force;
@@ -454,6 +477,8 @@ namespace armarx
rt2ControlBuffer.getWriteBuffer().currentTime += deltaT;
rt2ControlBuffer.commitWrite();
+
+
// --------------------------------------------- get ft sensor ---------------------------------------------
// static compensation
Eigen::Vector3f gravity;
@@ -504,11 +529,20 @@ namespace armarx
}
}
+ // pass sensor value to statechart
+ controlInterfaceBuffer.getWriteBuffer().currentObjPose = boxCurrentPose;
+ controlInterfaceBuffer.getWriteBuffer().currentObjVel = boxCurrentTwist.head(3);
+ controlInterfaceBuffer.getWriteBuffer().currentObjForce = objFTValue.head(3);
+ controlInterfaceBuffer.commitWrite();
+
// --------------------------------------------- get MP target ---------------------------------------------
Eigen::Matrix4f boxPose = rtGetControlStruct().boxPose;
Eigen::VectorXf boxTwist = rtGetControlStruct().boxTwist;
+
+
+
// --------------------------------------------- obj admittance control ---------------------------------------------
// admittance
Eigen::VectorXf objPoseError(6);
@@ -744,9 +778,109 @@ namespace armarx
// LockGuardType guard(controllerMutex);
ARMARX_INFO << "setting via points ";
objectDMP->setViaPose(u, viapoint);
+ }
+
+ void NJointBimanualObjLevelController::removeAllViaPoints(const Ice::Current&)
+ {
+ objectDMP->removeAllViaPoints();
+ }
+
+ void NJointBimanualObjLevelController::setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 12);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KpImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+
+ }
+
+ void NJointBimanualObjLevelController::setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 12);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KdImpedance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ void NJointBimanualObjLevelController::setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KpAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
}
+ void NJointBimanualObjLevelController::setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KdAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+ std::vector<float> NJointBimanualObjLevelController::getCurrentObjVel(const Ice::Current&)
+ {
+ Eigen::Vector3f tvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjVel;
+ std::vector<float> tvelvec = {tvel(0), tvel(1), tvel(2)};
+ return tvelvec;
+ }
+
+ std::vector<float> NJointBimanualObjLevelController::getCurrentObjForce(const Ice::Current&)
+ {
+ Eigen::Vector3f fvel = controlInterfaceBuffer.getUpToDateReadBuffer().currentObjForce;
+ std::vector<float> fvelvec = {fvel(0), fvel(1), fvel(2)};
+ return fvelvec;
+ }
+
+ void NJointBimanualObjLevelController::setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&)
+ {
+ Eigen::VectorXf setpoint;
+ setpoint.setZero(value.size());
+ ARMARX_CHECK_EQUAL(value.size(), 6);
+
+ for (size_t i = 0; i < value.size(); ++i)
+ {
+ setpoint(i) = value.at(i);
+ }
+
+ LockGuardType guard {interfaceDataMutex};
+ interface2rtBuffer.getWriteBuffer().KmAdmittance = setpoint;
+ interface2rtBuffer.commitWrite();
+ }
+
+
void NJointBimanualObjLevelController::onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx& debugObs)
{
@@ -891,6 +1025,8 @@ namespace armarx
void NJointBimanualObjLevelController::onInitNJointController()
{
+
+
ARMARX_INFO << "init ...";
runTask("NJointBimanualObjLevelController", [&]
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
index 7f1c10d711309e81271887feaade945462c23aab..341de9689094e1eee9c9a497c638f79fd2bbfa1d 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/BimanualForceControllers/NJointBimanualObjLevelController.h
@@ -70,11 +70,22 @@ namespace armarx
void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&);
void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&);
+ void removeAllViaPoints(const Ice::Current&);
+
double getVirtualTime(const Ice::Current&)
{
return virtualtimer;
}
+ void setKpImpedance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKdImpedance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKmAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKpAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+ void setKdAdmittance(const Ice::FloatSeq& value, const Ice::Current&);
+
+ std::vector<float> getCurrentObjVel(const Ice::Current&);
+ std::vector<float> getCurrentObjForce(const Ice::Current&);
+
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&);
@@ -175,10 +186,25 @@ namespace armarx
{
Eigen::Matrix4f currentLeftPose;
Eigen::Matrix4f currentRightPose;
+ Eigen::Matrix4f currentObjPose;
+ Eigen::Vector3f currentObjVel;
+ Eigen::Vector3f currentObjForce;
};
TripleBuffer<ControlInterfaceData> controlInterfaceBuffer;
+ struct Inferface2rtData
+ {
+ Eigen::VectorXf KpImpedance;
+ Eigen::VectorXf KdImpedance;
+ Eigen::VectorXf KmAdmittance;
+ Eigen::VectorXf KpAdmittance;
+ Eigen::VectorXf KdAdmittance;
+ };
+ TripleBuffer<Inferface2rtData> interface2rtBuffer;
+
+
+
std::vector<ControlTarget1DoFActuatorTorque*> leftTargets;
std::vector<const SensorValue1DoFActuatorAcceleration*> leftAccelerationSensors;
std::vector<const SensorValue1DoFActuatorVelocity*> leftVelocitySensors;
@@ -203,6 +229,7 @@ namespace armarx
double virtualtimer;
mutable MutexType controllerMutex;
+ mutable MutexType interfaceDataMutex;
Eigen::VectorXf leftDesiredJointValues;
Eigen::VectorXf rightDesiredJointValues;