diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 20abfb8c4aafe07dc034006f08fc3b5b20113c8f..160c1d04bb4f93f0d84c5859b97d1432513d6b31 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -13,6 +13,7 @@
<Variant baseType="::armarx::LinkedDirectionBase" dataType="::armarx::LinkedDirection" humanName="LinkedDirection" />
<Variant baseType="::armarx::OrientedPointBase" dataType="::armarx::OrientedPoint" humanName="OrientedPoint" />
<Variant baseType="::armarx::FramedOrientedPointBase" dataType="::armarx::FramedOrientedPoint" humanName="FramedOrientedPoint" />
+ <Variant baseType="::armarx::TrajectoryBase" dataType="::armarx::Trajectory" humanName="Trajectory"/>
</Lib>
<Lib name="RobotAPIInterfaces">
<Proxy include="RobotAPI/interface/units/KinematicUnitInterface.h"
diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp
index 8cfbf6e7d8059d8e79bd425fb5153fa705e31e11..c3f8a56c719298d92f49e512c776a738ad33ca8e 100644
--- a/source/RobotAPI/libraries/core/Trajectory.cpp
+++ b/source/RobotAPI/libraries/core/Trajectory.cpp
@@ -30,12 +30,12 @@
namespace armarx
{
-Trajectory::~Trajectory()
-{
+ Trajectory::~Trajectory()
+ {
-}
+ }
-void Trajectory::ice_preMarshal()
+ void Trajectory::ice_preMarshal()
{
dataVec.clear();
timestamps.clear();
@@ -47,7 +47,14 @@ void Trajectory::ice_preMarshal()
new2DVec.reserve(data.size());
for (DoubleSeqPtr & subVec : data)
{
- new2DVec.emplace_back(*subVec);
+ if (subVec)
+ {
+ new2DVec.emplace_back(*subVec);
+ }
+ else
+ {
+ new2DVec.emplace_back(Ice::DoubleSeq());
+ }
}
dataVec.emplace_back(new2DVec);
}
@@ -113,7 +120,7 @@ void Trajectory::ice_preMarshal()
{
newRow.push_back(atof(v.c_str()));
}
- addDimension(timestamps, newRow);
+ addDimension(newRow, timestamps);
}
obj->getStringArray("dimensionNames", dimensionNames);
}
@@ -159,12 +166,6 @@ void Trajectory::ice_preMarshal()
- Trajectory::Trajectory(const Ice::DoubleSeq& x, const Ice::DoubleSeq& y, const std::string& dimensionName)
- {
- addDimension(x, y, dimensionName);
- }
-
-
Trajectory::Trajectory(const std::map<double, Ice::DoubleSeq >& data)
{
if (data.size() == 0)
@@ -912,14 +913,6 @@ void Trajectory::ice_preMarshal()
}
-
-
-
-
-
-
-
-
Trajectory& Trajectory::operator +=(const Trajectory traj)
{
int dims = traj.dim();
@@ -1419,7 +1412,7 @@ void Trajectory::ice_preMarshal()
entries.push_back(__gaussianFilter(filterRadius, it, d, 0));
}
- filteredTraj.addDimension(timestamps, entries);
+ filteredTraj.addDimension(entries, timestamps);
}
CopyData(filteredTraj, *this);
@@ -1494,7 +1487,7 @@ void Trajectory::ice_preMarshal()
for (size_t dim = 0; dim < source.dim(); dim++)
{
- destination.addDimension(timestamps, source.getDimensionData(dim));
+ destination.addDimension(source.getDimensionData(dim), timestamps);
}
}
@@ -1570,7 +1563,7 @@ void Trajectory::ice_preMarshal()
for (size_t dim = 0; dim < traj.dim(); dim++)
{
Ice::DoubleSeq dimensionData = traj.getDimensionData(dim);
- normExampleTraj.addDimension(normTimestamps, dimensionData);
+ normExampleTraj.addDimension(dimensionData, normTimestamps);
}
return normExampleTraj;
@@ -1584,18 +1577,25 @@ void Trajectory::ice_preMarshal()
- size_t Trajectory::addDimension(const Ice::DoubleSeq& x, const Ice::DoubleSeq& y, const std::string name)
+
+ size_t Trajectory::addDimension(const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps, const std::string name)
{
+ const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : GenerateTimestamps(values);
+
size_t newDim = dim() + 1;
__addDimension();
- addPositionsToDimension(newDim - 1, x, y);
- if(dimensionNames.size() > newDim-1)
- dimensionNames.at(newDim-1) = name;
+ addPositionsToDimension(newDim - 1, values, tempTimestamps);
+ if (dimensionNames.size() > newDim - 1)
+ {
+ dimensionNames.at(newDim - 1) = name;
+ }
else
+ {
dimensionNames.push_back(name);
+ }
return newDim - 1;
}
@@ -1680,21 +1680,21 @@ void Trajectory::ice_preMarshal()
- void Trajectory::addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& x, const Ice::DoubleSeq& y)
+ void Trajectory::addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps)
{
if (dimension >= dim() && dim() > 0)
{
- addDimension(x, y);
+ addDimension(values, timestamps);
}
else
{
- BOOST_ASSERT(x.size() == y.size());
+ BOOST_ASSERT(timestamps.size() == values.size());
- for (size_t i = 0; i < x.size(); ++i)
+ for (size_t i = 0; i < timestamps.size(); ++i)
{
- checkValue(x[i]);
- checkValue(y[i]);
- setPositionEntry(x[i], dimension, y[i]);
+ checkValue(timestamps[i]);
+ checkValue(values[i]);
+ setPositionEntry(timestamps[i], dimension, values[i]);
}
}
}
@@ -1727,12 +1727,14 @@ void Trajectory::ice_preMarshal()
double Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const
{
- if(!trajectory)
+ if (!trajectory)
+ {
throw LocalException("Ptr to trajectory is NULL");
+ }
return trajectory->getState(timestamp, dim, derivation);
}
- const std::vector<DoubleSeqPtr> &Trajectory::TrajData::getData() const
+ const std::vector<DoubleSeqPtr>& Trajectory::TrajData::getData() const
{
return data;
}
diff --git a/source/RobotAPI/libraries/core/Trajectory.h b/source/RobotAPI/libraries/core/Trajectory.h
index 16678be210ede2d5bec9496d19197f4d75d83944..e2e9523ff89df6f6c24b022c9157357cdb337826 100644
--- a/source/RobotAPI/libraries/core/Trajectory.h
+++ b/source/RobotAPI/libraries/core/Trajectory.h
@@ -37,6 +37,7 @@
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/variant/Variant.h>
+#include <boost/type_traits/is_arithmetic.hpp>
#include <Eigen/Core>
@@ -90,15 +91,15 @@ namespace armarx
struct TrajData
{
- TrajData(){}
- TrajData(Trajectory *traj);
+ TrajData() {}
+ TrajData(Trajectory* traj);
DoubleSeqPtr operator[](size_t dim) const;
inline TrajData& operator=(const TrajData& s)
{
timestamp = s.timestamp;
data.resize(s.data.size());
int i = 0;
- for(auto& elem : s.data)
+ for (auto & elem : s.data)
{
data[i++] = elem;
}
@@ -115,10 +116,13 @@ namespace armarx
friend std::ostream& operator<<(std::ostream& stream, const TrajData& rhs)
{
stream << rhs.timestamp << ": ";
- for (size_t d = 0; d < rhs.data.size(); ++d) {
+ 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)
+ if (d <= rhs.data.size() - 1)
+ {
stream << ", ";
+ }
}
return stream;
}
@@ -151,7 +155,16 @@ namespace armarx
Trajectory() {}
Trajectory(const Trajectory& source);
- Trajectory(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, const std::string& dimensionName = "");
+ template <typename T>
+ Trajectory(const std::vector<T>& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const std::string& dimensionName = "", typename boost::enable_if<boost::is_arithmetic< T > >::type* t = 0)
+ {
+ if (data.size() == 0)
+ {
+ return;
+ }
+ Ice::DoubleSeq dvec(data.begin(), data.end());
+ addDimension(dvec, timestamps, dimensionName);
+ }
/**
* Constructor to add n-dimensions with m-values.
@@ -161,25 +174,18 @@ namespace armarx
*
*/
template <typename T>
- Trajectory(const std::vector<std::vector<T>>& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const Ice::StringSeq& dimensionNames = "")
+ Trajectory(const std::vector<std::vector<T>>& data, const Ice::DoubleSeq timestamps = Ice::DoubleSeq(), const Ice::StringSeq& dimensionNames = {})
{
if (data.size() == 0)
{
return;
}
- Ice::DoubleSeq generatedTimestamps;
- ARMARX_CHECK_EXPRESSION(timestamps.size() == 0 || data.at(0).size() == timestamps.size());
- ARMARX_CHECK_EXPRESSION(data.size() == dimensionNames.size());
- if (timestamps.size() == 0)
- {
- generatedTimestamps = GenerateTimestamps(0, 1, 1.0 / (data.at(0).size() - 1));
- }
- const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : generatedTimestamps;
+ const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : GenerateTimestamps(data.at(0));
size_t i = 0;
for (const auto & subvec : data)
{
Ice::DoubleSeq dvec(subvec.begin(), subvec.end());
- addDimension(tempTimestamps, dvec, dimensionNames.at(i++));
+ addDimension(dvec, tempTimestamps, dimensionNames.size() > i - 1 ? dimensionNames.at(i++) : "");
}
}
@@ -189,8 +195,8 @@ namespace armarx
Trajectory& operator=(const Trajectory& source);
// Basic Manipulators
- size_t addDimension(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, const std::string name = "");
- void addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& x, const Ice::DoubleSeq& y);
+ size_t addDimension(const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps = Ice::DoubleSeq(), const std::string name = "");
+ void addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps);
void addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs);
Trajectory& operator+=(const Trajectory traj);
void removeDimension(size_t dimension);
@@ -317,6 +323,12 @@ namespace armarx
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);
+ template <typename T>
+ static Ice::DoubleSeq GenerateTimestamps(const std::vector<T>& values)
+ {
+ return GenerateTimestamps(0, 1, 1.0 / (values.size() - 1));
+ }
+
void shiftTime(double shift);
void shiftValue(const Ice::DoubleSeq& shift);
@@ -347,6 +359,7 @@ namespace armarx
Ice::ObjectPtr ice_clone() const;
+
protected:
void __addDimension();
void __fillAllEmptyFields();
diff --git a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
index 64f35db4e7d0d3609127ddde3437b8abc860ab56..834cec402239ba3743b902246c6c5108759c9afd 100644
--- a/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
+++ b/source/RobotAPI/libraries/core/test/TrajectoryTest.cpp
@@ -29,134 +29,45 @@
using namespace armarx;
-//BOOST_AUTO_TEST_CASE(testTrajectoryLoading)
-//{
-// // 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 = 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));
-// std::cout << "end" << std::endl;
-//}
-
-//#define SIGN_FACTOR(x) ((x>=0)?1:-1)
-
-//BOOST_AUTO_TEST_CASE(testExtrapolation)
-//{
-// // 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 = 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);
-// BOOST_CHECK(before < SIGN_FACTOR(derivFirst) * first);
-// std::cout << "end" << std::endl;
-//}
-
-//BOOST_AUTO_TEST_CASE(testInplaceInterpolation)
-//{
-// // 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();
-// 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);
-// BOOST_CHECK(before < interpolation && after > interpolation);
-// std::cout << "end" << std::endl;
-//}
-
-//BOOST_AUTO_TEST_CASE(testDerivation)
-//{
-// // 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);
-// 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));
-// std::cout << "end" << std::endl;
-//}
-
-
-
-//BOOST_AUTO_TEST_CASE(testInplaceDerivation)
-//{
-// // 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);
-// BOOST_CHECK(d2 != 0);
-// std::cout << "end" << std::endl;
-//}
-
BOOST_AUTO_TEST_CASE(TrajectoryBasicUsage)
{
//! [TrajectoryDocumentation BasicUsage]
- Ice::DoubleSeq myPositionValues{0,5,1}; // fill with your values;
- Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(0, 1, 1.0/(myPositionValues.size()-1)); // if you dont have timestamps
- TrajectoryPtr trajectory(new Trajectory);
- trajectory->addDimension(timestamps, myPositionValues, "Shoulder 1 L");
-
- double timestamp = 0.1;
- double dimension = 0;
- double derivation = 0;
- double pos = trajectory->getState(timestamp, dimension, derivation);
- // or
- Ice::DoubleSeq positions = trajectory->getDimensionData(dimension, derivation);
- //! [TrajectoryDocumentation BasicUsage]
- ARMARX_INFO_S << VAROUT(pos) << VAROUT(positions);
+ Ice::DoubleSeq myPositionValues {0, 5, 1}; // fill with your values;
+ Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(0, 1, 1.0 / (myPositionValues.size() - 1)); // if you dont have timestamps
+ TrajectoryPtr trajectory(new Trajectory);
+ trajectory->addDimension(myPositionValues, timestamps, "Shoulder 1 L");
+
+ double timestamp = 0.1;
+ double dimension = 0;
+ double derivation = 0;
+ double pos = trajectory->getState(timestamp, dimension, derivation);
+ // or
+ Ice::DoubleSeq positions = trajectory->getDimensionData(dimension, derivation);
+ //! [TrajectoryDocumentation BasicUsage]
+ ARMARX_INFO_S << VAROUT(pos) << VAROUT(positions);
}
BOOST_AUTO_TEST_CASE(TrajectoryIteratorUsage)
{
- Ice::DoubleSeq myPositionValues{0,5,1}; // fill with your values;
- Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(0, 1, 1.0/(myPositionValues.size()-1)); // if you dont have timestamps
- TrajectoryPtr trajectory(new Trajectory);
- trajectory->addDimension(timestamps, myPositionValues, "Shoulder 1 L");
- //! [TrajectoryDocumentation IteratorUsage]
- Trajectory::ordered_view::iterator it; // or just auto it;
- it = trajectory->begin();
- for(; it != trajectory->end(); it++)
- {
- const Trajectory::TrajData& data = *it;
- ARMARX_INFO_S << "Position at " << data.getTimestamp() << " :" << data.getPosition(0);
- }
-
- // or with c++11 for loop:
- for(const Trajectory::TrajData& element : *trajectory)
- {
- ARMARX_INFO_S << "Position at " << element.getTimestamp() << " :" << element.getPosition(0);
- }
- //! [TrajectoryDocumentation IteratorUsage]
+ Ice::DoubleSeq myPositionValues {0, 5, 1}; // fill with your values;
+ Ice::DoubleSeq timestamps = Trajectory::GenerateTimestamps(0, 1, 1.0 / (myPositionValues.size() - 1)); // if you dont have timestamps
+ TrajectoryPtr trajectory(new Trajectory);
+ trajectory->addDimension(myPositionValues, timestamps, "Shoulder 1 L");
+ //! [TrajectoryDocumentation IteratorUsage]
+ Trajectory::ordered_view::iterator it; // or just auto it;
+ it = trajectory->begin();
+ for (; it != trajectory->end(); it++)
+ {
+ const Trajectory::TrajData& data = *it;
+ ARMARX_INFO_S << "Position at " << data.getTimestamp() << " :" << data.getPosition(0);
+ }
+
+ // or with c++11 for loop:
+ for (const Trajectory::TrajData & element : *trajectory)
+ {
+ ARMARX_INFO_S << "Position at " << element.getTimestamp() << " :" << element.getPosition(0);
+ }
+ //! [TrajectoryDocumentation IteratorUsage]
}
BOOST_AUTO_TEST_CASE(testInplaceDerivation2)
@@ -165,8 +76,8 @@ BOOST_AUTO_TEST_CASE(testInplaceDerivation2)
Ice::DoubleSeq timestamps { 0, 1, 2 };
Ice::DoubleSeq positions { 1, 2, 5 };
- TrajectoryPtr traj = new Trajectory(timestamps, positions);
- TrajectoryPtr traj2 = new Trajectory(timestamps, positions);
+ TrajectoryPtr traj = new Trajectory(positions, timestamps);
+ TrajectoryPtr traj2 = new Trajectory(positions, timestamps);
traj->differentiateDiscretly(1);
BOOST_CHECK_EQUAL(traj->getState(-1, 0, 1), traj2->getState(-1, 0, 1));
}
@@ -177,8 +88,8 @@ BOOST_AUTO_TEST_CASE(testInplaceDerivation3)
Ice::DoubleSeq timestamps { 0, 1, 2 };
Ice::DoubleSeq positions { 1, 2, 5 };
- TrajectoryPtr traj = new Trajectory(timestamps, positions);
- TrajectoryPtr traj2 = new Trajectory(timestamps, positions);
+ TrajectoryPtr traj = new Trajectory(positions, timestamps);
+ TrajectoryPtr traj2 = new Trajectory(positions, timestamps);
traj->differentiateDiscretly(2);
BOOST_CHECK_EQUAL(traj->getState(-1, 0, 2), traj2->getState(-1, 0, 2));
}
@@ -189,7 +100,7 @@ BOOST_AUTO_TEST_CASE(testDerivationRemoval)
Ice::DoubleSeq timestamps { 0, 1, 2 };
Ice::DoubleSeq positions { 1, 2, 5 };
- TrajectoryPtr traj = new Trajectory(timestamps, positions);
+ TrajectoryPtr traj = new Trajectory(positions, timestamps);
// TrajectoryPtr traj2 = new Trajectory(timestamps, positions);
traj->differentiateDiscretly(2);
BOOST_CHECK_EQUAL(traj->begin()->getData().at(0)->size(), 3);
@@ -205,7 +116,7 @@ BOOST_AUTO_TEST_CASE(TrajectoryMarshalTest)
Ice::DoubleSeq timestamps { 0, 1, 2 };
Ice::DoubleSeq positions { 1, 2, 5 };
- TrajectoryPtr traj = new Trajectory(timestamps, positions);
+ TrajectoryPtr traj = new Trajectory(positions, timestamps);
traj->ice_preMarshal();
TrajectoryPtr traj2 = TrajectoryPtr::dynamicCast(traj->clone());
traj2->clear();
@@ -231,7 +142,7 @@ BOOST_AUTO_TEST_CASE(TrajectoryPerformanceTest)
{
p.push_back(rand());
}
- traj->addDimension(timestamps, p);
+ traj->addDimension(p, timestamps);
}
ARMARX_INFO_S << "Starting";
traj->ice_preMarshal();
@@ -270,7 +181,7 @@ BOOST_AUTO_TEST_CASE(TrajectoryLengthTest)
FloatSeqSeq positions {{ 0, 0, 1 }, { 0, 1, 1 }};
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->getLength(), (Eigen::Vector3f(positions[0].data()) - Eigen::Vector3f(positions[1].data())).norm());
}
BOOST_AUTO_TEST_CASE(TrajectoryCopyIfTest)
@@ -281,11 +192,11 @@ BOOST_AUTO_TEST_CASE(TrajectoryCopyIfTest)
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)
+ [](const Trajectory::TrajData & elem)
{
- return fabs(elem.getDeriv(0,1)) > 0;
+ return fabs(elem.getDeriv(0, 1)) > 0;
});
- selection.resize(std::distance(selection.begin(),it));
+ selection.resize(std::distance(selection.begin(), it));
//! [TrajectoryDocumentation CopyIf]
ARMARX_INFO_S << VAROUT(*traj);
ARMARX_INFO_S << VAROUT(selection);