added debug info

.gitignore

@@ -58,3 +58,4 @@ scenarios/*/*/ttyACM*.log
 scenarios/*/*/config/datapath.cfg
 
 *.icegrid.xml
+data/RobotAPI/logs

scenarios/OrientedTactileSensor/OrientedTactileSensor.scx

 <?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>
 

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

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;

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;
     };