diff --git a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
index db6fbca38e68d4638b5c7c862d5542c7357f29a1..e0cc504677d6123ac32261f7a3fbe6105a97ddda 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
+++ b/data/RobotAPI/robots/Armar3/ArmarIII-Head.xml
@@ -273,6 +273,7 @@
<Transform>
<Translation x="0" y="-80" z="0" units='mm'/>
<rollpitchyaw roll="90" pitch="0" yaw="0" unitsAngle="degree"/>
+ <rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
</Transform>
<Visualization enable="true">
diff --git a/data/RobotAPI/robots/Head/KA-Head.xml b/data/RobotAPI/robots/Head/KA-Head.xml
index 53be47e3a014cbe728bc8be1c135664555933db8..4fbdcda252ecae4a50952d0cedd8e1d1bc5c5bd9 100644
--- a/data/RobotAPI/robots/Head/KA-Head.xml
+++ b/data/RobotAPI/robots/Head/KA-Head.xml
@@ -283,6 +283,7 @@
<Transform>
<Translation x="0" y="-80" z="0" units='mm'/>
<rollpitchyaw roll="90" pitch="0" yaw="0" unitsAngle="degree"/>
+ <rollpitchyaw roll="0" pitch="0" yaw="180" unitsAngle="degree"/>
</Transform>
<Visualization enable="true">
diff --git a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
index 03536408afa2d13024647c9946775b7a62190227..77659db2299c279c1289b34f261e812e7785513b 100644
--- a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
+++ b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
@@ -25,7 +25,6 @@
#include "XsensIMU.h"
-
using namespace armarx;
using namespace IMU;
@@ -37,6 +36,38 @@ PropertyDefinitionsPtr XsensIMU::createPropertyDefinitions()
void XsensIMU::frameAcquisitionTaskLoop()
{
+
+ std::vector<float> offset(3, 0.0);
+
+ int count = 0;
+
+ IceUtil::Time startTime = armarx::TimeUtil::GetTime();
+
+ if (getProperty<bool>("EnableSimpleCalibration").getValue())
+ {
+ ARMARX_WARNING << "Estimation of the offset for the IMU, please do not move the IMU";
+ while (((armarx::TimeUtil::GetTime() - startTime) < IceUtil::Time::seconds(5)) && !sensorTask->isStopped())
+ {
+
+ while (HasPendingEvents())
+ {
+ ProcessPendingEvent();
+
+ offset[0] += m_GyroscopeRotation[0];
+ offset[1] += m_GyroscopeRotation[1];
+ offset[2] += m_GyroscopeRotation[2];
+
+ count ++;
+ }
+ }
+
+ offset[0] /= count;
+ offset[1] /= count;
+ offset[2] /= count;
+
+ }
+
+
while (!sensorTask->isStopped())
{
@@ -52,9 +83,9 @@ void XsensIMU::frameAcquisitionTaskLoop()
data.acceleration.push_back(m_Accelaration[1]);
data.acceleration.push_back(m_Accelaration[2]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[0]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[1]);
- data.gyroscopeRotation.push_back(m_GyroscopeRotation[2]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[0] - offset[0]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[1] - offset[1]);
+ data.gyroscopeRotation.push_back(m_GyroscopeRotation[2] - offset[2]);
data.magneticRotation.push_back(m_MagneticRotation[0]);
diff --git a/source/RobotAPI/drivers/XsensIMU/XsensIMU.h b/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
index 7d91988e55dc80b9ebd0591a49a2f8a5d6499451..1243182f68134ab2214a50941ccf7b6533be2b4d 100644
--- a/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
+++ b/source/RobotAPI/drivers/XsensIMU/XsensIMU.h
@@ -52,6 +52,7 @@ namespace armarx
defineOptionalProperty<std::string>("frequency", "", "");
defineOptionalProperty<std::string>("maxPendingEvents", "", "");
+ defineOptionalProperty<bool>("EnableSimpleCalibration", false, "");
}
};
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index e6774b8203148bfde9eb9744f7e2fcf1e895ea1d..ad4200c6dd2b43c3512afd8c5eee0af691d02504 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -23,6 +23,7 @@
*/
#include "PIDController.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
using namespace armarx;
@@ -50,17 +51,17 @@ void PIDController::reset()
firstRun = true;
previousError = 0;
integral = 0;
- lastUpdateTime = IceUtil::Time::now();
+ lastUpdateTime = TimeUtil::GetTime();
}
void PIDController::update(double measuredValue, double targetValue)
{
ScopedRecursiveLock lock(mutex);
- IceUtil::Time now = IceUtil::Time::now();
+ IceUtil::Time now = TimeUtil::GetTime();
if (firstRun)
{
- lastUpdateTime = IceUtil::Time::now();
+ lastUpdateTime = TimeUtil::GetTime();
}
double dt = (now - lastUpdateTime).toSecondsDouble();
@@ -110,10 +111,13 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
firstRun = false;
double oldControlValue = controlValue;
controlValue = Kp * error + Ki * integral + Kd * derivative;
- double acc = (controlValue - oldControlValue) / deltaSec;
- if (fabs(acc) > maxAcceleration)
+ if (deltaSec > 0.0)
{
- controlValue = oldControlValue + maxAcceleration * deltaSec * math::MathUtils::Sign(acc);
+ double acc = (controlValue - oldControlValue) / deltaSec;
+ if (fabs(acc) > maxAcceleration)
+ {
+ controlValue = oldControlValue + maxAcceleration * deltaSec * math::MathUtils::Sign(acc);
+ }
}
controlValue = std::min(fabs(controlValue), maxControlValue) * math::MathUtils::Sign(controlValue);
ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
@@ -185,15 +189,18 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
}
controlValue = direction * (Kp * error + Ki * integral + Kd * derivative);
- Eigen::VectorXf accVec = (controlValue - oldControlValue) / deltaSec;
- float maxNewJointAcc = accVec.maxCoeff();
- float minNewJointAcc = accVec.minCoeff();
- maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
- if (maxNewJointAcc > maxAcceleration)
+ if (deltaSec > 0.0)
{
- auto newValue = oldControlValue + accVec * maxAcceleration / maxNewJointAcc * deltaSec;
- ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxAcceleration) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(oldControlValue) << VAROUT(newValue);
- controlValue = newValue;
+ Eigen::VectorXf accVec = (controlValue - oldControlValue) / deltaSec;
+ float maxNewJointAcc = accVec.maxCoeff();
+ float minNewJointAcc = accVec.minCoeff();
+ maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
+ if (maxNewJointAcc > maxAcceleration)
+ {
+ auto newValue = oldControlValue + accVec * maxAcceleration / maxNewJointAcc * deltaSec;
+ ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxAcceleration) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(oldControlValue) << VAROUT(newValue);
+ controlValue = newValue;
+ }
}
@@ -219,11 +226,11 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
void MultiDimPIDController::update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
{
ScopedRecursiveLock lock(mutex);
- IceUtil::Time now = IceUtil::Time::now();
+ IceUtil::Time now = TimeUtil::GetTime();
if (firstRun)
{
- lastUpdateTime = IceUtil::Time::now();
+ lastUpdateTime = TimeUtil::GetTime();
}
double dt = (now - lastUpdateTime).toSecondsDouble();
@@ -242,7 +249,7 @@ void MultiDimPIDController::reset()
firstRun = true;
previousError = 0;
integral = 0;
- lastUpdateTime = IceUtil::Time::now();
+ lastUpdateTime = TimeUtil::GetTime();
// controlValue.setZero();
// processValue.setZero();
// target.setZero();