diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 26d42594bfdc6eb7b1cc4c9444947a0f4a7b500b..6642bc9f6b5d5b9a0eb53d348d5905c62eb50b40 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -30,7 +30,12 @@
namespace armarx
{
- void Trajectory::ice_preMarshal()
+Trajectory::~Trajectory()
+{
+
+}
+
+void Trajectory::ice_preMarshal()
{
dataVec.clear();
timestamps.clear();
@@ -120,7 +125,14 @@ namespace armarx
std::string Trajectory::output(const Ice::Current&) const
{
- return "Trajectory with " + std::to_string(dim()) + " dimensions";
+ const ordered_view& ordv = dataMap.get<TagOrdered>();
+ typename ordered_view::const_iterator itMap = ordv.begin();
+ std::stringstream s;
+ for (; itMap != ordv.end(); itMap++)
+ {
+ s << *itMap << std::endl;
+ }
+ return s.str();
}
Ice::Int Trajectory::getType(const Ice::Current&) const
@@ -164,7 +176,7 @@ namespace armarx
for (; it != data.end(); it++)
{
- TrajData dataEntry(*this);
+ TrajData dataEntry(this);
dataEntry.timestamp = it->first;
const Ice::DoubleSeq& dataVec = it->second;
@@ -472,12 +484,12 @@ namespace armarx
}
}
- double Trajectory::getSpaceLength(size_t derivation) const
+ double Trajectory::getLength(size_t derivation) const
{
- return getSpaceLength(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+ return getLength(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- double Trajectory::getSpaceLength(size_t derivation, double startTime, double endTime) const
+ double Trajectory::getLength(size_t derivation, double startTime, double endTime) const
{
double length = 0.0;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -514,12 +526,12 @@ namespace armarx
return length;
}
- double Trajectory::getSpaceLength(size_t dimension, size_t derivation) const
+ double Trajectory::getLength(size_t dimension, size_t derivation) const
{
- return getSpaceLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+ return getLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- double Trajectory::getSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const
+ double Trajectory::getLength(size_t dimension, size_t derivation, double startTime, double endTime) const
{
double length = 0.0;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -543,12 +555,12 @@ namespace armarx
return length;
}
- double Trajectory::getSquaredSpaceLength(size_t dimension, size_t derivation) const
+ double Trajectory::getSquaredLength(size_t dimension, size_t derivation) const
{
- return getSquaredSpaceLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
+ return getSquaredLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
}
- double Trajectory::getSquaredSpaceLength(size_t dimension, size_t derivation, double startTime, double endTime) const
+ double Trajectory::getSquaredLength(size_t dimension, size_t derivation, double startTime, double endTime) const
{
double length = 0.0;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -605,7 +617,7 @@ namespace armarx
return bestValue;
}
- double Trajectory::getTrajectorySpace(size_t dimension, size_t derivation, double startTime, double endTime) const
+ double Trajectory::getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const
{
return getMax(dimension, derivation, startTime, endTime) - getMin(dimension, derivation, startTime, endTime);
}
@@ -645,7 +657,7 @@ namespace armarx
return result;
}
- Ice::DoubleSeq Trajectory::getMinimaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const
+ Ice::DoubleSeq Trajectory::getMinimaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -714,7 +726,7 @@ namespace armarx
return result;
}
- Ice::DoubleSeq Trajectory::getMaximaPositions(size_t dimension, size_t derivation, double startTime, double endTime) const
+ Ice::DoubleSeq Trajectory::getMaximaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
{
Ice::DoubleSeq result;
const ordered_view& ordv = dataMap.get<TagOrdered>();
@@ -806,7 +818,7 @@ namespace armarx
}
}
- TrajData entry(*this);
+ TrajData entry(this);
entry.timestamp = t;
entry.data.resize(dim());
dataMap.insert(entry);
@@ -957,7 +969,7 @@ namespace armarx
if (itMap == dataMap.end() || dim() == 0)
{
// double dura = getTimeLength();
- TrajData newData(*this);
+ TrajData newData(this);
newData.timestamp = t;
newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
newData.data[dimIndex] = DoubleSeqPtr(new Ice::DoubleSeq(y));
@@ -985,7 +997,7 @@ namespace armarx
if (itMap == dataMap.end() || dim() == 0)
{
- TrajData newData(*this);
+ TrajData newData(this);
newData.timestamp = t;
newData.data = std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
newData.data[dimIndex] = DoubleSeqPtr(new Ice::DoubleSeq(1, y));
@@ -1572,7 +1584,7 @@ namespace armarx
- Ice::DoubleSeq Trajectory::normalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime, const double endTime)
+ Ice::DoubleSeq Trajectory::NormalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime, const double endTime)
{
Ice::DoubleSeq normTimestamps;
normTimestamps.resize(timestamps.size());
@@ -1589,7 +1601,7 @@ namespace armarx
}
- Trajectory Trajectory::normalizeTimestamps(const Trajectory& traj, const double startTime, const double endTime)
+ Trajectory Trajectory::NormalizeTimestamps(const Trajectory& traj, const double startTime, const double endTime)
{
if (traj.size() <= 1 || (traj.begin()->timestamp == startTime && traj.rbegin()->timestamp == endTime))
@@ -1601,7 +1613,7 @@ namespace armarx
Ice::DoubleSeq timestamps = traj.getTimestamps();
- Ice::DoubleSeq normTimestamps = normalizeTimestamps(timestamps, startTime, endTime);
+ Ice::DoubleSeq normTimestamps = NormalizeTimestamps(timestamps, startTime, endTime);
Trajectory normExampleTraj;
for (size_t dim = 0; dim < traj.dim(); dim++)
@@ -1742,10 +1754,9 @@ namespace armarx
setEntries(t, dimension, derivs);
}
- Trajectory::TrajData::TrajData(Trajectory& traj) :
- trajectory(traj)
+ Trajectory::TrajData::TrajData(Trajectory* traj)
{
-
+ trajectory = traj;
}
void Trajectory::shiftTime(double shift)
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 3245c52b482c76afdd15e62d02dfaf628c96257e..0255e219a6ffed139ba821d4312e4db2e128e07a 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -52,6 +52,7 @@ namespace armarx
const VariantTypeId Trajectory = Variant::addTypeName("::armarx::TrajectoryBase");
}
+
typedef boost::shared_ptr<Ice::DoubleSeq> DoubleSeqPtr;
class Trajectory;
@@ -82,6 +83,8 @@ namespace armarx
*/
class Trajectory : public TrajectoryBase
{
+ protected:
+ ~Trajectory();
public:
// Object interface
void ice_preMarshal();
@@ -105,11 +108,24 @@ namespace armarx
struct TrajData
{
- TrajData(Trajectory& traj);
- DoubleSeqPtr operator[](size_t dim)
+ TrajData(){}
+ TrajData(Trajectory *traj);
+ inline DoubleSeqPtr operator[](size_t dim)
{
return data.at(dim);
}
+ inline TrajData& operator=(const TrajData& s)
+ {
+ timestamp = s.timestamp;
+ data.resize(s.data.size());
+ int i = 0;
+ for(auto& elem : s.data)
+ {
+ data[i++] = elem;
+ }
+ trajectory = nullptr;
+ return *this;
+ }
inline double getTimestamp() const
{
return timestamp;
@@ -120,16 +136,30 @@ namespace armarx
}
inline double getDeriv(size_t dim, size_t derivation) const
{
- return trajectory.getState(timestamp, dim, derivation);
+ if(!trajectory)
+ throw LocalException("Ptr to trajectory is NULL");
+ return trajectory->getState(timestamp, dim, derivation);
}
inline const std::vector< DoubleSeqPtr >& getData() const
{
return data;
}
+ friend std::ostream& operator<<(std::ostream& stream, const TrajData& rhs)
+ {
+ stream << rhs.timestamp << ": ";
+ for (size_t d = 0; d < rhs.data.size(); ++d) {
+ stream << (rhs.data[d]->size() > 0 ? std::to_string(rhs.data[d]->at(0)) : std::string("-"));
+ if(d <= rhs.data.size()-1)
+ stream << ", ";
+ }
+ return stream;
+ }
+
double timestamp;
mutable std::vector< DoubleSeqPtr > data; // mutable so that it can be changed
- Trajectory& trajectory;
+ protected:
+ Trajectory* trajectory = nullptr;
friend class Trajectory;
};
@@ -252,20 +282,32 @@ namespace armarx
size_t dim() const;
size_t size() const;
double getTimeLength() 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;
+ /**
+ * @brief Returns the sum of a all subsequent distances of the entries in the trajectories over all dimensions
+ * @param derivation Derivation for which the length is calculated
+ */
+ double getLength(size_t derivation = 0) const;
+ double getLength(size_t derivation, double startTime, double endTime) const;
+ double getLength(size_t dimension, size_t derivation) const;
+ double getLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ double getSquaredLength(size_t dimension, size_t derivation) const;
+ double getSquaredLength(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;
+ double getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ /**
+ * @brief Calculate *all* minima.
+ * @param dimension
+ * @param derivation
+ * @param startTime
+ * @param endTime
+ * @return
+ */
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;
+ Ice::DoubleSeq getMinimaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const;
+ Ice::DoubleSeq getMaximaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const;
// calculations
@@ -290,8 +332,8 @@ namespace armarx
*/
void gaussianFilter(double filterRadius);
- static Trajectory normalizeTimestamps(const Trajectory& traj, const double startTime = 0.0, const double endTime = 1.0);
- static Ice::DoubleSeq normalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime = 0.0, const double endTime = 1.0);
+ static Trajectory NormalizeTimestamps(const Trajectory& traj, const double startTime = 0.0, const double endTime = 1.0);
+ static Ice::DoubleSeq NormalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime = 0.0, const double endTime = 1.0);
static Ice::DoubleSeq GenerateTimestamps(double startTime = 0.0, double endTime = 1.0, double stepSize = 0.001);
void shiftTime(double shift);
void shiftValue(const Ice::DoubleSeq& shift);
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
index 54ac0bc4c7f52f97be6649ae7de20b758f8901c2..e81204ba7797f21e1a207da83ae0514bcbdb63c1 100644
--- a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -33,16 +33,16 @@ using namespace armarx;
//{
// // Load the trajectory
// Trajectory traj;
-// // traj.readFromCSVFile("pouring_x_short.csv");
-// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
-// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
-// BOOST_CHECK(traj.begin()->getPosition(0) == 445.35);
+// // traj->readFromCSVFile("pouring_x_short.csv");
+// // traj->readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj->readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// BOOST_CHECK(traj->begin()->getPosition(0) == 445.35);
// traj = Trajectory::normalizeTimestamps(traj, 0, 1);
-// Ice::DoubleSeq timestamps = traj.getTimestamps();
-// traj.differentiateDiscretly(2);
-// Ice::DoubleSeq d0 = traj.getDimensionData(0, 0);
-// Ice::DoubleSeq d1 = traj.getDimensionData(0, 1);
-// BOOST_CHECK(traj.begin()->getDeriv(0, 1) == d1.at(0));
+// Ice::DoubleSeq timestamps = traj->getTimestamps();
+// traj->differentiateDiscretly(2);
+// Ice::DoubleSeq d0 = traj->getDimensionData(0, 0);
+// Ice::DoubleSeq d1 = traj->getDimensionData(0, 1);
+// BOOST_CHECK(traj->begin()->getDeriv(0, 1) == d1.at(0));
// std::cout << "end" << std::endl;
//}
@@ -52,16 +52,16 @@ using namespace armarx;
//{
// // Load the trajectory
// Trajectory traj;
-// // traj.readFromCSVFile("pouring_x_short.csv");
-// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
-// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// // traj->readFromCSVFile("pouring_x_short.csv");
+// // traj->readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj->readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
// // traj = Trajectory::normalizeTimestamps(traj,0,1);
-// Ice::DoubleSeq timestamps = traj.getTimestamps();
-// traj.differentiateDiscretly(2);
-// double first = traj.begin()->getPosition(0);
-// double derivFirst = traj.begin()->getDeriv(0, 1);
-// double before = traj.getState(timestamps.at(0) - 5, 0, 0);
+// Ice::DoubleSeq timestamps = traj->getTimestamps();
+// traj->differentiateDiscretly(2);
+// double first = traj->begin()->getPosition(0);
+// double derivFirst = traj->begin()->getDeriv(0, 1);
+// double before = traj->getState(timestamps.at(0) - 5, 0, 0);
// BOOST_CHECK(before < SIGN_FACTOR(derivFirst) * first);
// std::cout << "end" << std::endl;
//}
@@ -70,14 +70,14 @@ using namespace armarx;
//{
// // Load the trajectory
// Trajectory traj;
-// // traj.readFromCSVFile("pouring_x_short.csv");
-// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
-// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
-// Ice::DoubleSeq timestamps = traj.getTimestamps();
+// // traj->readFromCSVFile("pouring_x_short.csv");
+// // traj->readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj->readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// Ice::DoubleSeq timestamps = traj->getTimestamps();
// double middle = (timestamps.at(1) + timestamps.at(0)) * 0.5;
-// double before = traj.getState(timestamps.at(0));
-// double after = traj.getState(timestamps.at(1));
-// double interpolation = traj.getState(middle);
+// double before = traj->getState(timestamps.at(0));
+// double after = traj->getState(timestamps.at(1));
+// double interpolation = traj->getState(middle);
// BOOST_CHECK(before < interpolation && after > interpolation);
// std::cout << "end" << std::endl;
//}
@@ -86,21 +86,21 @@ using namespace armarx;
//{
// // Load the trajectory
// Trajectory traj;
-// // traj.readFromCSVFile("pouring_x_short.csv");
-// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
-// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
-// traj.differentiateDiscretly(1);
-// traj.differentiateDiscretly(2);
+// // traj->readFromCSVFile("pouring_x_short.csv");
+// // traj->readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj->readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// traj->differentiateDiscretly(1);
+// traj->differentiateDiscretly(2);
// Trajectory traj2;
-// traj2.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
-// traj2.differentiateDiscretly(2);
-
-// BOOST_CHECK_EQUAL(traj.begin()->getDeriv(0, 2), traj2.begin()->getDeriv(0, 2));
-// BOOST_CHECK_EQUAL(traj.rbegin()->getDeriv(0, 2), traj2.rbegin()->getDeriv(0, 2));
-// BOOST_CHECK_EQUAL(traj.rbegin()->getDeriv(0, 1), traj2.rbegin()->getDeriv(0, 1));
-// double middleTimestamp = traj.getTimestamps().at(0) + traj.getTimeLength() / 2;
-// BOOST_CHECK_EQUAL(traj.getState(middleTimestamp, 0, 2), traj2.getState(middleTimestamp, 0, 2));
-// BOOST_CHECK_EQUAL(traj.getState(middleTimestamp, 0, 1), traj2.getState(middleTimestamp, 0, 1));
+// traj2->readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// traj2->differentiateDiscretly(2);
+
+// BOOST_CHECK_EQUAL(traj->begin()->getDeriv(0, 2), traj2->begin()->getDeriv(0, 2));
+// BOOST_CHECK_EQUAL(traj->rbegin()->getDeriv(0, 2), traj2->rbegin()->getDeriv(0, 2));
+// BOOST_CHECK_EQUAL(traj->rbegin()->getDeriv(0, 1), traj2->rbegin()->getDeriv(0, 1));
+// double middleTimestamp = traj->getTimestamps().at(0) + traj->getTimeLength() / 2;
+// BOOST_CHECK_EQUAL(traj->getState(middleTimestamp, 0, 2), traj2->getState(middleTimestamp, 0, 2));
+// BOOST_CHECK_EQUAL(traj->getState(middleTimestamp, 0, 1), traj2->getState(middleTimestamp, 0, 1));
// std::cout << "end" << std::endl;
//}
@@ -110,78 +110,78 @@ using namespace armarx;
//{
// // Load the trajectory
// Trajectory traj;
-// // traj.readFromCSVFile("pouring_x_short.csv");
-// // traj.readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
-// traj.readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
-// double d2 = traj.begin()->getDeriv(0, 2);
+// // traj->readFromCSVFile("pouring_x_short.csv");
+// // traj->readFromCSVFile("sampletraj_withvel_shortened.csv.txt");
+// traj->readFromCSVFile(std::string(DATA_DIR) + "/ExampleTrajectories/pouring_x_short.csv");
+// double d2 = traj->begin()->getDeriv(0, 2);
// BOOST_CHECK(d2 != 0);
// std::cout << "end" << std::endl;
//}
BOOST_AUTO_TEST_CASE(testInplaceDerivation2)
{
- // Load the trajectory
+
Ice::DoubleSeq timestamps { 0, 1, 2 };
Ice::DoubleSeq positions { 1, 2, 5 };
- Trajectory traj(timestamps, positions);
- Trajectory traj2(timestamps, positions);
- traj.differentiateDiscretly(1);
- BOOST_CHECK_EQUAL(traj.getState(-1, 0, 1), traj2.getState(-1, 0, 1));
+ TrajectoryPtr traj = new Trajectory(timestamps, positions);
+ TrajectoryPtr traj2 = new Trajectory(timestamps, positions);
+ traj->differentiateDiscretly(1);
+ BOOST_CHECK_EQUAL(traj->getState(-1, 0, 1), traj2->getState(-1, 0, 1));
}
BOOST_AUTO_TEST_CASE(testInplaceDerivation3)
{
- // Load the trajectory
+
Ice::DoubleSeq timestamps { 0, 1, 2 };
Ice::DoubleSeq positions { 1, 2, 5 };
- Trajectory traj(timestamps, positions);
- Trajectory traj2(timestamps, positions);
- traj.differentiateDiscretly(2);
- BOOST_CHECK_EQUAL(traj.getState(-1, 0, 2), traj2.getState(-1, 0, 2));
+ TrajectoryPtr traj = new Trajectory(timestamps, positions);
+ TrajectoryPtr traj2 = new Trajectory(timestamps, positions);
+ traj->differentiateDiscretly(2);
+ BOOST_CHECK_EQUAL(traj->getState(-1, 0, 2), traj2->getState(-1, 0, 2));
}
BOOST_AUTO_TEST_CASE(testDerivationRemoval)
{
- // Load the trajectory
+
Ice::DoubleSeq timestamps { 0, 1, 2 };
Ice::DoubleSeq positions { 1, 2, 5 };
- Trajectory traj(timestamps, positions);
- // Trajectory traj2(timestamps, positions);
- traj.differentiateDiscretly(2);
- BOOST_CHECK_EQUAL(traj.begin()->getData().at(0)->size(), 3);
- traj.removeDerivation(1);
- BOOST_CHECK_EQUAL(traj.begin()->getData().at(0)->size(), 1);
+ TrajectoryPtr traj = new Trajectory(timestamps, positions);
+ // TrajectoryPtr traj2 = new Trajectory(timestamps, positions);
+ traj->differentiateDiscretly(2);
+ BOOST_CHECK_EQUAL(traj->begin()->getData().at(0)->size(), 3);
+ traj->removeDerivation(1);
+ BOOST_CHECK_EQUAL(traj->begin()->getData().at(0)->size(), 1);
}
BOOST_AUTO_TEST_CASE(TrajectoryMarshalTest)
{
- // Load the trajectory
+
Ice::DoubleSeq timestamps { 0, 1, 2 };
Ice::DoubleSeq positions { 1, 2, 5 };
- Trajectory traj(timestamps, positions);
- traj.ice_preMarshal();
- TrajectoryPtr traj2 = TrajectoryPtr::dynamicCast(traj.clone());
+ TrajectoryPtr traj = new Trajectory(timestamps, positions);
+ traj->ice_preMarshal();
+ TrajectoryPtr traj2 = TrajectoryPtr::dynamicCast(traj->clone());
traj2->clear();
traj2->ice_postUnmarshal();
- BOOST_CHECK_EQUAL(traj.getState(0), traj2->getState(0));
+ BOOST_CHECK_EQUAL(traj->getState(0), traj2->getState(0));
}
BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
{
- // Load the trajectory
+
Ice::DoubleSeq timestamps;
int count = 1000;
for (int i = 0; i < count; ++i)
{
timestamps.push_back(rand());
}
- Trajectory traj;
+ TrajectoryPtr traj(new Trajectory());
for (int var = 0; var < 8; ++var)
{
Ice::DoubleSeq p;
@@ -190,49 +190,63 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
{
p.push_back(rand());
}
- traj.addDimension(timestamps, p);
+ traj->addDimension(timestamps, p);
}
ARMARX_INFO_S << "Starting";
- traj.ice_preMarshal();
+ traj->ice_preMarshal();
ARMARX_INFO_S << "Marshalled";
- TrajectoryPtr traj2 = TrajectoryPtr::dynamicCast(traj.clone());
+ TrajectoryPtr traj2 = TrajectoryPtr::dynamicCast(traj->clone());
ARMARX_INFO_S << "Cloned";
traj2->clear();
ARMARX_INFO_S << "Cleared";
traj2->ice_postUnmarshal();
ARMARX_INFO_S << "Unmarshalled";
- traj.getSpaceLength();
+ traj->getLength();
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);
+ BOOST_CHECK_EQUAL(traj->getState(0), traj2->getState(0));
}
BOOST_AUTO_TEST_CASE(TrajectorySerializationTest)
{
- // Load the trajectory
+
FloatSeqSeq positions {{ 1, 2, 5 }, { 10, 2332, 53425 }};
JSONObjectPtr json = new JSONObject();
- Trajectory traj(positions, {}, {"joint1", "joint2"});
- traj.serialize(json);
+ TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
+ traj->serialize(json);
ARMARX_INFO_S << json->asString(true);
- 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_EQUAL(traj.getDimensionName(0), traj2.getDimensionName(0));
+ TrajectoryPtr traj2 = new Trajectory();
+ 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_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"});
+ TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
+
+ BOOST_CHECK_EQUAL(traj->getLength(), (Eigen::Vector3f(positions[0].data())- Eigen::Vector3f(positions[1].data())).norm());
+}
- BOOST_CHECK_EQUAL(traj.getSpaceLength(), (Eigen::Vector3f(positions[0].data())- Eigen::Vector3f(positions[1].data())).norm());
+BOOST_AUTO_TEST_CASE(TrajectoryCopyIfTest)
+{
+
+ FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
+ TrajectoryPtr traj = new Trajectory(positions, {}, {"joint1", "joint2"});
+ std::vector<Trajectory::TrajData> selection(traj->size());
+ // select all elements with velocity > 0
+ auto it = std::copy_if(traj->begin(), traj->end(), selection.begin(),
+ [](const Trajectory::TrajData& elem)
+ {
+ return fabs(elem.getDeriv(0,1)) > 0;
+ });
+ selection.resize(std::distance(selection.begin(),it));
+ ARMARX_INFO_S << VAROUT(*traj);
+ ARMARX_INFO_S << VAROUT(selection);
}