From bcd89bba1d54522b757008b0abe64f51c5de9335 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Tue, 10 Mar 2015 18:03:53 +0100
Subject: [PATCH] changed channel names for haptic data
---
.../components/units/HapticObserver.cpp | 31 ++++++++++---------
.../components/units/TCPControlUnit.cpp | 16 +++++-----
.../HapticUnitPlugin/HapticUnitGuiPlugin.cpp | 5 +++
3 files changed, 31 insertions(+), 21 deletions(-)
diff --git a/source/RobotAPI/components/units/HapticObserver.cpp b/source/RobotAPI/components/units/HapticObserver.cpp
index 6517a665f..8f5d82933 100644
--- a/source/RobotAPI/components/units/HapticObserver.cpp
+++ b/source/RobotAPI/components/units/HapticObserver.cpp
@@ -56,23 +56,26 @@ void HapticObserver::reportSensorValues(const std::string& device, const std::st
Eigen::MatrixXf eigenMatrix = matrix->toEigen();
float max = eigenMatrix.maxCoeff();
float mean = eigenMatrix.mean();
- if(!existsChannel(device))
+ std::string channelName = name;
+ if(!existsChannel(channelName))
{
- offerChannel(device, "Haptic data");
- offerDataFieldWithDefault(device, "name", Variant(name), "Name of the tactile sensor");
- offerDataFieldWithDefault(device, "matrix", matrix, "Raw tactile matrix data");
- offerDataFieldWithDefault(device, "max", Variant(max), "Maximum value");
- offerDataFieldWithDefault(device, "mean", Variant(mean), "Mean value");
- offerDataFieldWithDefault(device, "timestamp", timestampPtr, "Timestamp");
- offerDataFieldWithDefault(device, "rate", Variant(0.0f), "Sample rate");
+ offerChannel(channelName, "Haptic data");
+ offerDataFieldWithDefault(channelName, "device", Variant(device), "Device of the tactile sensor");
+ offerDataFieldWithDefault(channelName, "name", Variant(name), "Name of the tactile sensor");
+ offerDataFieldWithDefault(channelName, "matrix", matrix, "Raw tactile matrix data");
+ offerDataFieldWithDefault(channelName, "max", Variant(max), "Maximum value");
+ offerDataFieldWithDefault(channelName, "mean", Variant(mean), "Mean value");
+ offerDataFieldWithDefault(channelName, "timestamp", timestampPtr, "Timestamp");
+ offerDataFieldWithDefault(channelName, "rate", Variant(0.0f), "Sample rate");
}
else
{
- setDataField(device, "name", Variant(name));
- setDataField(device, "matrix", matrix);
- setDataField(device, "max", Variant(max));
- setDataField(device, "mean", Variant(mean));
- setDataField(device, "timestamp", timestampPtr);
+ setDataField(channelName, "device", Variant(device));
+ setDataField(channelName, "name", Variant(name));
+ setDataField(channelName, "matrix", matrix);
+ setDataField(channelName, "max", Variant(max));
+ setDataField(channelName, "mean", Variant(mean));
+ setDataField(channelName, "timestamp", timestampPtr);
}
/*if(statistics.count(device) > 0)
{
@@ -87,7 +90,7 @@ void HapticObserver::reportSensorValues(const std::string& device, const std::st
statistics.insert(std::map<std::string,HapticSampleStatistics>::value_type(device, HapticSampleStatistics(100, timestamp->timestamp)));
}*/
- updateChannel(device);
+ updateChannel(channelName);
}
PropertyDefinitionsPtr HapticObserver::createPropertyDefinitions()
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index 32660272f..f21953806 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -342,7 +342,7 @@ namespace armarx
if(data.orientationVelocity)
{
data.orientationVelocity = FramedVector3::ChangeFrame(localRobot, *data.orientationVelocity, refFrame);
-// ARMARX_INFO << deactivateSpam(1) << "Orientation in " << refFrame << ": " << data.orientationVelocity->output();
+ ARMARX_INFO << deactivateSpam(1) << "Orientation in " << refFrame << ": " << data.orientationVelocity->output();
Eigen::Matrix3f rot;
rot = Eigen::AngleAxisf(data.orientationVelocity->z*cycleTime*0.001, Eigen::Vector3f::UnitZ())
* Eigen::AngleAxisf(data.orientationVelocity->y*cycleTime*0.001, Eigen::Vector3f::UnitY())
@@ -359,7 +359,7 @@ namespace armarx
if(data.translationVelocity)
{
data.translationVelocity = FramedVector3::ChangeFrame(localRobot, *data.translationVelocity, refFrame);
-// ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": " << data.translationVelocity->toEigen();
+ ARMARX_INFO << deactivateSpam(1) << "Translation in " << refFrame << ": " << data.translationVelocity->toEigen();
m.block(0,3,3,1) = tcpNode->getGlobalPose().block(0,3,3,1) + data.translationVelocity->toEigen()*cycleTime*0.001;
}
@@ -396,7 +396,7 @@ namespace armarx
// ARMARX_VERBOSE << deactivateSpam(1) << "Old Pos: \n" << robotNodeSet->getTCP()->getGlobalPose();
// dIK->setVerbose(true);
Eigen::VectorXf jointDelta;
- dIK->computeSteps(jointDelta, 0.8f, 0.001, 1, &DifferentialIK::computeStep); // 1.0, 0.00001, 50
+ dIK->computeSteps(jointDelta, 0.8f, 0.001, 1, &EDifferentialIK::computeStep); // 1.0, 0.00001, 50
// ARMARX_VERBOSE << deactivateSpam(1) << "New Goal: \n" << robotNodeSet->getTCP()->getGlobalPose();
MatrixXf fullJac = dIK->calcFullJacobian();
@@ -666,10 +666,11 @@ namespace armarx
for (unsigned int i=0; i<nodes.size();i++)
{
+ ARMARX_INFO_S << VAROUT(nodes[i]->getJointValue()) << VAROUT(dTheta[i]);
jv[i] = (nodes[i]->getJointValue() + dTheta[i]);
if (boost::math::isnan(jv[i]) || boost::math::isinf(jv[i]))
{
- VR_WARNING << "Aborting, invalid joint value (nan)" << endl;
+ ARMARX_WARNING_S << "Aborting, invalid joint value (nan)" << endl;
dofWeights = tempDOFWeights;
return false;
}
@@ -795,10 +796,11 @@ namespace armarx
}
- applyDOFWeightsToJacobian(Jacobian);
-
+// applyDOFWeightsToJacobian(Jacobian);
+ ARMARX_INFO_S << VAROUT(Jacobian);
MatrixXf pseudo = computePseudoInverseJacobianMatrix(Jacobian);
-
+ ARMARX_INFO_S << VAROUT(Jacobian);
+ ARMARX_INFO_S << VAROUT(error);
return pseudo * error;
}
diff --git a/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp b/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
index 56acc084c..ff77ae283 100644
--- a/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp
@@ -170,6 +170,11 @@ void HapticUnitWidget::createMatrixWidgets()
void HapticUnitWidget::updateDisplayWidgets()
{
+ QLayoutItem *child;
+ while ((child = layout->takeAt(0)) != 0) {
+ delete child;
+ }
+
int i = 0;
ChannelRegistry channels = hapticObserverProxy->getAvailableChannels(false);
for(std::pair<std::string, ChannelRegistryEntry> pair : channels)
--
GitLab