From eef447918f837d4778fd1ef58bff094e82b76e2c Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Fri, 17 Apr 2020 19:30:58 +0200
Subject: [PATCH] CartesianNaturalPositionController: added position only mode
---
...ointCartesianNaturalPositionController.cpp | 43 +++++-----
...NJointCartesianNaturalPositionController.h | 24 ++++--
...artesianNaturalPositionControllerWidget.ui | 48 ++++++-----
...uralPositionControllerWidgetController.cpp | 49 ++++++++++-
...aturalPositionControllerWidgetController.h | 6 ++
...ointCartesianNaturalPositionController.ice | 2 +-
.../CartesianNaturalPositionController.cpp | 10 ++-
.../core/CartesianNaturalPositionController.h | 2 +
.../CartesianVelocityControllerWithRamp.cpp | 14 +++-
.../CartesianVelocityControllerWithRamp.h | 3 +
...artesianNaturalPositionControllerProxy.cpp | 82 ++++++++++++++-----
.../CartesianNaturalPositionControllerProxy.h | 34 ++++++--
12 files changed, 238 insertions(+), 79 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index fe33db6ac..08f5a512c 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -143,22 +143,29 @@ namespace armarx
{
auto& rt2nonrtBuf = _tripRt2NonRt.getWriteBuffer();
- if (_tripBufPosCtrl.updateReadBuffer())
+ if (_tripBufCfg.updateReadBuffer())
{
- ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer");
- auto& r = _tripBufPosCtrl._getNonConstReadBuffer();
- if (r.cfgUpdated)
+ ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer cfg");
+ auto& r = _tripBufCfg._getNonConstReadBuffer();
+ if (r.updated)
{
- r.cfgUpdated = false;
+ r.updated = false;
_rtPosController->setConfig(r.cfg);
ARMARX_RT_LOGF_IMPORTANT("updated the controller config");
}
- if (r.targetUpdated)
+ }
+ if (_tripBufTarget.updateReadBuffer())
+ {
+ ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer target");
+ auto& r = _tripBufTarget._getNonConstReadBuffer();
+ if (r.updated)
{
- r.targetUpdated = false;
+ r.updated = false;
+ _rtSetOrientation = r.setOrientation;
_rtPosController->setTarget(r.tcpTarget, r.elbowTarget);
}
}
+
//run
//bool reachedTarget = false;
//bool reachedFtLimit = false;
@@ -206,7 +213,8 @@ namespace armarx
{
- const Eigen::VectorXf& goal = _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble());
+ VirtualRobot::IKSolver::CartesianSelection mode = _rtSetOrientation ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position;
+ const Eigen::VectorXf& goal = _rtPosController->calculate(timeSinceLastIteration.toSecondsDouble(), mode);
//write targets
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelTargets))
@@ -229,25 +237,22 @@ namespace armarx
void NJointCartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current&)
{
std::lock_guard g{_tripBufPosCtrlMut};
- auto& w = _tripBufPosCtrl.getWriteBuffer();
- w.targetUpdated = false;
- w.cfgUpdated = true;
- w.feedForwardVelocityUpdated = false;
+ auto& w = _tripBufCfg.getWriteBuffer();
w.cfg = cfg;
- _tripBufPosCtrl.commitWrite();
+ w.updated = true;
+ _tripBufCfg.commitWrite();
ARMARX_IMPORTANT << "set new config\n" << cfg;
}
- void NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, const Ice::Current&)
+ void NJointCartesianNaturalPositionController::setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Ice::Current&)
{
std::lock_guard g{_tripBufPosCtrlMut};
- auto& w = _tripBufPosCtrl.getWriteBuffer();
- w.targetUpdated = true;
- w.cfgUpdated = false;
- w.feedForwardVelocityUpdated = false;
+ auto& w = _tripBufTarget.getWriteBuffer();
w.tcpTarget = tcpTarget;
w.elbowTarget = elbowTarget;
- _tripBufPosCtrl.commitWrite();
+ w.setOrientation = setOrientation;
+ w.updated = true;
+ _tripBufTarget.commitWrite();
ARMARX_IMPORTANT << "set new target\n" << tcpTarget << "\nelbow: " << elbowTarget.transpose();
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
index 55d4f70b3..4bfd448af 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -41,7 +41,7 @@ namespace armarx
//bool hasReachedForceLimit(const Ice::Current& = Ice::emptyCurrent) override;
void setConfig(const CartesianNaturalPositionControllerConfig& cfg, const Ice::Current& = Ice::emptyCurrent) override;
- void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, const Ice::Current& = Ice::emptyCurrent) override;
+ void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget, bool setOrientation, const Ice::Current& = Ice::emptyCurrent) override;
void setFeedForwardVelocity(const Eigen::Vector6f& vel, const Ice::Current&) override;
void stopMovement(const Ice::Current& = Ice::emptyCurrent) override;
@@ -54,15 +54,22 @@ namespace armarx
//structs
private:
- struct CtrlData
+ struct TB_Target
{
Eigen::Matrix4f tcpTarget;
Eigen::Vector3f elbowTarget;
+ bool setOrientation;
+ bool updated = false;
+ };
+ struct TB_FFvel
+ {
Eigen::Vector6f feedForwardVelocity;
+ bool updated = false;
+ };
+ struct TB_Cfg
+ {
CartesianNaturalPositionControllerConfig cfg;
- bool targetUpdated = false;
- bool cfgUpdated = false;
- bool feedForwardVelocityUpdated = false;
+ bool updated = false;
};
struct RtToNonRtData
@@ -91,6 +98,7 @@ namespace armarx
VirtualRobot::RobotNodePtr _rtRobotRoot;
std::unique_ptr<Ctrl> _rtPosController;
+ bool _rtSetOrientation = true;
Eigen::VectorXf _rtJointVelocityFeedbackBuffer;
std::vector<const float*> _rtVelSensors;
@@ -112,8 +120,10 @@ namespace armarx
//buffers
mutable std::recursive_mutex _tripBufPosCtrlMut;
- TripleBuffer<CtrlData> _tripBufPosCtrl;
- TripleBuffer<Eigen::Matrix4f> _tripBufTarget;
+ //TripleBuffer<CtrlData> _tripBufPosCtrl;
+ TripleBuffer<TB_Target> _tripBufTarget;
+ TripleBuffer<TB_FFvel> _tripBufFFvel;
+ TripleBuffer<TB_Cfg> _tripBufCfg;
mutable std::recursive_mutex _tripRt2NonRtMutex;
TripleBuffer<RtToNonRtData> _tripRt2NonRt;
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
index 32f83881e..a9c862e33 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
@@ -184,6 +184,9 @@
<property name="text">
<string>Set Orientation</string>
</property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
</widget>
</item>
<item row="3" column="0">
@@ -270,60 +273,67 @@
<string>Parameters</string>
</property>
<layout class="QGridLayout" name="gridLayout_5">
- <item row="0" column="2">
- <widget class="QLabel" name="labelKpElbow">
- <property name="text">
- <string>0.0</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
+ <item row="1" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>KpElbow</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QSlider" name="sliderKpElbow">
+ <item row="2" column="1">
+ <widget class="QSlider" name="sliderKpJla">
<property name="maximum">
<number>200</number>
</property>
- <property name="singleStep">
- <number>1</number>
- </property>
- <property name="value">
- <number>100</number>
- </property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
- <item row="1" column="0">
+ <item row="2" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>KpJLA</string>
</property>
</widget>
</item>
+ <item row="2" column="2">
+ <widget class="QLabel" name="labelKpJla">
+ <property name="text">
+ <string>0.0</string>
+ </property>
+ </widget>
+ </item>
<item row="1" column="1">
- <widget class="QSlider" name="sliderKpJla">
+ <widget class="QSlider" name="sliderKpElbow">
<property name="maximum">
<number>200</number>
</property>
+ <property name="singleStep">
+ <number>1</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="1" column="2">
- <widget class="QLabel" name="labelKpJla">
+ <widget class="QLabel" name="labelKpElbow">
<property name="text">
<string>0.0</string>
</property>
</widget>
</item>
+ <item row="0" column="0" colspan="2">
+ <widget class="QCheckBox" name="checkBoxAutoKp">
+ <property name="text">
+ <string>Automatic Kp</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</widget>
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
index 37549b77c..8a0907fad 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
@@ -44,6 +44,8 @@ namespace armarx
connect(_ui.pushButtonCreateController, SIGNAL(clicked()), this, SLOT(on_pushButtonCreateController_clicked()));
connect(_ui.sliderKpElbow, SIGNAL(valueChanged(int)), this, SLOT(on_sliders_valueChanged(int)));
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)));
auto addBtn = [&](QPushButton * btn, float px, float py, float pz, float rx, float ry, float rz)
{
@@ -155,6 +157,7 @@ namespace armarx
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));
_controller->init();
@@ -173,7 +176,15 @@ namespace armarx
Eigen::AngleAxisf aa(deltaOri.norm(), deltaOri.normalized());
math::Helpers::Orientation(_tcpTarget) = aa.toRotationMatrix() * math::Helpers::Orientation(_tcpTarget);
}
- _controller->setTarget(_tcpTarget);
+ updateTarget();
+ }
+ void CartesianNaturalPositionControllerWidgetController::updateTarget()
+ {
+ _controller->setTarget(_tcpTarget, _setOri);
+ if (_controller->getDynamicKp().enabled)
+ {
+ updateKpSliders();
+ }
}
void CartesianNaturalPositionControllerWidgetController::readRunCfgFromUi()
@@ -181,12 +192,46 @@ namespace armarx
_runCfg.elbowKp = _ui.sliderKpElbow->value() * 0.01f;
_runCfg.jointLimitAvoidanceKp = _ui.sliderKpJla->value() * 0.01f;
}
+ void CartesianNaturalPositionControllerWidgetController::updateKpSliderLabels()
+ {
+ _ui.labelKpElbow->setText(QString::number(_runCfg.elbowKp, 'f', 2));
+ _ui.labelKpJla->setText(QString::number(_runCfg.jointLimitAvoidanceKp, 'f', 2));
+ }
+ void CartesianNaturalPositionControllerWidgetController::updateKpSliders()
+ {
+ _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);
+ }
void CartesianNaturalPositionControllerWidgetController::on_sliders_valueChanged(int)
{
+ //ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
readRunCfgFromUi();
- ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
+ updateKpSliderLabels();
_controller->setRuntimeConfig(_runCfg);
+
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_checkBoxAutoKp_stateChanged(int)
+ {
+ CartesianNaturalPositionControllerProxy::DynamicKp dynamicKp;
+ dynamicKp.enabled = _ui.checkBoxAutoKp->isChecked();
+ _controller->setDynamicKp(dynamicKp);
+ if (_controller->getDynamicKp().enabled)
+ {
+ updateKpSliders();
+ }
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_checkBoxSetOri_stateChanged(int)
+ {
+ _setOri = _ui.checkBoxSetOri->isChecked();
+ updateTarget();
}
//void CartesianNaturalPositionControllerWidgetController::on_pushButtonSendSettings_clicked()
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
index c42100984..f8600c5f8 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
@@ -105,6 +105,8 @@ namespace armarx
void on_pushButtonCreateController_clicked();
void on_anyDeltaPushButton_clicked();
void on_sliders_valueChanged(int);
+ void on_checkBoxAutoKp_stateChanged(int);
+ void on_checkBoxSetOri_stateChanged(int);
signals:
/* QT signal declarations */
@@ -113,6 +115,9 @@ namespace armarx
void deleteOldController();
void readRunCfgFromUi();
void timerEvent(QTimerEvent*) override;
+ void updateTarget();
+ void updateKpSliders();
+ void updateKpSliderLabels();
@@ -127,6 +132,7 @@ namespace armarx
VirtualRobot::RobotPtr _robot;
std::vector<Eigen::Matrix4f> _lastParsedWPs;
bool _supportsFT{false};
+ bool _setOri = true;
mutable std::recursive_mutex _allMutex;
int _timer;
Eigen::Matrix4f _tcpTarget;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
index 2ff9815f1..f3e2415eb 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
@@ -50,7 +50,7 @@ module armarx
//idempotent bool hasReachedForceLimit();
void setConfig(CartesianNaturalPositionControllerConfig cfg);
- void setTarget(Eigen::Matrix4f tcpTarget, Eigen::Vector3f elbowTarget);
+ void setTarget(Eigen::Matrix4f tcpTarget, Eigen::Vector3f elbowTarget, bool setOrientation);
void setFeedForwardVelocity(Eigen::Vector6f vel);
//void setConfigAndWaypoints(NJointCartesianNaturalPositionControllerRuntimeConfig cfg, Eigen::Matrix4fSeq wps);
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index 8a55fe263..4d2e857bd 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -48,6 +48,7 @@ namespace armarx
pcTcp(tcp ? tcp : rns->getTCP()),
vcElb(rns, elbow),
pcElb(elbow),
+ lastJointVelocity(Eigen::VectorXf::Constant(rns->getSize(), 0.f)),
rns(rns)
{
ARMARX_CHECK_NOT_NULL(rns);
@@ -81,7 +82,7 @@ namespace armarx
}
Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
- ARMARX_IMPORTANT << deactivateSpam(0.1) << VAROUT(pdElb) << VAROUT(elbowTarget) << VAROUT(pcElb.getTcp()->getPositionInRootFrame());
+ //ARMARX_IMPORTANT << deactivateSpam(0.1) << VAROUT(pdElb) << VAROUT(elbowTarget) << VAROUT(pcElb.getTcp()->getPositionInRootFrame());
Eigen::VectorXf cartesianVelElb(3);
cartesianVelElb.block<3, 1>(0, 0) = pdElb;
Eigen::VectorXf jnv = Eigen::VectorXf::Zero(rns->getSize());
@@ -94,7 +95,11 @@ namespace armarx
}
//ARMARX_IMPORTANT << VAROUT(jnv);
- Eigen::VectorXf jv = vcTcp.calculate(cartesianVel, jnv, mode);
+ if (vcTcp.getMode() != mode)
+ {
+ vcTcp.switchMode(lastJointVelocity, mode);
+ }
+ Eigen::VectorXf jv = vcTcp.calculate(cartesianVel, jnv, dt);
//ARMARX_IMPORTANT << VAROUT(jv);
Eigen::VectorXf jvClamped = LimitInfNormTo(jv, maxJointVelocity);
@@ -104,6 +109,7 @@ namespace armarx
clearFeedForwardVelocity();
}
+ lastJointVelocity = jvClamped;
return jvClamped;
}
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
index 9c86ea7f6..59d8dc1ea 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
@@ -88,6 +88,7 @@ namespace armarx
CartesianPositionController pcTcp;
CartesianVelocityController vcElb;
CartesianPositionController pcElb;
+ Eigen::VectorXf lastJointVelocity;
Eigen::Matrix4f tcpTarget;
Eigen::Vector3f elbowTarget;
@@ -99,6 +100,7 @@ namespace armarx
float maxJointVelocity = -1;
+
Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
Eigen::Vector6f cartesianVelocity = Eigen::Vector6f::Zero();
bool autoClearFeedForward = true;
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
index 3c9f18f9b..228f765b8 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp
@@ -41,7 +41,7 @@ namespace armarx
void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
{
Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
- Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
+ //Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));
Eigen::FullPivLU<Eigen::MatrixXf> lu_decomp(jacobi);
@@ -57,6 +57,18 @@ namespace armarx
}
+ void CartesianVelocityControllerWithRamp::switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode)
+ {
+ Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
+ cartesianVelocityRamp.setState(jacobi * currentJointVelocity, mode);
+ this->mode = mode;
+ }
+
+ VirtualRobot::IKSolver::CartesianSelection CartesianVelocityControllerWithRamp::getMode()
+ {
+ return mode;
+ }
+
Eigen::VectorXf CartesianVelocityControllerWithRamp::calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt)
{
Eigen::VectorXf nullspaceVel = controller.calculateNullspaceVelocity(cartesianVel, jointLimitAvoidanceScale, mode);
diff --git a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
index 0c47ae498..ff1ff8453 100644
--- a/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
+++ b/source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h
@@ -52,6 +52,9 @@ namespace armarx
[[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);
+ void switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode);
+ VirtualRobot::IKSolver::CartesianSelection getMode();
+
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt);
Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt);
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
index 6b7390c4b..522a513c7 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.cpp
@@ -23,13 +23,15 @@
#include "CartesianNaturalPositionControllerProxy.h"
#include <VirtualRobot/math/Helpers.h>
+#include <ArmarXCore/core/logging/Logging.h>
using namespace armarx;
CartesianNaturalPositionControllerProxy::CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config)
- : ik(ik), robotUnit(robotUnit), controllerName(controllerName), config(config)
+ : _ik(ik), _robotUnit(robotUnit), _controllerName(controllerName), _config(config)
{
+ _runCfg = _config->runCfg;
}
NJointCartesianNaturalPositionControllerConfigPtr CartesianNaturalPositionControllerProxy::MakeConfig(const std::string& rns, const std::string& elbowNode)
@@ -42,50 +44,92 @@ NJointCartesianNaturalPositionControllerConfigPtr CartesianNaturalPositionContro
void CartesianNaturalPositionControllerProxy::init()
{
- NJointControllerInterfacePrx ctrl = robotUnit->getNJointController(controllerName);
+ NJointControllerInterfacePrx ctrl = _robotUnit->getNJointController(_controllerName);
if (ctrl)
{
- controllerCreated = false;
- controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
- controller->setConfig(config->runCfg);
+ _controllerCreated = false;
+ _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
+ _controller->setConfig(_runCfg);
}
else
{
- ctrl = robotUnit->createNJointController("NJointCartesianNaturalPositionController", controllerName, config);
- controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
- controllerCreated = true;
+ ctrl = _robotUnit->createNJointController("NJointCartesianNaturalPositionController", _controllerName, _config);
+ _controller = NJointCartesianNaturalPositionControllerInterfacePrx::checkedCast(ctrl);
+ _controllerCreated = true;
}
- controller->activateController();
+ _controller->activateController();
}
-void CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, std::optional<float> minElbowHeigh)
+void CartesianNaturalPositionControllerProxy::setTarget(const Eigen::Matrix4f& tcpTarget, bool setOrientation, std::optional<float> minElbowHeigh)
{
- NaturalIK::SoechtingForwardPositions fwd = ik.solveSoechtingIK(math::Helpers::Position(tcpTarget), minElbowHeigh);
- controller->setTarget(tcpTarget, fwd.elbow);
+ _tcpTarget = tcpTarget;
+ _fwd = _ik.solveSoechtingIK(math::Helpers::Position(tcpTarget), minElbowHeigh);
+ _controller->setTarget(_tcpTarget, _fwd.elbow, setOrientation);
+ updateDynamicKp();
+}
+void CartesianNaturalPositionControllerProxy::updateDynamicKp()
+{
+ if (_dynamicKp.enabled)
+ {
+ float error = (math::Helpers::Position(_tcpTarget) - _fwd.wrist).norm();
+ float KpElb, KpJla;
+ ARMARX_IMPORTANT << VAROUT(error);
+ _dynamicKp.calculate(error, KpElb, KpJla);
+ //ARMARX_IMPORTANT << VAROUT()
+ _runCfg.elbowKp = KpElb;
+ _runCfg.jointLimitAvoidanceKp = KpJla;
+ _controller->setConfig(_runCfg);
+ ARMARX_IMPORTANT << VAROUT(_runCfg.elbowKp) << VAROUT(_runCfg.jointLimitAvoidanceKp);
+
+ }
+}
+
+void CartesianNaturalPositionControllerProxy::DynamicKp::calculate(float error, float& KpElb, float& KpJla)
+{
+ float f = std::exp(-0.5f * (error * error) / (sigmaMM * sigmaMM));
+ KpElb = f * maxKp;
+ KpJla = (1 - f) * maxKp;
}
void CartesianNaturalPositionControllerProxy::setRuntimeConfig(CartesianNaturalPositionControllerConfig runCfg)
{
- controller->setConfig(runCfg);
- this->config->runCfg = runCfg;
+ _controller->setConfig(runCfg);
+ this->_runCfg = runCfg;
+}
+
+CartesianNaturalPositionControllerConfig CartesianNaturalPositionControllerProxy::getRuntimeConfig()
+{
+ return _runCfg;
}
void CartesianNaturalPositionControllerProxy::cleanup()
{
- if (controllerCreated)
+ if (_controllerCreated)
{
// delete controller only if it was created
- controller->deactivateAndDeleteController();
+ _controller->deactivateAndDeleteController();
}
else
{
// if the controller existed, only deactivate it
- controller->deactivateController();
+ _controller->deactivateController();
}
- controllerCreated = false;
+ _controllerCreated = false;
}
NJointCartesianNaturalPositionControllerInterfacePrx CartesianNaturalPositionControllerProxy::getInternalController()
{
- return controller;
+ return _controller;
+}
+
+void CartesianNaturalPositionControllerProxy::setDynamicKp(DynamicKp dynamicKp)
+{
+ _dynamicKp = dynamicKp;
+ updateDynamicKp();
}
+
+CartesianNaturalPositionControllerProxy::DynamicKp CartesianNaturalPositionControllerProxy::getDynamicKp()
+{
+ return _dynamicKp;
+}
+
diff --git a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
index 64807afcb..2455f1d2d 100644
--- a/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
+++ b/source/RobotAPI/libraries/natik/CartesianNaturalPositionControllerProxy.h
@@ -36,14 +36,22 @@ namespace armarx
class CartesianNaturalPositionControllerProxy
{
public:
- CartesianNaturalPositionControllerProxy(const NaturalIK& ik, const RobotUnitInterfacePrx& robotUnit, const std::string& controllerName, NJointCartesianNaturalPositionControllerConfigPtr config);
+ struct DynamicKp
+ {
+ bool enabled = false;
+ float maxKp = 1;
+ float sigmaMM = 50;
+ void calculate(float error, float& KpElb, float& KpJla);
+ };
+ CartesianNaturalPositionControllerProxy(const NaturalIK& _ik, const RobotUnitInterfacePrx& _robotUnit, const std::string& _controllerName, NJointCartesianNaturalPositionControllerConfigPtr _config);
static NJointCartesianNaturalPositionControllerConfigPtr MakeConfig(const std::string& rns, const std::string& elbowNode);
void init();
- void setTarget(const Eigen::Matrix4f& tcpTarget, std::optional<float> minElbowHeight = std::nullopt);
+ void setTarget(const Eigen::Matrix4f& tcpTarget, bool setOrientation, std::optional<float> minElbowHeight = std::nullopt);
void setRuntimeConfig(CartesianNaturalPositionControllerConfig runCfg);
+ CartesianNaturalPositionControllerConfig getRuntimeConfig();
//void setTargetVelocity(const Eigen::VectorXf& cv);
@@ -53,14 +61,22 @@ namespace armarx
NJointCartesianNaturalPositionControllerInterfacePrx getInternalController();
+ void setDynamicKp(DynamicKp dynamicKp);
+ DynamicKp getDynamicKp();
private:
- NaturalIK ik;
- RobotUnitInterfacePrx robotUnit;
- std::string side;
- std::string controllerName;
- NJointCartesianNaturalPositionControllerConfigPtr config;
- bool controllerCreated = false;
- NJointCartesianNaturalPositionControllerInterfacePrx controller;
+ void updateDynamicKp();
+
+ NaturalIK _ik;
+ RobotUnitInterfacePrx _robotUnit;
+ std::string _side;
+ std::string _controllerName;
+ NJointCartesianNaturalPositionControllerConfigPtr _config;
+ CartesianNaturalPositionControllerConfig _runCfg;
+ bool _controllerCreated = false;
+ NJointCartesianNaturalPositionControllerInterfacePrx _controller;
+ DynamicKp _dynamicKp;
+ Eigen::Matrix4f _tcpTarget;
+ NaturalIK::SoechtingForwardPositions _fwd;
};
}
--
GitLab