From 378e61fb3944660e13fd7a1119a5c05303928463 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Fri, 16 Nov 2018 11:39:15 +0100
Subject: [PATCH] added limitless joint support for trajectory controller
---
.../libraries/core/TrajectoryController.cpp | 14 +++++++++++++-
1 file changed, 13 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index ef74dd1fc..5328d534a 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -48,6 +48,7 @@ namespace armarx
//}
positions.resize(traj->dim(), 1);
veloctities.resize(traj->dim(), 1);
+ currentError.resize(traj->dim(), 1);
}
const Eigen::VectorXf& TrajectoryController::update(double deltaT, const Eigen::VectorXf& currentPosition)
@@ -82,7 +83,18 @@ namespace armarx
veloctities(i) = 0;
}
}
- currentError = positions - currentPosition;
+ for (size_t i = 0; i < dim; ++i)
+ {
+ if (pid->limitless.size() > i && pid->limitless.at(i))
+ {
+ currentError(i) = math::MathUtils::AngleDelta(currentPosition(i), positions(i));
+ }
+ else
+ {
+ currentError(i) = positions(i) - currentPosition(i);
+ }
+
+ }
pid->update(std::abs(deltaT), currentPosition, positions);
veloctities += pid->getControlValue();
--
GitLab