Skip to content
Snippets Groups Projects
Commit da78fbf4 authored by Raphael Grimm's avatar Raphael Grimm
Browse files

Start extracting the widget to configure an impedance controller

parent 59ba37ee
No related branches found
No related tags found
1 merge request!76Update cartesian impedance and arviz
Hide whitespace changes
Inline Side-by-side
Showing
with 512 additions and 2 deletions
source/RobotAPI/gui-plugins/CartesianImpedanceController/CMakeLists.txt
+ 1
− 1
View file @ da78fbf4
......@@ -11,7 +11,7 @@ set(HEADERS
)
set(GUI_MOC_HDRS ${HEADERS})
set(GUI_UIS CartesianImpedanceControllerWidget.ui)
set(COMPONENT_LIBS NJointControllerGuiPluginUtility)
set(COMPONENT_LIBS NJointControllerGuiPluginUtility RobotAPINJointControllerWidgets)
if(ArmarXGui_FOUND)
armarx_gui_library(CartesianImpedanceControllerGuiPlugin "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
......
source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.h
+ 2
− 0
View file @ da78fbf4
......@@ -25,6 +25,8 @@
#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h>
#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h>
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h>
#include <RobotAPI/gui-plugins/CartesianImpedanceController/ui_CartesianImpedanceControllerWidget.h>
namespace armarx
......
source/RobotAPI/libraries/CMakeLists.txt
+ 2
− 1
View file @ da78fbf4
......@@ -20,4 +20,5 @@ add_subdirectory(natik)
add_subdirectory(armem)
add_subdirectory(aron)
add_subdirectory(NJointControllerGuiPluginUtility)
\ No newline at end of file
add_subdirectory(NJointControllerGuiPluginUtility)
add_subdirectory(RobotAPINJointControllerWidgets)
\ No newline at end of file
source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CMakeLists.txt 0 → 100644
+ 17
− 0
View file @ da78fbf4
set(LIB_NAME RobotAPINJointControllerWidgets)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
RobotUnit
)
set(SOURCES CartesianImpedanceControllerConfigWidget.cpp)
set(HEADERS CartesianImpedanceControllerConfigWidget.h)
set(GUI_MOC_HDRS CartesianImpedanceControllerConfigWidget.h)
set(GUI_UIS CartesianImpedanceControllerConfigWidget.ui)
if(ArmarXGui_FOUND)
armarx_gui_library("${LIB_NAME}" "${SOURCES}" "${GUI_MOC_HDRS}" "${GUI_UIS}" "" "${LIBS}")
endif()
source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp 0 → 100644
+ 238
− 0
View file @ da78fbf4
#include "CartesianImpedanceControllerConfigWidget.h"
namespace armarx
{
void clearLayout(QLayout* layout)
{
QLayoutItem* item;
while ((item = layout->takeAt(0)))
{
if (item->layout())
{
clearLayout(item->layout());
delete item->layout();
}
if (item->widget())
{
delete item->widget();
}
delete item;
}
}
CartesianImpedanceControllerConfigWidget::CartesianImpedanceControllerConfigWidget(QWidget* parent)
: QWidget(parent)
{
ui.setupUi(this);
kxyz.addWidget(ui.doubleSpinBoxKTX);
kxyz.addWidget(ui.doubleSpinBoxKTY);
kxyz.addWidget(ui.doubleSpinBoxKTZ);
kxyz.setMinMax(0, 1000);
kxyz.set(500);
krpy.addWidget(ui.doubleSpinBoxKRX);
krpy.addWidget(ui.doubleSpinBoxKRY);
krpy.addWidget(ui.doubleSpinBoxKRZ);
krpy.setMinMax(0, 1000);
dxyz.set(1);
dxyz.addWidget(ui.doubleSpinBoxDTX);
dxyz.addWidget(ui.doubleSpinBoxDTY);
dxyz.addWidget(ui.doubleSpinBoxDTZ);
dxyz.setMinMax(0, 1000);
dxyz.set(0);
drpy.addWidget(ui.doubleSpinBoxDRX);
drpy.addWidget(ui.doubleSpinBoxDRY);
drpy.addWidget(ui.doubleSpinBoxDRZ);
drpy.setMinMax(0, 1000);
drpy.set(0);
using T = CartesianImpedanceControllerConfigWidget;
connect(ui.pushButtonNullspaceUpdateJoints, &QPushButton::clicked, this, &T::on_pushButtonNullspaceUpdateJoints_clicked);
connect(ui.pushButtonNullspaceSend, &QPushButton::clicked, this, &T::on_pushButtonNullspaceSend_clicked);
connect(ui.pushButtonSettingsSend, &QPushButton::clicked, this, &T::on_pushButtonSettingsSend_clicked);
}
void CartesianImpedanceControllerConfigWidget::loadSettings(QSettings* settings, const QString& prefix)
{
if (prefix.size())
{
settings->beginGroup(prefix);
}
///TODO
if (prefix.size())
{
settings->endGroup();
}
}
void CartesianImpedanceControllerConfigWidget::saveSettings(QSettings* settings, const QString& prefix)
{
if (prefix.size())
{
settings->beginGroup("layer");
}
///TODO
if (prefix.size())
{
settings->endGroup();
}
}
NJointTaskSpaceImpedanceControlConfigPtr
CartesianImpedanceControllerConfigWidget::readFullCFG(const Eigen::Vector3f& targPos, const Eigen::Quaternionf& targOri) const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(_rns);
NJointTaskSpaceImpedanceControlConfigPtr cfg = new NJointTaskSpaceImpedanceControlConfig;
cfg->nodeSetName = _rns->getName();
cfg->desiredPosition = targPos;
cfg->desiredOrientation = targOri;
kxyz.get(cfg->Kpos);
krpy.get(cfg->Kori);
dxyz.get(cfg->Dpos);
drpy.get(cfg->Dori);
auto [joint, knull, dnull] = readNullspaceCFG();
cfg->desiredJointPositions = joint;
cfg->Knull = knull;
cfg->Dnull = dnull;
cfg->torqueLimit = ui.doubleSpinBoxTorqueLim->value();
return cfg;
}
NJointTaskSpaceImpedanceControlRuntimeConfig
CartesianImpedanceControllerConfigWidget::readRuntimeCFG() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(_rns);
NJointTaskSpaceImpedanceControlRuntimeConfig cfg;
kxyz.get(cfg.Kpos);
krpy.get(cfg.Kori);
dxyz.get(cfg.Dpos);
drpy.get(cfg.Dori);
auto [joint, knull, dnull] = readNullspaceCFG();
cfg.desiredJointPositions = joint;
cfg.Knull = knull;
cfg.Dnull = dnull;
cfg.torqueLimit = ui.doubleSpinBoxTorqueLim->value();
return cfg;
}
std::tuple<Eigen::VectorXf, Eigen::VectorXf, Eigen::VectorXf>
CartesianImpedanceControllerConfigWidget::readNullspaceCFG() const
{
ARMARX_TRACE;
return
{
jointValues.get<Eigen::VectorXf>(),
jointKnull.get<Eigen::VectorXf>(),
jointDnull.get<Eigen::VectorXf>()
};
}
void CartesianImpedanceControllerConfigWidget::on_pushButtonNullspaceSend_clicked()
{
ARMARX_TRACE;
if (!_controller)
{
return;
}
auto [joint, knull, dnull] = readNullspaceCFG();
_controller->setNullspaceConfig(joint, knull, dnull);
}
void CartesianImpedanceControllerConfigWidget::on_pushButtonNullspaceUpdateJoints_clicked()
{
ARMARX_TRACE;
if (!_rns)
{
return;
}
jointValues.set(_rns);
}
void CartesianImpedanceControllerConfigWidget::on_pushButtonSettingsSend_clicked()
{
ARMARX_TRACE;
if (!_controller)
{
return;
}
_controller->setConfig(readRuntimeCFG());
}
void CartesianImpedanceControllerConfigWidget::setController(
const NJointTaskSpaceImpedanceControlInterfacePrx& prx
)
{
_controller = prx;
}
void CartesianImpedanceControllerConfigWidget::setRNS(
const VirtualRobot::RobotNodeSetPtr& rns
)
{
ARMARX_TRACE;
_rns = rns;
auto lay = ui.gridLayoutNullspace;
clearLayout(lay);
lay->addWidget(new QLabel{"Joint"}, 0, 0);
lay->addWidget(new QLabel{"Target"}, 0, 1);
lay->addWidget(new QLabel{"Knull"}, 0, 2);
lay->addWidget(new QLabel{"Dnull"}, 0, 3);
jointValues.clear();
jointKnull.clear();
jointDnull.clear();
if (!_rns)
{
return;
}
int i = 1;
for (const auto& rn : _rns->getAllRobotNodes())
{
ARMARX_TRACE;
const auto&& n = rn->getName();
lay->addWidget(new QLabel{QString::fromStdString(n)}, i, 0);
{
auto b = new QDoubleSpinBox;
jointValues.addWidget(b);
lay->addWidget(b, i, 1);
const auto lo = rn->getJointLimitLow();
const auto hi = rn->getJointLimitHigh();
b->setMinimum(lo);
b->setMaximum(hi);
b->setValue((lo + hi) / 2);
}
{
auto b = new QDoubleSpinBox;
jointKnull.addWidget(b);
lay->addWidget(b, i, 2);
b->setMinimum(0);
b->setMaximum(1000);
b->setValue(1);
}
{
auto b = new QDoubleSpinBox;
jointDnull.addWidget(b);
lay->addWidget(b, i, 3);
b->setMinimum(0);
b->setMaximum(1000);
b->setValue(0);
}
++i;
}
jointKnull.set(5);
jointDnull.set(0);
on_pushButtonNullspaceUpdateJoints_clicked();
}
}
source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.h 0 → 100644
+ 53
− 0
View file @ da78fbf4
#pragma once
#include <QWidget>
#include <QSettings>
#include <VirtualRobot/RobotNodeSet.h>
#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToVector.h>
#include <RobotAPI/libraries/NJointControllerGuiPluginUtility/SpinBoxToPose.h>
#include <RobotAPI/interface/units/RobotUnit/TaskSpaceActiveImpedanceControl.h>
#include <RobotAPI/libraries/RobotAPINJointControllerWidgets/ui_CartesianImpedanceControllerConfigWidget.h>
namespace armarx
{
class CartesianImpedanceControllerConfigWidget : public QWidget
{
public:
CartesianImpedanceControllerConfigWidget(QWidget* parent = nullptr);
void loadSettings(QSettings* settings, const QString& prefix = "");
void saveSettings(QSettings* settings, const QString& prefix = "");
void on_pushButtonNullspaceUpdateJoints_clicked();
void on_pushButtonNullspaceSend_clicked();
void on_pushButtonSettingsSend_clicked();
NJointTaskSpaceImpedanceControlConfigPtr
readFullCFG(const Eigen::Vector3f& targPos, const Eigen::Quaternionf& targOri) const;
NJointTaskSpaceImpedanceControlRuntimeConfig
readRuntimeCFG() const;
std::tuple<Eigen::VectorXf, Eigen::VectorXf, Eigen::VectorXf>
readNullspaceCFG() const;
///if null -> send buttons deactivated
void setController(const NJointTaskSpaceImpedanceControlInterfacePrx& prx);
void setRNS(const VirtualRobot::RobotNodeSetPtr& rns);
Ui::CartesianImpedanceControllerConfigWidget ui;
SpinBoxToVector<QDoubleSpinBox, 3> kxyz;
SpinBoxToVector<QDoubleSpinBox, 3> krpy;
SpinBoxToVector<QDoubleSpinBox, 3> dxyz;
SpinBoxToVector<QDoubleSpinBox, 3> drpy;
SpinBoxToVector<QDoubleSpinBox> jointValues;
SpinBoxToVector<QDoubleSpinBox> jointKnull;
SpinBoxToVector<QDoubleSpinBox> jointDnull;
private:
NJointTaskSpaceImpedanceControlInterfacePrx _controller;
VirtualRobot::RobotNodeSetPtr _rns;
};
}
source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui 0 → 100644
+ 199
− 0
View file @ da78fbf4
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>CartesianImpedanceControllerConfigWidget</class>
<widget class="QWidget" name="CartesianImpedanceControllerConfigWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>540</width>
<height>303</height>
</rect>
</property>
<property name="windowTitle">
<string>CartesianImpedanceControllerWidget</string>
</property>
<layout class="QGridLayout" name="gridLayout">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item row="0" column="0">
<widget class="QWidget" name="widget" native="true">
<layout class="QGridLayout" name="gridLayout_3">
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBoxKRX"/>
</item>
<item row="4" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBoxDRZ"/>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>K RPY</string>
</property>
</widget>
</item>
<item row="5" column="1" colspan="3">
<widget class="QDoubleSpinBox" name="doubleSpinBoxTorqueLim"/>
</item>
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBoxKTX"/>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_3">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>K XYZ</string>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QDoubleSpinBox" name="doubleSpinBoxKRY"/>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Torque Limit</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QDoubleSpinBox" name="doubleSpinBoxKTY"/>
</item>
<item row="4" column="2">
<widget class="QDoubleSpinBox" name="doubleSpinBoxDRY"/>
</item>
<item row="4" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBoxDRX"/>
</item>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBoxDTX"/>
</item>
<item row="0" column="0" colspan="4">
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
<string>Nullspace</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QWidget" name="widget_3" native="true">
<layout class="QGridLayout" name="gridLayout_4">
<item row="0" column="0" colspan="2">
<layout class="QGridLayout" name="gridLayoutNullspace">
<item row="0" column="1">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Pos</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_9">
<property name="text">
<string>Joint</string>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QLabel" name="label_10">
<property name="text">
<string>Dnull</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Knull</string>
</property>
</widget>
</item>
</layout>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="pushButtonNullspaceSend">
<property name="text">
<string>Send</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QPushButton" name="pushButtonNullspaceUpdateJoints">
<property name="text">
<string>Set to current</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>
<item row="3" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBoxDTZ"/>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>D RPY</string>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QDoubleSpinBox" name="doubleSpinBoxDTY"/>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>D XYZ</string>
</property>
</widget>
</item>
<item row="2" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBoxKRZ"/>
</item>
<item row="1" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBoxKTZ"/>
</item>
<item row="6" column="0" colspan="4">
<widget class="QPushButton" name="pushButtonSettingsSend">
<property name="text">
<string>Send</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
<resources/>
<connections/>
</ui>
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment