diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 08f5a512cd04670a0575adf645acbdeeff8ae21a..8069aeafdd1ed58ed6b8a45df986daff19b4ea83 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -19,12 +19,14 @@ namespace armarx
<< "KpPos " << cfg.KpPos << '\n'
<< "KpOri " << cfg.KpOri << '\n'
- << "maxPosVel " << cfg.maxPosVel << '\n'
- << "maxOriVel " << cfg.maxOriVel << '\n'
+ << "maxTcpPosVel " << cfg.maxTcpPosVel << '\n'
+ << "maxTcpOriVel " << cfg.maxTcpOriVel << '\n'
+ << "maxElpPosVel " << cfg.maxElbPosVel << '\n'
<< "jointLimitAvoidanceKp " << cfg.jointLimitAvoidanceKp << '\n'
- << "jointLimitAvoidanceScale " << cfg.jointLimitAvoidanceScale << '\n'
<< "elbowKp " << cfg.elbowKp << '\n'
+ << "nullspaceTargetKp " << cfg.nullspaceTargetKp << '\n'
+
;
return out;
}
@@ -165,6 +167,26 @@ namespace armarx
_rtPosController->setTarget(r.tcpTarget, r.elbowTarget);
}
}
+ if (_tripBufNullspaceTarget.updateReadBuffer())
+ {
+ ARMARX_RT_LOGF_IMPORTANT("updates in tripple buffer NullspaceTarget");
+ auto& r = _tripBufNullspaceTarget._getNonConstReadBuffer();
+ if (r.updated)
+ {
+ r.updated = false;
+ if (r.clearRequested)
+ {
+ _rtPosController->clearNullspaceTarget();
+ }
+ else
+ {
+ ARMARX_RT_LOGF_IMPORTANT("_rtPosController->setNullspaceTarget");
+ _rtPosController->setNullspaceTarget(r.nullspaceTarget);
+ }
+ r.clearRequested = false;
+
+ }
+ }
//run
//bool reachedTarget = false;
@@ -293,6 +315,27 @@ namespace armarx
_stopNowRequested = true;
}
+ void NJointCartesianNaturalPositionController::setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufPosCtrlMut};
+ auto& w = _tripBufNullspaceTarget.getWriteBuffer();
+ w.nullspaceTarget = nullspaceTarget;
+ w.clearRequested = false;
+ w.updated = true;
+ _tripBufNullspaceTarget.commitWrite();
+ ARMARX_IMPORTANT << "set new nullspaceTarget\n" << nullspaceTarget;
+ }
+
+ void NJointCartesianNaturalPositionController::clearNullspaceTarget(const Ice::Current&)
+ {
+ std::lock_guard g{_tripBufPosCtrlMut};
+ auto& w = _tripBufNullspaceTarget.getWriteBuffer();
+ w.clearRequested = true;
+ w.updated = true;
+ _tripBufNullspaceTarget.commitWrite();
+ ARMARX_IMPORTANT << "reset nullspaceTarget";
+ }
+
void NJointCartesianNaturalPositionController::onPublish(
const SensorAndControl&,
const DebugDrawerInterfacePrx& drawer,
@@ -374,3 +417,5 @@ namespace armarx
}
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
index 4bfd448af1a0f1b40337ec92b58fe03ed3cdfc50..3cb3403dd7e081bc4db25a074e5baa9cbbcd5cb9 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.h
@@ -44,6 +44,8 @@ namespace armarx
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;
+ void setNullspaceTarget(const Ice::FloatSeq& nullspaceTarget, const Ice::Current&) override;
+ void clearNullspaceTarget(const Ice::Current&) override;
void setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p, const Ice::Current& = Ice::emptyCurrent) override;
void resetVisualizationRobotGlobalPose(const Ice::Current& = Ice::emptyCurrent) override;
@@ -71,6 +73,12 @@ namespace armarx
CartesianNaturalPositionControllerConfig cfg;
bool updated = false;
};
+ struct TB_NT
+ {
+ std::vector<float> nullspaceTarget;
+ bool clearRequested = false;
+ bool updated = false;
+ };
struct RtToNonRtData
{
@@ -122,6 +130,7 @@ namespace armarx
mutable std::recursive_mutex _tripBufPosCtrlMut;
//TripleBuffer<CtrlData> _tripBufPosCtrl;
TripleBuffer<TB_Target> _tripBufTarget;
+ TripleBuffer<TB_NT> _tripBufNullspaceTarget;
TripleBuffer<TB_FFvel> _tripBufFFvel;
TripleBuffer<TB_Cfg> _tripBufCfg;
@@ -140,5 +149,7 @@ namespace armarx
//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 a9c862e336c5d771825234f05307db653f1c8c8c..dd7b4452001a5913c35ca23c85c549e56682657c 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidget.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>697</width>
- <height>553</height>
+ <width>767</width>
+ <height>493</height>
</rect>
</property>
<property name="windowTitle">
@@ -336,6 +336,64 @@
</item>
</layout>
</widget>
+ <widget class="QGroupBox" name="groupBoxNullspaceTargets">
+ <property name="geometry">
+ <rect>
+ <x>390</x>
+ <y>160</y>
+ <width>371</width>
+ <height>291</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Nullspace Targets</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_6">
+ <item row="1" column="1">
+ <widget class="QWidget" name="widgetNullspaceTargets" native="true">
+ <layout class="QGridLayout" name="gridLayoutNullspaceTargets">
+ <item row="0" column="1">
+ <widget class="QSlider" name="horizontalSliderExampleJoint1">
+ <property name="maximum">
+ <number>360</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QCheckBox" name="checkBoxExampleJoint1">
+ <property name="text">
+ <string>Joint1</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="labelExampleJoint1">
+ <property name="text">
+ <string>0</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <spacer name="verticalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </widget>
</widget>
<resources/>
<connections/>
diff --git a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
index 8a0907fad01eb9684bbb6f117f9d233e56e23c17..1536cac11e7c8627cfe9ca73b1d79c6f07846f56 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.cpp
@@ -35,6 +35,8 @@ namespace armarx
std::lock_guard g{_allMutex};
_ui.setupUi(getWidget());
+ connect(this, SIGNAL(invokeConnectComponentQt()), this, SLOT(onConnectComponentQt()), Qt::QueuedConnection);
+ connect(this, SIGNAL(invokeDisconnectComponentQt()), this, SLOT(onDisconnectComponentQt()), Qt::QueuedConnection);
//connect(_ui.pushButtonStop, SIGNAL(clicked()), this, SLOT(on_pushButtonStop_clicked()));
@@ -116,10 +118,14 @@ namespace armarx
_robot = RemoteRobot::createLocalCloneFromFile(_robotStateComponent, VirtualRobot::RobotIO::eStructure);
}
+ _controller.reset();
+ invokeConnectComponentQt();
+ }
+ void CartesianNaturalPositionControllerWidgetController::onConnectComponentQt()
+ {
_ui.comboBoxSide->addItem("Right");
_ui.comboBoxSide->addItem("Left");
_ui.comboBoxSide->setCurrentIndex(0);
- _controller.reset();
}
void CartesianNaturalPositionControllerWidgetController::onDisconnectComponent()
@@ -137,6 +143,31 @@ namespace armarx
VirtualRobot::RobotNodeSetPtr rns = _robot->getRobotNodeSet(side + "Arm");
+ _ui.gridLayoutNullspaceTargets->removeWidget(_ui.checkBoxExampleJoint1);
+ _ui.gridLayoutNullspaceTargets->removeWidget(_ui.labelExampleJoint1);
+ _ui.gridLayoutNullspaceTargets->removeWidget(_ui.horizontalSliderExampleJoint1);
+
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ QCheckBox* checkBox = new QCheckBox(QString::fromStdString(rns->getNode(i)->getName()));
+ QSlider* slider = new QSlider(Qt::Orientation::Horizontal);
+ slider->setMinimum(rns->getNode(i)->getJointLimitLo() / M_PI * 180);
+ slider->setMaximum(rns->getNode(i)->getJointLimitHi() / M_PI * 180);
+ slider->setValue(rns->getNode(i)->getJointValue() / M_PI * 180);
+ QLabel* label = new QLabel(QString::number(slider->value()));
+ _ui.gridLayoutNullspaceTargets->addWidget(checkBox, i, 0);
+ _ui.gridLayoutNullspaceTargets->addWidget(slider, i, 1);
+ _ui.gridLayoutNullspaceTargets->addWidget(label, i, 2);
+ connect(checkBox, SIGNAL(stateChanged(int)), this, SLOT(on_anyNullspaceCheckbox_stateChanged(int)));
+ connect(slider, SIGNAL(valueChanged(int)), this, SLOT(on_anyNullspaceSlider_valueChanged(int)));
+ NullspaceTarget nt;
+ nt.checkBox = checkBox;
+ nt.slider = slider;
+ nt.label = label;
+ nt.i = i;
+ _nullspaceTargets.emplace_back(nt);
+ }
+
VirtualRobot::RobotNodePtr sho1 = rns->getNode(1);
Eigen::Vector3f shoulder = sho1->getPositionInRootFrame();
@@ -234,6 +265,26 @@ namespace armarx
updateTarget();
}
+ void CartesianNaturalPositionControllerWidgetController::updateNullspaceTargets()
+ {
+ std::vector<float> nsTargets;
+ for (const NullspaceTarget& nt : _nullspaceTargets)
+ {
+ nsTargets.push_back(nt.checkBox->isChecked() ? nt.slider->value() * M_PI / 180 : std::nanf(""));
+ }
+ _controller->getInternalController()->setNullspaceTarget(nsTargets);
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_anyNullspaceCheckbox_stateChanged(int)
+ {
+ updateNullspaceTargets();
+ }
+
+ void CartesianNaturalPositionControllerWidgetController::on_anyNullspaceSlider_valueChanged(int)
+ {
+ updateNullspaceTargets();
+ }
+
//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 f8600c5f83f46920c3af787bb54fdc516a2a36ea..08b0f52714eae1eb6d33331b2433d9829c4bba4a 100644
--- a/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianNaturalPositionController/CartesianNaturalPositionControllerWidgetController.h
@@ -64,6 +64,14 @@ namespace armarx
Q_OBJECT
public:
+ struct NullspaceTarget
+ {
+ QCheckBox* checkBox;
+ QSlider* slider;
+ QLabel* label;
+ size_t i;
+ };
+
/**
* Controller Constructor
*/
@@ -102,14 +110,19 @@ namespace armarx
public slots:
/* QT slot declarations */
+ void onConnectComponentQt();
void on_pushButtonCreateController_clicked();
void on_anyDeltaPushButton_clicked();
void on_sliders_valueChanged(int);
void on_checkBoxAutoKp_stateChanged(int);
void on_checkBoxSetOri_stateChanged(int);
+ void on_anyNullspaceCheckbox_stateChanged(int);
+ void on_anyNullspaceSlider_valueChanged(int);
signals:
/* QT signal declarations */
+ void invokeConnectComponentQt();
+ void invokeDisconnectComponentQt();
private:
void deleteOldController();
@@ -118,6 +131,7 @@ namespace armarx
void updateTarget();
void updateKpSliders();
void updateKpSliderLabels();
+ void updateNullspaceTargets();
@@ -140,6 +154,8 @@ namespace armarx
std::map<QObject*, Eigen::Vector3f> _deltaMapPos;
std::map<QObject*, Eigen::Vector3f> _deltaMapOri;
CartesianNaturalPositionControllerConfig _runCfg;
+
+ std::vector<NullspaceTarget> _nullspaceTargets;
};
}
diff --git a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
index 86e63f5024c6e6091ec7a9834a9dac72fad3064a..9ea75137bd5490f7c2b68f41e4fee490aeeb9e9b 100644
--- a/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
+++ b/source/RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice
@@ -22,6 +22,9 @@
#pragma once
+//#include <ArmarXCore/interface/serialization/Eigen.ice>
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
+
module armarx
{
struct CartesianNaturalPositionControllerConfig
@@ -29,11 +32,13 @@ module armarx
float maxPositionAcceleration = 500;
float maxOrientationAcceleration = 1;
float maxNullspaceAcceleration = 2;
- float maxJointVelocity = 999;
+
+ Ice::FloatSeq maxJointVelocity;
+ Ice::FloatSeq maxNullspaceVelocity;
float jointLimitAvoidanceKp = 0.01;
- float jointLimitAvoidanceScale = 0;
float elbowKp = 1;
+ float nullspaceTargetKp = 1;
float thresholdOrientationNear = 0.1;
float thresholdOrientationReached = 0.05;
@@ -41,8 +46,9 @@ module armarx
float thresholdPositionReached = 5;
- float maxOriVel = 1;
- float maxPosVel = 80;
+ float maxTcpPosVel = 80;
+ float maxTcpOriVel = 1;
+ float maxElbPosVel = 80;
float KpOri = 1;
float KpPos = 1;
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
index f3e2415eb87434be1ea6b5c4a647ade0b7903dc6..1dc159fe82c83f3c81d4cbb7375a2bcf304caa0b 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianNaturalPositionController.ice
@@ -26,6 +26,7 @@
#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/core/CartesianNaturalPositionControllerConfig.ice>
+#include <ArmarXCore/interface/core/BasicVectorTypes.ice>
module armarx
{
@@ -52,6 +53,8 @@ module armarx
void setConfig(CartesianNaturalPositionControllerConfig cfg);
void setTarget(Eigen::Matrix4f tcpTarget, Eigen::Vector3f elbowTarget, bool setOrientation);
void setFeedForwardVelocity(Eigen::Vector6f vel);
+ void setNullspaceTarget(Ice::FloatSeq nullspaceTarget);
+ void clearNullspaceTarget();
//void setConfigAndWaypoints(NJointCartesianNaturalPositionControllerRuntimeConfig cfg, Eigen::Matrix4fSeq wps);
//void setConfigAndWaypoint(NJointCartesianNaturalPositionControllerRuntimeConfig cfg, Eigen::Matrix4f wp);
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
index 4d2e857bdb9d52db8ad197f472b9a0e9bfd05555..298626cd868ae9bfea02c7a104bb8889860ae551 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.cpp
@@ -52,46 +52,90 @@ namespace armarx
rns(rns)
{
ARMARX_CHECK_NOT_NULL(rns);
+ clearNullspaceTarget();
}
- Eigen::VectorXf CartesianNaturalPositionController::LimitInfNormTo(Eigen::VectorXf vec, float maxValue)
+ Eigen::VectorXf CartesianNaturalPositionController::LimitInfNormTo(const Eigen::VectorXf& vec, const std::vector<float>& maxValue)
{
- float infNorm = vec.lpNorm<Eigen::Infinity>();
- if (infNorm > maxValue)
+ if (maxValue.size() == 0)
{
- vec = vec / infNorm * maxValue;
+ return vec;
}
- return vec;
+ if (maxValue.size() != 1 && (int)maxValue.size() != vec.rows())
+ {
+ throw std::invalid_argument("maxValue.size != 1 and != maxValue.size");
+ }
+ float scale = 1;
+ for (int i = 0; i < vec.rows(); i++)
+ {
+ int j = maxValue.size() == 1 ? 0 : i;
+ if (std::abs(vec(i)) > maxValue.at(j) && maxValue.at(j) >= 0)
+ {
+ scale = std::min(scale, maxValue.at(j) / std::abs(vec(i)));
+ }
+ }
+ return vec / scale;
}
- const Eigen::VectorXf CartesianNaturalPositionController::calculate(float dt, VirtualRobot::IKSolver::CartesianSelection mode)
+ const Eigen::VectorXf CartesianNaturalPositionController::calculateNullspaceTargetDifference()
{
-
- int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
- int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
- Eigen::Vector3f pdTcp = posLen ? pcTcp.getPositionDiff(tcpTarget) : Eigen::Vector3f::Zero();
- Eigen::Vector3f odTcp = oriLen ? pcTcp.getOrientationDiff(tcpTarget) : Eigen::Vector3f::Zero();
- Eigen::VectorXf cartesianVel(posLen + oriLen);
- if (posLen)
- {
- cartesianVel.block<3, 1>(0, 0) = pdTcp;
- }
- if (oriLen)
+ Eigen::VectorXf jvNT = Eigen::VectorXf(rns->getSize());
+ for (size_t i = 0; i < rns->getSize(); i++)
{
- cartesianVel.block<3, 1>(posLen, 0) = odTcp;
+ if (std::isnan(nullspaceTarget(i)))
+ {
+ jvNT(i) = 0;
+ }
+ else
+ {
+ float diff = nullspaceTarget(i) - rns->getNode(i)->getJointValue();
+ if (rns->getNode(i)->isLimitless())
+ {
+ diff = math::Helpers::AngleModPI(diff);
+ }
+ jvNT(i) = diff;
+ }
}
+ return jvNT;
+ }
- Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
- //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());
+ const Eigen::VectorXf CartesianNaturalPositionController::calculate(float dt, VirtualRobot::IKSolver::CartesianSelection mode)
+ {
+
+ //int posLen = mode & VirtualRobot::IKSolver::Position ? 3 : 0;
+ //int oriLen = mode & VirtualRobot::IKSolver::Orientation ? 3 : 0;
+ //Eigen::Vector3f pdTcp = posLen ? pcTcp.getPositionDiff(tcpTarget) : Eigen::Vector3f::Zero();
+ //Eigen::Vector3f odTcp = oriLen ? pcTcp.getOrientationDiff(tcpTarget) : Eigen::Vector3f::Zero();
+ //Eigen::VectorXf cartesianVel(posLen + oriLen);
+ //if (posLen)
+ //{
+ // cartesianVel.block<3, 1>(0, 0) = pdTcp;
+ //}
+ //if (oriLen)
+ //{
+ // cartesianVel.block<3, 1>(posLen, 0) = odTcp;
+ //}
+ Eigen::VectorXf cartesianVelTcp = pcTcp.calculate(tcpTarget, mode);
+
+ //Eigen::Vector3f pdElb = pcElb.getPositionDiffVec3(elbowTarget);
+ ////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 cartesianVelElb = pcElb.calculatePos(elbowTarget);
+ Eigen::VectorXf jnvClamped = Eigen::VectorXf::Zero(rns->getSize());
if (nullSpaceControlEnabled)
{
- Eigen::VectorXf jvElb = elbowKp * vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
+ Eigen::VectorXf jnv = Eigen::VectorXf::Zero(rns->getSize());
+ Eigen::VectorXf jvElb = vcElb.calculate(cartesianVelElb, VirtualRobot::IKSolver::Position);
+
Eigen::VectorXf jvLA = jointLimitAvoidanceKp * vcTcp.controller.calculateJointLimitAvoidance();
+ Eigen::VectorXf jvNT = nullspaceTargetKp * calculateNullspaceTargetDifference();
+ ARMARX_IMPORTANT << deactivateSpam(1) << VAROUT(jvNT);
+
//ARMARX_IMPORTANT << VAROUT(jvElb);
- jnv = jvElb + jvLA;
+ jnv = jvElb + jvLA + jvNT;
+ jnvClamped = LimitInfNormTo(jnv, maxNullspaceVelocity);
}
//ARMARX_IMPORTANT << VAROUT(jnv);
@@ -99,7 +143,7 @@ namespace armarx
{
vcTcp.switchMode(lastJointVelocity, mode);
}
- Eigen::VectorXf jv = vcTcp.calculate(cartesianVel, jnv, dt);
+ Eigen::VectorXf jv = vcTcp.calculate(cartesianVelTcp, jnvClamped, dt);
//ARMARX_IMPORTANT << VAROUT(jv);
Eigen::VectorXf jvClamped = LimitInfNormTo(jv, maxJointVelocity);
@@ -132,6 +176,25 @@ namespace armarx
feedForwardVelocity.block<3, 1>(3, 0) = feedForwardVelocityOri;
}
+ void CartesianNaturalPositionController::setNullspaceTarget(const Eigen::VectorXf& nullspaceTarget)
+ {
+ this->nullspaceTarget = nullspaceTarget;
+ }
+
+ void CartesianNaturalPositionController::setNullspaceTarget(const std::vector<float>& nullspaceTarget)
+ {
+ ARMARX_CHECK_EXPRESSION(rns->getSize() == nullspaceTarget.size());
+ for (size_t i = 0; i < rns->getSize(); i++)
+ {
+ this->nullspaceTarget(i) = nullspaceTarget.at(i);
+ }
+ }
+
+ void CartesianNaturalPositionController::clearNullspaceTarget()
+ {
+ this->nullspaceTarget = Eigen::VectorXf::Constant(rns->getSize(), std::nanf(""));
+ }
+
void CartesianNaturalPositionController::clearFeedForwardVelocity()
{
feedForwardVelocity = Eigen::Vector6f::Zero();
@@ -172,6 +235,23 @@ namespace armarx
return elbowTarget;
}
+ const Eigen::VectorXf& CartesianNaturalPositionController::getCurrentNullspaceTarget() const
+ {
+ return nullspaceTarget;
+ }
+
+ bool CartesianNaturalPositionController::hasNullspaceTarget() const
+ {
+ for (int i = 0; i < nullspaceTarget.rows(); i++)
+ {
+ if (!std::isnan(nullspaceTarget(i)))
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
void CartesianNaturalPositionController::setNullSpaceControl(bool enabled)
{
nullSpaceControlEnabled = enabled;
@@ -188,8 +268,6 @@ namespace armarx
void CartesianNaturalPositionController::setConfig(const CartesianNaturalPositionControllerConfig& cfg)
{
jointLimitAvoidanceKp = cfg.jointLimitAvoidanceKp;
- jointLimitAvoidanceScale = cfg.jointLimitAvoidanceScale;
- elbowKp = cfg.elbowKp;
thresholdOrientationNear = cfg.thresholdOrientationNear;
thresholdOrientationReached = cfg.thresholdOrientationReached;
@@ -197,11 +275,16 @@ namespace armarx
thresholdPositionReached = cfg.thresholdPositionReached;
maxJointVelocity = cfg.maxJointVelocity;
+ maxNullspaceVelocity = cfg.maxNullspaceVelocity;
+
+ nullspaceTargetKp = cfg.nullspaceTargetKp;
- pcTcp.maxOriVel = cfg.maxOriVel;
- pcTcp.maxPosVel = cfg.maxPosVel;
+ pcTcp.maxPosVel = cfg.maxTcpPosVel;
+ pcTcp.maxOriVel = cfg.maxTcpOriVel;
pcTcp.KpOri = cfg.KpOri;
pcTcp.KpPos = cfg.KpPos;
+ pcElb.maxPosVel = cfg.maxElbPosVel;
+ pcElb.KpPos = cfg.elbowKp;
vcTcp.setMaxAccelerations(
cfg.maxPositionAcceleration,
diff --git a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
index 59d8dc1eac6005a5cdc4bc5fecf3a0c0d376c28e..68ea5f35b597900f64a706c443ea4d19b4b8342c 100644
--- a/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianNaturalPositionController.h
@@ -58,12 +58,16 @@ namespace armarx
);
- static Eigen::VectorXf LimitInfNormTo(Eigen::VectorXf vec, float maxValue);
+ static Eigen::VectorXf LimitInfNormTo(const Eigen::VectorXf& vec, const std::vector<float>& maxValue);
+ const Eigen::VectorXf calculateNullspaceTargetDifference();
const Eigen::VectorXf calculate(float dt, VirtualRobot::IKSolver::CartesianSelection mode = VirtualRobot::IKSolver::All);
void setTarget(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& elbowTarget);
void setFeedForwardVelocity(const Eigen::Vector6f& feedForwardVelocity);
void setFeedForwardVelocity(const Eigen::Vector3f& feedForwardVelocityPos, const Eigen::Vector3f& feedForwardVelocityOri);
+ void setNullspaceTarget(const Eigen::VectorXf& nullspaceTarget);
+ void setNullspaceTarget(const std::vector<float>& nullspaceTarget);
+ void clearNullspaceTarget();
void clearFeedForwardVelocity();
float getPositionError() const;
@@ -77,6 +81,8 @@ namespace armarx
const Eigen::Matrix4f& getCurrentTarget() const;
const Eigen::Vector3f getCurrentTargetPosition() const;
const Eigen::Vector3f& getCurrentElbowTarget() const;
+ const Eigen::VectorXf& getCurrentNullspaceTarget() const;
+ bool hasNullspaceTarget() const;
void setNullSpaceControl(bool enabled);
@@ -92,13 +98,15 @@ namespace armarx
Eigen::Matrix4f tcpTarget;
Eigen::Vector3f elbowTarget;
+ Eigen::VectorXf nullspaceTarget = Eigen::VectorXf(0);
float thresholdPositionReached = 0.f;
float thresholdOrientationReached = 0.f;
float thresholdPositionNear = 0.f;
float thresholdOrientationNear = 0.f;
- float maxJointVelocity = -1;
+ std::vector<float> maxJointVelocity;
+ std::vector<float> maxNullspaceVelocity;
Eigen::Vector6f feedForwardVelocity = Eigen::Vector6f::Zero();
@@ -109,7 +117,7 @@ namespace armarx
bool nullSpaceControlEnabled = true;
float jointLimitAvoidanceScale = 0.f;
float jointLimitAvoidanceKp = 0.f;
- float elbowKp = 0;
+ float nullspaceTargetKp = 0;
private:
VirtualRobot::RobotNodeSetPtr rns;
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index efe266741f146f560da821f0c8f17ecc1cdb45fd..25a88e737bb0685abeb07b421158487039b023ac 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
@@ -69,6 +69,20 @@ namespace armarx
return cartesianVel;
}
+ Eigen::VectorXf CartesianPositionController::calculatePos(const Eigen::Vector3f& targetPos) const
+ {
+ Eigen::VectorXf cartesianVel(3);
+ Eigen::Vector3f currentPos = tcp->getPositionInRootFrame();
+ Eigen::Vector3f errPos = targetPos - currentPos;
+ Eigen::Vector3f posVel = errPos * KpPos;
+ if (maxPosVel > 0)
+ {
+ posVel = math::MathUtils::LimitTo(posVel, maxPosVel);
+ }
+ cartesianVel.block<3, 1>(0, 0) = posVel;
+ return cartesianVel;
+ }
+
float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose) const
{
Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index cde4cd4734cf8daf6f21551e741e56c04629819a..a25b14e28aec91d34d7a0caa7b64e2bd4a0944b7 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/source/RobotAPI/libraries/core/CartesianPositionController.h
@@ -47,6 +47,7 @@ namespace armarx
CartesianPositionController& operator=(CartesianPositionController&&) = default;
Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode) const;
+ Eigen::VectorXf calculatePos(const Eigen::Vector3f& targetPos) const;
float getPositionError(const Eigen::Matrix4f& targetPose) const;
float getOrientationError(const Eigen::Matrix4f& targetPose) const;