diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index ee7495af91d4e6a32558b3238536659dcd603c02..082e6fa7a3c2acbea62c3863dac7c3951226e5a1 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -31,7 +31,6 @@ namespace armarx
Trajectory::~Trajectory()
{
-
}
void Trajectory::ice_preMarshal()
@@ -63,6 +62,7 @@ namespace armarx
void Trajectory::ice_postUnmarshal()
{
int i = 0;
+ dataMap.clear();
for (DoubleSeqSeq& _2DVec : dataVec)
{
@@ -185,8 +185,7 @@ namespace armarx
checkValue(dataVec[i]);
dataEntry.data.push_back(DoubleSeqPtr(new Ice::DoubleSeq(1, dataVec[i])));
}
-
- dataMap.insert(dataEntry);
+ dataMap.insert(std::move(dataEntry));
}
}
@@ -489,6 +488,10 @@ namespace armarx
double Trajectory::getLength(size_t derivation) const
{
+ if (size() == 0)
+ {
+ return 0;
+ }
return getLength(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
@@ -496,8 +499,8 @@ namespace armarx
{
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);
+ ordered_view::const_iterator itO = ordv.lower_bound(startTime);
+ ordered_view::const_iterator itOEnd = ordv.upper_bound(endTime);
if (itO == ordv.end())
{
return 0.0;
@@ -506,31 +509,52 @@ namespace armarx
Ice::DoubleSeq prevValue(dimensions);
for (size_t d = 0; d < dimensions; ++d)
{
- prevValue[d] = itO->getDeriv(d, derivation);
+ prevValue[d] = getState(startTime, d, derivation);
}
+
itO++;
+
+ double segmentLength = 0;
for (;
- itO != itOEnd;
+ itO != itOEnd && itO != ordv.end();
itO++
)
{
- double segmentLength = 0;
+ if (itO->getTimestamp() >= endTime)
+ {
+ break;
+ }
+ segmentLength = 0;
double value;
for (size_t d = 0; d < dimensions; ++d)
{
value = itO->getDeriv(d, derivation);
- segmentLength += pow(prevValue[d] - value, 2);
+ double diff = value - prevValue[d];
+ segmentLength += diff * diff;
prevValue[d] = value;
}
- length = sqrt(segmentLength);
+ length += sqrt(segmentLength);
}
-
+ segmentLength = 0;
+ // interpolate end values
+ for (size_t d = 0; d < dimensions; ++d)
+ {
+ double value = getState(endTime, d, derivation);
+ double diff = value - prevValue[d];
+ segmentLength += diff * diff;
+ prevValue[d] = value;
+ }
+ length += sqrt(segmentLength);
return length;
}
double Trajectory::getLength(size_t dimension, size_t derivation) const
{
+ if (size() == 0)
+ {
+ return 0;
+ }
return getLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
@@ -544,17 +568,21 @@ namespace armarx
{
return 0.0;
}
- double prevValue = itO->getDeriv(dimension, derivation);
+ double prevValue = getState(startTime, dimension, derivation);
for (;
- itO != itOEnd;
+ itO != itOEnd && itO != ordv.end();
itO++
)
{
+ if (itO->getTimestamp() >= endTime)
+ {
+ break;
+ }
length += fabs(prevValue - itO->getDeriv(dimension, derivation));
prevValue = itO->getDeriv(dimension, derivation);
}
-
+ length += fabs(prevValue - getState(endTime, dimension, derivation));
return length;
}
@@ -968,7 +996,7 @@ namespace armarx
newData.timestamp = t;
newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
newData.data[dimIndex] = DoubleSeqPtr(new Ice::DoubleSeq(y));
- dataMap.insert(newData);
+ dataMap.insert(std::move(newData));
}
else
{
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index e6805755c6ca256ce2f544f8f4d96e4a7b4cc901..33422bc0098b83a4fb23ffee3bfc099d77f46d58 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -102,7 +102,7 @@ namespace armarx
int i = 0;
for (auto& elem : s.data)
{
- data[i++] = elem;
+ data[i++].reset(new Ice::DoubleSeq(*elem));
}
trajectory = nullptr;
return *this;
@@ -176,7 +176,7 @@ namespace armarx
mutable std::vector< DoubleSeqPtr > data; // mutable so that it can be changed
protected:
Trajectory* trajectory = nullptr;
- friend class Trajectory;
+ // friend class Trajectory;
};
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
index db6263acd9606a9789b0f45f501f0da8c96a30a0..68b3b520837379c72d92275caa15cfc6772e30d8 100644
--- a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -65,7 +65,7 @@ BOOST_AUTO_TEST_CASE(TrajectoryIteratorUsage)
}
// or with c++11 for loop:
- for (const Trajectory::TrajData & element : *trajectory)
+ for (const Trajectory::TrajData& element : *trajectory)
{
ARMARX_INFO_S << "Position at " << element.getTimestamp() << " :" << element.getPosition(0);
}
@@ -161,6 +161,34 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
BOOST_CHECK_EQUAL(traj->getState(0), traj2->getState(0));
}
+BOOST_AUTO_TEST_CASE(TrajectoryUnmarshalTest)
+{
+ Ice::DoubleSeq timestamps;
+ int count = 1000;
+ for (int i = 0; i < count; ++i)
+ {
+ timestamps.push_back(i);
+ }
+ TrajectoryPtr traj(new Trajectory());
+ for (int var = 0; var < 8; ++var)
+ {
+ Ice::DoubleSeq p;
+ p.reserve(count);
+ for (int i = 0; i < count; ++i)
+ {
+ p.push_back(rand());
+ }
+ traj->addDimension(p, timestamps);
+ }
+ ARMARX_INFO_S << "Starting";
+ traj->ice_preMarshal();
+
+ traj->clear();
+ traj->ice_postUnmarshal();
+ ARMARX_INFO << "length: " << traj->getLength();
+ traj->getLength(0, 50, 350);
+}
+
BOOST_AUTO_TEST_CASE(TrajectorySerializationTest)
{
@@ -180,10 +208,10 @@ BOOST_AUTO_TEST_CASE(TrajectorySerializationTest)
BOOST_AUTO_TEST_CASE(TrajectoryLengthTest)
{
- FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
- TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
+ FloatSeqSeq jointtrajectories {{ 0, 0, 1 }, { 0, 1, 1 }};
+ TrajectoryPtr traj = new Trajectory(jointtrajectories, {}, {"joint1", "joint2"});
- BOOST_CHECK_EQUAL(traj->getLength(), (Eigen::Vector3f(positions[0].data()) - Eigen::Vector3f(positions[1].data())).norm());
+ BOOST_CHECK_EQUAL(traj->getLength(), 2);
}
BOOST_AUTO_TEST_CASE(TrajectoryCopyIfTest)