diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index d4d68c30e9a198e5a50ba7238774f2cfaf3cb9b5..88b1806f31933d8d4b188b9005968f3017be4327 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -25,6 +25,8 @@
#include <VirtualRobot/math/Helpers.h>
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+
#include <RobotAPI/libraries/core/Pose.h>
using namespace math;
@@ -38,6 +40,7 @@ DebugDrawerHelper::DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerP
{
}
+//1st order
void DebugDrawerHelper::drawPose(const std::string& name, const Eigen::Matrix4f& pose)
{
CHECK_AND_ADD(name, DrawElementType::Pose)
@@ -74,12 +77,6 @@ void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f&
debugDrawerPrx->setLineVisu(layerName, name, makeGlobal(p1), makeGlobal(p2), width, color);
}
-void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
-{
- CHECK_AND_ADD(name, DrawElementType::Line)
- drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
-}
-
void DebugDrawerHelper::drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size)
{
CHECK_AND_ADD(name, DrawElementType::Text)
@@ -123,6 +120,62 @@ void DebugDrawerHelper::setRobotConfig(const std::string& name, const std::map<s
debugDrawerPrx->updateRobotConfig(layerName, name, config);
}
+//2nd order
+void DebugDrawerHelper::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses)
+{
+ for (const auto& [idx, pose] : MakeIndexedContainer(poses))
+ {
+ drawPose(prefix + std::to_string(idx), pose);
+ }
+}
+void DebugDrawerHelper::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale)
+{
+ for (const auto& [idx, pose] : MakeIndexedContainer(poses))
+ {
+ drawPose(prefix + std::to_string(idx), pose, scale);
+ }
+}
+
+void DebugDrawerHelper::drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2)
+{
+ drawLine(name, p1, p2, defaults.lineWidth, defaults.lineColor);
+}
+
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps, float width, const DrawColor& color)
+{
+ for (std::size_t idx = 1; idx < ps.size(); ++idx)
+ {
+ drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx), width, color);
+ }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps)
+{
+ for (std::size_t idx = 1; idx < ps.size(); ++idx)
+ {
+ drawLine(prefix + std::to_string(idx), ps.at(idx - 1), ps.at(idx));
+ }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps, float width, const DrawColor& color)
+{
+ for (std::size_t idx = 1; idx < ps.size(); ++idx)
+ {
+ drawLine(prefix + std::to_string(idx),
+ ps.at(idx - 1).topRightCorner<3, 1>(),
+ ps.at(idx).topRightCorner<3, 1>(),
+ width, color);
+ }
+}
+void DebugDrawerHelper::drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps)
+{
+ for (std::size_t idx = 1; idx < ps.size(); ++idx)
+ {
+ drawLine(prefix + std::to_string(idx),
+ ps.at(idx - 1).topRightCorner<3, 1>(),
+ ps.at(idx).topRightCorner<3, 1>());
+ }
+}
+
+//utility
void DebugDrawerHelper::clearLayer()
{
debugDrawerPrx->clearLayer(layerName);
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 334fa002eaa8cc1c8be7023757a48edd9cf5e620..521ed9cf56d9fbabbc81be3bb41337912d628ce5 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -69,6 +69,7 @@ namespace armarx
DebugDrawerHelper(const DebugDrawerInterfacePrx& debugDrawerPrx, const std::string& layerName, const VirtualRobot::RobotPtr& robot);
+ //1st order draw functions (direct calls to proxy)
void drawPose(const std::string& name, const Eigen::Matrix4f& pose);
void drawPose(const std::string& name, const Eigen::Matrix4f& pose, float scale);
@@ -77,7 +78,6 @@ namespace armarx
void drawBox(const std::string& name, const Eigen::Matrix4f& pose, const Eigen::Vector3f& size, const DrawColor& color);
void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2, float width, const DrawColor& color);
- void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
void drawText(const std::string& name, const Eigen::Vector3f& p1, const std::string& text, const DrawColor& color, int size);
@@ -90,6 +90,18 @@ namespace armarx
void drawRobot(const std::string& name, const std::string& robotFile, const std::string& armarxProject, const Eigen::Matrix4f& pose, const DrawColor& color);
void setRobotConfig(const std::string& name, const std::map<std::string, float>& config);
+ //2nd order draw functions (call 1st order)
+ void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses);
+ void drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale);
+
+ void drawLine(const std::string& name, const Eigen::Vector3f& p1, const Eigen::Vector3f& p2);
+
+ void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps, float width, const DrawColor& color);
+ void drawLines(const std::string& prefix, const std::vector<Eigen::Vector3f>& ps);
+ void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps, float width, const DrawColor& color);
+ void drawLines(const std::string& prefix, const std::vector<Eigen::Matrix4f>& ps);
+
+ //utility
void clearLayer();
void cyclicCleanup();
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index c0f082947b658e46d526df7f6edebb4bf11169cd..f7087a226d59906ad7794a123742953d68e4c589 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -8,6 +8,29 @@
namespace armarx
{
+ std::ostream& operator<<(std::ostream& out, const CartesianWaypointControllerConfigWithForceLimit& cfg)
+ {
+ out << "maxPositionAcceleration " << cfg.wpCfg.maxPositionAcceleration << '\n'
+ << "maxOrientationAcceleration " << cfg.wpCfg.maxOrientationAcceleration << '\n'
+ << "maxNullspaceAcceleration " << cfg.wpCfg.maxNullspaceAcceleration << '\n'
+
+ << "kpJointLimitAvoidance " << cfg.wpCfg.kpJointLimitAvoidance << '\n'
+ << "jointLimitAvoidanceScale " << cfg.wpCfg.jointLimitAvoidanceScale << '\n'
+
+ << "thresholdPositionNear " << cfg.wpCfg.thresholdPositionNear << '\n'
+ << "thresholdPositionReached " << cfg.wpCfg.thresholdPositionReached << '\n'
+ << "maxPosVel " << cfg.wpCfg.maxPosVel << '\n'
+ << "kpPos " << cfg.wpCfg.kpPos << '\n'
+
+ << "thresholdOrientationNear " << cfg.wpCfg.thresholdOrientationNear << '\n'
+ << "thresholdOrientationReached " << cfg.wpCfg.thresholdOrientationReached << '\n'
+ << "maxOriVel " << cfg.wpCfg.maxOriVel << '\n'
+ << "kpOri " << cfg.wpCfg.kpOri << '\n'
+
+ << "forceThreshold " << cfg.forceThreshold << '\n';
+ return out;
+ }
+
NJointControllerRegistration<NJointCartesianWaypointController> registrationControllerNJointCartesianWaypointController("NJointCartesianWaypointController");
@@ -36,17 +59,18 @@ namespace armarx
}
//ctrl
{
- auto rtRobot = useSynchronizedRtRobot();
- ARMARX_CHECK_NOT_NULL(rtRobot);
+ _rtRobot = useSynchronizedRtRobot();
+ ARMARX_CHECK_NOT_NULL(_rtRobot);
- auto rns = rtRobot->getRobotNodeSet(config->rns);
- ARMARX_CHECK_NOT_NULL(rns);
- ARMARX_CHECK_NOT_NULL(rns->getTCP());
+ _rtRns = _rtRobot->getRobotNodeSet(config->rns);
+ ARMARX_CHECK_NOT_NULL(_rtRns);
+ _rtTcp = _rtRns->getTCP();
+ ARMARX_CHECK_NOT_NULL(_rtTcp);
- _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(rns->getSize()));
+ _rtJointVelocityFeedbackBuffer = Eigen::VectorXf::Zero(static_cast<int>(_rtRns->getSize()));
_rtWpController = std::make_unique<CartesianWaypointController>(
- rns,
+ _rtRns,
_rtJointVelocityFeedbackBuffer,
config->runCfg.wpCfg.maxPositionAcceleration,
config->runCfg.wpCfg.maxOrientationAcceleration,
@@ -55,9 +79,9 @@ namespace armarx
_rtWpController->setConfig({});
- for (size_t i = 0; i < rns->getSize(); ++i)
+ for (size_t i = 0; i < _rtRns->getSize(); ++i)
{
- std::string jointName = rns->getNode(i)->getName();
+ std::string jointName = _rtRns->getNode(i)->getName();
auto ct = useControlTarget<ControlTarget1DoFActuatorVelocity>(jointName, ControlModes::Velocity1DoF);
auto sv = useSensorValue<SensorValue1DoFActuatorVelocity>(jointName);
ARMARX_CHECK_NOT_NULL(ct);
@@ -96,6 +120,8 @@ namespace armarx
void NJointCartesianWaypointController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
+ auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
+
if (_tripBufWpCtrl.updateReadBuffer())
{
ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer");
@@ -126,11 +152,9 @@ namespace armarx
if (_rtForceSensor)
{
- auto& ft = _tripBufFT.getWriteBuffer();
- ft.force = *_rtForceSensor;
- ft.torque = *_rtTorqueSensor;
- ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
- _tripBufFT.commitWrite();
+ rt2nonrtBuf.ft.force = *_rtForceSensor;
+ rt2nonrtBuf.ft.torque = *_rtTorqueSensor;
+ rt2nonrtBuf.ft.timestampInUs = sensorValuesTimestamp.toMicroSeconds();
if (_setFTOffset)
{
@@ -147,6 +171,8 @@ namespace armarx
_publishWpsCur = _rtHasWps ? _rtWpController->currentWaypointIndex : 0;
+ rt2nonrtBuf.rootPose = _rtRobot->getGlobalPose();
+ rt2nonrtBuf.tcp = _rtTcp->getPoseInRootFrame();
if (_rtHasWps)
{
const float errorPos = _rtWpController->getPositionError();
@@ -166,6 +192,11 @@ namespace armarx
reachedTarget = (errorPos < _rtWpController->thresholdPositionReached) &&
(errorOri < _rtWpController->thresholdOrientationReached);
}
+ rt2nonrtBuf.tcpTarg = _rtWpController->getCurrentTarget();
+ }
+ else
+ {
+ rt2nonrtBuf.tcpTarg = rt2nonrtBuf.tcp;
}
_rtStopConditionReached = _rtStopConditionReached || reachedFtLimit || reachedTarget;
_publishIsAtTarget = reachedTarget;
@@ -183,6 +214,7 @@ namespace armarx
*ptr = goal(idx);
}
}
+ _tripRt2NonRt.commitWrite();
}
void NJointCartesianWaypointController::rtPostDeactivateController()
@@ -202,7 +234,7 @@ namespace armarx
w.cfgUpdated = true;
w.cfg = cfg;
_tripBufWpCtrl.commitWrite();
- ARMARX_IMPORTANT << "set new config";
+ ARMARX_IMPORTANT << "set new config\n" << cfg;
}
void NJointCartesianWaypointController::setWaypoints(const std::vector<Eigen::Matrix4f>& wps, const Ice::Current&)
{
@@ -212,7 +244,7 @@ namespace armarx
w.cfgUpdated = false;
w.wps = wps;
_tripBufWpCtrl.commitWrite();
- ARMARX_IMPORTANT << "set " << wps.size() << " new waypoints";
+ ARMARX_IMPORTANT << "set new waypoints\n" << wps;
}
void NJointCartesianWaypointController::setWaypoint(const Eigen::Matrix4f& wp, const Ice::Current&)
{
@@ -223,7 +255,7 @@ namespace armarx
w.wps.clear();
w.wps.emplace_back(wp);
_tripBufWpCtrl.commitWrite();
- ARMARX_IMPORTANT << "set new waypoint";
+ ARMARX_IMPORTANT << "set new waypoint\n" << wp;
}
void NJointCartesianWaypointController::setConfigAndWaypoints(const CartesianWaypointControllerConfigWithForceLimit& cfg,
const std::vector<Eigen::Matrix4f>& wps,
@@ -236,7 +268,7 @@ namespace armarx
w.cfg = cfg;
w.wps = wps;
_tripBufWpCtrl.commitWrite();
- ARMARX_IMPORTANT << "set new config and" << wps.size() << " new waypoints";
+ ARMARX_IMPORTANT << "set new config\n" << cfg << " and new waypoints\n" << wps;
}
void NJointCartesianWaypointController::setConfigAndWaypoint(const CartesianWaypointControllerConfigWithForceLimit& cfg,
const Eigen::Matrix4f& wp,
@@ -250,7 +282,7 @@ namespace armarx
w.wps.clear();
w.wps.emplace_back(wp);
_tripBufWpCtrl.commitWrite();
- ARMARX_IMPORTANT << "set new config and a new waypoint";
+ ARMARX_IMPORTANT << "set new config\n" << cfg << "and a new waypoint\n" << wp;
}
void NJointCartesianWaypointController::setNullVelocity()
{
@@ -268,8 +300,8 @@ namespace armarx
FTSensorValue NJointCartesianWaypointController::getFTSensorValue(const Ice::Current&)
{
ARMARX_CHECK_NOT_NULL(_rtForceSensor);
- std::lock_guard g{_tripBufFTMut};
- return _tripBufFT.getUpToDateReadBuffer();
+ std::lock_guard g{_tripRt2NonRtMutex};
+ return _tripRt2NonRt.getUpToDateReadBuffer().ft;
}
void NJointCartesianWaypointController::setCurrentFTAsOffset(const Ice::Current&)
{
@@ -289,7 +321,7 @@ namespace armarx
void NJointCartesianWaypointController::onPublish(
const SensorAndControl&,
- const DebugDrawerInterfacePrx&,
+ const DebugDrawerInterfacePrx& drawer,
const DebugObserverInterfacePrx& obs)
{
const std::size_t wpsNum = _publishWpsNum;
@@ -325,5 +357,28 @@ namespace armarx
<< ", perror " << errorPos << " / " << errorPosMax
<< ", oerror " << errorOri << " / " << errorOriMax
<< ')';
+
+ {
+ std::lock_guard g{_tripRt2NonRtMutex};
+ const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();
+ if (buf.tcp != buf.tcpTarg)
+ {
+ const Eigen::Matrix4f gtcp = buf.rootPose * buf.tcp;
+ const Eigen::Matrix4f gtcptarg = buf.rootPose * buf.tcpTarg;
+ drawer->setPoseVisu(getName(), "tcp", new Pose(gtcp));
+ drawer->setPoseVisu(getName(), "tcptarg", new Pose(gtcptarg));
+ drawer->setLineVisu(
+ getName(), "tcp2targ",
+ new Vector3(Eigen::Vector3f{gtcp.topRightCorner<3, 1>()}),
+ new Vector3(Eigen::Vector3f{gtcptarg.topRightCorner<3, 1>()}),
+ 3, {0, 0, 1, 1});
+ }
+ else
+ {
+ drawer->removePoseVisu(getName(), "tcp");
+ drawer->removePoseVisu(getName(), "tcptarg");
+ drawer->removeLineVisu(getName(), "tcp2targ");
+ }
+ }
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
index 50b76c2430b8d36bb5979acbb0c1fe80a895316e..16ebac8c13fa0acba8a0e96ce4950a428461aa68 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h
@@ -76,11 +76,23 @@ namespace armarx
bool cfgUpdated = false;
};
+ struct RtToNonRtData
+ {
+ FTSensorValue ft;
+ Eigen::Matrix4f rootPose = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f tcp = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f tcpTarg = Eigen::Matrix4f::Identity();
+ };
+
//data
private:
void setNullVelocity();
//rt data
+ VirtualRobot::RobotPtr _rtRobot;
+ VirtualRobot::RobotNodeSetPtr _rtRns;
+ VirtualRobot::RobotNodePtr _rtTcp;
+
std::unique_ptr<CartesianWaypointController> _rtWpController;
Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
@@ -104,8 +116,9 @@ namespace armarx
//buffers
mutable std::recursive_mutex _tripBufWpCtrlMut;
TripleBuffer<CtrlData> _tripBufWpCtrl;
- mutable std::recursive_mutex _tripBufFTMut;
- TripleBuffer<FTSensorValue> _tripBufFT;
+
+ mutable std::recursive_mutex _tripRt2NonRtMutex;
+ TripleBuffer<RtToNonRtData> _tripRt2NonRt;
//publish data
std::atomic_size_t _publishWpsNum{0};
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
index a4d546f478e0c1b002ca19e9e1257e2fd180263f..a23cb5332f54d5107e7aa15a38e1577ad374c36c 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
@@ -163,7 +163,135 @@
<item row="0" column="0">
<widget class="QWidget" name="widgetSettings" native="true">
<layout class="QGridLayout" name="gridLayout_5">
+ <item row="2" column="4">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxPosReached">
+ <property name="maximum">
+ <double>500.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>5.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="6">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxPosMaxVel">
+ <property name="maximum">
+ <double>500.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>80.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>Position</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_15">
+ <property name="text">
+ <string>Orientation</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="5">
+ <widget class="QLabel" name="label_20">
+ <property name="text">
+ <string>Max vel</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxOriNear">
+ <property name="maximum">
+ <double>5.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLabel" name="label_16">
+ <property name="text">
+ <string>Near</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="3">
+ <widget class="QLabel" name="label_17">
+ <property name="text">
+ <string>Reached</string>
+ </property>
+ </widget>
+ </item>
<item row="1" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>Joint limit avoidance</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="4">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxOriReached">
+ <property name="maximum">
+ <double>5.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.050000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLabel" name="label_14">
+ <property name="text">
+ <string>Near</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxPosNear">
+ <property name="maximum">
+ <double>500.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>50.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccOri">
+ <property name="maximum">
+ <double>4.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_9">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Pos</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
<widget class="QLabel" name="label_4">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
@@ -176,7 +304,131 @@
</property>
</widget>
</item>
- <item row="2" column="0" colspan="2">
+ <item row="0" column="6">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccNull">
+ <property name="maximum">
+ <double>5.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>2.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="6">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxOriMaxVel">
+ <property name="maximum">
+ <double>5.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Max acc</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="8">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxPosKP">
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="4">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxLimitAvoidScale">
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>2.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLabel" name="label_12">
+ <property name="text">
+ <string>KP</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="5">
+ <widget class="QLabel" name="label_11">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Nullspace</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="3">
+ <widget class="QLabel" name="label_13">
+ <property name="text">
+ <string>Scale</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="3">
+ <widget class="QLabel" name="label_10">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Ori</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="8">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxOriKP">
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="5">
+ <widget class="QLabel" name="label_19">
+ <property name="text">
+ <string>Max vel</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxMaxAccPos">
+ <property name="maximum">
+ <double>2000.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>500.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxLimitAvoidKP">
+ <property name="maximum">
+ <double>10.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0" colspan="9">
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<widget class="QPushButton" name="pushButtonZeroFT">
@@ -194,7 +446,34 @@
</item>
</layout>
</item>
- <item row="1" column="1">
+ <item row="3" column="3">
+ <widget class="QLabel" name="label_18">
+ <property name="text">
+ <string>Reached</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="7">
+ <widget class="QLabel" name="label_21">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Kp</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="7">
+ <widget class="QLabel" name="label_22">
+ <property name="text">
+ <string>Kp</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2" colspan="7">
<widget class="QDoubleSpinBox" name="doubleSpinBoxFTLimit">
<property name="minimum">
<double>-1.000000000000000</double>
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
index df1458f3f5589e42f9167af964d8f2f9dfb362a9..09683de6a49e6a0948fd79532b170518751ed671 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
@@ -188,7 +188,6 @@ namespace armarx
{
_lastParsedWPs.clear();
_ui.labelParsingSuccess->setText("<pre parsing>");
- ///TODO _lastParsedWPs labelParsingSuccess
if (_ui.radioButtonWPJson->isChecked())
{
//parse json
@@ -220,8 +219,25 @@ namespace armarx
CartesianWaypointControllerConfigWithForceLimit CartesianWaypointControlGuiWidgetController::readRunCfg() const
{
CartesianWaypointControllerConfigWithForceLimit cfg;
- cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
- ///TODO add more gui elemetns to change the execution parameters
+
+ cfg.wpCfg.maxPositionAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
+ cfg.wpCfg.maxOrientationAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
+ cfg.wpCfg.maxNullspaceAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
+
+ cfg.wpCfg.kpJointLimitAvoidance = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
+ cfg.wpCfg.jointLimitAvoidanceScale = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
+
+ cfg.wpCfg.thresholdOrientationNear = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
+ cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
+ cfg.wpCfg.thresholdPositionNear = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
+ cfg.wpCfg.thresholdPositionReached = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
+
+ cfg.wpCfg.maxOriVel = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
+ cfg.wpCfg.maxPosVel = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
+ cfg.wpCfg.kpOri = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
+ cfg.wpCfg.kpPos = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
+
+ cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
return cfg;
}
diff --git a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
index 7be0e04be38888d8af0ec895ebb2a804eedffed7..f5fc0ff5453ededba9f8743c09db6bffc1e51ff4 100644
--- a/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
+++ b/source/RobotAPI/libraries/ControllerUIUtility/CartesianWaypointControllerConfig/RemoteGui.cpp
@@ -30,134 +30,127 @@ namespace armarx::RemoteGui
{
return RemoteGui::makeGroupBox(name)
.addChild(
- RemoteGui::makeSimpleGridLayout().cols(3)
+ RemoteGui::makeSimpleGridLayout().cols(10)
+
.addChild(RemoteGui::makeTextLabel("Max accelerations:"))
+ .addChild(RemoteGui::makeTextLabel("Pos:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_maxAcc_Pos")
+ .min(0)
+ .max(10000)
+ .value(val.maxPositionAcceleration)
+ .decimals(3)
+ )
+ .addChild(RemoteGui::makeTextLabel("Ori:"))
.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Pos:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_maxAcc_Pos")
- .min(0)
- .max(10000)
- .value(val.kpJointLimitAvoidance)
- .decimals(3)
- )
- .addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeTextLabel("Ori:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_maxAcc_Ori")
- .min(0)
- .max(10)
- .value(val.jointLimitAvoidanceScale)
- .decimals(3)
- )
- .addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeTextLabel("Nullspace:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_maxAcc_Null")
- .min(0)
- .max(10)
- .value(val.jointLimitAvoidanceScale)
- .decimals(3)
- )
+ RemoteGui::makeFloatSpinBox(name + "_maxAcc_Ori")
+ .min(0)
+ .max(10)
+ .value(val.maxOrientationAcceleration)
+ .decimals(3)
)
+ .addChild(RemoteGui::makeTextLabel("Nullspace:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_maxAcc_Null")
+ .min(0)
+ .max(10)
+ .value(val.maxNullspaceAcceleration)
+ .decimals(3)
+ )
+ .addChild(new RemoteGui::Widget())
+ .addChild(new RemoteGui::Widget())
.addChild(new RemoteGui::HSpacer)
+
.addChild(RemoteGui::makeTextLabel("JointLimitAvoidance:"))
+ .addChild(RemoteGui::makeTextLabel("KP:"))
.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("KP:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_KP")
- .min(0)
- .max(10)
- .value(val.kpJointLimitAvoidance)
- .decimals(3)
- )
- .addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeTextLabel("Scale:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_Scale")
- .min(0)
- .max(10)
- .value(val.jointLimitAvoidanceScale)
- .decimals(3)
- )
+ RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_KP")
+ .min(0)
+ .max(10)
+ .value(val.kpJointLimitAvoidance)
+ .decimals(3)
)
+ .addChild(RemoteGui::makeTextLabel("Scale:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_JointLimitAvoidance_Scale")
+ .min(0)
+ .max(10)
+ .value(val.jointLimitAvoidanceScale)
+ .decimals(3)
+ )
+ .addChild(new RemoteGui::Widget())
+ .addChild(new RemoteGui::Widget())
+ .addChild(new RemoteGui::Widget())
+ .addChild(new RemoteGui::Widget())
.addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeTextLabel("Thresholds:"))
+
+ .addChild(RemoteGui::makeTextLabel("Position:"))
+ .addChild(RemoteGui::makeTextLabel("Near:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Near")
+ .min(0)
+ .max(1000)
+ .value(val.thresholdPositionNear)
+ .decimals(3)
+ )
+ .addChild(RemoteGui::makeTextLabel("Reached:"))
.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Ori near:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Near")
- .min(0)
- .max(3.14f)
- .value(val.thresholdOrientationNear)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Ori reached:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Reached")
- .min(0)
- .max(3.14f)
- .value(val.thresholdOrientationReached)
- .decimals(3)
- )
- .addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeTextLabel("Pos near:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Near")
- .min(0)
- .max(1000)
- .value(val.thresholdPositionNear)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Pos reached:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Reached")
- .min(0)
- .max(1000)
- .value(val.thresholdPositionReached)
- .decimals(3)
- )
+ RemoteGui::makeFloatSpinBox(name + "_Thresholds_Pos_Reached")
+ .min(0)
+ .max(1000)
+ .value(val.thresholdPositionReached)
+ .decimals(3)
+ )
+ .addChild(RemoteGui::makeTextLabel("Max vel:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_Max_vel_pos")
+ .min(0)
+ .max(1000)
+ .value(val.maxPosVel)
+ .decimals(3)
+ )
+ .addChild(RemoteGui::makeTextLabel("KP:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_KP_pos")
+ .min(0)
+ .max(10)
+ .value(val.kpPos)
+ .decimals(3)
)
.addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeTextLabel(""))
+
+ .addChild(RemoteGui::makeTextLabel("Orientation:"))
+ .addChild(RemoteGui::makeTextLabel("Near:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Near")
+ .min(0)
+ .max(3.14f)
+ .value(val.thresholdOrientationNear)
+ .decimals(3)
+ )
+ .addChild(RemoteGui::makeTextLabel("Reached:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_Thresholds_Ori_Reached")
+ .min(0)
+ .max(3.14f)
+ .value(val.thresholdOrientationReached)
+ .decimals(3)
+ )
+ .addChild(RemoteGui::makeTextLabel("Max vel:"))
+ .addChild(
+ RemoteGui::makeFloatSpinBox(name + "_Max_vel_ori")
+ .min(0)
+ .max(31.4f)
+ .value(val.maxOriVel)
+ .decimals(3)
+ )
+ .addChild(RemoteGui::makeTextLabel("KP:"))
.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Max vel ori:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Max_vel_ori")
- .min(0)
- .max(31.4f)
- .value(val.maxOriVel)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Ori KP:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_KP_ori")
- .min(0)
- .max(10)
- .value(val.kpOri)
- .decimals(3)
- )
- .addChild(new RemoteGui::HSpacer)
- .addChild(RemoteGui::makeTextLabel("Max vel pos:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_Max_vel_pos")
- .min(0)
- .max(400)
- .value(val.maxPosVel)
- .decimals(3)
- )
- .addChild(RemoteGui::makeTextLabel("Pos KP:"))
- .addChild(
- RemoteGui::makeFloatSpinBox(name + "_KP_pos")
- .min(0)
- .max(10)
- .value(val.kpPos)
- .decimals(3)
- )
+ RemoteGui::makeFloatSpinBox(name + "_KP_ori")
+ .min(0)
+ .max(10)
+ .value(val.kpOri)
+ .decimals(3)
)
.addChild(new RemoteGui::HSpacer)
);
@@ -182,8 +175,8 @@ namespace armarx::RemoteGui
cfg.thresholdPositionReached = getValue<float>(values, name + "_Thresholds_Pos_Reached");
cfg.maxOriVel = getValue<float>(values, name + "_Max_vel_ori");
- cfg.maxPosVel = getValue<float>(values, name + "_KP_ori");
- cfg.kpOri = getValue<float>(values, name + "_Max_vel_pos");
+ cfg.maxPosVel = getValue<float>(values, name + "_Max_vel_pos");
+ cfg.kpOri = getValue<float>(values, name + "_KP_ori");
cfg.kpPos = getValue<float>(values, name + "_KP_pos");
return cfg;
}