From c7d3cf73ab973e606bf8e71e8a39e5e7f0c13046 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Mon, 30 Sep 2019 16:11:40 +0200
Subject: [PATCH] Improve NJointCartesianWaypointController (it optionally
keeps optimizing the Nullspace even if the target was reached)
Also rename CartesianWaypointControllerConfigWithForceLimit to NJointCartesianWaypointControllerRuntimeConfig
---
.../NJointCartesianWaypointController.cpp | 12 +++++++-----
.../NJointCartesianWaypointController.h | 9 +++++----
...artesianWaypointControlGuiWidgetController.cpp | 4 ++--
.../CartesianWaypointControlGuiWidgetController.h | 2 +-
.../NJointCartesianWaypointController.ice | 11 ++++++-----
.../RemoteGui.cpp | 15 ++++++++++++---
.../RemoteGui.h | 4 ++--
7 files changed, 35 insertions(+), 22 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 0078a0e05..edb5b230b 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -8,7 +8,7 @@
namespace armarx
{
- std::ostream& operator<<(std::ostream& out, const CartesianWaypointControllerConfigWithForceLimit& cfg)
+ std::ostream& operator<<(std::ostream& out, const NJointCartesianWaypointControllerRuntimeConfig& cfg)
{
out << "maxPositionAcceleration " << cfg.wpCfg.maxPositionAcceleration << '\n'
<< "maxOrientationAcceleration " << cfg.wpCfg.maxOrientationAcceleration << '\n'
@@ -137,6 +137,7 @@ namespace armarx
_rtWpController->setConfig(r.cfg.wpCfg);
_rtForceThreshold = r.cfg.forceThreshold;
_publishForceThreshold = _rtForceThreshold;
+ _rtOptimizeNullspaceIfTargetWasReached = r.cfg.optimizeNullspaceIfTargetWasReached;
ARMARX_RT_LOGF_IMPORTANT("updated the controller config");
}
if (r.wpsUpdated)
@@ -203,7 +204,8 @@ namespace armarx
{
rt2nonrtBuf.tcpTarg = rt2nonrtBuf.tcp;
}
- _rtStopConditionReached = _rtStopConditionReached || reachedFtLimit || reachedTarget;
+ _rtStopConditionReached = _rtStopConditionReached || reachedFtLimit ||
+ (reachedTarget && !_rtOptimizeNullspaceIfTargetWasReached);
_publishIsAtTarget = reachedTarget;
if (!_rtHasWps || _rtStopConditionReached)
{
@@ -231,7 +233,7 @@ namespace armarx
{
return _publishIsAtTarget;
}
- void NJointCartesianWaypointController::setConfig(const CartesianWaypointControllerConfigWithForceLimit& cfg, const Ice::Current&)
+ void NJointCartesianWaypointController::setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current&)
{
std::lock_guard g{_tripBufWpCtrlMut};
auto& w = _tripBufWpCtrl.getWriteBuffer();
@@ -262,7 +264,7 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new waypoint\n" << wp;
}
- void NJointCartesianWaypointController::setConfigAndWaypoints(const CartesianWaypointControllerConfigWithForceLimit& cfg,
+ void NJointCartesianWaypointController::setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
const std::vector<Eigen::Matrix4f>& wps,
const Ice::Current&)
{
@@ -275,7 +277,7 @@ namespace armarx
_tripBufWpCtrl.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg << " and new waypoints\n" << wps;
}
- void NJointCartesianWaypointController::setConfigAndWaypoint(const CartesianWaypointControllerConfigWithForceLimit& cfg,
+ void NJointCartesianWaypointController::setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg,
const Eigen::Matrix4f& wp,
const Ice::Current&)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index d16afbcbe..af526869c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -52,11 +52,11 @@ namespace armarx
bool hasReachedTarget(const Ice::Current& = Ice::emptyCurrent) override;
bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
- void setConfig(const CartesianWaypointControllerConfigWithForceLimit& cfg, const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfig(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
void setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
void setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
- void setConfigAndWaypoints(const CartesianWaypointControllerConfigWithForceLimit& cfg, const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
- void setConfigAndWaypoint(const CartesianWaypointControllerConfigWithForceLimit& cfg, const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfigAndWaypoints(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const std::vector<Eigen::Matrix4f>& wps, const Ice::Current& = Ice::emptyCurrent) override;
+ void setConfigAndWaypoint(const NJointCartesianWaypointControllerRuntimeConfig& cfg, const Eigen::Matrix4f& wp, const Ice::Current& = Ice::emptyCurrent) override;
void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
FTSensorValue getFTSensorValue(const Ice::Current& = Ice::emptyCurrent) override;
@@ -73,7 +73,7 @@ namespace armarx
struct CtrlData
{
std::vector<Eigen::Matrix4f> wps;
- CartesianWaypointControllerConfigWithForceLimit cfg;
+ NJointCartesianWaypointControllerRuntimeConfig cfg;
float skipRad2mmFactor = 500;
bool wpsUpdated = false;
bool cfgUpdated = false;
@@ -108,6 +108,7 @@ namespace armarx
Eigen::Vector3f _rtForceOffset;
float _rtForceThreshold = -1;
+ bool _rtOptimizeNullspaceIfTargetWasReached = false;
bool _rtHasWps = false;
bool _rtStopConditionReached = false;
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
index 93bb8b33a..06495d829 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
@@ -217,9 +217,9 @@ namespace armarx
_ui.labelParsingSuccess->setText("Parsed " + QString::number(_lastParsedWPs.size()) + " waypoints.");
}
- CartesianWaypointControllerConfigWithForceLimit CartesianWaypointControlGuiWidgetController::readRunCfg() const
+ NJointCartesianWaypointControllerRuntimeConfig CartesianWaypointControlGuiWidgetController::readRunCfg() const
{
- CartesianWaypointControllerConfigWithForceLimit cfg;
+ NJointCartesianWaypointControllerRuntimeConfig cfg;
cfg.wpCfg.maxPositionAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
cfg.wpCfg.maxOrientationAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
index 4816a63fe..e8cf03487 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
@@ -103,7 +103,7 @@ namespace armarx
void triggerParsing();
private:
- CartesianWaypointControllerConfigWithForceLimit readRunCfg() const;
+ NJointCartesianWaypointControllerRuntimeConfig readRunCfg() const;
void deleteOldController();
private:
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
index 765afbb10..00a1419fc 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -29,10 +29,11 @@
module armarx
{
- struct CartesianWaypointControllerConfigWithForceLimit
+ struct NJointCartesianWaypointControllerRuntimeConfig
{
CartesianWaypointControllerConfig wpCfg;
float forceThreshold = -1; // < 0 -> no limit
+ bool optimizeNullspaceIfTargetWasReached = false;
};
struct FTSensorValue
@@ -45,7 +46,7 @@ module armarx
class NJointCartesianWaypointControllerConfig extends NJointControllerConfig
{
string rns;
- CartesianWaypointControllerConfigWithForceLimit runCfg;
+ NJointCartesianWaypointControllerRuntimeConfig runCfg;
string ftName; //optional
};
@@ -54,11 +55,11 @@ module armarx
idempotent bool hasReachedTarget();
idempotent bool hasReachedForceLimit();
- void setConfig(CartesianWaypointControllerConfigWithForceLimit cfg);
+ void setConfig(NJointCartesianWaypointControllerRuntimeConfig cfg);
void setWaypoints(Eigen::Matrix4fSeq wps);
void setWaypoint(Eigen::Matrix4f wp);
- void setConfigAndWaypoints(CartesianWaypointControllerConfigWithForceLimit cfg, Eigen::Matrix4fSeq wps);
- void setConfigAndWaypoint(CartesianWaypointControllerConfigWithForceLimit cfg, Eigen::Matrix4f wp);
+ void setConfigAndWaypoints(NJointCartesianWaypointControllerRuntimeConfig cfg, Eigen::Matrix4fSeq wps);
+ void setConfigAndWaypoint(NJointCartesianWaypointControllerRuntimeConfig cfg, Eigen::Matrix4f wp);
FTSensorValue getFTSensorValue();
void setCurrentFTAsOffset();
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
index 2bd5c3cae..7998ef600 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
+++ b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.cpp
@@ -25,10 +25,11 @@ namespace armarx::RemoteGui
{
detail::GroupBoxBuilder makeConfigGui(
const std::string& name,
- const CartesianWaypointControllerConfigWithForceLimit& val)
+ const NJointCartesianWaypointControllerRuntimeConfig& val)
{
detail::GroupBoxBuilder builder = makeConfigGui(name, val.wpCfg);
auto& cs = builder.child(0)->children;
+
cs.emplace_back(RemoteGui::makeTextLabel("Force limit:"));
cs.emplace_back(RemoteGui::makeFloatSpinBox(name + "_forceThreshold")
.min(-1)
@@ -36,17 +37,25 @@ namespace armarx::RemoteGui
.value(val.forceThreshold)
.decimals(3));
cs.emplace_back(new RemoteGui::HSpacer);
+
+ cs.emplace_back(new RemoteGui::Widget);
+ cs.emplace_back(RemoteGui::makeCheckBox(name + "_optimizeNullspaceIfTargetWasReached")
+ .value(val.optimizeNullspaceIfTargetWasReached)
+ .label("Optimize nullspace if target was reached"));
+ cs.emplace_back(new RemoteGui::HSpacer);
+
return builder;
}
template <>
- CartesianWaypointControllerConfigWithForceLimit getValue<CartesianWaypointControllerConfigWithForceLimit>(
+ NJointCartesianWaypointControllerRuntimeConfig getValue<NJointCartesianWaypointControllerRuntimeConfig>(
ValueMap const& values,
std::string const& name)
{
- CartesianWaypointControllerConfigWithForceLimit cfg;
+ NJointCartesianWaypointControllerRuntimeConfig cfg;
getValue(cfg.wpCfg, values, name);
getValue(cfg.forceThreshold, values, name + "_forceThreshold");
+ getValue(cfg.optimizeNullspaceIfTargetWasReached, values, name + "_optimizeNullspaceIfTargetWasReached");
return cfg;
}
}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
index 4951d5efa..3f5f2b121 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
+++ b/source/RobotAPI/libraries/ControllerUIUtility/NJointCartesianWaypointControllerConfig/RemoteGui.h
@@ -30,10 +30,10 @@ namespace armarx::RemoteGui
{
detail::GroupBoxBuilder makeConfigGui(
const std::string& name,
- const CartesianWaypointControllerConfigWithForceLimit& val);
+ const NJointCartesianWaypointControllerRuntimeConfig& val);
template <>
- CartesianWaypointControllerConfigWithForceLimit getValue<CartesianWaypointControllerConfigWithForceLimit>(
+ NJointCartesianWaypointControllerRuntimeConfig getValue<NJointCartesianWaypointControllerRuntimeConfig>(
ValueMap const& values,
std::string const& name);
}
--
GitLab