From 315e03830706b51b437d27f73276cf5bc7fa9896 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Tue, 21 Apr 2020 17:43:01 +0200
Subject: [PATCH] added scalable max vel
---
...ointCartesianNaturalPositionController.cpp | 58 +++++++++++++++----
...NJointCartesianNaturalPositionController.h | 20 +++++--
...artesianNaturalPositionControllerWidget.ui | 41 ++++++++++---
...uralPositionControllerWidgetController.cpp | 53 ++++++++++-------
...aturalPositionControllerWidgetController.h | 9 +--
.../CartesianNaturalPositionController.cpp | 12 +++-
.../core/CartesianNaturalPositionController.h | 3 +
...artesianNaturalPositionControllerProxy.cpp | 53 ++++++++++++++++-
.../CartesianNaturalPositionControllerProxy.h | 23 +++++++-
9 files changed, 220 insertions(+), 52 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 8069aeafd..6bbc17af4 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -11,6 +11,19 @@
namespace armarx
{
+ std::string vec2str(const std::vector<float>& vec)
+ {
+ std::stringstream ss;
+ for (size_t i = 0; i < vec.size(); i++)
+ {
+ if (i > 0)
+ {
+ ss << ", ";
+ }
+ ss << vec.at(i);
+ }
+ return ss.str();
+ }
std::ostream& operator<<(std::ostream& out, const CartesianNaturalPositionControllerConfig& cfg)
{
out << "maxPositionAcceleration " << cfg.maxPositionAcceleration << '\n'
@@ -23,6 +36,9 @@ namespace armarx
<< "maxTcpOriVel " << cfg.maxTcpOriVel << '\n'
<< "maxElpPosVel " << cfg.maxElbPosVel << '\n'
+ << "maxJointVelocity " << vec2str(cfg.maxJointVelocity) << '\n'
+ << "maxNullspaceVelocity " << vec2str(cfg.maxNullspaceVelocity) << '\n'
+
<< "jointLimitAvoidanceKp " << cfg.jointLimitAvoidanceKp << '\n'
<< "elbowKp " << cfg.elbowKp << '\n'
<< "nullspaceTargetKp " << cfg.nullspaceTargetKp << '\n'
@@ -104,7 +120,8 @@ namespace armarx
config->runCfg.maxNullspaceAcceleration
);
- _rtPosController->setConfig({});
+ _rtPosController->setConfig(config->runCfg);
+ ARMARX_IMPORTANT << "controller config: " << config->runCfg;
for (size_t i = 0; i < _rtRns->getSize(); ++i)
{
@@ -129,13 +146,17 @@ namespace armarx
void NJointCartesianNaturalPositionController::rtPreActivateController()
{
- _publishIsAtTarget = false;
+ //_publishIsAtTarget = false;
//_rtForceOffset = Eigen::Vector3f::Zero();
- _publishErrorPos = 0;
- _publishErrorOri = 0;
- _publishErrorPosMax = 0;
- _publishErrorOriMax = 0;
+ _publish.errorPos = 0;
+ _publish.errorOri = 0;
+ _publish.errorPosMax = 0;
+ _publish.errorOriMax = 0;
+ _publish.tcpPosVel = 0;
+ _publish.tcpOriVel = 0;
+ _publish.elbPosVel = 0;
+ _publish.targetReached = false;
//_publishForceThreshold = _rtForceThreshold;
_rtPosController->setTarget(_rtTcp->getPoseInRootFrame(), _rtElbow->getPositionInRootFrame());
@@ -237,6 +258,14 @@ namespace armarx
{
VirtualRobot::IKSolver::CartesianSelection mode = _rtSetOrientation ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
const Eigen::VectorXf& goal = _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble(), mode);
+
+ const Eigen::VectorXf actualTcpVel = _rtPosController->actualTcpVel(goal);
+ const Eigen::VectorXf actualElbVel = _rtPosController->actualElbVel(goal);
+ //ARMARX_IMPORTANT << VAROUT(actualTcpVel) << VAROUT(actualElbVel);
+ _publish.tcpPosVel = actualTcpVel.block<3, 1>(0, 0).norm();
+ _publish.tcpOriVel = actualTcpVel.block<3, 1>(3, 0).norm();
+ _publish.elbPosVel = actualElbVel.block<3, 1>(0, 0).norm();
+
//write targets
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelTargets))
@@ -254,7 +283,7 @@ namespace armarx
bool NJointCartesianNaturalPositionController::hasReachedTarget(const Ice::Current&)
{
- return _publishIsAtTarget;
+ return _publish.targetReached;
}
void NJointCartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current&)
{
@@ -341,12 +370,15 @@ namespace armarx
const DebugDrawerInterfacePrx& drawer,
const DebugObserverInterfacePrx& obs)
{
- const float errorPos = _publishErrorPos;
- const float errorOri = _publishErrorOri;
- const float errorPosMax = _publishErrorPosMax;
- const float errorOriMax = _publishErrorOriMax;
+ const float errorPos = _publish.errorPos;
+ const float errorOri = _publish.errorOri;
+ const float errorPosMax = _publish.errorPosMax;
+ const float errorOriMax = _publish.errorOriMax;
//const float forceThreshold = _publishForceThreshold;
//const float forceCurrent = _publishForceCurrent;
+ const float tcpPosVel = _publish.tcpPosVel;
+ const float tcpOriVel = _publish.tcpOriVel;
+ const float elbPosVel = _publish.elbPosVel;
{
StringVariantBaseMap datafields;
@@ -356,6 +388,10 @@ namespace armarx
datafields["errorPosMax"] = new Variant(errorPosMax);
datafields["errorOriMax"] = new Variant(errorOriMax);
+ datafields["tcpPosVel"] = new Variant(tcpPosVel);
+ datafields["tcpOriVel"] = new Variant(tcpOriVel);
+ datafields["elbPosVel"] = new Variant(elbPosVel);
+
//datafields["forceThreshold"] = new Variant(forceThreshold);
//datafields["forceCurrent"] = new Variant(forceCurrent);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
index 3cb3403dd..0fef1f90e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -91,6 +91,18 @@ namespace armarx
//Eigen::Vector3f ftUsed = Eigen::Vector3f::Zero();
};
+ struct PublishData
+ {
+ std::atomic<float> errorPos{0};
+ std::atomic<float> errorOri{0};
+ std::atomic<float> errorPosMax{0};
+ std::atomic<float> errorOriMax{0};
+ std::atomic<float> tcpPosVel{0};
+ std::atomic<float> tcpOriVel{0};
+ std::atomic<float> elbPosVel{0};
+ std::atomic<bool> targetReached;
+ };
+
//data
private:
void setNullVelocity();
@@ -121,7 +133,7 @@ namespace armarx
//bool _rtStopConditionReached = false;
//flags
- std::atomic_bool _publishIsAtTarget{false};
+ //std::atomic_bool _publishIsAtTarget{false};
//std::atomic_bool _publishIsAtForceLimit{false};
//std::atomic_bool _setFTOffset{false};
std::atomic_bool _stopNowRequested{false};
@@ -141,10 +153,8 @@ namespace armarx
TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
//publish data
- std::atomic<float> _publishErrorPos{0};
- std::atomic<float> _publishErrorOri{0};
- std::atomic<float> _publishErrorPosMax{0};
- std::atomic<float> _publishErrorOriMax{0};
+ PublishData _publish;
+
//std::atomic<float> _publishForceThreshold{0};
//std::atomic<float> _publishForceCurrent{0};
//std::atomic_bool _publishForceThresholdInRobotRootZ{0};
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
index dd7b44520..ba6daf104 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
@@ -273,14 +273,14 @@
<string>Parameters</string>
</property>
<layout class="QGridLayout" name="gridLayout_5">
- <item row="1" column="0">
+ <item row="2" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>KpElbow</string>
</property>
</widget>
</item>
- <item row="2" column="1">
+ <item row="3" column="1">
<widget class="QSlider" name="sliderKpJla">
<property name="maximum">
<number>200</number>
@@ -290,21 +290,21 @@
</property>
</widget>
</item>
- <item row="2" column="0">
+ <item row="3" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>KpJLA</string>
</property>
</widget>
</item>
- <item row="2" column="2">
+ <item row="3" column="2">
<widget class="QLabel" name="labelKpJla">
<property name="text">
<string>0.0</string>
</property>
</widget>
</item>
- <item row="1" column="1">
+ <item row="2" column="1">
<widget class="QSlider" name="sliderKpElbow">
<property name="maximum">
<number>200</number>
@@ -320,20 +320,47 @@
</property>
</widget>
</item>
- <item row="1" column="2">
+ <item row="2" column="2">
<widget class="QLabel" name="labelKpElbow">
<property name="text">
<string>0.0</string>
</property>
</widget>
</item>
- <item row="0" column="0" colspan="2">
+ <item row="1" column="0" colspan="2">
<widget class="QCheckBox" name="checkBoxAutoKp">
<property name="text">
<string>Automatic Kp</string>
</property>
</widget>
</item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>PosVel</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QSlider" name="sliderPosVel">
+ <property name="maximum">
+ <number>200</number>
+ </property>
+ <property name="value">
+ <number>80</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="labelPosVel">
+ <property name="text">
+ <string>80</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBoxNullspaceTargets">
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
index 1536cac11..a04a15377 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
@@ -48,6 +48,7 @@ namespace armarx
connect(_ui.sliderKpJla, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
connect(_ui.checkBoxAutoKp, SIGNAL(stateChanged(int)), this, SLOT(on_checkBoxAutoKp_stateChanged(int)));
connect(_ui.checkBoxSetOri, SIGNAL(stateChanged(int)), this, SLOT(on_checkBoxSetOri_stateChanged(int)));
+ connect(_ui.sliderPosVel, SIGNAL(valueChanged(int)), this, SLOT(on_horizontalSliderPosVel_valueChanged(int)));
auto addBtn = [&](QPushButton * btn, float px, float py, float pz, float rx, float ry, float rz)
{
@@ -186,11 +187,11 @@ namespace armarx
ik.setUpperArmLength(upper_arm_length);
ik.setLowerArmLength(lower_arm_length);
NJointCartesianNaturalPositionControllerConfigPtr config = CartesianNaturalPositionControllerProxy::MakeConfig(rns->getName(), elb->getName());
- _runCfg = config->runCfg;
- readRunCfgFromUi();
- updateKpSliderLabels();
- config->runCfg = _runCfg;
- _controller.reset(new CartesianNaturalPositionControllerProxy(ik, _robotUnit, side + "NaturalPosition", config));
+ CartesianNaturalPositionControllerConfig runCfg = config->runCfg;
+ readRunCfgFromUi(runCfg);
+ updateKpSliderLabels(runCfg);
+ config->runCfg = runCfg;
+ _controller.reset(new CartesianNaturalPositionControllerProxy(ik, rns, _robotUnit, side + "NaturalPosition", config));
_controller->init();
}
@@ -214,37 +215,37 @@ namespace armarx
_controller->setTarget(_tcpTarget, _setOri);
if (_controller->getDynamicKp().enabled)
{
- updateKpSliders();
+ updateKpSliders(_controller->getRuntimeConfig());
}
}
- void CartesianNaturalPositionControllerWidgetController::readRunCfgFromUi()
+ void CartesianNaturalPositionControllerWidgetController::readRunCfgFromUi(CartesianNaturalPositionControllerConfig& runCfg)
{
- _runCfg.elbowKp = _ui.sliderKpElbow->value() * 0.01f;
- _runCfg.jointLimitAvoidanceKp = _ui.sliderKpJla->value() * 0.01f;
+ runCfg.elbowKp = _ui.sliderKpElbow->value() * 0.01f;
+ runCfg.jointLimitAvoidanceKp = _ui.sliderKpJla->value() * 0.01f;
}
- void CartesianNaturalPositionControllerWidgetController::updateKpSliderLabels()
+ void CartesianNaturalPositionControllerWidgetController::updateKpSliderLabels(const CartesianNaturalPositionControllerConfig& runCfg)
{
- _ui.labelKpElbow->setText(QString::number(_runCfg.elbowKp, 'f', 2));
- _ui.labelKpJla->setText(QString::number(_runCfg.jointLimitAvoidanceKp, 'f', 2));
+ _ui.labelKpElbow->setText(QString::number(runCfg.elbowKp, 'f', 2));
+ _ui.labelKpJla->setText(QString::number(runCfg.jointLimitAvoidanceKp, 'f', 2));
}
- void CartesianNaturalPositionControllerWidgetController::updateKpSliders()
+ void CartesianNaturalPositionControllerWidgetController::updateKpSliders(const CartesianNaturalPositionControllerConfig& runCfg)
{
- _runCfg = _controller->getRuntimeConfig();
const QSignalBlocker blockKpElb(_ui.sliderKpElbow);
const QSignalBlocker blockKpJla(_ui.sliderKpJla);
- _ui.sliderKpElbow->setValue(_runCfg.elbowKp * 100);
- _ui.sliderKpJla->setValue(_runCfg.jointLimitAvoidanceKp * 100);
- updateKpSliderLabels();
- ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
+ _ui.sliderKpElbow->setValue(runCfg.elbowKp * 100);
+ _ui.sliderKpJla->setValue(runCfg.jointLimitAvoidanceKp * 100);
+ updateKpSliderLabels(runCfg);
+ ARMARX_IMPORTANT << VAROUT(runCfg.elbowKp) << VAROUT(runCfg.jointLimitAvoidanceKp);
}
void CartesianNaturalPositionControllerWidgetController::on_sliders_valueChanged(int)
{
+ CartesianNaturalPositionControllerConfig runCfg = _controller->getRuntimeConfig();
//ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
- readRunCfgFromUi();
- updateKpSliderLabels();
- _controller->setRuntimeConfig(_runCfg);
+ readRunCfgFromUi(runCfg);
+ updateKpSliderLabels(runCfg);
+ _controller->setRuntimeConfig(runCfg);
}
@@ -255,7 +256,7 @@ namespace armarx
_controller->setDynamicKp(dynamicKp);
if (_controller->getDynamicKp().enabled)
{
- updateKpSliders();
+ updateKpSliders(_controller->getRuntimeConfig());
}
}
@@ -285,6 +286,14 @@ namespace armarx
updateNullspaceTargets();
}
+ void CartesianNaturalPositionControllerWidgetController::on_horizontalSliderPosVel_valueChanged(int)
+ {
+ _ui.labelPosVel->setText(QString::number(_ui.sliderPosVel->value()));
+ float posVel = _ui.sliderPosVel->value();
+ _controller->setMaxVelocities(posVel);
+ //_runCfg = _controller->getRuntimeConfig();
+ }
+
//void CartesianNaturalPositionControllerWidgetController::on_pushButtonSendSettings_clicked()
//{
// std::lock_guard g{_allMutex};
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
index 08b0f5271..6cbd0242f 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
@@ -118,6 +118,7 @@ namespace armarx
void on_checkBoxSetOri_stateChanged(int);
void on_anyNullspaceCheckbox_stateChanged(int);
void on_anyNullspaceSlider_valueChanged(int);
+ void on_horizontalSliderPosVel_valueChanged(int);
signals:
/* QT signal declarations */
@@ -126,11 +127,11 @@ namespace armarx
private:
void deleteOldController();
- void readRunCfgFromUi();
+ void readRunCfgFromUi(CartesianNaturalPositionControllerConfig& runCfg);
void timerEvent(QTimerEvent*) override;
void updateTarget();
- void updateKpSliders();
- void updateKpSliderLabels();
+ void updateKpSliders(const CartesianNaturalPositionControllerConfig& runCfg);
+ void updateKpSliderLabels(const CartesianNaturalPositionControllerConfig& runCfg);
void updateNullspaceTargets();
@@ -153,7 +154,7 @@ namespace armarx
std::map<QObject*, Eigen::Vector3f> _deltaMapPos;
std::map<QObject*, Eigen::Vector3f> _deltaMapOri;
- CartesianNaturalPositionControllerConfig _runCfg;
+ //CartesianNaturalPositionControllerConfig _runCfg;
std::vector<NullspaceTarget> _nullspaceTargets;
};
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index 298626cd8..4b0b4be7d 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -74,7 +74,7 @@ namespace armarx
scale = std::min(scale, maxValue.at(j) / std::abs(vec(i)));
}
}
- return vec / scale;
+ return vec * scale;
}
const Eigen::VectorXf CartesianNaturalPositionController::calculateNullspaceTargetDifference()
@@ -292,4 +292,14 @@ namespace armarx
cfg.maxNullspaceAcceleration
);
}
+
+ Eigen::VectorXf CartesianNaturalPositionController::actualTcpVel(const Eigen::VectorXf& jointVel)
+ {
+ return (vcTcp.controller.ik->getJacobianMatrix(VirtualRobot::IKSolver::All) * jointVel);
+ }
+
+ Eigen::VectorXf CartesianNaturalPositionController::actualElbVel(const Eigen::VectorXf& jointVel)
+ {
+ return (vcElb.jacobi * jointVel);
+ }
}
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
index 68ea5f35b..81c51f351 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
@@ -90,6 +90,9 @@ namespace armarx
void setConfig(const CartesianNaturalPositionControllerConfig& cfg);
+ Eigen::VectorXf actualTcpVel(const Eigen::VectorXf& jointVel);
+ Eigen::VectorXf actualElbVel(const Eigen::VectorXf& jointVel);
+
CartesianVelocityControllerWithRamp vcTcp;
CartesianPositionController pcTcp;
CartesianVelocityController vcElb;
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index 522a513c7..6bea00678 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -28,10 +28,13 @@
using namespace armarx;
-CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
+CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const VirtualRobot::RobotNodeSetPtr& rns, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
: _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config)
{
_runCfg = _config->runCfg;
+ _velocityBaseSettings.basePosVel = _runCfg.maxTcpPosVel;
+ _velocityBaseSettings.baseOriVel = _runCfg.maxTcpOriVel;
+ _velocityBaseSettings.baseJointVelocity = CalculateMaxJointVelocity(rns, _runCfg.maxTcpPosVel);
}
NJointCartesianNaturalPositionControllerConfigPtr CartesianNaturalPositionControllerProxy::MakeConfig(const std::string& rns, const std::string& elbowNode)
@@ -102,6 +105,54 @@ CartesianNaturalPositionControllerConfig CartesianNaturalPositionControllerProxy
return _runCfg;
}
+std::vector<float> CartesianNaturalPositionControllerProxy::CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr& rns, float maxPosVel)
+{
+ size_t len = rns->getSize();
+ std::vector<Eigen::Vector3f> positions;
+ for (size_t i = 0; i < len; i++)
+ {
+ positions.push_back(rns->getNode(i)->getPositionInRootFrame());
+ }
+ positions.push_back(rns->getTCP()->getPositionInRootFrame());
+
+ std::vector<float> dists;
+ for (size_t i = 0; i < len; i++)
+ {
+ dists.push_back((positions.at(i) - positions.at(i + 1)).norm());
+ }
+
+ std::vector<float> result(len, 0);
+ float dist = 0;
+ for (int i = len - 1; i >= 0; i--)
+ {
+ dist += dists.at(i);
+ result.at(i) = maxPosVel / dist;
+ }
+ return result;
+}
+
+std::vector<float> CartesianNaturalPositionControllerProxy::ScaleVec(const std::vector<float>& vec, float scale)
+{
+ std::vector<float> result(vec.size(), 0);
+ for (size_t i = 0; i < vec.size(); i++)
+ {
+ result.at(i) = vec.at(i) * scale;
+ }
+ return result;
+}
+
+void CartesianNaturalPositionControllerProxy::setMaxVelocities(float referencePosVel)
+{
+ VelocityBaseSettings& v = _velocityBaseSettings;
+ float scale = referencePosVel / v.basePosVel;
+ _runCfg.maxTcpPosVel = v.basePosVel * v.scaleTcpPosVel * scale;
+ _runCfg.maxTcpOriVel = v.baseOriVel * v.scaleTcpOriVel * scale;
+ _runCfg.maxElbPosVel = v.basePosVel * v.scaleElbPosVel * scale;
+ _runCfg.maxJointVelocity = ScaleVec(v.baseJointVelocity, v.scaleJointVelocities * scale);
+ _runCfg.maxNullspaceVelocity = ScaleVec(v.baseJointVelocity, v.scaleNullspaceVelocities * scale);
+ _controller->setConfig(_runCfg);
+}
+
void CartesianNaturalPositionControllerProxy::cleanup()
{
if (_controllerCreated)
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
index 2455f1d2d..794e1e727 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
@@ -43,7 +43,21 @@ namespace armarx
float sigmaMM = 50;
void calculate(float error, float& KpElb, float& KpJla);
};
- CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config);
+
+ struct VelocityBaseSettings
+ {
+ float scaleTcpPosVel = 1;
+ float scaleTcpOriVel = 1;
+ float scaleElbPosVel = 1;
+ float scaleJointVelocities = 1;
+ float scaleNullspaceVelocities = 1;
+
+ Ice::FloatSeq baseJointVelocity;
+ float basePosVel;
+ float baseOriVel;
+ };
+
+ CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const VirtualRobot::RobotNodeSetPtr& rns, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config);
static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string& rns, const std::string& elbowNode);
void init();
@@ -57,12 +71,18 @@ namespace armarx
//void setNullSpaceControl(bool enabled);
+ static std::vector<float> CalculateMaxJointVelocity(const VirtualRobot::RobotNodeSetPtr& rns, float maxPosVel);
+
+ void setMaxVelocities(float referencePosVel);
+
+
void cleanup();
NJointCartesianNaturalPositionControllerInterfacePrx getInternalController();
void setDynamicKp(DynamicKp dynamicKp);
DynamicKp getDynamicKp();
+ static std::vector<float> ScaleVec(const std::vector<float>& vec, float scale);
private:
void updateDynamicKp();
@@ -78,5 +98,6 @@ namespace armarx
DynamicKp _dynamicKp;
Eigen::Matrix4f _tcpTarget;
NaturalIK::SoechtingForwardPositions _fwd;
+ VelocityBaseSettings _velocityBaseSettings;
};
}
--
GitLab