diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 9a151bdb8fb2a9b04f696b9a6e17c6c7fe3903b9..24a754acb477f73020bcf123138ec1bd76d6eca2 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -51,6 +51,7 @@ set(LIB_FILES
set(LIB_HEADERS
EigenStl.h
PIDController.h
+ MultiDimPIDController.h
Trajectory.h
TrajectoryController.h
VectorHelpers.h
diff --git a/source/RobotAPI/libraries/core/MultiDimPIDController.h b/source/RobotAPI/libraries/core/MultiDimPIDController.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d0cce9ec7904331f09394c0bb65f61fba62a09b
--- /dev/null
+++ b/source/RobotAPI/libraries/core/MultiDimPIDController.h
@@ -0,0 +1,238 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX
+ * @author Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2018
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <Eigen/Core>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <RobotAPI/libraries/core/math/MathUtils.h>
+
+namespace armarx
+{
+ template <int dimensions = Eigen::Dynamic>
+ class MultiDimPIDControllerTemplate :
+ public Logging
+ {
+ public:
+ typedef Eigen::Matrix<float, dimensions, 1> PIDVectorX;
+
+ 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();
+ 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;
+
+ 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 derivative;
+ double previousError;
+ PIDVectorX processValue;
+ PIDVectorX target;
+ IceUtil::Time lastUpdateTime;
+ PIDVectorX controlValue;
+ double maxControlValue;
+ double maxDerivation;
+ bool firstRun;
+ mutable RecursiveMutex mutex;
+ bool threadSafe = true;
+ std::vector<bool> limitless;
+ private:
+
+ struct StackAllocationHelper
+ {
+ PIDVectorX errorVec;
+ PIDVectorX direction;
+ PIDVectorX oldControlValue;
+ PIDVectorX zeroVec;
+ } stackAllocations;
+
+ ScopedRecursiveLockPtr getLock() const
+ {
+ if (threadSafe)
+ {
+ return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
+ }
+ else
+ {
+ return ScopedRecursiveLockPtr();
+ }
+ }
+ };
+ typedef MultiDimPIDControllerTemplate<> MultiDimPIDController;
+ typedef boost::shared_ptr<MultiDimPIDControllerTemplate<>> MultiDimPIDControllerPtr;
+}
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 928e8451aee81823951866979f5d0a30321ca70b..e364f8d9bcdc0e688a986cec002390acf32deae0 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -25,7 +25,6 @@
#include "PIDController.h"
#include <ArmarXCore/core/time/TimeUtil.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
-#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
using namespace armarx;
@@ -162,158 +161,16 @@ double PIDController::getControlValue() const
}
-MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool threadSafe, 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 MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue)
-{
- ScopedRecursiveLockPtr lock = getLock();
- processValue = measuredValue;
- target = targetValue;
- Eigen::VectorXf errorVec = target - processValue;
- if (limitless.size() != 0)
- {
- ARMARX_CHECK_EQUAL(limitless.size(), (size_t)errorVec.rows());
- for (size_t i = 0; i < limitless.size(); i++)
- {
- if (limitless.at(i))
- {
- errorVec(i) = math::MathUtils::angleModPI(errorVec(i));
- }
- }
- }
- double error = errorVec.norm();
- //double dt = (now - lastUpdateTime).toSecondsDouble();
- // ARMARX_INFO << deactivateSpam() << VAROUT(dt);
- if (!firstRun)
- {
- integral += error * deltaSec;
- if (deltaSec > 0.0)
- {
- derivative = (error - previousError) / deltaSec;
- }
- }
-
- firstRun = false;
- Eigen::VectorXf direction = targetValue; // copy size
-
- if (error > 0)
- {
- direction = errorVec.normalized();
- }
- else
- {
- direction.setZero();
- }
- Eigen::VectorXf oldControlValue;
- if (controlValue.rows() > 0)
- {
- oldControlValue = controlValue;
- }
- else
- {
- oldControlValue = Eigen::VectorXf::Zero(measuredValue.rows());
- }
- controlValue = direction * (Kp * error + Ki * integral + Kd * derivative);
- if (deltaSec > 0.0)
- {
- Eigen::VectorXf accVec = (controlValue - oldControlValue) / deltaSec;
- float maxNewJointAcc = accVec.maxCoeff();
- float minNewJointAcc = accVec.minCoeff();
- maxNewJointAcc = std::max<float>(fabs(minNewJointAcc), fabs(maxNewJointAcc));
- if (maxNewJointAcc > maxDerivation)
- {
- auto newValue = oldControlValue + accVec * maxDerivation / maxNewJointAcc * deltaSec;
- ARMARX_DEBUG << deactivateSpam(0.5) << VAROUT(maxDerivation) << VAROUT(maxNewJointAcc) << VAROUT(controlValue) << VAROUT(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 MultiDimPIDController::update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& 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 Eigen::VectorXf& MultiDimPIDController::getControlValue() const
-{
- return controlValue;
-}
-
-void MultiDimPIDController::reset()
-{
- ScopedRecursiveLockPtr lock = getLock();
- firstRun = true;
- previousError = 0;
- integral = 0;
- lastUpdateTime = TimeUtil::GetTime();
- // controlValue.setZero();
- // processValue.setZero();
- // target.setZero();
-}
-
-ScopedRecursiveLockPtr MultiDimPIDController::getLock() const
-{
- if (threadSafe)
- {
- return ScopedRecursiveLockPtr(new ScopedRecursiveLock(mutex));
- }
- else
- {
- return ScopedRecursiveLockPtr();
- }
-}
-
-void MultiDimPIDController::setMaxControlValue(double value)
-{
- ScopedRecursiveLockPtr lock = getLock();
- maxControlValue = value;
-}
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index e6e9976b94e31241d086ee513f117d8b3aa81fed..8fa7fba0a9b6a8096e03b343a2cf8cd90f6e2c0a 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <Eigen/Core>
+#include "MultiDimPIDController.h"
namespace armarx
{
@@ -69,41 +70,6 @@ namespace armarx
};
typedef boost::shared_ptr<PIDController> PIDControllerPtr;
- class MultiDimPIDController :
- public Logging
- {
- public:
- MultiDimPIDController(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 = {});
- void update(const double deltaSec, const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
- void update(const Eigen::VectorXf& measuredValue, const Eigen::VectorXf& targetValue);
- const Eigen::VectorXf& getControlValue() const;
- void setMaxControlValue(double value);
- void reset();
- // protected:
- float Kp, Ki, Kd;
- double integral;
- double derivative;
- double previousError;
- Eigen::VectorXf processValue;
- Eigen::VectorXf target;
- IceUtil::Time lastUpdateTime;
- Eigen::VectorXf controlValue;
- double maxControlValue;
- double maxDerivation;
- bool firstRun;
- mutable RecursiveMutex mutex;
- bool threadSafe = true;
- std::vector<bool> limitless;
- private:
- ScopedRecursiveLockPtr getLock() const;
- };
- typedef boost::shared_ptr<MultiDimPIDController> MultiDimPIDControllerPtr;
}