diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 0078a0e05e3743fd076f257e7a45777ca8253873..edb5b230b98170649235f345ad416aaba70d3140 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 d16afbcbe35a604e85dba2ff80972db59fb3b227..af526869cc86ce553e84c1dd772f5985deb622da 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 93bb8b33a3d60d24e224475cf21ac35e15f881f0..06495d8290f40fc5dd14aebabe6460262cf9714b 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 4816a63feab9995b559f27e5a32a541a26dcf8d3..e8cf03487dd0d30be29158713bd1ccd2a9e943b3 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 765afbb10068a59ecec091e416dfdff73b0d2ea7..00a1419fca440de821d31c407d32dbfbee9bef4f 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 2bd5c3cae52f6bb9ccc245eacd162e14b06e4458..7998ef600a583592b7aae38022f9f19435291df8 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 4951d5efac47f1197b36a8c99379962b6524128b..3f5f2b12167071338a4ca2384c4f1ff840be6d7b 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);
}