diff --git a/.gitignore b/.gitignore
index 04042697d35f52aada539610775896e6966564e9..8531981952d0ccc91a80991878edde84467f5363 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 14b01f42a3da337953534f2b1b259454cee716e4..f12dab3cfb468c375d8141f530a6e6891397998a 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 e9a50e15698650c98d5f759e8e0f231569578782..b74a4804f6a97a615946626b8dcbb5cfce06df33 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 017f09da0220d63982601158ff28210eb0e06b6b..10ade5e6575d9088905cd808ffb074aeed7349ac 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 2770df9a026f905887802e0f61ead97623f6672c..56d5f1d353a09307772ea8289c7ec9c9c1b88e70 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;
};