diff --git a/source/RobotAPI/interface/core/Trajectory.ice b/source/RobotAPI/interface/core/Trajectory.ice
index daeb1ce6fca713dab8cc238f90a495aa9380d92d..3bcf4960af7e7571a7fd8199c7e48aa6ee175ac2 100644
--- a/source/RobotAPI/interface/core/Trajectory.ice
+++ b/source/RobotAPI/interface/core/Trajectory.ice
@@ -30,6 +30,15 @@ module armarx
{
sequence<DoubleSeqSeq> DoubleSeqSeqSeq;
+ struct LimitlessState
+ {
+ bool enabled;
+ double limitLo;
+ double limitHi;
+ };
+
+ sequence<LimitlessState> LimitlessStateSeq;
+
/**
* [TrajectoryBase] defines a n-dimension trajectory with n deriviations.
*
@@ -47,6 +56,8 @@ module armarx
*/
["protected"] DoubleSeqSeqSeq dataVec;
["protected"] Ice::DoubleSeq timestamps;
+
+ ["protected"] LimitlessStateSeq limitless;
};
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 106f8a723c75bc884a6efd8323cb4965cd325a3a..cea8fa2744f760265647b2d7f1ad18f5085733bb 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -25,11 +25,12 @@
#include "PIDController.h"
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
using namespace armarx;
-PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation) :
+PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool limitless, double limitlessLimitHi) :
Kp(Kp),
Ki(Ki),
Kd(Kd),
@@ -41,7 +42,9 @@ PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValu
controlValue(0),
controlValueDerivation(0),
maxControlValue(maxControlValue),
- maxDerivation(maxDerivation)
+ maxDerivation(maxDerivation),
+ limitless(limitless),
+ limitlessLimitHi(limitlessLimitHi)
{
reset();
}
@@ -95,7 +98,14 @@ void PIDController::update(double deltaSec, double measuredValue, double targetV
processValue = measuredValue;
target = targetValue;
+
double error = target - processValue;
+ //ARMARX_INFO << VAROUT(target) << ", " << VAROUT(processValue) << ", " << VAROUT(error);
+ if (limitless)
+ {
+ error = math::MathUtils::fmod(error, limitlessLimitHi - 2 * M_PI, limitlessLimitHi);
+ //ARMARX_INFO << "Limitless after mod:" << VAROUT(error);
+ }
//double dt = (now - lastUpdateTime).toSecondsDouble();
// ARMARX_INFO << deactivateSpam() << VAROUT(dt);
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index cf7373d20fba53ef0da244d75b715f60fb9076a3..4e1f5528b4164f5800a585bb327861e74e2265b7 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -35,7 +35,13 @@ namespace armarx
public Logging
{
public:
- PIDController(float Kp, float Ki, float Kd, double maxControlValue = std::numeric_limits<double>::max(), double maxDerivation = std::numeric_limits<double>::max());
+ PIDController(float Kp,
+ float Ki,
+ float Kd,
+ double maxControlValue = std::numeric_limits<double>::max(),
+ double maxDerivation = std::numeric_limits<double>::max(),
+ bool limitless = false,
+ double limitlessLimitHi = 2 * M_PI);
void update(double deltaSec, double measuredValue, double targetValue);
void update(double measuredValue, double targetValue);
void update(double measuredValue);
@@ -57,6 +63,8 @@ namespace armarx
double maxControlValue;
double maxDerivation;
bool firstRun;
+ bool limitless;
+ double limitlessLimitHi;
mutable RecursiveMutex mutex;
};
typedef boost::shared_ptr<PIDController> PIDControllerPtr;
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 33a0698f8c15a77469838339703caeae308a37c1..07530219cc7b65f415117e5cc66617ba1c4d6947 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include "VectorHelpers.h"
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include "math/MathUtils.h"
namespace armarx
{
@@ -1265,8 +1266,45 @@ namespace armarx
}
double duration = tNext - tBefore;
- double delta = next - before;
+
+ double delta;
+ if (trajDimension < limitless.size() && limitless.at(trajDimension).enabled)
+ {
+ double range = limitless.at(trajDimension).limitHi - limitless.at(trajDimension).limitLo;
+
+ double dist1 = next - before;
+ double dist2 = next - (before + range);
+ double dist3 = next - (before - range);
+
+ if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
+ {
+ // no issue with bounds
+ //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
+ delta = dist1;
+ }
+ else if (fabs(dist2) <= fabs(dist3) && fabs(dist2) <= fabs(dist3))
+ {
+ // go over hi bound
+ //ARMARX_IMPORTANT << "LIMITLESS deriv HIGH: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
+ delta = dist2;
+ }
+ else
+ {
+ // go over lo bound
+ //ARMARX_IMPORTANT << "LIMITLESS deriv LOW: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
+ delta = dist3;
+ }
+
+ //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ": delta is " << delta;
+ }
+ else
+ {
+ // no limitless joint
+ delta = next - before;
+ }
+
delta = delta / duration;
+ //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ": delta/duration is " << delta;
checkValue(delta);
return delta;
}
@@ -1292,8 +1330,6 @@ namespace armarx
}
-
-
void Trajectory::reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState)
{
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -1438,10 +1474,43 @@ namespace armarx
double weightPrev = fabs(t - itNext->timestamp) / distance;
double weightNext = fabs(itPrev->timestamp - t) / distance;
- return weightNext * next + weightPrev * previous;
-
+ if (dimension < limitless.size() && limitless.at(dimension).enabled)
+ {
+ double result = 0;
+ double range = limitless.at(dimension).limitHi - limitless.at(dimension).limitLo;
+ double dist1 = next - previous;
+ double dist2 = next - (previous + range);
+ double dist3 = next - (previous - range);
+ //ARMARX_IMPORTANT << "LIMITLESS: checking dim " << dimension << ", prev:" << previous << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
+ if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
+ {
+ // no issue with bounds
+ result = weightNext * next + weightPrev * previous;
+ }
+ else if (fabs(dist2) <= fabs(dist3) && fabs(dist2) <= fabs(dist3))
+ {
+ // go over hi bound
+ result = weightNext * next + weightPrev * (previous + range);
+ result = math::MathUtils::fmod(result, limitless.at(dimension).limitLo, limitless.at(dimension).limitHi);
+ //ARMARX_IMPORTANT << "LIMITLESS - HIGH: checking dim " << dimension << ": high bounds, result: " << result << ", orig result would be " << weightNext* next + weightPrev* previous;
+ }
+ else
+ {
+ // go over lo bound
+ result = weightNext * next + weightPrev * (previous - range);
+ result = math::MathUtils::fmod(result, limitless.at(dimension).limitLo, limitless.at(dimension).limitHi);
+ //ARMARX_IMPORTANT << "LIMITLESS - LOW: checking dim " << dimension << ": low bounds, result: " << result << ", orig result would be " << weightNext* next + weightPrev* previous;
+ }
+ return result;
+ }
+ else
+ {
+ //ARMARX_IMPORTANT << "LIMITLESS: dim " << dimension << ": not limitless, limitless.size()=" << limitless.size();
+ return weightNext * next + weightPrev * previous;
+ }
}
}
+
void Trajectory::gaussianFilter(double filterRadius)
{
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -1538,6 +1607,8 @@ namespace armarx
destination.addDimension(source.getDimensionData(dim), timestamps);
}
destination.setDimensionNames(source.getDimensionNames());
+
+ destination.setLimitless(source.getLimitless());
}
void Trajectory::clear()
@@ -1612,6 +1683,7 @@ namespace armarx
normExampleTraj.addDimension(dimensionData, normTimestamps);
}
normExampleTraj.setDimensionNames(traj.getDimensionNames());
+ normExampleTraj.setLimitless(traj.getLimitless());
return normExampleTraj;
@@ -1849,4 +1921,15 @@ namespace armarx
}
+ void Trajectory::setLimitless(const LimitlessStateSeq& limitlessStates)
+ {
+ limitless = limitlessStates;
+ }
+
+ LimitlessStateSeq Trajectory::getLimitless() const
+ {
+ return limitless;
+ }
+
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index db38b1e6657b9717dd2f0f98bd418b9d5a02e234..f1b481febf2b8b62786115e3ce287b5d660e0125 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -424,6 +424,8 @@ namespace armarx
dimensionNames = dimNames;
}
+ void setLimitless(const LimitlessStateSeq& limitlessStates);
+ LimitlessStateSeq getLimitless() const;
protected:
void __addDimension();
diff --git a/source/RobotAPI/libraries/core/math/MathUtils.h b/source/RobotAPI/libraries/core/math/MathUtils.h
index bef829c58f7f55ae16416e8524a434cce2c9d987..eae0128a8508b7963ea5820394cc9a13cbf71173 100644
--- a/source/RobotAPI/libraries/core/math/MathUtils.h
+++ b/source/RobotAPI/libraries/core/math/MathUtils.h
@@ -124,6 +124,28 @@ namespace armarx
return max;
}
+ static float fmod(float value, float boundLow, float boundHigh)
+ {
+ if (value < 0)
+ {
+ return std::fmod(value + boundLow, boundHigh - boundLow) - boundLow;
+ }
+ else
+ {
+ return std::fmod(value - boundLow, boundHigh - boundLow) + boundLow;
+ }
+ }
+
+ static float angleMod2PI(float value)
+ {
+ return fmod(value, 0, 2 * M_PI);
+ }
+
+ static float angleModPI(float value)
+ {
+ return angleMod2PI(value) - M_PI;
+ }
+
};
}
}