From fb080baa821e5a1f36731218c586124b3a0f0cd2 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Sat, 25 Jun 2016 17:54:57 +0200
Subject: [PATCH] added function to calculate the length of a trajectory over
all dimensions
---
source/RobotAPI/libraries/core/Trajectory.cpp | 131 ++++++++++++------
source/RobotAPI/libraries/core/Trajectory.h | 27 ++--
.../libraries/core/test/TrajectoryTest.cpp | 28 +++-
3 files changed, 123 insertions(+), 63 deletions(-)
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 544072b37..26d42594b 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -472,82 +472,118 @@ namespace armarx
}
}
- double Trajectory::getSpaceLength(size_t dimension, size_t stateDim) const
+ double Trajectory::getSpaceLength(size_t derivation) const
{
- return getSquaredSpaceLength(dimension, stateDim, begin()->getTimestamp(), rbegin()->getTimestamp());
+ return getSpaceLength(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- double Trajectory::getSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ double Trajectory::getSpaceLength(size_t derivation, double startTime, double endTime) const
{
double length = 0.0;
const ordered_view& ordv = dataMap.get<TagOrdered>();
- // timestamp_view::iterator it = dataMap.find(startTime);
- // Ice::DoubleSeq data = getDimensionData(0);
- // timestamp_view::iterator itEnd= dataMap.find(endTime);
ordered_view::iterator itO = ordv.lower_bound(startTime);
ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
if (itO == ordv.end())
{
return 0.0;
}
- double prevValue = itO->getDeriv(dimension, stateDim);
+ size_t dimensions = dim();
+ Ice::DoubleSeq prevValue(dimensions);
+ for (size_t d = 0; d < dimensions; ++d)
+ {
+ prevValue[d] = itO->getDeriv(d, derivation);
+ }
+ itO++;
+ for (;
+ itO != itOEnd;
+ itO++
+ )
+ {
+ double segmentLength = 0;
+ double value;
+ for (size_t d = 0; d < dimensions; ++d)
+ {
+ value = itO->getDeriv(d, derivation);
+ segmentLength += pow(prevValue[d] - value, 2);
+ prevValue[d] = value;
+ }
+ length = sqrt(segmentLength);
+
+ }
+
+ return length;
+ }
+
+ double Trajectory::getSpaceLength(size_t dimension, size_t derivation) const
+ {
+ return getSpaceLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+ }
+
+ double Trajectory::getSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const
+ {
+ double length = 0.0;
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ ordered_view::iterator itO = ordv.lower_bound(startTime);
+ ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
+ if (itO == ordv.end())
+ {
+ return 0.0;
+ }
+ double prevValue = itO->getDeriv(dimension, derivation);
for (;
itO != itOEnd;
itO++
)
{
- length += fabs(prevValue - itO->getDeriv(dimension, stateDim));
- prevValue = itO->getDeriv(dimension, stateDim);
+ length += fabs(prevValue - itO->getDeriv(dimension, derivation));
+ prevValue = itO->getDeriv(dimension, derivation);
}
return length;
}
- double Trajectory::getSquaredSpaceLength(size_t dimension, size_t stateDim) const
+ double Trajectory::getSquaredSpaceLength(size_t dimension, size_t derivation) const
{
- return getSquaredSpaceLength(dimension, stateDim, begin()->getTimestamp(), rbegin()->getTimestamp());
+ return getSquaredSpaceLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- double Trajectory::getSquaredSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ double Trajectory::getSquaredSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const
{
double length = 0.0;
const ordered_view& ordv = dataMap.get<TagOrdered>();
- // timestamp_view::iterator it = dataMap.find(startTime);
- // Ice::DoubleSeq data = getDimensionData(0);
- // timestamp_view::iterator itEnd= dataMap.find(endTime);
ordered_view::iterator itO = ordv.lower_bound(startTime);
ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
if (itO == ordv.end())
{
return 0.0;
}
- // double prevValue = itO->data[dimension]->at(stateDim);
- double prevValue = itO->getDeriv(dimension, stateDim);
+
+ double prevValue = itO->getDeriv(dimension, derivation);
for (;
itO != itOEnd;
itO++
)
{
- length += fabs(pow(prevValue, 2.0) - pow(itO->getDeriv(dimension, stateDim), 2.0));
- prevValue = itO->getDeriv(dimension, stateDim);
+ length += fabs(pow(prevValue, 2.0) - pow(itO->getDeriv(dimension, derivation), 2.0));
+ prevValue = itO->getDeriv(dimension, derivation);
}
return length;
}
- double Trajectory::getMax(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ double Trajectory::getMax(size_t dimension, size_t derivation, double startTime, double endTime) const
{
- return getWithFunc(&std::max<double>, -std::numeric_limits<double>::max(), dimension, stateDim, startTime, endTime);
+ return getWithFunc(&std::max<double>, -std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
}
- double Trajectory::getMin(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ double Trajectory::getMin(size_t dimension, size_t derivation, double startTime, double endTime) const
{
- return getWithFunc(&std::min<double>, std::numeric_limits<double>::max(), dimension, stateDim, startTime, endTime);
+ return getWithFunc(&std::min<double>, std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
}
- double Trajectory::getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t stateDim, double startTime, double endTime) const
+ double Trajectory::getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const
{
double bestValue = initValue;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -563,18 +599,18 @@ namespace armarx
itO++
)
{
- bestValue = foo(bestValue, itO->getDeriv(dimension, stateDim));
+ bestValue = foo(bestValue, itO->getDeriv(dimension, derivation));
}
return bestValue;
}
- double Trajectory::getTrajectorySpace(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ double Trajectory::getTrajectorySpace(size_t dimension, size_t derivation, double startTime, double endTime) const
{
- return getMax(dimension, stateDim, startTime, endTime) - getMin(dimension, stateDim, startTime, endTime);
+ return getMax(dimension, derivation, startTime, endTime) - getMin(dimension, derivation, startTime, endTime);
}
- Ice::DoubleSeq Trajectory::getMinima(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ Ice::DoubleSeq Trajectory::getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -585,20 +621,20 @@ namespace armarx
{
return result;
}
- double preValue = itO->getDeriv(dimension, stateDim);
+ double preValue = itO->getDeriv(dimension, derivation);
for (;
itO != itOEnd;
)
{
// double t = itO->getTimestamp();
- double cur = itO->getDeriv(dimension, stateDim);
+ double cur = itO->getDeriv(dimension, derivation);
itO++;
if (itO == ordv.end())
{
break;
}
- double next = itO->getDeriv(dimension, stateDim);
+ double next = itO->getDeriv(dimension, derivation);
if (cur <= preValue && cur <= next)
{
result.push_back(cur);
@@ -609,7 +645,7 @@ namespace armarx
return result;
}
- Ice::DoubleSeq Trajectory::getMinimaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ Ice::DoubleSeq Trajectory::getMinimaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -620,20 +656,20 @@ namespace armarx
{
return result;
}
- double preValue = itO->getDeriv(dimension, stateDim);
+ double preValue = itO->getDeriv(dimension, derivation);
for (;
itO != itOEnd;
)
{
// double t = itO->getTimestamp();
- double cur = itO->getDeriv(dimension, stateDim);
+ double cur = itO->getDeriv(dimension, derivation);
itO++;
if (itO == ordv.end())
{
break;
}
- double next = itO->getDeriv(dimension, stateDim);
+ double next = itO->getDeriv(dimension, derivation);
if (cur <= preValue && cur <= next)
{
result.push_back(itO->getTimestamp());
@@ -644,7 +680,7 @@ namespace armarx
return result;
}
- Ice::DoubleSeq Trajectory::getMaxima(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ Ice::DoubleSeq Trajectory::getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -655,19 +691,19 @@ namespace armarx
{
return result;
}
- double preValue = itO->getDeriv(dimension, stateDim);
+ double preValue = itO->getDeriv(dimension, derivation);
for (;
itO != itOEnd;
)
{
- double cur = itO->getDeriv(dimension, stateDim);
+ double cur = itO->getDeriv(dimension, derivation);
itO++;
if (itO == ordv.end())
{
break;
}
- double next = itO->getDeriv(dimension, stateDim);
+ double next = itO->getDeriv(dimension, derivation);
if (cur >= preValue && cur >= next)
{
result.push_back(cur);
@@ -678,7 +714,7 @@ namespace armarx
return result;
}
- Ice::DoubleSeq Trajectory::getMaximaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const
+ Ice::DoubleSeq Trajectory::getMaximaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -689,19 +725,19 @@ namespace armarx
{
return result;
}
- double preValue = itO->getDeriv(dimension, stateDim);
+ double preValue = itO->getDeriv(dimension, derivation);
for (;
itO != itOEnd;
)
{
- double cur = itO->getDeriv(dimension, stateDim);
+ double cur = itO->getDeriv(dimension, derivation);
itO++;
if (itO == ordv.end())
{
break;
}
- double next = itO->getDeriv(dimension, stateDim);
+ double next = itO->getDeriv(dimension, derivation);
if (cur >= preValue && cur >= next)
{
@@ -1254,7 +1290,7 @@ namespace armarx
double previousValue = valueAtFirstTimestamp;
double previousTimestamp = it->timestamp;
- // size_t stateDim = double().size();
+ // size_t derivation = double().size();
for (it++; it != ordv.end(); it++)
{
double slope;
@@ -1593,7 +1629,10 @@ namespace armarx
__addDimension();
addPositionsToDimension(newDim - 1, x, y);
- dimensionNames.push_back(name);
+ if(dimensionNames.size() > newDim-1)
+ dimensionNames.at(newDim-1) = name;
+ else
+ dimensionNames.push_back(name);
return newDim - 1;
}
@@ -1742,4 +1781,6 @@ namespace armarx
}
+
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 3573e5dd8..3245c52b4 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -239,7 +239,6 @@ namespace armarx
* @param dimension
* @return
*/
- // Ice::DoubleSeq getDimensionData(size_t dimension = 0) const;
Ice::DoubleSeq getDimensionData(size_t dimension, size_t deriv = 0) const;
Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation) const;
Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation, double startTime, double endTime) const;
@@ -253,18 +252,20 @@ namespace armarx
size_t dim() const;
size_t size() const;
double getTimeLength() const;
- double getSpaceLength(size_t dimension, size_t stateDim) const;
- double getSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const;
- double getSquaredSpaceLength(size_t dimension, size_t stateDim) const;
- double getSquaredSpaceLength(size_t dimension, size_t stateDim, double startTime, double endTime) const;
- double getMax(size_t dimension, size_t stateDim, double startTime, double endTime) const;
- double getMin(size_t dimension, size_t stateDim, double startTime, double endTime) const;
- double getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t stateDim, double startTime, double endTime) const;
- double getTrajectorySpace(size_t dimension, size_t stateDim, double startTime, double endTime) const;
- Ice::DoubleSeq getMinima(size_t dimension, size_t stateDim, double startTime, double endTime) const;
- Ice::DoubleSeq getMaxima(size_t dimension, size_t stateDim, double startTime, double endTime) const;
- Ice::DoubleSeq getMinimaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const;
- Ice::DoubleSeq getMaximaPositions(size_t dimension, size_t stateDim, double startTime, double endTime) const;
+ double getSpaceLength(size_t derivation = 0) const;
+ double getSpaceLength(size_t derivation, double startTime, double endTime) const;
+ double getSpaceLength(size_t dimension, size_t derivation) const;
+ double getSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double getSquaredSpaceLength(size_t dimension, size_t derivation) const;
+ double getSquaredSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double getMax(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double getMin(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double getTrajectorySpace(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Ice::DoubleSeq getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Ice::DoubleSeq getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Ice::DoubleSeq getMinimaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Ice::DoubleSeq getMaximaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const;
// calculations
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
index 65289894b..54ac0bc4c 100644
--- a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -177,14 +177,17 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
// Load the trajectory
Ice::DoubleSeq timestamps;
int count = 1000;
- for (int i = 0; i < count; ++i) {
+ for (int i = 0; i < count; ++i)
+ {
timestamps.push_back(rand());
}
Trajectory traj;
- for (int var = 0; var < 8; ++var) {
+ for (int var = 0; var < 8; ++var)
+ {
Ice::DoubleSeq p;
p.reserve(count);
- for (int i = 0; i < count; ++i) {
+ for (int i = 0; i < count; ++i)
+ {
p.push_back(rand());
}
traj.addDimension(timestamps, p);
@@ -199,8 +202,12 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
ARMARX_INFO_S << "Cleared";
traj2->ice_postUnmarshal();
ARMARX_INFO_S << "Unmarshalled";
-
+ traj.getSpaceLength();
+ ARMARX_INFO_S << "Length calculated";
BOOST_CHECK_EQUAL(traj.getState(0), traj2->getState(0));
+ JSONObjectPtr json = new JSONObject();
+ traj.serialize(json);
+ ARMARX_INFO_S << json->asString(true);
}
@@ -215,8 +222,19 @@ BOOST_AUTO_TEST_CASE(TrajectorySerializationTest)
Trajectory traj2;
traj2.deserialize(json);
BOOST_CHECK_CLOSE(traj.getState(0), traj2.getState(0), 0.01);
- BOOST_CHECK_CLOSE(traj.getState(0.4,1), traj2.getState(0.4,1), 0.01);
+ BOOST_CHECK_CLOSE(traj.getState(0.4, 1), traj2.getState(0.4, 1), 0.01);
BOOST_CHECK_EQUAL(traj.getDimensionName(0), traj2.getDimensionName(0));
}
+BOOST_AUTO_TEST_CASE(TrajectoryLengthTest)
+{
+ // Load the trajectory
+ FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
+ Trajectory traj(positions, {}, {"joint1", "joint2"});
+
+ BOOST_CHECK_EQUAL(traj.getSpaceLength(), (Eigen::Vector3f(positions[0].data())- Eigen::Vector3f(positions[1].data())).norm());
+}
+
+
+
--
GitLab