From e1ab835b5abaedad74cbfb9442f0b289da350736 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Mon, 2 Sep 2019 17:14:22 +0200
Subject: [PATCH] Added NJointCartesianWaypointController and a gui for it
* Added NJointCartesianWaypointController
* Added CartesianWaypointControlGuiGuiPlugin
* changed CartesianWaypointControllerConfig to include the acc ramps
---
etc/doxygen/pages/GuiPlugins.dox | 2 +
.../components/units/RobotUnit/CMakeLists.txt | 2 +
...intCartesianVelocityControllerWithRamp.cpp | 6 +
.../NJointCartesianWaypointController.cpp | 329 +++++++++++++++++
.../NJointCartesianWaypointController.h | 120 +++++++
source/RobotAPI/gui-plugins/CMakeLists.txt | 3 +-
.../CMakeLists.txt | 30 ++
.../CartesianWaypointControlGuiGuiPlugin.cpp | 33 ++
.../CartesianWaypointControlGuiGuiPlugin.h | 50 +++
.../CartesianWaypointControlGuiWidget.ui | 335 ++++++++++++++++++
...sianWaypointControlGuiWidgetController.cpp | 254 +++++++++++++
...tesianWaypointControlGuiWidgetController.h | 124 +++++++
.../MatrixDatafieldDisplayWidget.cpp | 1 -
source/RobotAPI/interface/CMakeLists.txt | 1 +
.../CartesianWaypointControllerConfig.ice | 16 +-
.../NJointCartesianWaypointController.ice | 67 ++++
.../CartesianVelocityControllerWithRamp.cpp | 3 +
.../CartesianVelocityControllerWithRamp.h | 1 +
.../core/CartesianWaypointController.cpp | 13 +
.../core/CartesianWaypointController.h | 6 +-
20 files changed, 1384 insertions(+), 12 deletions(-)
create mode 100644 source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
create mode 100644 source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
create mode 100644 source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
create mode 100644 source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp
create mode 100644 source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h
create mode 100644 source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
create mode 100644 source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
create mode 100644 source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
create mode 100644 source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
diff --git a/etc/doxygen/pages/GuiPlugins.dox b/etc/doxygen/pages/GuiPlugins.dox
index b8922c0ea..e090dfd48 100644
--- a/etc/doxygen/pages/GuiPlugins.dox
+++ b/etc/doxygen/pages/GuiPlugins.dox
@@ -26,4 +26,6 @@ The following Gui Plugins are available:
\subpage ArmarXGui-GuiPlugins-RobotUnitPlugin
\subpage ArmarXGui-GuiPlugins-GuiHealthClient
+
+\subpage RobotAPI-GuiPlugins-CartesianWaypointControlGui
*/
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 60ce76b94..2fbf0d924 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -49,6 +49,7 @@ set(LIB_FILES
NJointControllers/NJointCartesianTorqueController.cpp
NJointControllers/NJointTaskSpaceImpedanceController.cpp
NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+ NJointControllers/NJointCartesianWaypointController.cpp
#NJointControllers/NJointCartesianActiveImpedanceController.cpp
Devices/SensorDevice.cpp
@@ -117,6 +118,7 @@ set(LIB_HEADERS
NJointControllers/NJointCartesianVelocityController.h
NJointControllers/NJointCartesianTorqueController.h
NJointControllers/NJointCartesianVelocityControllerWithRamp.h
+ NJointControllers/NJointCartesianWaypointController.h
#NJointControllers/NJointCartesianActiveImpedanceController.h
Devices/DeviceBase.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 0d3d1b5b4..6edb465ad 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -85,12 +85,15 @@ namespace armarx
void NJointCartesianVelocityControllerWithRamp::rtPreActivateController()
{
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
Eigen::VectorXf currentJointVelocities(velocitySensors.size());
for (size_t i = 0; i < velocitySensors.size(); i++)
{
currentJointVelocities(i) = *velocitySensors.at(i);
}
controller->setCurrentJointVelocity(currentJointVelocities);
+#pragma GCC diagnostic pop
ARMARX_IMPORTANT << "initial joint velocities: " << currentJointVelocities.transpose();
}
@@ -159,7 +162,10 @@ namespace armarx
void NJointCartesianVelocityControllerWithRamp::immediateHardStop(const Ice::Current&)
{
LockGuardType guard {controlDataMutex};
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
controller->setCurrentJointVelocity(Eigen::VectorXf::Zero(velocitySensors.size()));
+#pragma GCC diagnostic pop
getWriterControlStruct().xVel = 0;
getWriterControlStruct().yVel = 0;
getWriterControlStruct().zVel = 0;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
new file mode 100644
index 000000000..c0f082947
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -0,0 +1,329 @@
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+
+#include "NJointCartesianWaypointController.h"
+
+namespace armarx
+{
+
+ NJointControllerRegistration<NJointCartesianWaypointController> registrationControllerNJointCartesianWaypointController("NJointCartesianWaypointController");
+
+ std::string NJointCartesianWaypointController::getClassName(const Ice::Current&) const
+ {
+ return "NJointCartesianWaypointController";
+ }
+
+ NJointCartesianWaypointController::NJointCartesianWaypointController(
+ RobotUnitPtr,
+ const NJointCartesianWaypointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&
+ )
+ {
+ ARMARX_CHECK_NOT_NULL(config);
+
+ //sensor
+ {
+ if (!config->ftName.empty())
+ {
+ const auto svalFT = useSensorValue<SensorValueForceTorque>(config->ftName);
+ ARMARX_CHECK_NOT_NULL(svalFT);
+ _rtForceSensor = &(svalFT->force);
+ _rtTorqueSensor = &(svalFT->force);
+ }
+ }
+ //ctrl
+ {
+ auto rtRobot = useSynchronizedRtRobot();
+ ARMARX_CHECK_NOT_NULL(rtRobot);
+
+ auto rns = rtRobot->getRobotNodeSet(config->rns);
+ ARMARX_CHECK_NOT_NULL(rns);
+ ARMARX_CHECK_NOT_NULL(rns->getTCP());
+
+ _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(rns->getSize()));
+
+ _rtWpController = std::make_unique<CartesianWaypointController>(
+ rns,
+ _rtJointVelocityFeedbackBuffer,
+ config->runCfg.wpCfg.maxPositionAcceleration,
+ config->runCfg.wpCfg.maxOrientationAcceleration,
+ config->runCfg.wpCfg.maxNullspaceAcceleration
+ );
+
+ _rtWpController->setConfig({});
+
+ for (size_t i = 0; i < rns->getSize(); ++i)
+ {
+ std::string jointName = rns->getNode(i)->getName();
+ auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(jointName, ControlModes::Velocity1DoF);
+ auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
+ ARMARX_CHECK_NOT_NULL(ct);
+ ARMARX_CHECK_NOT_NULL(sv);
+
+ _rtVelTargets.emplace_back(&(ct->velocity));
+ _rtVelSensors.emplace_back(&(sv->velocity));
+ }
+ }
+ }
+
+ void NJointCartesianWaypointController::rtPreActivateController()
+ {
+ _publishIsAtTarget = false;
+ _publishIsAtForceLimit = false;
+ _rtForceOffset = Eigen::Vector3f::Zero();
+
+ _publishErrorPos = 0;
+ _publishErrorOri = 0;
+ _publishErrorPosMax = 0;
+ _publishErrorOriMax = 0;
+ _publishForceThreshold = _rtForceThreshold;
+
+ //feedback
+ {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+ for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelSensors))
+ {
+ _rtJointVelocityFeedbackBuffer(idx) = *ptr;
+ }
+ _rtWpController->setCurrentJointVelocity(_rtJointVelocityFeedbackBuffer);
+#pragma GCC diagnostic pop
+ }
+ }
+
+ void NJointCartesianWaypointController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ if (_tripBufWpCtrl.updateReadBuffer())
+ {
+ ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer");
+ auto& r = _tripBufWpCtrl._getNonConstReadBuffer();
+ if (r.cfgUpdated)
+ {
+ r.cfgUpdated = false;
+ _rtWpController->setConfig(r.cfg.wpCfg);
+ _rtForceThreshold = r.cfg.forceThreshold;
+ _publishForceThreshold = _rtForceThreshold;
+ ARMARX_RT_LOGF_IMPORTANT("updated the controller config");
+ }
+ if (r.wpsUpdated)
+ {
+ _rtStopConditionReached = false;
+ _publishIsAtTarget = false;
+ r.wpsUpdated = false;
+ ARMARX_RT_LOGF_IMPORTANT("updated waypoints (# = %u)", r.wps.size());
+ _publishWpsNum = r.wps.size();
+ _rtHasWps = !r.wps.empty();
+ _rtWpController->swapWaypoints(r.wps);
+ _rtWpController->skipToClosestWaypoint(r.skipRad2mmFactor);
+ }
+ }
+ //run
+ bool reachedTarget = false;
+ bool reachedFtLimit = false;
+
+ if (_rtForceSensor)
+ {
+ auto& ft = _tripBufFT.getWriteBuffer();
+ ft.force = *_rtForceSensor;
+ ft.torque = *_rtTorqueSensor;
+ ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
+ _tripBufFT.commitWrite();
+
+ if (_setFTOffset)
+ {
+ _rtForceOffset = *_rtForceSensor;
+ _setFTOffset = false;
+ }
+
+ const float currentForce = (*_rtForceSensor - _rtForceOffset).norm() ;
+
+ reachedFtLimit = (_rtForceThreshold >= 0) && currentForce > _rtForceThreshold;
+ _publishForceCurrent = currentForce;
+ _publishIsAtForceLimit = reachedFtLimit;
+ }
+
+ _publishWpsCur = _rtHasWps ? _rtWpController->currentWaypointIndex : 0;
+
+ if (_rtHasWps)
+ {
+ const float errorPos = _rtWpController->getPositionError();
+ const float errorOri = _rtWpController->getOrientationError();
+
+ _publishErrorPos = errorPos;
+ _publishErrorOri = errorOri;
+ if (!_rtWpController->isLastWaypoint())
+ {
+ _publishErrorPosMax = _rtWpController->thresholdPositionNear;
+ _publishErrorOriMax = _rtWpController->thresholdOrientationNear;
+ }
+ else
+ {
+ _publishErrorPosMax = _rtWpController->thresholdPositionReached;
+ _publishErrorOriMax = _rtWpController->thresholdOrientationReached;
+ reachedTarget = (errorPos < _rtWpController->thresholdPositionReached) &&
+ (errorOri < _rtWpController->thresholdOrientationReached);
+ }
+ }
+ _rtStopConditionReached = _rtStopConditionReached || reachedFtLimit || reachedTarget;
+ _publishIsAtTarget = reachedTarget;
+ if (!_rtHasWps || _rtStopConditionReached)
+ {
+ setNullVelocity();
+ }
+ else
+ {
+ const Eigen::VectorXf& goal = _rtWpController->calculate(timeSinceLastIteration.toSecondsDouble());
+ //write targets
+ ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
+ for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelTargets))
+ {
+ *ptr = goal(idx);
+ }
+ }
+ }
+
+ void NJointCartesianWaypointController::rtPostDeactivateController()
+ {
+
+ }
+
+ bool NJointCartesianWaypointController::hasReachedTarget(const Ice::Current&)
+ {
+ return _publishIsAtTarget;
+ }
+ void NJointCartesianWaypointController::setConfig(const CartesianWaypointControllerConfigWithForceLimit& cfg, const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufWpCtrlMut};
+ auto& w = _tripBufWpCtrl.getWriteBuffer();
+ w.wpsUpdated = false;
+ w.cfgUpdated = true;
+ w.cfg = cfg;
+ _tripBufWpCtrl.commitWrite();
+ ARMARX_IMPORTANT << "set new config";
+ }
+ void NJointCartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufWpCtrlMut};
+ auto& w = _tripBufWpCtrl.getWriteBuffer();
+ w.wpsUpdated = true;
+ w.cfgUpdated = false;
+ w.wps = wps;
+ _tripBufWpCtrl.commitWrite();
+ ARMARX_IMPORTANT << "set " << wps.size() << " new waypoints";
+ }
+ void NJointCartesianWaypointController::setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufWpCtrlMut};
+ auto& w = _tripBufWpCtrl.getWriteBuffer();
+ w.wpsUpdated = true;
+ w.cfgUpdated = false;
+ w.wps.clear();
+ w.wps.emplace_back(wp);
+ _tripBufWpCtrl.commitWrite();
+ ARMARX_IMPORTANT << "set new waypoint";
+ }
+ void NJointCartesianWaypointController::setConfigAndWaypoints(const CartesianWaypointControllerConfigWithForceLimit& cfg,
+ const std::vector<Eigen::Matrix4f>& wps,
+ const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufWpCtrlMut};
+ auto& w = _tripBufWpCtrl.getWriteBuffer();
+ w.wpsUpdated = true;
+ w.cfgUpdated = true;
+ w.cfg = cfg;
+ w.wps = wps;
+ _tripBufWpCtrl.commitWrite();
+ ARMARX_IMPORTANT << "set new config and" << wps.size() << " new waypoints";
+ }
+ void NJointCartesianWaypointController::setConfigAndWaypoint(const CartesianWaypointControllerConfigWithForceLimit& cfg,
+ const Eigen::Matrix4f& wp,
+ const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufWpCtrlMut};
+ auto& w = _tripBufWpCtrl.getWriteBuffer();
+ w.wpsUpdated = true;
+ w.cfgUpdated = true;
+ w.cfg = cfg;
+ w.wps.clear();
+ w.wps.emplace_back(wp);
+ _tripBufWpCtrl.commitWrite();
+ ARMARX_IMPORTANT << "set new config and a new waypoint";
+ }
+ void NJointCartesianWaypointController::setNullVelocity()
+ {
+ for (auto ptr : _rtVelTargets)
+ {
+ *ptr = 0;
+ }
+ }
+
+ bool NJointCartesianWaypointController::hasReachedForceLimit(const Ice::Current&)
+ {
+ ARMARX_CHECK_NOT_NULL(_rtForceSensor);
+ return _publishIsAtForceLimit;
+ }
+ FTSensorValue NJointCartesianWaypointController::getFTSensorValue(const Ice::Current&)
+ {
+ ARMARX_CHECK_NOT_NULL(_rtForceSensor);
+ std::lock_guard g{_tripBufFTMut};
+ return _tripBufFT.getUpToDateReadBuffer();
+ }
+ void NJointCartesianWaypointController::setCurrentFTAsOffset(const Ice::Current&)
+ {
+ ARMARX_CHECK_NOT_NULL(_rtForceSensor);
+ _setFTOffset = true;
+ }
+ void NJointCartesianWaypointController::stopMovement(const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufWpCtrlMut};
+ auto& w = _tripBufWpCtrl.getWriteBuffer();
+ w.wpsUpdated = true;
+ w.cfgUpdated = false;
+ w.wps.clear();
+ _tripBufWpCtrl.commitWrite();
+ ARMARX_IMPORTANT << "stop movement by setting 0 waypoints";
+ }
+
+ void NJointCartesianWaypointController::onPublish(
+ const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& obs)
+ {
+ const std::size_t wpsNum = _publishWpsNum;
+ const std::size_t wpsCur = wpsNum ? _publishWpsCur + 1 : 0;
+
+ const float errorPos = _publishErrorPos;
+ const float errorOri = _publishErrorOri;
+ const float errorPosMax = _publishErrorPosMax;
+ const float errorOriMax = _publishErrorOriMax;
+ const float forceThreshold = _publishForceThreshold;
+ const float forceCurrent = _publishForceCurrent;
+
+ {
+ StringVariantBaseMap datafields;
+ datafields["WpNum"] = new Variant(static_cast<int>(wpsNum));
+ datafields["WpCur"] = new Variant(static_cast<int>(wpsCur));
+
+ datafields["errorPos"] = new Variant(errorPos);
+ datafields["errorOri"] = new Variant(errorOri);
+ datafields["errorPosMax"] = new Variant(errorPosMax);
+ datafields["errorOriMax"] = new Variant(errorOriMax);
+
+ datafields["forceThreshold"] = new Variant(forceThreshold);
+ datafields["forceCurrent"] = new Variant(forceCurrent);
+
+ obs->setDebugChannel(getInstanceName(), datafields);
+ }
+
+ ARMARX_INFO << deactivateSpam(1) << std::setprecision(4)
+ << "At waypoint " << wpsCur << " / " << wpsNum
+ << " (last " << _publishIsAtTarget
+ << ", ft limit " << _publishIsAtForceLimit
+ << ", perror " << errorPos << " / " << errorPosMax
+ << ", oerror " << errorOri << " / " << errorOriMax
+ << ')';
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
new file mode 100644
index 000000000..50b76c243
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -0,0 +1,120 @@
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+
+#include <RobotAPI/libraries/core/CartesianWaypointController.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointCartesianWaypointController);
+
+ /**
+ * @brief The NJointCartesianWaypointController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointCartesianWaypointController :
+ public NJointController,
+ public NJointCartesianWaypointControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointCartesianWaypointControllerConfigPtr;
+ NJointCartesianWaypointController(
+ RobotUnitPtr robotUnit,
+ const NJointCartesianWaypointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&);
+
+ // NJointControllerInterface interface
+ std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
+ // WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = Ice::emptyCurrent) const override;
+ // void callDescribedFunction(const std::string& name, const StringVariantBaseMap& valueMap, const Ice::Current& = Ice::emptyCurrent) override;
+
+ // NJointController interface
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ // static WidgetDescription::WidgetPtr GenerateConfigDescription(
+ // const VirtualRobot::RobotPtr&,
+ // const std::map<std::string, ConstControlDevicePtr>&,
+ // const std::map<std::string, ConstSensorDevicePtr>&);
+
+ // static NJointCartesianWaypointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& values);
+ // NJointCartesianWaypointController(
+ // RobotUnitPtr prov,
+ // NJointCartesianWaypointControllerConfigPtr config,
+ // const VirtualRobot::RobotPtr& r);
+
+ void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
+
+ public:
+ 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 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 stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
+
+ FTSensorValue getFTSensorValue(const Ice::Current& = Ice::emptyCurrent) override;
+ void setCurrentFTAsOffset(const Ice::Current& = Ice::emptyCurrent) override;
+ protected:
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+
+ //structs
+ private:
+ struct CtrlData
+ {
+ std::vector<Eigen::Matrix4f> wps;
+ CartesianWaypointControllerConfigWithForceLimit cfg;
+ float skipRad2mmFactor = 500;
+ bool wpsUpdated = false;
+ bool cfgUpdated = false;
+ };
+
+ //data
+ private:
+ void setNullVelocity();
+
+ //rt data
+ std::unique_ptr<CartesianWaypointController> _rtWpController;
+ Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
+
+ std::vector<const float*> _rtVelSensors;
+ std::vector<float*> _rtVelTargets;
+
+ const Eigen::Vector3f* _rtForceSensor = nullptr;
+ const Eigen::Vector3f* _rtTorqueSensor = nullptr;
+
+ Eigen::Vector3f _rtForceOffset;
+
+ float _rtForceThreshold = -1;
+ bool _rtHasWps = false;
+ bool _rtStopConditionReached = false;
+
+ //flags
+ std::atomic_bool _publishIsAtTarget{false};
+ std::atomic_bool _publishIsAtForceLimit{false};
+ std::atomic_bool _setFTOffset{false};
+
+ //buffers
+ mutable std::recursive_mutex _tripBufWpCtrlMut;
+ TripleBuffer<CtrlData> _tripBufWpCtrl;
+ mutable std::recursive_mutex _tripBufFTMut;
+ TripleBuffer<FTSensorValue> _tripBufFT;
+
+ //publish data
+ std::atomic_size_t _publishWpsNum{0};
+ std::atomic_size_t _publishWpsCur{0};
+ std::atomic<float> _publishErrorPos{0};
+ std::atomic<float> _publishErrorOri{0};
+ std::atomic<float> _publishErrorPosMax{0};
+ std::atomic<float> _publishErrorOriMax{0};
+ std::atomic<float> _publishForceThreshold{0};
+ std::atomic<float> _publishForceCurrent{0};
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/CMakeLists.txt b/source/RobotAPI/gui-plugins/CMakeLists.txt
index ee50015cb..a7264e2ef 100644
--- a/source/RobotAPI/gui-plugins/CMakeLists.txt
+++ b/source/RobotAPI/gui-plugins/CMakeLists.txt
@@ -10,4 +10,5 @@ add_subdirectory(LaserScannerPlugin)
add_subdirectory(RobotUnitPlugin)
add_subdirectory(HomogeneousMatrixCalculator)
-add_subdirectory(GuiHealthClient)
\ No newline at end of file
+add_subdirectory(GuiHealthClient)
+add_subdirectory(CartesianWaypointControlGui)
\ No newline at end of file
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
new file mode 100644
index 000000000..38578c06d
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CMakeLists.txt
@@ -0,0 +1,30 @@
+armarx_set_target("CartesianWaypointControlGuiGuiPlugin")
+
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+set(SOURCES
+ CartesianWaypointControlGuiGuiPlugin.cpp
+ CartesianWaypointControlGuiWidgetController.cpp
+)
+
+set(HEADERS
+ CartesianWaypointControlGuiGuiPlugin.h
+ CartesianWaypointControlGuiWidgetController.h
+)
+
+set(GUI_MOC_HDRS ${HEADERS})
+
+set(GUI_UIS CartesianWaypointControlGuiWidget.ui)
+
+set(COMPONENT_LIBS
+ VirtualRobot
+
+ SimpleConfigDialog
+
+ RobotAPIInterfaces
+ RobotAPICore
+)
+
+if(ArmarXGui_FOUND)
+ armarx_gui_library(CartesianWaypointControlGuiGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+endif()
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp
new file mode 100644
index 000000000..fea32168c
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.cpp
@@ -0,0 +1,33 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package RobotAPI::gui-plugins::CartesianWaypointControlGuiGuiPlugin
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2019
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "CartesianWaypointControlGuiGuiPlugin.h"
+
+#include "CartesianWaypointControlGuiWidgetController.h"
+
+namespace armarx
+{
+ CartesianWaypointControlGuiGuiPlugin::CartesianWaypointControlGuiGuiPlugin()
+ {
+ addWidget < CartesianWaypointControlGuiWidgetController > ();
+ }
+}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h
new file mode 100644
index 000000000..b4b523dcc
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiGuiPlugin.h
@@ -0,0 +1,50 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package RobotAPI::gui-plugins::CartesianWaypointControlGui
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2019
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+
+namespace armarx
+{
+ /**
+ * \class CartesianWaypointControlGuiGuiPlugin
+ * \ingroup ArmarXGuiPlugins
+ * \brief CartesianWaypointControlGuiGuiPlugin brief description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT CartesianWaypointControlGuiGuiPlugin:
+ public armarx::ArmarXGuiPlugin
+ {
+ Q_OBJECT
+ Q_INTERFACES(ArmarXGuiInterface)
+ Q_PLUGIN_METADATA(IID "ArmarXGuiInterface/1.00")
+ public:
+ /**
+ * All widgets exposed by this plugin are added in the constructor
+ * via calls to addWidget()
+ */
+ CartesianWaypointControlGuiGuiPlugin();
+ };
+}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
new file mode 100644
index 000000000..a4d546f47
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
@@ -0,0 +1,335 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>CartesianWaypointControlGuiWidget</class>
+ <widget class="QWidget" name="CartesianWaypointControlGuiWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>907</width>
+ <height>739</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>CartesianWaypointControlGuiWidget</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="2" column="0" colspan="2">
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="title">
+ <string>Waypoints</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_4">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item row="0" column="0" colspan="2">
+ <widget class="QWidget" name="widgetWPs" native="true">
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="1" column="0" colspan="2">
+ <widget class="QTextEdit" name="textEditWPs"/>
+ </item>
+ <item row="2" column="0">
+ <widget class="QPushButton" name="pushButtonExecute">
+ <property name="text">
+ <string>Execute Waypoints</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0" colspan="2">
+ <layout class="QHBoxLayout" name="horizontalLayout">
+ <item>
+ <widget class="QLabel" name="label_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Format</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="radioButtonWPJson">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>json</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QRadioButton" name="radioButton4f">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Matrix4f</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QLabel" name="labelParsingSuccess">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="2" column="1">
+ <widget class="QPushButton" name="pushButtonStop">
+ <property name="text">
+ <string>Stop</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="1" column="0" colspan="2">
+ <widget class="QGroupBox" name="groupBox_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="title">
+ <string>Settings</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <item row="0" column="0">
+ <widget class="QWidget" name="widgetSettings" native="true">
+ <layout class="QGridLayout" name="gridLayout_5">
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>FT limit</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0" colspan="2">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="pushButtonZeroFT">
+ <property name="text">
+ <string>Zero FT</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="pushButtonSendSettings">
+ <property name="text">
+ <string>Send</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="1">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxFTLimit">
+ <property name="minimum">
+ <double>-1.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1000.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>-1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="0" column="0" colspan="2">
+ <widget class="QGroupBox" name="groupBox">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="title">
+ <string>Controller Creation</string>
+ </property>
+ <property name="checkable">
+ <bool>true</bool>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="1" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Kinematic Chain</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="lineEditFTName"/>
+ </item>
+ <item row="3" column="0" colspan="3">
+ <widget class="QPushButton" name="pushButtonCreateController">
+ <property name="text">
+ <string>Create</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>FT Sensor (optional)</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QComboBox" name="comboBoxChain"/>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="labelCurrentControllerChain">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLabel" name="labelCurrentControllerFT">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>Current</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections>
+ <connection>
+ <sender>groupBox</sender>
+ <signal>clicked(bool)</signal>
+ <receiver>groupBox</receiver>
+ <slot>setVisible(bool)</slot>
+ <hints>
+ <hint type="sourcelabel">
+ <x>24</x>
+ <y>16</y>
+ </hint>
+ <hint type="destinationlabel">
+ <x>176</x>
+ <y>32</y>
+ </hint>
+ </hints>
+ </connection>
+ <connection>
+ <sender>groupBox_3</sender>
+ <signal>clicked(bool)</signal>
+ <receiver>groupBox_3</receiver>
+ <slot>setVisible(bool)</slot>
+ <hints>
+ <hint type="sourcelabel">
+ <x>51</x>
+ <y>157</y>
+ </hint>
+ <hint type="destinationlabel">
+ <x>105</x>
+ <y>159</y>
+ </hint>
+ </hints>
+ </connection>
+ <connection>
+ <sender>groupBox_2</sender>
+ <signal>clicked(bool)</signal>
+ <receiver>groupBox_2</receiver>
+ <slot>setVisible(bool)</slot>
+ <hints>
+ <hint type="sourcelabel">
+ <x>154</x>
+ <y>268</y>
+ </hint>
+ <hint type="destinationlabel">
+ <x>204</x>
+ <y>270</y>
+ </hint>
+ </hints>
+ </connection>
+ </connections>
+</ui>
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
new file mode 100644
index 000000000..eb3e88376
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
@@ -0,0 +1,254 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * \package RobotAPI::gui-plugins::CartesianWaypointControlGuiWidgetController
+ * \author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * \date 2019
+ * \copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <random>
+#include <string>
+
+#include <VirtualRobot/Util/json.h>
+
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include "CartesianWaypointControlGuiWidgetController.h"
+
+namespace Eigen
+{
+ void from_json(const nlohmann::json& j, Eigen::Matrix4f& mx)
+ {
+ mx = VirtualRobot::json::posquat2eigen4f(j);
+ }
+}
+
+namespace armarx
+{
+ CartesianWaypointControlGuiWidgetController::CartesianWaypointControlGuiWidgetController()
+ {
+ _ui.setupUi(getWidget());
+ connect(_ui.radioButtonWPJson, SIGNAL(clicked()), this, SLOT(triggerParsing()));
+ connect(_ui.radioButton4f, SIGNAL(clicked()), this, SLOT(triggerParsing()));
+ connect(_ui.textEditWPs, SIGNAL(textChanged()), this, SLOT(triggerParsing()));
+
+ connect(_ui.pushButtonStop, SIGNAL(clicked()), this, SLOT(on_pushButtonStop_clicked()));
+ connect(_ui.pushButtonExecute, SIGNAL(clicked()), this, SLOT(on_pushButtonExecute_clicked()));
+ connect(_ui.pushButtonZeroFT, SIGNAL(clicked()), this, SLOT(on_pushButtonZeroFT_clicked()));
+ connect(_ui.pushButtonSendSettings, SIGNAL(clicked()), this, SLOT(on_pushButtonSendSettings_clicked()));
+ connect(_ui.pushButtonCreateController, SIGNAL(clicked()), this, SLOT(on_pushButtonCreateController_clicked()));
+ }
+
+ CartesianWaypointControlGuiWidgetController::~CartesianWaypointControlGuiWidgetController() = default;
+
+
+ void CartesianWaypointControlGuiWidgetController::loadSettings(QSettings* settings)
+ {
+ _robotStateComponentName = settings->value("rsc", "Armar6StateComponent").toString().toStdString();
+ _robotUnitName = settings->value("ru", "Armar6Unit").toString().toStdString();
+ }
+
+ void CartesianWaypointControlGuiWidgetController::saveSettings(QSettings* settings)
+ {
+ settings->setValue("rsc", QString::fromStdString(_robotStateComponentName));
+ settings->setValue("ru", QString::fromStdString(_robotUnitName));
+ }
+
+
+ void CartesianWaypointControlGuiWidgetController::onInitComponent()
+ {
+ usingProxy(_robotStateComponentName);
+ usingProxy(_robotUnitName);
+ }
+
+
+ void CartesianWaypointControlGuiWidgetController::onConnectComponent()
+ {
+ //proxies
+ {
+ _robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(_robotStateComponentName);
+ _robotUnit = getProxy<RobotUnitInterfacePrx>(_robotUnitName);
+ }
+ //robot
+ {
+ _robot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent, VirtualRobot::RobotIO::eStructure);
+ }
+ //fill rns combo box
+ {
+ bool found = false;
+ for (const auto& rnsn : _robot->getRobotNodeSetNames())
+ {
+ _ui.comboBoxChain->addItem(QString::fromStdString(rnsn));
+ if (rnsn == "RightArm")
+ {
+ _ui.comboBoxChain->setCurrentIndex(_ui.comboBoxChain->count() - 1);
+ found = true;
+ }
+ }
+ if(found && _robot->hasRobotNode("FT R"))
+ {
+ _ui.lineEditFTName->setText("FT R");
+ }
+ }
+ }
+ void CartesianWaypointControlGuiWidgetController::onDisconnectComponent()
+ {
+ deleteOldController();
+ }
+ void CartesianWaypointControlGuiWidgetController::on_pushButtonCreateController_clicked()
+ {
+ deleteOldController();
+
+ static const std::string njointControllerClassName = "NJointCartesianWaypointController";
+
+ NJointCartesianWaypointControllerConfigPtr cfg = new NJointCartesianWaypointControllerConfig;
+ cfg->rns = _ui.comboBoxChain->currentText().toStdString();
+ cfg->ftName = _ui.lineEditFTName->text().toStdString();
+ cfg->runCfg = readRunCfg();
+ _supportsFT = !cfg->ftName.empty();
+
+ _controllerName = getName() + "_" + njointControllerClassName +
+ std::to_string(std::random_device{}());
+
+ _ui.labelCurrentControllerChain->setText(QString::fromStdString(cfg->rns));
+ _ui.labelCurrentControllerFT->setText(QString::fromStdString(cfg->ftName));
+
+ ARMARX_IMPORTANT << "Creating " << njointControllerClassName << " '"
+ << _controllerName << "'";
+ _controller = NJointCartesianWaypointControllerInterfacePrx::checkedCast(
+ _robotUnit->createNJointController(
+ njointControllerClassName,
+ _controllerName,
+ cfg));
+ ARMARX_CHECK_NOT_NULL(_controller);
+ }
+
+ void CartesianWaypointControlGuiWidgetController::on_pushButtonSendSettings_clicked()
+ {
+ if (_controller)
+ {
+ ARMARX_IMPORTANT << "sending new config to " << _controllerName;
+ _controller->setConfig(readRunCfg());
+ }
+ }
+
+ void CartesianWaypointControlGuiWidgetController::on_pushButtonZeroFT_clicked()
+ {
+ if (_controller && _supportsFT)
+ {
+ ARMARX_IMPORTANT << "setting ft offset for " << _controllerName;
+ _controller->setCurrentFTAsOffset();
+ }
+ }
+
+ void CartesianWaypointControlGuiWidgetController::on_pushButtonExecute_clicked()
+ {
+ if (_controller)
+ {
+ _controller->activateController();
+ ARMARX_IMPORTANT << "trigger execution of " << _lastParsedWPs.size()
+ << " waypoints on " << _controllerName;
+ _controller->setWaypoints(_lastParsedWPs);
+ }
+ }
+
+ void CartesianWaypointControlGuiWidgetController::on_pushButtonStop_clicked()
+ {
+ if (_controller)
+ {
+ ARMARX_IMPORTANT << "sending stop command to controller " << _controllerName;
+ _controller->stopMovement();
+ }
+ }
+
+ QPointer<QDialog> CartesianWaypointControlGuiWidgetController::getConfigDialog(QWidget* parent)
+ {
+ if (!_dialog)
+ {
+ _dialog = new SimpleConfigDialog(parent);
+ _dialog->addProxyFinder<RobotUnitInterfacePrx>({"ru", "Robot Unit", "*Unit"});
+ _dialog->addProxyFinder<RobotStateComponentInterfacePrx>({"rsc", "Robot State Component", "*Component"});
+ }
+ return qobject_cast<SimpleConfigDialog*>(_dialog);
+ }
+
+ void CartesianWaypointControlGuiWidgetController::configured()
+ {
+ _robotStateComponentName = _dialog->getProxyName("rsc");
+ _robotUnitName = _dialog->getProxyName("ru");
+ }
+
+ void CartesianWaypointControlGuiWidgetController::triggerParsing()
+ {
+ _lastParsedWPs.clear();
+ _ui.labelParsingSuccess->setText("<pre parsing>");
+ ///TODO _lastParsedWPs labelParsingSuccess
+ if (_ui.radioButtonWPJson->isChecked())
+ {
+ //parse json
+ try
+ {
+ auto json = ::nlohmann::json::parse(_ui.textEditWPs->toPlainText().toStdString());
+ ARMARX_INFO << "parsed string as json";
+ _lastParsedWPs = json.get<std::vector<Eigen::Matrix4f>>();
+ }
+ catch (...)
+ {
+ _ui.labelParsingSuccess->setText("Failed to parse json array!");
+ return;
+ }
+ }
+ else if (_ui.radioButton4f->isChecked())
+ {
+ //parse lines
+ ///TODO parse Matrix4f style input
+ _ui.labelParsingSuccess->setText("NYI");
+ return;
+ }
+ //test reachability
+ {
+ ///TODO test reachability and visualize
+ }
+ _ui.labelParsingSuccess->setText("Parsed " + QString::number(_lastParsedWPs.size()) + " waypoints.");
+ }
+
+ CartesianWaypointControllerConfigWithForceLimit CartesianWaypointControlGuiWidgetController::readRunCfg() const
+ {
+ CartesianWaypointControllerConfigWithForceLimit cfg;
+ cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
+ ///TODO add more gui elemetns to change the execution parameters
+ return cfg;
+ }
+
+ void CartesianWaypointControlGuiWidgetController::deleteOldController()
+ {
+ if (_controller)
+ {
+ ARMARX_IMPORTANT << "deleting old controller '" << _controllerName << "'";
+ try
+ {
+ _controller->deactivateAndDeleteController();
+ }
+ catch (...)
+ {
+ _controller = nullptr;
+ }
+ }
+ }
+}
+
+
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
new file mode 100644
index 000000000..4816a63fe
--- /dev/null
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
@@ -0,0 +1,124 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::gui-plugins::CartesianWaypointControlGuiWidgetController
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXGuiPlugin.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include <RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.h>
+
+#include <RobotAPI/gui-plugins/CartesianWaypointControlGui/ui_CartesianWaypointControlGuiWidget.h>
+
+namespace armarx
+{
+ /**
+ \page RobotAPI-GuiPlugins-CartesianWaypointControlGui CartesianWaypointControlGui
+ \brief The CartesianWaypointControlGui allows visualizing ...
+
+ \image html CartesianWaypointControlGui.png
+ The user can
+
+ API Documentation \ref CartesianWaypointControlGuiWidgetController
+
+ \see CartesianWaypointControlGuiGuiPlugin
+ */
+
+ /**
+ * \class CartesianWaypointControlGuiWidgetController
+ * \brief CartesianWaypointControlGuiWidgetController brief one line description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT
+ CartesianWaypointControlGuiWidgetController:
+ public armarx::ArmarXComponentWidgetControllerTemplate < CartesianWaypointControlGuiWidgetController >
+ {
+ Q_OBJECT
+
+ public:
+ /// Controller Constructor
+ explicit CartesianWaypointControlGuiWidgetController();
+
+ /// Controller destructor
+ virtual ~CartesianWaypointControlGuiWidgetController();
+
+ /// @see ArmarXWidgetController::loadSettings()
+ void loadSettings(QSettings* settings) override;
+
+ /// @see ArmarXWidgetController::saveSettings()
+ void saveSettings(QSettings* settings) override;
+
+ /**
+ * Returns the Widget name displayed in the ArmarXGui to create an
+ * instance of this class.
+ */
+ static QString GetWidgetName()
+ {
+ return "RobotControl.CartesianWaypointControlGui";
+ }
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+ void onDisconnectComponent() override;
+
+ QPointer<QDialog> getConfigDialog(QWidget* parent) override;
+ void configured() override;
+
+ signals:
+ /* QT signal declarations */
+
+ private slots:
+ void on_pushButtonStop_clicked();
+ void on_pushButtonExecute_clicked();
+ void on_pushButtonZeroFT_clicked();
+ void on_pushButtonSendSettings_clicked();
+ void on_pushButtonCreateController_clicked();
+
+ void triggerParsing();
+
+ private:
+ CartesianWaypointControllerConfigWithForceLimit readRunCfg() const;
+ void deleteOldController();
+
+ private:
+ std::string _robotStateComponentName;
+ std::string _robotUnitName;
+ RobotStateComponentInterfacePrx _robotStateComponent;
+ RobotUnitInterfacePrx _robotUnit;
+ Ui::CartesianWaypointControlGuiWidget _ui;
+ QPointer<SimpleConfigDialog> _dialog;
+ NJointCartesianWaypointControllerInterfacePrx _controller;
+ std::string _controllerName;
+ VirtualRobot::RobotPtr _robot;
+ std::vector<Eigen::Matrix4f> _lastParsedWPs;
+ bool _supportsFT{false};
+ };
+}
+
+
diff --git a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
index 294707b12..8a05e556a 100644
--- a/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
+++ b/source/RobotAPI/gui-plugins/HapticUnitPlugin/MatrixDatafieldDisplayWidget.cpp
@@ -22,7 +22,6 @@
* GNU General Public License
*/
#include "MatrixDatafieldDisplayWidget.h"
-//#include "ui_MatrixDatafieldDisplayWidget.h"
#include <QPainter>
#include <pthread.h>
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index a14744594..273194576 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -56,6 +56,7 @@ set(SLICE_FILES
units/RobotUnit/NJointCartesianTorqueController.ice
units/RobotUnit/NJointCartesianActiveImpedanceController.ice
units/RobotUnit/NJointCartesianVelocityControllerWithRamp.ice
+ units/RobotUnit/NJointCartesianWaypointController.ice
units/RobotUnit/RobotUnitInterface.ice
units/RobotUnit/NJointJointSpaceDMPController.ice
diff --git a/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice b/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice
index 1e2a1a8e9..5a509ea7b 100644
--- a/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice
+++ b/source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice
@@ -26,13 +26,17 @@ module armarx
{
struct CartesianWaypointControllerConfig
{
- float kpJointLimitAvoidance = 1;
- float jointLimitAvoidanceScale = 2;
+ float maxPositionAcceleration = 500;
+ float maxOrientationAcceleration = 1;
+ float maxNullspaceAcceleration = 2;
- float thresholdOrientationNear = 0.1;
- float thresholdOrientationReached = 0.05;
- float thresholdPositionNear = 50;
- float thresholdPositionReached = 5;
+ float kpJointLimitAvoidance = 1;
+ float jointLimitAvoidanceScale = 2;
+
+ float thresholdOrientationNear = 0.1;
+ float thresholdOrientationReached = 0.05;
+ float thresholdPositionNear = 50;
+ float thresholdPositionReached = 5;
float maxOriVel = 1;
float maxPosVel = 80;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
new file mode 100644
index 000000000..4d722bb95
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianWaypointController.ice
@@ -0,0 +1,67 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::RobotAPI
+* @author Raphael Grimm
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <ArmarXCore/interface/serialization/Eigen.ice>
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/CartesianWaypointControllerConfig.ice>
+
+module armarx
+{
+ struct CartesianWaypointControllerConfigWithForceLimit
+ {
+ CartesianWaypointControllerConfig wpCfg;
+ float forceThreshold = -1; // < 0 -> no limit
+ };
+
+ struct FTSensorValue
+ {
+ long timestampInUs = 0;
+ Eigen::Vector3f force;
+ Eigen::Vector3f torque;
+ };
+
+ class NJointCartesianWaypointControllerConfig extends NJointControllerConfig
+ {
+ string rns;
+ CartesianWaypointControllerConfigWithForceLimit runCfg;
+ string ftName; //optional
+ };
+
+ interface NJointCartesianWaypointControllerInterface extends NJointControllerInterface
+ {
+ idempotent bool hasReachedTarget();
+ idempotent bool hasReachedForceLimit();
+
+ void setConfig(CartesianWaypointControllerConfigWithForceLimit 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);
+
+ FTSensorValue getFTSensorValue();
+ void setCurrentFTAsOffset();
+ void stopMovement();
+ };
+};
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index bd336320b..775d29544 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -32,7 +32,10 @@ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const V
: controller(rns, tcp), mode(mode)
{
setMaxAccelerations(maxPositionAcceleration, maxOrientationAcceleration, maxNullspaceAcceleration);
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
setCurrentJointVelocity(currentJointVelocity);
+#pragma GCC diagnostic pop
}
void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
index 3f0932ab9..0c47ae498 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -49,6 +49,7 @@ namespace armarx
CartesianVelocityControllerWithRamp(CartesianVelocityControllerWithRamp&&) = default;
CartesianVelocityControllerWithRamp& operator=(CartesianVelocityControllerWithRamp&&) = default;
+ [[deprecated("compued null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt);
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
index 975cc058a..675dc505f 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.cpp
@@ -223,5 +223,18 @@ namespace armarx
ctrlCartesianPos2Vel.maxPosVel = cfg.maxPosVel;
ctrlCartesianPos2Vel.KpOri = cfg.kpOri;
ctrlCartesianPos2Vel.KpPos = cfg.kpPos;
+
+ ctrlCartesianVelWithRamps.setMaxAccelerations(
+ cfg.maxPositionAcceleration,
+ cfg.maxOrientationAcceleration,
+ cfg.maxNullspaceAcceleration
+ );
+ }
+ void CartesianWaypointController::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
+ {
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+ ctrlCartesianVelWithRamps.setCurrentJointVelocity(currentJointVelocity);
+#pragma GCC diagnostic pop
}
}
diff --git a/source/RobotAPI/libraries/core/CartesianWaypointController.h b/source/RobotAPI/libraries/core/CartesianWaypointController.h
index e199af936..59932ae8d 100644
--- a/source/RobotAPI/libraries/core/CartesianWaypointController.h
+++ b/source/RobotAPI/libraries/core/CartesianWaypointController.h
@@ -58,10 +58,8 @@ namespace armarx
);
- void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
- {
- ctrlCartesianVelWithRamps.setCurrentJointVelocity(currentJointVelocity);
- }
+ [[deprecated("compued null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
+ void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
const Eigen::VectorXf& calculate(float dt);
--
GitLab