From 095762d5320e0c7352f0a208accc251ae411bc74 Mon Sep 17 00:00:00 2001
From: andreeatulbure <andreea_tulbure@yahoo.de>
Date: Thu, 1 Jun 2017 16:33:51 +0200
Subject: [PATCH] added debug info
---
.gitignore | 1 +
.../OrientedTactileSensor.scx | 6 +--
.../config/OrientedTactileSensorUnitApp.cfg | 4 +-
.../OrientedTactileSensorUnit.cpp | 46 +++++++++++++++++--
.../OrientedTactileSensorUnit.h | 4 +-
5 files changed, 50 insertions(+), 11 deletions(-)
diff --git a/.gitignore b/.gitignore
index 04042697d..853198195 100644
--- a/.gitignore
+++ b/.gitignore
@@ -58,3 +58,4 @@ scenarios/*/*/ttyACM*.log
scenarios/*/*/config/datapath.cfg
*.icegrid.xml
+data/RobotAPI/logs
diff --git a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
index 14b01f42a..f12dab3cf 100644
--- a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
+++ b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="OrientedTactileSensor" lastChange="2017-03-22.17:10:15" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
- <application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI"/>
- <application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI"/>
+<scenario name="OrientedTactileSensor" creation="2017-02-27.01:48:55 PM" lastChange="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
+ <application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI" enabled="true"/>
+ <application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI" enabled="true"/>
</scenario>
diff --git a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
index e9a50e156..b74a4804f 100644
--- a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
@@ -79,7 +79,7 @@
# - Default: 65524 3 12 65534 65534 1 1208 119 58726 1000 943
# - Case sensitivity: no
# - Required: no
-ArmarX.OrientedTactileSensorUnit.CalibrationData = 65524 3 12 65534 65534 1 1208 119 58726 1000 943
+ArmarX.OrientedTactileSensorUnit.CalibrationData = 65535 65535 65535 65535 65535 65535 65535 65535 65535 65535 65535
# ArmarX.OrientedTactileSensorUnit.EnableProfiling: enable profiler which is used for logging performance events
@@ -159,7 +159,7 @@ ArmarX.OrientedTactileSensorUnit.calibrateSensor = 0
# - Default: 1
# - Case sensitivity: no
# - Required: no
-ArmarX.OrientedTactileSensorUnit.logData = 0
+ArmarX.OrientedTactileSensorUnit.logData = 1
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 017f09da0..10ade5e65 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -120,6 +120,14 @@ void OrientedTactileSensorUnit::onInitComponent()
if (loadCalibration())
{
ARMARX_IMPORTANT << "loaded calibration";
+ first = true;
+ /*std::string line;
+ getline(arduinoIn, line, '\n');
+ ARMARX_IMPORTANT<<line;
+ SensorData dataInit = getValues(line.c_str());
+ ARMARX_IMPORTANT<<dataInit.qw<<" "dataInit.qx<<" "<<dataInit.qy<<" "<<dataInit.qz;
+ Eigen::Quaternionf initialOrientation = Eigen::Quaternionf(dataInit.qw, dataInit.qx, dataInit.qy, dataInit.qz);
+ inverseOrientation = initialOrientation.inverse();*/
}
}
readTask = new RunningTask<OrientedTactileSensorUnit>(this, &OrientedTactileSensorUnit::run);
@@ -158,6 +166,7 @@ void OrientedTactileSensorUnit::run()
RotationRate sampleRotation;
sampleRotation.timestamp = now;
sampleRotation.orientation = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
+ //sampleRotation.orientation = sampleRotation.orientation * inverseOrientation;
if (samplesRotation.size() < maxSamplesRotation)
{
samplesRotation.push_back(sampleRotation);
@@ -171,6 +180,8 @@ void OrientedTactileSensorUnit::run()
oldsampleRotation.timestamp = samplesRotation.at(sampleIndexRotation).timestamp;
oldsampleRotation.orientation = samplesRotation.at(sampleIndexRotation).orientation;
Eigen::AngleAxisf aa(sampleRotation.orientation * oldsampleRotation.orientation.inverse());
+ //Eigen::AngleAxisf ax(sampleRotation.orientation);
+ ARMARX_IMPORTANT << "aa: " << aa.axis() << " " << aa.angle();
rotationRate = aa.angle() / (sampleRotation.timestamp - oldsampleRotation.timestamp).toSecondsDouble();
}
}
@@ -233,15 +244,40 @@ void OrientedTactileSensorUnit::run()
if (getProperty<bool>("logData").getValue())
{
Eigen::Quaternionf orientationQuaternion = Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz);
- Eigen::Matrix3f quaternionMatrix = orientationQuaternion.toRotationMatrix();
+ //Eigen::Matrix3f quaternionMatrix = orientationQuaternion.toRotationMatrix();
+ //Eigen::Matrix4f quat4Matrix = Eigen::Matrix4f::Identity();
+ //quat4Matrix.block(0, 0, 3, 3) = quaternionMatrix;
+ //ARMARX_IMPORTANT << "first" << first << " " << orientationQuaternion.w() << " " << orientationQuaternion.x() << " " << orientationQuaternion.y() << " " << orientationQuaternion.z();
+ if (i < 50)
+ {
+ inverseOrientation = orientationQuaternion.inverse();
+ i++;
+ }
+ Eigen::Quaternionf transformedOrientation = orientationQuaternion * inverseOrientation;
+ Eigen::Matrix3f quatMatrix = transformedOrientation.toRotationMatrix();
Eigen::Matrix4f quat4Matrix = Eigen::Matrix4f::Identity();
- quat4Matrix.block(0, 0, 3, 3) = quaternionMatrix;
+ //ARMARX_IMPORTANT << "quaternionInv: " << transformedOrientation.w() << " " << transformedOrientation.x() << " " << transformedOrientation.y() << " " << transformedOrientation.z();
+ //ARMARX_IMPORTANT << " data.pressure" << data.pressure << "matrix 31 " << quatMatrix(3, 1);
+ //quatMatrix = quatMatrix*rotationY*rotationZ;
+ quat4Matrix.block(0, 0, 3, 3) = quatMatrix;
+ //Eigen::Vector3f offsetContactCoordinate = Eigen::Vector3f(x,y,z);
+ //quat4Matrix.col(3).head(3) = offsetContactCoordinate;
+ //float angleY = std::atan2(2 * (transformedOrientation.w() * transformedOrientation.x() + transformedOrientation.y() * transformedOrientation.z()), 1 - 2 * (transformedOrientation.x() * transformedOrientation.x() + transformedOrientation.y() * transformedOrientation.y())) * 57.2957795;
+ //float angleX = std::asin(2 * (transformedOrientation.w() * transformedOrientation.y() - transformedOrientation.z() * transformedOrientation.x())) * 57.2957795;
+ //float angleZ = std::atan2(2 * (transformedOrientation.w() * transformedOrientation.z() + transformedOrientation.x() * transformedOrientation.y()), 1 - 2 * (transformedOrientation.y() * transformedOrientation.y() + transformedOrientation.z() * transformedOrientation.z())) * 57.2957795;
+ //ARMARX_IMPORTANT << " angle " << std::abs(angleX) << " " << std::abs(angleY) << " " << std::abs(angleZ);
SimpleJsonLoggerEntry e;
e.AddTimestamp();
e.Add("Pressure", data.pressure);
e.Add("RotationRate", rotationRate);
e.AddAsArr("Orientation", quat4Matrix);
- logger->log(e);
+ //e.Add("Anglex", angleX);
+ //e.Add("Angley", angleY);
+ //e.Add("Anglez", angleZ);
+ if (pressureRate > 50)
+ {
+ logger->log(e);
+ }
}
}
if (getProperty<bool>("logData").getValue())
@@ -276,14 +312,14 @@ bool OrientedTactileSensorUnit::loadCalibration()
while (arduinoLine.find("mode") == std::string::npos)
{
getline(arduinoIn, arduinoLine, '\n');
- //ARMARX_INFO << arduinoIn;
+ ARMARX_INFO << arduinoIn;
}
arduinoOut << "load";
arduinoOut.flush();
while (arduinoLine.find("calibration data") == std::string::npos)
{
getline(arduinoIn, arduinoLine, '\n');
- //ARMARX_INFO << arduinoIn;
+ ARMARX_INFO << arduinoIn;
}
arduinoOut << calibrationStream;
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index 2770df9a0..56d5f1d35 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -138,6 +138,7 @@ namespace armarx
int fd;
CalibrationData calibration;
+ Eigen::Quaternionf inverseOrientation;
std::vector<RotationRate> samplesRotation;
std::vector<PressureRate> samplesPressure;
std::vector<AccelerationRate> samplesAcceleration;
@@ -150,7 +151,8 @@ namespace armarx
int sampleIndexAcceleration;
int sampleIndexPressureRate;
float sumPressureRates;
-
+ bool first;
+ int i = 0;
SimpleJsonLoggerPtr logger;
std::string prefix;
};
--
GitLab