Skip to content
Snippets Groups Projects
Commit b9f88181 authored by Mirko Wächter's avatar Mirko Wächter
Browse files

Merge branch 'kinematic_unit' into 'master' 

Kinematic unit GUI

Add visualization of failures.
Remove column  temperature.

Only tested in simulation!

See merge request !17
parents 8191f12a 54b06956
No related branches found
No related tags found
1 merge request!17Kinematic unit GUI
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.ui
+ 81
− 15
View file @ b9f88181
......@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>464</width>
<height>190</height>
<height>255</height>
</rect>
</property>
<property name="sizePolicy">
......@@ -45,13 +45,6 @@
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="labelTopic">
<property name="text">
<string>&lt;not set&gt;</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="labelRobotNodeSetName">
<property name="text">
......@@ -73,13 +66,6 @@
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>RobotState Topic Name</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="labelKinematicUnitName">
<property name="sizePolicy">
......@@ -93,6 +79,86 @@
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>RobotState Topic Name</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="labelTopic">
<property name="text">
<string>&lt;not set&gt;</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Enable ValueValidator</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QCheckBox" name="checkBox">
<property name="enabled">
<bool>true</bool>
</property>
<property name="text">
<string>Enabled</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="labelHistoryLength">
<property name="text">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;History length&lt;/p&gt;&lt;p&gt;(period length to calculate errors)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QSpinBox" name="spinBoxHistory">
<property name="suffix">
<string> ms</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>10000</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="labelMaxMinCurrent">
<property name="text">
<string>Max/Min value for current</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBoxMaxMinCurrent">
<property name="prefix">
<string>+/- </string>
</property>
<property name="maximum">
<double>50.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>5.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>
......
source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+ 99
− 45
View file @ b9f88181
......@@ -76,6 +76,9 @@ KinematicUnitGuiPlugin::KinematicUnitGuiPlugin()
KinematicUnitWidgetController::KinematicUnitWidgetController() :
kinematicUnitNode(nullptr),
enableValueValidator(true),
historyTime(100000), // 1/10 s
currentValueMax(5.0f),
selectedControlMode(ePositionControl)
{
rootVisu = NULL;
......@@ -310,16 +313,26 @@ void KinematicUnitWidgetController::configured()
{
ARMARX_VERBOSE << "KinematicUnitWidget::configured()";
kinematicUnitName = dialog->proxyFinder->getSelectedProxyName().toStdString();
enableValueValidator = dialog->ui->checkBox->isChecked();
historyTime = dialog->ui->spinBoxHistory->value() * 1000;
currentValueMax = dialog->ui->doubleSpinBoxMaxMinCurrent->value();
}
void KinematicUnitWidgetController::loadSettings(QSettings* settings)
{
kinematicUnitName = settings->value("kinematicUnitName", KINEMATIC_UNIT_NAME_DEFAULT).toString().toStdString();
enableValueValidator = settings->value("enableValueValidator", true).toBool();
historyTime = settings->value("historyTime", 100).toInt() * 1000;
currentValueMax = settings->value("currentValueMax", 5.0).toFloat();
}
void KinematicUnitWidgetController::saveSettings(QSettings* settings)
{
settings->setValue("kinematicUnitName", QString::fromStdString(kinematicUnitName));
settings->setValue("enableValueValidator", enableValueValidator);
assert(historyTime % 1000 == 0);
settings->setValue("historyTime", (int)(historyTime / 1000));
settings->setValue("currentValueMax", currentValueMax);
}
......@@ -372,7 +385,6 @@ void KinematicUnitWidgetController::connectSlots()
connect(this, SIGNAL(jointTorquesReported()), this, SLOT(updateJointTorquesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointCurrentsReported()), this, SLOT(updateJointCurrentsTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointControlModesReported()), this, SLOT(updateControlModesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointMotorTemperaturesReported()), this, SLOT(updateJointMotorTemperaturesTable()), Qt::QueuedConnection);
connect(this, SIGNAL(jointStatusesReported()), this, SLOT(updateJointStatusesTable()), Qt::QueuedConnection);
connect(ui.tableJointList, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(selectJointFromTableWidget(int, int)), Qt::QueuedConnection);
......@@ -611,7 +623,7 @@ bool KinematicUnitWidgetController::initGUIComboBox(VirtualRobot::RobotNodeSetPt
bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet)
{
uint numberOfColumns = 11;
uint numberOfColumns = 10;
//dont use clear! It is not required here and somehow causes the tabel to have
//numberOfColumns additional empty columns and rn.size() additional empty rows.
......@@ -642,7 +654,6 @@ bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNod
<< "Velocity [rad/s]"
<< "Torque [Nm]"
<< "Current [A]"
<< "Temperature [C]"
<< "Operation"
<< "Error"
<< "Enabled"
......@@ -1084,12 +1095,13 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (!getWidget() || !robotNodeSet)
if (!getWidget() || !robotNodeSet || jointCurrentHistory.size() == 0)
{
return;
}
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
QTableWidgetItem* newItem;
NameValueMap reportedJointCurrents = jointCurrentHistory.back().second;
NameValueMap::const_iterator it;
for (unsigned int i = 0; i < rn.size(); i++)
......@@ -1105,34 +1117,8 @@ void KinematicUnitWidgetController::updateJointCurrentsTable()
newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
ui.tableJointList->setItem(i, eTabelColumnCurrent, newItem);
}
}
void KinematicUnitWidgetController::updateJointMotorTemperaturesTable()
{
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (!getWidget() || !robotNodeSet)
{
return;
}
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
QTableWidgetItem* newItem;
NameValueMap::const_iterator it;
for (unsigned int i = 0; i < rn.size(); i++)
{
it = reportedJointMotorTemperatures.find(rn[i]->getName());
if (it == reportedJointMotorTemperatures.end())
{
continue;
}
const float currentValue = it->second;
newItem = new QTableWidgetItem(QString::number(cutJitter(currentValue)));
ui.tableJointList->setItem(i, eTabelColumnTemperature, newItem);
}
highlightCriticalValues();
}
void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
......@@ -1195,28 +1181,26 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
if (!aValueChanged && reportedJointCurrents.size() > 0)
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
if (aValueChanged && jointCurrents.size() > 0)
{
return;
jointCurrentHistory.push_back(std::pair<Ice::Long, NameValueMap>(timestamp, jointCurrents));
}
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
reportedJointCurrents = jointCurrents;
while (jointCurrentHistory.size() > 1 && jointCurrentHistory.back().first - jointCurrentHistory.front().first > historyTime)
{
jointCurrentHistory.pop_front();
}
emit jointCurrentsReported();
if (jointCurrentHistory.size() > 0)
{
emit jointCurrentsReported();
}
}
void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
if (!aValueChanged && reportedJointMotorTemperatures.size() > 0)
{
return;
}
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
reportedJointMotorTemperatures = jointMotorTemperatures;
emit jointMotorTemperaturesReported();
//Not implemented
}
void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current&)
......@@ -1264,6 +1248,76 @@ void KinematicUnitWidgetController::updateModel()
robot->setJointValues(usedNodes, jv);
}
void KinematicUnitWidgetController::highlightCriticalValues()
{
if (!enableValueValidator)
{
return;
}
boost::recursive_mutex::scoped_lock lock(mutexNodeSet);
std::vector< RobotNodePtr > rn = robotNodeSet->getAllRobotNodes();
// get standard line colors
static std::vector<QBrush> standardColors;
if (standardColors.size() == 0)
{
for (unsigned int i = 0; i < rn.size(); i++)
{
// all cells of a row have the same color
standardColors.push_back(ui.tableJointList->item(i, eTabelColumnCurrent)->background());
}
}
// check robot current value of nodes
for (unsigned int i = 0; i < rn.size(); i++)
{
bool isZero = true;
float smoothedValue = 0;
for (auto historyIt = jointCurrentHistory.begin(); historyIt != jointCurrentHistory.end(); historyIt++)
{
NameValueMap reportedJointCurrents = historyIt->second;
NameValueMap::const_iterator it = reportedJointCurrents.find(rn[i]->getName());
if (it == reportedJointCurrents.end())
{
continue;
}
const float currentValue = std::fabs(it->second);
smoothedValue = 0.5f*currentValue + 0.5f*smoothedValue;
if (currentValue != 0)
{
isZero = false;
}
}
NameStatusMap::const_iterator it;
it = reportedJointStatuses.find(rn[i]->getName());
JointStatus currentStatus = it->second;
if (isZero && currentStatus.enabled)
{
// current value is zero, but joint is enabled
ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::darkYellow);
}
else if (smoothedValue > currentValueMax)
{
// current value is too high
ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(Qt::red);
}
else
{
// everything seems to work as expected
ui.tableJointList->item(i, eTabelColumnCurrent)->setBackground(standardColors[i]);
}
}
}
void KinematicUnitWidgetController::setMutex3D(boost::shared_ptr<boost::recursive_mutex> mutex3D)
{
//ARMARX_IMPORTANT << "KinematicUnitWidgetController controller " << getInstanceName() << ": set mutex " << mutex3D.get();
......
source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+ 8
− 5
View file @ b9f88181
......@@ -124,7 +124,6 @@ namespace armarx
eTabelColumnVelocity,
eTabelColumnTorque,
eTabelColumnCurrent,
eTabelColumnTemperature,
eTabelColumnOperation,
eTabelColumnError,
eTabelColumnEnabled,
......@@ -180,7 +179,6 @@ namespace armarx
void jointControlModesReported();
void jointTorquesReported();
void jointCurrentsReported();
void jointMotorTemperaturesReported();
void jointStatusesReported();
......@@ -205,7 +203,6 @@ namespace armarx
void updateJointVelocitiesTable();
void updateJointTorquesTable();
void updateJointCurrentsTable();
void updateJointMotorTemperaturesTable();
void updateJointStatusesTable();
void updateControlModesTable();
void updateKinematicUnitListInDialog() {}
......@@ -258,6 +255,8 @@ namespace armarx
void updateModel();
void highlightCriticalValues();
protected slots:
void showVisuLayers(bool show);
private:
......@@ -270,12 +269,16 @@ namespace armarx
bool initGUIComboBox(VirtualRobot::RobotNodeSetPtr robotNodeSet);
bool initGUIJointListTable(VirtualRobot::RobotNodeSetPtr robotNodeSet);
bool enableValueValidator;
Ice::Long historyTime;
float currentValueMax;
NameValueMap reportedJointAngles;
NameValueMap reportedJointVelocities;
NameControlModeMap reportedJointControlModes;
NameValueMap reportedJointTorques;
NameValueMap reportedJointCurrents;
NameValueMap reportedJointMotorTemperatures;
std::deque<std::pair<Ice::Long, NameValueMap>> jointCurrentHistory;
NameStatusMap reportedJointStatuses;
std::vector<float> dirty_squaresum_old;
......
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