diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 75e57d387b8e4541ef29f439942728538d3feca7..22ada2251abc6416998c3fb43aa1b9681a5ca0d5 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -178,10 +178,14 @@
<stateMethod header="const VirtualRobot::RobotPtr getRobot() const">return %getContext%->getRobot();</stateMethod>
<method header="const VirtualRobot::RobotPtr getLocalRobot() const">return localRobot;</method>
+ <method header="const VirtualRobot::RobotPtr getLocalCollisionRobot() const">return localCollisionRobot;</method>
<member>VirtualRobot::RobotPtr localRobot;</member>
+ <member>VirtualRobot::RobotPtr localCollisionRobot;</member>
<onConnect>// initialize local robot</onConnect>
<onConnect>localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eStructure);</onConnect>
+ <onConnect>localCollisionRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eCollisionModel);</onConnect>
<stateMethod header="const VirtualRobot::RobotPtr getLocalRobot() const">return %getContext%->getLocalRobot();</stateMethod>
+ <stateMethod header="const VirtualRobot::RobotPtr getLocalCollisionRobot() const">return %getContext%->getLocalCollisionRobot();</stateMethod>
</Proxy>
<Proxy include="RobotAPI/interface/components/ViewSelectionInterface.h"
humanName="Automatic View Selection"
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
index 2706e2c4524cfc6e473f13226fb54a714bed4cf1..a4fb96c2f34f565f4de12c0054c9b7b7f778d435 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp
@@ -61,14 +61,14 @@ namespace armarx
{
TrajectoryController& contr = *rtGetControlStruct().trajectoryCtrl;
- auto dimNames = contr.getTraj()->getDimensionNames();
+ const auto& dimNames = contr.getTraj()->getDimensionNames();
for (size_t i = 0; i < dimNames.size(); i++)
{
const auto& jointName = dimNames.at(i);
- currentPos(i) = (sensors.count(jointName) == 1) ? sensors[jointName]->position : 0.0f;
+ currentPos(i) = (sensors.count(jointName) == 1) ? sensors.at(jointName)->position : 0.0f;
}
- auto newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
+ auto& newVelocities = contr.update(timeSinceLastIteration.toSecondsDouble() * direction, currentPos);
currentTimestamp = contr.getCurrentTimestamp();
finished = (currentTimestamp >= contr.getTraj()->rbegin()->getTimestamp() && direction == 1.0)
|| (currentTimestamp <= contr.getTraj()->begin()->getTimestamp() && direction == -1.0);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 1abf7cecd98e67e872adf7fc735c36d83fa801ce..f3dd49b631aa213b9232182c5782ce0be478c429 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -1283,7 +1283,7 @@ namespace armarx
std::move(ctrlDeviceUsedBitmap),
std::move(ctrlDeviceUsedIndices),
deletable, internal);
- getArmarXManager()->addObjectAsync(nJointCtrl, instanceName, false, false);
+ getArmarXManager()->addObject(nJointCtrl, instanceName, false, false);
nJointControllers[instanceName] = std::move(nJointCtrl);
ARMARX_CHECK_EXPRESSION(listenerPrx);
listenerPrx->nJointControllerCreated(instanceName);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
index 137be0396b07d845cccc8686fa42b48ce34a4dda..ba802511259c99c9c01c0fc10f7c3b5ba5d90687 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -744,7 +744,7 @@ namespace armarx
while (!interfaceData.updateReadBuffer())
{
-
+ usleep(100);
}
Eigen::Matrix4f leftPose = interfaceData.getReadBuffer().currentLeftPose;
Eigen::Matrix4f rightPose = interfaceData.getReadBuffer().currentRightPose;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index b1332376e1f025a8cf8c144015def710431d21f5..21db16e876446bb1ac5d7d74fd0723e5bbea64db 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -320,7 +320,7 @@ namespace armarx
while (!interfaceData.updateReadBuffer())
{
-
+ usleep(100);
}
Eigen::Matrix4f pose = interfaceData.getReadBuffer().currentTcpPose;
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;
}
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index ffd7288aae77878a2b49853aaa14fb5c808d7e51..64cd1ac6dcffd96674bcc732183fa9ca3de8b73c 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -276,7 +276,7 @@ namespace armarx
{
if (derivation == 0)
{
- return __calcBaseDataAtTimestamp(t).at(dim)->at(derivation); // interpolates and retrieves
+ return __interpolate(t, dim, derivation);
}
else
{
@@ -336,7 +336,7 @@ namespace armarx
return dimensionNames.at(dim);
}
- Ice::StringSeq Trajectory::getDimensionNames() const
+ const Ice::StringSeq& Trajectory::getDimensionNames() const
{
return dimensionNames;
}
@@ -821,7 +821,6 @@ namespace armarx
double newValue = __interpolate(t, dimension, 0);
checkValue(newValue);
result.push_back(DoubleSeqPtr(new Ice::DoubleSeq(1, newValue)));
-
}
return result;
@@ -1210,7 +1209,7 @@ namespace armarx
{
if (derivation == 0)
{
- getState(t, trajDimension, derivation);
+ return getState(t, trajDimension, derivation);
}
typename timestamp_view::iterator it = dataMap.find(t);
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 15cef1251f47b3f1c7d782b4dc54c1aea4e96217..f7a88f977d472ca15e996a5664c2c8f27e2d61c9 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -162,7 +162,7 @@ namespace armarx
stream << rhs.timestamp << ": ";
for (size_t d = 0; d < rhs.data.size(); ++d)
{
- stream << (rhs.data[d]->size() > 0 ? to_string(rhs.data[d]->at(0)) : std::string("-"));
+ stream << (rhs.data[d] && rhs.data[d]->size() > 0 ? to_string(rhs.data[d]->at(0)) : std::string("-"));
if (d <= rhs.data.size() - 1)
{
stream << ", ";
@@ -296,7 +296,7 @@ namespace armarx
std::vector<Ice::DoubleSeq > getAllStates(double t, int maxDeriv = 1);
Ice::DoubleSeq getDerivations(double t, size_t dimension, size_t derivations) const;
std::string getDimensionName(size_t dim) const;
- Ice::StringSeq getDimensionNames() const;
+ const Ice::StringSeq& getDimensionNames() const;
TrajDataContainer& data();
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.cpp b/source/RobotAPI/libraries/core/TrajectoryController.cpp
index 0884d4cff46f75a3d58e873e8b05a76998f08771..77207aa85cc7f30480fa3e23cee49b676d0f7cbd 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.cpp
+++ b/source/RobotAPI/libraries/core/TrajectoryController.cpp
@@ -24,6 +24,7 @@
#include "TrajectoryController.h"
#include <RobotAPI/libraries/core/math/LinearizeAngularTrajectory.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
+
namespace armarx
{
@@ -36,6 +37,7 @@ namespace armarx
limitless.push_back(ls.enabled);
}
pid.reset(new MultiDimPIDController(kp, ki, kd, std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), threadSafe, limitless));
+ pid->preallocate(traj->dim());
ARMARX_CHECK_EXPRESSION(traj);
currentTimestamp = traj->begin()->getTimestamp();
//for (size_t i = 0; i < traj->dim(); i++)
@@ -51,22 +53,36 @@ namespace armarx
{
ARMARX_CHECK_EXPRESSION(pid);
ARMARX_CHECK_EXPRESSION(traj);
+ ARMARX_CHECK_EXPRESSION(traj->size() > 0);
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(currentPosition.rows()), traj->dim());
size_t dim = traj->dim();
currentTimestamp = currentTimestamp + deltaT;
-
+ const Trajectory& traj = *this->traj;
if (currentTimestamp < 0.0)
{
currentTimestamp = 0.0;
}
+ if (currentTimestamp <= traj.rbegin()->getTimestamp())
+ {
+ for (size_t i = 0; i < dim; ++i)
+ {
- for (size_t i = 0; i < dim; ++i)
+ 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();
+ }
+ }
+ else // hold position in the end
{
- 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();
+ for (size_t i = 0; i < dim; ++i)
+ {
+ positions(i) = traj.rbegin()->getPosition(i);
+ veloctities(i) = 0;
+ }
}
+ currentError = positions - currentPosition;
+
pid->update(std::abs(deltaT), currentPosition, positions);
veloctities += pid->getControlValue();
return veloctities;
@@ -154,5 +170,15 @@ namespace armarx
}
}
+ const Eigen::VectorXf& TrajectoryController::getCurrentError() const
+ {
+ return currentError;
+ }
+
+ const Eigen::VectorXf& TrajectoryController::getPositions() const
+ {
+ return positions;
+ }
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/TrajectoryController.h b/source/RobotAPI/libraries/core/TrajectoryController.h
index f715d29aed2fccf9b077be81bf27207fd5dab555..3fdcd37c9b81553af1030d9770e223d9011810f6 100644
--- a/source/RobotAPI/libraries/core/TrajectoryController.h
+++ b/source/RobotAPI/libraries/core/TrajectoryController.h
@@ -44,6 +44,10 @@ namespace armarx
static void UnfoldLimitlessJointPositions(TrajectoryPtr traj);
static void FoldLimitlessJointPositions(TrajectoryPtr traj);
+ const Eigen::VectorXf& getCurrentError() const;
+
+ const Eigen::VectorXf& getPositions() const;
+
protected:
TrajectoryPtr traj;
MultiDimPIDControllerPtr pid;
@@ -51,6 +55,7 @@ namespace armarx
double currentTimestamp;
Eigen::VectorXf positions;
Eigen::VectorXf veloctities;
+ Eigen::VectorXf currentError;
};
typedef std::shared_ptr<TrajectoryController> TrajectoryControllerPtr;