Skip to content
Snippets Groups Projects
Commit 4a9d0cbd authored by Johann Mantel's avatar Johann Mantel
Browse files

set controlValues to zero when calling reset()

parent c8d87fc9
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/core/MultiDimPIDController.h
+ 205
− 204
View file @ 4a9d0cbd
......@@ -37,214 +37,215 @@ namespace armarx
{
template <int dimensions = Eigen::Dynamic>
class MultiDimPIDControllerTemplate :
public Logging
public Logging
{
public:
using PIDVectorX = Eigen::Matrix<float, dimensions, 1>;
MultiDimPIDControllerTemplate(float Kp,
float Ki,
float Kd,
double maxControlValue = std::numeric_limits<double>::max(),
double maxDerivation = std::numeric_limits<double>::max(),
bool threadSafe = true,
std::vector<bool> limitless = {}) :
Kp(Kp),
Ki(Ki),
Kd(Kd),
integral(0),
derivative(0),
previousError(0),
maxControlValue(maxControlValue),
maxDerivation(maxDerivation),
threadSafe(threadSafe),
limitless(limitless)
{
reset();
}
void preallocate(size_t size)
{
stackAllocations.zeroVec = PIDVectorX::Zero(size);
stackAllocations.errorVec = stackAllocations.zeroVec;
stackAllocations.direction = stackAllocations.zeroVec;
stackAllocations.oldControlValue = stackAllocations.zeroVec;
}
~MultiDimPIDControllerTemplate() {}
void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
{
ScopedRecursiveLockPtr lock = getLock();
if (stackAllocations.zeroVec.rows() == 0)
{
preallocate(measuredValue.rows());
}
ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
processValue = measuredValue;
target = targetValue;
stackAllocations.errorVec = target - processValue;
if (limitless.size() != 0)
{
ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
for (size_t i = 0; i < limitless.size(); i++)
{
if (limitless.at(i))
{
stackAllocations.errorVec(i) = math::MathUtils::angleModPI(stackAllocations.errorVec(i));
}
}
}
double error = stackAllocations.errorVec.norm();
//double dt = (now - lastUpdateTime).toSecondsDouble();
// ARMARX_INFO << deactivateSpam() << VAROUT(dt);
if (!firstRun)
{
integral += error * deltaSec;
integral = std::min(integral, maxIntegral);
if (deltaSec > 0.0)
{
derivative = (error - previousError) / deltaSec;
}
}
firstRun = false;
stackAllocations.direction = targetValue; // copy size
if (error > 0)
{
stackAllocations.direction = stackAllocations.errorVec.normalized();
}
else
{
stackAllocations.direction.setZero();
}
if (controlValue.rows() > 0)
{
stackAllocations.oldControlValue = controlValue;
}
else
{
stackAllocations.oldControlValue = stackAllocations.zeroVec;
}
controlValue = stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
if (deltaSec > 0.0)
{
PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
float maxNewJointAcc = accVec.maxCoeff();
float minNewJointAcc = accVec.minCoeff();
maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
if (maxNewJointAcc > maxDerivation)
{
auto newValue = stackAllocations.oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
controlValue = newValue;
}
}
float max = controlValue.maxCoeff();
float min = controlValue.minCoeff();
max = std::max<float>(fabs(min), fabs(max));
if (max > maxControlValue)
{
auto newValue = controlValue * maxControlValue / max;
ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
controlValue = newValue;
}
ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
previousError = error;
lastUpdateTime += IceUtil::Time::seconds(deltaSec);
}
void update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
{
ScopedRecursiveLockPtr lock = getLock();
IceUtil::Time now = TimeUtil::GetTime();
if (firstRun)
{
lastUpdateTime = TimeUtil::GetTime();
}
double dt = (now - lastUpdateTime).toSecondsDouble();
update(dt, measuredValue, targetValue);
lastUpdateTime = now;
}
const PIDVectorX&
getControlValue() const
{
return controlValue;
}
void setMaxControlValue(double value)
{
ScopedRecursiveLockPtr lock = getLock();
maxControlValue = value;
}
void reset()
{
ScopedRecursiveLockPtr lock = getLock();
firstRun = true;
previousError = 0;
integral = 0;
lastUpdateTime = TimeUtil::GetTime();
// controlValue.setZero();
// processValue.setZero();
// target.setZero();
}
// protected:
float Kp, Ki, Kd;
double integral;
double maxIntegral = std::numeric_limits<double>::max();
double derivative;
double previousError;
PIDVectorX processValue;
PIDVectorX target;
IceUtil::Time lastUpdateTime;
PIDVectorX controlValue;
double maxControlValue;
double maxDerivation;
bool firstRun;
mutable std::recursive_mutex mutex;
bool threadSafe = true;
std::vector<bool> limitless;
using PIDVectorX = Eigen::Matrix<float, dimensions, 1>;
MultiDimPIDControllerTemplate(float Kp,
float Ki,
float Kd,
double maxControlValue = std::numeric_limits<double>::max(),
double maxDerivation = std::numeric_limits<double>::max(),
bool threadSafe = true,
std::vector<bool> limitless = {}) :
Kp(Kp),
Ki(Ki),
Kd(Kd),
integral(0),
derivative(0),
previousError(0),
maxControlValue(maxControlValue),
maxDerivation(maxDerivation),
threadSafe(threadSafe),
limitless(limitless)
{
reset();
}
void preallocate(size_t size)
{
stackAllocations.zeroVec = PIDVectorX::Zero(size);
stackAllocations.errorVec = stackAllocations.zeroVec;
stackAllocations.direction = stackAllocations.zeroVec;
stackAllocations.oldControlValue = stackAllocations.zeroVec;
}
~MultiDimPIDControllerTemplate() {}
void update(const double deltaSec, const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
{
ScopedRecursiveLockPtr lock = getLock();
if (stackAllocations.zeroVec.rows() == 0)
{
preallocate(measuredValue.rows());
}
ARMARX_CHECK_EQUAL(measuredValue.rows(), targetValue.rows());
ARMARX_CHECK_EQUAL(measuredValue.rows(), stackAllocations.zeroVec.rows());
processValue = measuredValue;
target = targetValue;
stackAllocations.errorVec = target - processValue;
if (limitless.size() != 0)
{
ARMARX_CHECK_EQUAL(limitless.size(), (size_t)stackAllocations.errorVec.rows());
for (size_t i = 0; i < limitless.size(); i++)
{
if (limitless.at(i))
{
stackAllocations.errorVec(i) = math::MathUtils::angleModPI(stackAllocations.errorVec(i));
}
}
}
double error = stackAllocations.errorVec.norm();
//double dt = (now - lastUpdateTime).toSecondsDouble();
// ARMARX_INFO << deactivateSpam() << VAROUT(dt);
if (!firstRun)
{
integral += error * deltaSec;
integral = std::min(integral, maxIntegral);
if (deltaSec > 0.0)
{
derivative = (error - previousError) / deltaSec;
}
}
firstRun = false;
stackAllocations.direction = targetValue; // copy size
if (error > 0)
{
stackAllocations.direction = stackAllocations.errorVec.normalized();
}
else
{
stackAllocations.direction.setZero();
}
if (controlValue.rows() > 0)
{
stackAllocations.oldControlValue = controlValue;
}
else
{
stackAllocations.oldControlValue = stackAllocations.zeroVec;
}
controlValue = stackAllocations.direction * (Kp * error + Ki * integral + Kd * derivative);
if (deltaSec > 0.0)
{
PIDVectorX accVec = (controlValue - stackAllocations.oldControlValue) / deltaSec;
float maxNewJointAcc = accVec.maxCoeff();
float minNewJointAcc = accVec.minCoeff();
maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
if (maxNewJointAcc > maxDerivation)
{
auto newValue = stackAllocations.oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(stackAllocations.oldControlValue) << VAROUT(newValue);
controlValue = newValue;
}
}
float max = controlValue.maxCoeff();
float min = controlValue.minCoeff();
max = std::max<float>(fabs(min), fabs(max));
if (max > maxControlValue)
{
auto newValue = controlValue * maxControlValue / max;
ARMARX_DEBUG << deactivateSpam(0.5) << " Control value to big: " << controlValue << " max value: " << maxControlValue << " new value: " << newValue;
controlValue = newValue;
}
ARMARX_DEBUG << deactivateSpam(0.5) << " error: " << error << " cV: " << (controlValue) << " i: " << (Ki * integral) << " d: " << (Kd * derivative) << " dt: " << deltaSec;
previousError = error;
lastUpdateTime += IceUtil::Time::seconds(deltaSec);
}
void update(const PIDVectorX& measuredValue, const PIDVectorX& targetValue)
{
ScopedRecursiveLockPtr lock = getLock();
IceUtil::Time now = TimeUtil::GetTime();
if (firstRun)
{
lastUpdateTime = TimeUtil::GetTime();
}
double dt = (now - lastUpdateTime).toSecondsDouble();
update(dt, measuredValue, targetValue);
lastUpdateTime = now;
}
const PIDVectorX&
getControlValue() const
{
return controlValue;
}
void setMaxControlValue(double value)
{
ScopedRecursiveLockPtr lock = getLock();
maxControlValue = value;
}
void reset()
{
ScopedRecursiveLockPtr lock = getLock();
firstRun = true;
previousError = 0;
integral = 0;
lastUpdateTime = TimeUtil::GetTime();
//reset control
controlValue.setZero();
processValue.setZero();
target.setZero();
}
// protected:
float Kp, Ki, Kd;
double integral;
double maxIntegral = std::numeric_limits<double>::max();
double derivative;
double previousError;
PIDVectorX processValue;
PIDVectorX target;
IceUtil::Time lastUpdateTime;
PIDVectorX controlValue;
double maxControlValue;
double maxDerivation;
bool firstRun;
mutable std::recursive_mutex mutex;
bool threadSafe = true;
std::vector<bool> limitless;
private:
struct StackAllocationHelper
{
PIDVectorX errorVec;
PIDVectorX direction;
PIDVectorX oldControlValue;
PIDVectorX zeroVec;
} stackAllocations;
using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
ScopedRecursiveLockPtr getLock() const
{
if (threadSafe)
{
return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
}
else
{
return ScopedRecursiveLockPtr();
}
}
struct StackAllocationHelper
{
PIDVectorX errorVec;
PIDVectorX direction;
PIDVectorX oldControlValue;
PIDVectorX zeroVec;
} stackAllocations;
using ScopedRecursiveLock = std::unique_lock<std::recursive_mutex>;
using ScopedRecursiveLockPtr = std::unique_ptr<ScopedRecursiveLock>;
ScopedRecursiveLockPtr getLock() const
{
if (threadSafe)
{
return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
}
else
{
return ScopedRecursiveLockPtr();
}
}
};
using MultiDimPIDController = MultiDimPIDControllerTemplate<>;
using MultiDimPIDControllerPtr = std::shared_ptr<MultiDimPIDControllerTemplate<>>;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment