From a2411546aa1613a6cfe3b547f877940b51f5a798 Mon Sep 17 00:00:00 2001
From: armar6a-demo <armar6-demo@i61pc043.itec.uni-karlsruhe.de>
Date: Fri, 5 Jan 2018 16:35:39 +0100
Subject: [PATCH] added support for limitless joints
---
.../RobotAPI/libraries/core/PIDController.cpp | 27 +++++++++++++++----
.../RobotAPI/libraries/core/PIDController.h | 4 ++-
.../libraries/core/TrajectoryController.cpp | 22 ++++++++++++---
.../libraries/core/TrajectoryController.h | 5 ++--
4 files changed, 46 insertions(+), 12 deletions(-)
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index e39f631ab..d3d1a61fb 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -26,6 +26,7 @@
#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;
@@ -148,7 +149,7 @@ double PIDController::getControlValue() const
}
-MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool threadSafe) :
+MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, double maxControlValue, double maxDerivation, bool threadSafe, std::vector<bool> limitless) :
Kp(Kp),
Ki(Ki),
Kd(Kd),
@@ -157,7 +158,8 @@ MultiDimPIDController::MultiDimPIDController(float Kp, float Ki, float Kd, doubl
previousError(0),
maxControlValue(maxControlValue),
maxDerivation(maxDerivation),
- threadSafe(threadSafe)
+ threadSafe(threadSafe),
+ limitless(limitless)
{
reset();
}
@@ -168,7 +170,22 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
processValue = measuredValue;
target = targetValue;
- double error = (target - processValue).norm();
+ 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);
@@ -185,9 +202,9 @@ void MultiDimPIDController::update(const double deltaSec, const Eigen::VectorXf&
firstRun = false;
Eigen::VectorXf direction = targetValue; // copy size
- if ((target - processValue).norm() > 0)
+ if (error > 0)
{
- direction = (target - processValue).normalized();
+ direction = errorVec.normalized();
}
else
{
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index 240baec49..c402574ed 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -76,7 +76,8 @@ namespace armarx
float Kd,
double maxControlValue = std::numeric_limits<double>::max(),
double maxDerivation = std::numeric_limits<double>::max(),
- bool threadSafe = true);
+ 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;
@@ -97,6 +98,7 @@ namespace armarx
bool firstRun;
mutable RecursiveMutex mutex;
bool threadSafe = true;
+ std::vector<bool> limitless;
private:
ScopedRecursiveLockPtr getLock() const;
};
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index b4f67eb13..cfe472298 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -29,9 +29,19 @@ namespace armarx
TrajectoryController::TrajectoryController(const TrajectoryPtr& traj, float kp, float ki, float kd, bool threadSafe) :
traj(traj)
{
- pid.reset(new MultiDimPIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), threadSafe));
+ std::vector<bool> limitless;
+ for (auto ls : traj->getLimitless())
+ {
+ limitless.push_back(ls.enabled);
+ }
+ pid.reset(new MultiDimPIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), threadSafe, limitless));
ARMARX_CHECK_EXPRESSION(traj);
currentTimestamp = traj->begin()->getTimestamp();
+ //for (size_t i = 0; i < traj->dim(); i++)
+ //{
+ // PIDControllerPtr pid(new PIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), traj->getLimitless().at(i).enabled));
+ // pids.push_back(pid);
+ //}
positions.resize(traj->dim(), 1);
veloctities.resize(traj->dim(), 1);
}
@@ -53,12 +63,15 @@ namespace armarx
{
positions(i) = traj->getState(currentTimestamp, i, 0);
veloctities(i) = (std::signbit(deltaT) ? -1.0 : 1.0) * traj->getState(currentTimestamp, i, 1);
+ //pids.at(i)->update(std::abs(deltaT), currentPosition(i), positions(i));
+ //veloctities(i) += pids.at(i)->getControlValue();
}
pid->update(std::abs(deltaT), currentPosition, positions);
veloctities += pid->getControlValue();
return veloctities;
}
+ /*
const MultiDimPIDControllerPtr& TrajectoryController::getPid() const
{
return pid;
@@ -67,7 +80,7 @@ namespace armarx
void TrajectoryController::setPid(const MultiDimPIDControllerPtr& value)
{
pid = value;
- }
+ }*/
double TrajectoryController::getCurrentTimestamp() const
{
@@ -105,8 +118,9 @@ namespace armarx
// double pi2Mult = floor(prevPos.at(i) / (pi2));
// double center = limitlessStates.at(i).limitHi - limitlessStates.at(i).limitLo;
// double prevPosModf = math::MathUtils::angleMod2PI(prevPos.at(i));
- double delta = math::MathUtils::AngleDelta(prevPos.at(i), pos);
- double newPos = prevPos.at(i) + delta;
+
+ //double delta = math::MathUtils::AngleDelta(prevPos.at(i), pos);
+ double newPos = prevPos.at(i) + math::MathUtils::angleModPI(pos - prevPos.at(i));
state.getData().at(i)->at(0) = newPos;
prevPos.at(i) = newPos;
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
index 64acb6de4..a12d592f4 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.h
+++ b/source/RobotAPI/libraries/core/TrajectoryController.h
@@ -34,8 +34,8 @@ namespace armarx
public:
TrajectoryController(const TrajectoryPtr& traj, float kp, float ki = 0.0f, float kd = 0.0f, bool threadSafe = true);
Eigen::VectorXf update(double deltaT, const Eigen::VectorXf currentPosition);
- const MultiDimPIDControllerPtr& getPid() const;
- void setPid(const MultiDimPIDControllerPtr& value);
+ //const MultiDimPIDControllerPtr& getPid() const;
+ //void setPid(const MultiDimPIDControllerPtr& value);
double getCurrentTimestamp() const;
@@ -47,6 +47,7 @@ namespace armarx
protected:
TrajectoryPtr traj;
MultiDimPIDControllerPtr pid;
+ //std::vector<PIDControllerPtr> pids;
double currentTimestamp;
Eigen::VectorXf positions;
Eigen::VectorXf veloctities;
--
GitLab