diff --git a/CMakeLists.txt b/CMakeLists.txt
index 12a6ea10e1a72d4710ebc588177703815bc812ae..c7bd1763dfe4608861a977d61c0dfb04a4dc3670 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,19 +17,8 @@ set(ARMARX_ENABLE_AUTO_CODE_FORMATTING TRUE)
armarx_project("RobotAPI")
depends_on_armarx_package(ArmarXGui)
-set(ArmarX_Simox_VERSION 2.3.72)
-option(REQUIRE_SIMOX "If enabled the Simox dependency is a required dependency" TRUE)
-
-if(REQUIRE_SIMOX)
- find_package(Simox ${ArmarX_Simox_VERSION} REQUIRED)
-else()
- find_package(Simox ${ArmarX_Simox_VERSION} QUIET)
-endif()
-
add_subdirectory(source)
-list(APPEND CPACK_DEBIAN_PACKAGE_DEPENDS "simox (= ${ArmarX_Simox_VERSION})")
-
install_project()
add_subdirectory(scenarios)
diff --git a/etc/cmake/UseRobotAPI.cmake b/etc/cmake/UseRobotAPI.cmake
index de1c501c0780d4b604ccb1d2637d7fa4a222de51..ddbb6ceada4a458087550ecb9ecff5032286c101 100644
--- a/etc/cmake/UseRobotAPI.cmake
+++ b/etc/cmake/UseRobotAPI.cmake
@@ -1,3 +1 @@
# This file contains macros for projects depending on RobotAPI
-find_package(Simox QUIET)
-find_package(Eigen3 QUIET)
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
index 5656bc799e424199554b7302d21bf98b12e5f82e..b2e14c046bce4e8a9ad572e3875aefe958fa0101 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.h
@@ -24,7 +24,7 @@
#pragma once
#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/math/OrientedBox.h>
+#include <SimoxUtility/eigen/OrientedBox.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -87,7 +87,7 @@ namespace armarx::detail::DebugDrawerHelper
void drawBox(const std::string& name, const Eigen::Vector3f& position, float size, const DrawColor& color);
void drawBox(const std::string& name, const Eigen::Matrix4f& pose, float size, const DrawColor& color);
template<class FloatT>
- void drawBox(const std::string& name, const VirtualRobot::OrientedBox<FloatT>& box, const DrawColor& color)
+ void drawBox(const std::string& name, const simox::OrientedBox<FloatT>& box, const DrawColor& color)
{
drawBox(name, box.template transformation_centered<float>(), box.template dimensions<float>(), color);
}
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 1c82523cb9ef65c454605423f4fbc44e56c92bf3..4499ff645801a7041b1c280b432dfa6526011d46 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,15 +1,15 @@
-add_subdirectory(core)
add_subdirectory(ArmarXEtherCAT)
-add_subdirectory(KITGripperEtherCAT)
-add_subdirectory(widgets)
-add_subdirectory(SimpleJsonLogger)
-add_subdirectory(RobotAPIVariantWidget)
-add_subdirectory(RobotAPINJointControllers)
-add_subdirectory(DMPController)
+add_subdirectory(ControllerUIUtility)
+add_subdirectory(core)
+add_subdirectory(DebugDrawerConfigWidget)
# disabled until Mathlib include is fixed...
+add_subdirectory(DMPController)
add_subdirectory(DSControllers)
+add_subdirectory(KITGripperEtherCAT)
+add_subdirectory(RobotAPIComponentPlugins)
+add_subdirectory(RobotAPINJointControllers)
+add_subdirectory(RobotAPIVariantWidget)
add_subdirectory(RobotStatechartHelpers)
-add_subdirectory(ControllerUIUtility)
-add_subdirectory(DebugDrawerConfigWidget)
-
-add_subdirectory(RobotAPIComponentPlugins)
\ No newline at end of file
+add_subdirectory(SimpleJsonLogger)
+add_subdirectory(SimpleTrajectory)
+add_subdirectory(widgets)
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt b/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5e24563b59a6d4d470916e244a451a6878d07436
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/CMakeLists.txt
@@ -0,0 +1,31 @@
+set(LIB_NAME RobotAPISimpleTrajectory)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+set(LIBS
+ ArmarXCoreInterfaces ArmarXCore
+)
+
+set(LIB_FILES
+ exceptions.cpp
+ Trajectory.cpp
+ Track.cpp
+
+ interpolate/linear.cpp
+)
+set(LIB_HEADERS
+ exceptions.h
+ Track.h
+ Trajectory.h
+ VariantValue.h
+
+ interpolate/linear.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+# add unit tests
+add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp b/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6f6bd2970e4f0233510753e1ea6c5354e5fbff0a
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Track.cpp
@@ -0,0 +1,16 @@
+#include "Track.h"
+
+
+namespace armarx::trajectory
+{
+
+ template <>
+ void Track<VariantValue>::checkValueType(const VariantValue& value)
+ {
+ if (!empty() && value.type() != keyframes.front().value.type())
+ {
+ throw error::WrongValueTypeInKeyframe(id, value.type(), keyframes.front().value.type());
+ }
+ }
+
+}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Track.h b/source/RobotAPI/libraries/SimpleTrajectory/Track.h
new file mode 100644
index 0000000000000000000000000000000000000000..6658b8df8856589c99fbe3cdfe3cfb2bc0f292ce
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Track.h
@@ -0,0 +1,284 @@
+#pragma once
+
+#include "VariantValue.h"
+#include "interpolate/linear.h"
+
+
+namespace armarx::trajectory
+{
+
+ /**
+ * @brief A keyframe, representing a value at a given time.
+ */
+ template <typename ValueT>
+ struct Keyframe
+ {
+ /// Constructor.
+ Keyframe(float time, const ValueT& value) : time(time), value(value)
+ {}
+
+ float time; ///< The time on the timeline.
+ ValueT value; ///< The value.
+ };
+
+ /// A keyframe with of type TValue.
+ using VariantKeyframe = Keyframe<VariantValue>;
+
+
+ /**
+ * @brief A track represents the timeline of a single value, identified by a track ID.
+ * A track is comprised of a sequence of keyframes and used to call a
+ * single update function.
+ */
+ template <typename ValueT>
+ class Track
+ {
+ public:
+ /// The update function type.
+ using UpdateFunc = std::function<void(ValueT)>;
+ // Forward declaration.
+ struct KeyframeProxy;
+
+ public:
+
+ /// Construct a track with given ID (and no update function).
+ Track(const TrackID& id) :
+ id(id)
+ {}
+
+ /// Construct a track with given ID and update function.
+ Track(const TrackID& id, UpdateFunc updateFunc) :
+ id(id), updateFunc(updateFunc)
+ {}
+
+
+ /// Indicate whether this track does not contain any keyframes.
+ bool empty() const;
+ /// Clear the track of all keyframes.
+ void clear();
+
+ /// Add a keyframe to this track.
+ void addKeyframe(const Keyframe<ValueT>& keyframe);
+ /// Add a keyframe to this track.
+ void addKeyframe(float time, const ValueT& value);
+
+ /// Add a keyframe by assignment: `track[time] = value;`
+ KeyframeProxy operator[](float time);
+
+
+ /**
+ * @brief Get the interpolated value at the given time.
+ * @throws `error::EmptyTrack` If the track is empty.
+ */
+ ValueT at(float time) const;
+
+ /**
+ * @brief Call the update function with the interpolated value at the given time.
+ *
+ * @param ignoreIfEmpty If true and the track is empty, the method does nothing.
+ * @throws `error::EmptyTrack` If the track is empty and `ignoreIfEmpty` is false.
+ */
+ void update(float time, bool ignoreIfEmpty = false);
+
+ template <typename V>
+ friend std::ostream& operator<<(std::ostream& os, const Track<V>& track);
+
+
+ private:
+
+ /// Sort the keyframes.
+ void sortKeyframes();
+
+ /**
+ * @throw `error::WrongValueTypeInKeyframe` If the given value's type
+ * conflicts with contained keyframes.
+ */
+ void checkValueType(const ValueT& value);
+
+
+ /// The track ID.
+ TrackID id;
+ /// The update function.
+ UpdateFunc updateFunc;
+ /// The sorted array of keyframes.
+ std::vector<Keyframe<ValueT>> keyframes;
+
+
+ public:
+
+ /// A proxy allowing for adding keyframes by: `track[time] = value;`
+ struct KeyframeProxy
+ {
+ public:
+ /// Get the value at time.
+ operator ValueT() const;
+ /// Add a keyframe on assignment.
+ void operator= (const ValueT& value);
+ /// Add a keyframe on assignment.
+ void operator= (const KeyframeProxy& value);
+
+ /// Get the value at time.
+ ValueT value() const;
+
+ private:
+ friend Track; // Allow TrackBase to use constructor.
+
+ KeyframeProxy(Track& track, float time);
+
+ Track& track;
+ float time;
+ };
+
+ };
+
+ /// A track with value type TValue.
+ using VariantTrack = Track<VariantValue>;
+
+ template <typename V>
+ bool Track<V>::empty() const
+ {
+ return keyframes.empty();
+ }
+
+ template<typename V>
+ void Track<V>::clear()
+ {
+ keyframes.clear();
+ }
+
+ template <typename V>
+ void Track<V>::addKeyframe(const Keyframe<V>& keyframe)
+ {
+ checkValueType(keyframe.value);
+ keyframes.push_back(keyframe);
+ sortKeyframes();
+ }
+
+ template <typename V>
+ void Track<V>::addKeyframe(float time, const V& value)
+ {
+ addKeyframe(Keyframe<V>(time, value));
+ }
+
+ template<typename V>
+ auto Track<V>::operator[](float time) -> KeyframeProxy
+ {
+ return KeyframeProxy{*this, time};
+ }
+
+ template <typename V>
+ void Track<V>::sortKeyframes()
+ {
+ std::sort(keyframes.begin(), keyframes.end(),
+ [](const Keyframe<V>& lhs, const Keyframe<V>& rhs)
+ {
+ return lhs.time < rhs.time;
+ });
+ }
+
+ /// In general, do nothing.
+ template <typename V>
+ void Track<V>::checkValueType(const V&)
+ {}
+
+ /**
+ * If the track is not empty, check whether the given type of the given
+ * value is the same as the type of the values already in the track.
+ *
+ * @throws `error::WrongValueTypeInKeyframe` if the types do not match.
+ */
+ template <>
+ void Track<VariantValue>::checkValueType(const VariantValue& value);
+
+
+ template <typename V>
+ V Track<V>::at(float time) const
+ {
+ if (empty())
+ {
+ throw error::EmptyTrack(id);
+ }
+
+ if (keyframes.size() == 1)
+ {
+ return keyframes.front().value;
+ }
+
+ if (time <= keyframes.front().time)
+ {
+ return keyframes.front().value;
+ }
+ if (time >= keyframes.back().time)
+ {
+ return keyframes.back().value;
+ }
+
+
+ std::size_t i = 0;
+ while (i + 1 <= keyframes.size() && keyframes[i + 1].time < time)
+ {
+ i++;
+ }
+
+ // interpolate between i and i+1
+ const Keyframe<V>& kf1 = keyframes.at(i);
+ const Keyframe<V>& kf2 = keyframes.at(i + 1);
+
+ float t = (time - kf1.time) / (kf2.time - kf1.time);
+ // t = 0 => full kf1, t = 1 => full kf2
+
+ return interpolate::linear<V>(t, kf1.value, kf2.value);
+ }
+
+
+ template <typename V>
+ void Track<V>::update(float time, bool ignoreIfEmpty)
+ {
+ if (updateFunc && !(ignoreIfEmpty && empty()))
+ {
+ updateFunc(at(time));
+ }
+ }
+
+
+ template<typename V>
+ Track<V>::KeyframeProxy::KeyframeProxy(Track& track, float time) :
+ track(track), time(time)
+ {}
+
+ template <typename V>
+ Track<V>::KeyframeProxy::operator V() const
+ {
+ return track.at(time);
+ }
+
+ template <typename V>
+ void Track<V>::KeyframeProxy::operator=(const V& value)
+ {
+ track.addKeyframe(time, value);
+ }
+
+ template <typename V>
+ void Track<V>::KeyframeProxy::operator=(const KeyframeProxy& other)
+ {
+ track.addKeyframe(time, other.value());
+ }
+
+ template <typename V>
+ auto Track<V>::KeyframeProxy::value() const -> V
+ {
+ return track.at(time);
+ }
+
+ template <typename ValueT>
+ std::ostream& operator<<(std::ostream& os, const Track<ValueT>& track)
+ {
+ os << "Track '" << track.id << "' with " << track.keyframes.size() << " keyframes: [";
+ for (const Keyframe<ValueT>& kf : track.keyframes)
+ {
+ os << kf.time << ", ";
+ }
+ return os << "]";
+ }
+
+}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5211a504403ce4dff2feca568998ac4cd88ca555
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.cpp
@@ -0,0 +1,105 @@
+#include "Trajectory.h"
+
+
+using namespace armarx::trajectory;
+
+
+Trajectory::Trajectory()
+{}
+
+void Trajectory::clear()
+{
+ tracks.clear();
+}
+
+VariantTrack& Trajectory::addTrack(const TrackID& id)
+{
+ return tracks.emplace(id, VariantTrack {id}).first->second;
+}
+
+VariantTrack& Trajectory::addTrack(const TrackID& id, const VariantTrack::UpdateFunc& updateFunc)
+{
+ return tracks.emplace(id, VariantTrack {id, updateFunc}).first->second;
+}
+
+void Trajectory::addKeyframe(const TrackID& id, const VariantKeyframe& keyframe)
+{
+ (*this)[id].addKeyframe(keyframe);
+}
+
+void Trajectory::addKeyframe(const TrackID& id, float time, const VariantValue& value)
+{
+ addKeyframe(id, VariantKeyframe {time, value});
+}
+
+void Trajectory::update(float time, bool ignoreEmptyTracks)
+{
+ for (auto& it : tracks)
+ {
+ it.second.update(time, ignoreEmptyTracks);
+ }
+}
+
+VariantTrack& Trajectory::operator[](const TrackID& id)
+{
+ try
+ {
+ return tracks.at(id);
+ }
+ catch (const std::out_of_range&)
+ {
+ throw error::NoTrackWithID(id);
+ }
+}
+
+const VariantTrack& Trajectory::operator[](const TrackID& id) const
+{
+ try
+ {
+ return tracks.at(id);
+ }
+ catch (const std::out_of_range&)
+ {
+ throw error::NoTrackWithID(id);
+ }
+}
+
+std::ostream& armarx::trajectory::operator<<(std::ostream& os, const Trajectory& trajectory)
+{
+ os << "Trajectory with " << trajectory.tracks.size() << " tracks: ";
+ for (const auto& [name, track] : trajectory.tracks)
+ {
+ os << "\n - " << track;
+ }
+ return os;
+}
+
+
+namespace armarx
+{
+
+ auto trajectory::toUpdateFunc(std::function<void (float)> func) -> VariantTrack::UpdateFunc
+ {
+ return [func](VariantValue value)
+ {
+ func(boost::get<float>(value));
+ };
+ }
+
+ auto trajectory::toUpdateFunc(std::function<void (const Eigen::MatrixXf&)> func) -> VariantTrack::UpdateFunc
+ {
+ return [func](VariantValue value)
+ {
+ func(boost::get<Eigen::MatrixXf>(value));
+ };
+ }
+
+ auto trajectory::toUpdateFunc(std::function<void (const Eigen::Quaternionf&)> func) -> VariantTrack::UpdateFunc
+ {
+ return [func](VariantValue value)
+ {
+ func(boost::get<Eigen::Quaternionf>(value));
+ };
+ }
+
+}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h
new file mode 100644
index 0000000000000000000000000000000000000000..5600a669fa6bf6afce56ff08d3f2a947912fd139
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/Trajectory.h
@@ -0,0 +1,105 @@
+#pragma once
+
+#include <map>
+
+#include "Track.h"
+
+
+namespace armarx::trajectory
+{
+
+ /**
+ * @brief This class is used to update entities based on trajectory
+ * defined by keyframes.
+ *
+ * A Trajectory is a collection of tracks, where each track represents a
+ * timeline of a single value. A track is composed of keyframes, where
+ * each keyframe represents a value at a specific time on the timeline.
+ *
+ * @see Track
+ */
+ class Trajectory
+ {
+ public:
+
+ /// Constructor.
+ Trajectory();
+
+
+ /// Clear the trajectory of all tracks.
+ void clear();
+
+
+ /**
+ * @brief Add track with the given ID (and no update function).
+ * If there is already a track with the given ID, it is overwritten.
+ */
+ VariantTrack& addTrack(const TrackID& id);
+
+ /**
+ * @brief Add track with the given ID and update function.
+ * If there is already a track with the given ID, it is overwritten.
+ */
+ VariantTrack& addTrack(const TrackID& id, const VariantTrack::UpdateFunc& updateFunc);
+
+
+ /// Add a keyframe to the specified track.
+ /// @throw `error::NoTrackWithID` if there is not track with the given ID
+ void addKeyframe(const TrackID& id, const VariantKeyframe& keyframe);
+
+ /// Add a keyframe to the specified track.
+ /// @throw `error::NoTrackWithID` if there is not track with the given ID
+ void addKeyframe(const TrackID& id, float time, const VariantValue& value);
+
+
+ /**
+ * @brief Update all tracks for the given time.
+ * @param ignoreEmptyTracks If true, empty tracks are ignored.
+ * @throws `error::EmptyTrack` If `ignoreEmptyTracks` is false and a track is empty.
+ */
+ void update(float time, bool ignoreEmptyTracks = false);
+
+
+ /// Get the track with the given ID.
+ /// @throw `error::NoTrackWithID` if there is not track with the given ID
+ VariantTrack& operator[](const TrackID& id);
+
+ /// Get the track with the given ID.
+ /// @throw `error::NoTrackWithID` if there is not track with the given ID
+ const VariantTrack& operator[](const TrackID& id) const;
+
+ friend std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory);
+
+
+ private:
+
+ /// The tracks.
+ std::map<TrackID, VariantTrack> tracks;
+
+ };
+
+
+ std::ostream& operator<<(std::ostream& os, const Trajectory& trajectory);
+
+
+ // UPDATE FUNCTION HELPERS
+
+ /// Get an update function for value assignments.
+ template <typename ValueT>
+ VariantTrack::UpdateFunc updateValue(ValueT& v)
+ {
+ return [&v](VariantValue value)
+ {
+ v = boost::get<ValueT>(value);
+ };
+ }
+
+ /// Wrap the function in a Track::UpdateFunc.
+ VariantTrack::UpdateFunc toUpdateFunc(std::function<void(float)> func);
+ /// Wrap the function in a Track::UpdateFunc.
+ VariantTrack::UpdateFunc toUpdateFunc(std::function<void(const Eigen::MatrixXf&)> func);
+ /// Wrap the function in a Track::UpdateFunc.
+ VariantTrack::UpdateFunc toUpdateFunc(std::function<void(const Eigen::Quaternionf&)> func);
+
+}
+
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h b/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7e6db669445a95033f304da8d4eb0da0fb9890b
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/VariantValue.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include <boost/variant.hpp>
+
+#include <Eigen/Eigen>
+
+
+namespace armarx::trajectory
+{
+
+ /// Variant for trajectory values.
+ using VariantValue = boost::variant<float, Eigen::MatrixXf, Eigen::Quaternionf>;
+
+ /// ID of tracks.
+ using TrackID = std::string;
+
+}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5e3db572f972e0b55bfa399b774d328c04603473
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.cpp
@@ -0,0 +1,51 @@
+#include "exceptions.h"
+
+using namespace armarx::trajectory::error;
+
+
+TrajectoryException::TrajectoryException(const std::string& msg) : std::logic_error(msg)
+{}
+
+
+InterpolateDifferentTypesError::InterpolateDifferentTypesError() :
+ TrajectoryException("Interpolating between two different types.")
+{}
+
+
+NoTrackWithID::NoTrackWithID(const TrackID& id) : TrajectoryException(makeMsg(id))
+{}
+
+std::string NoTrackWithID::makeMsg(const TrackID& id)
+{
+ std::stringstream ss;
+ ss << "No track with ID '" << id << "'. \n"
+ << "Add a track with ID '" << id << "' before before adding keyframes.";
+ return ss.str();
+}
+
+
+EmptyTrack::EmptyTrack(const TrackID& id) : TrajectoryException(makeMsg(id))
+{}
+
+std::string EmptyTrack::makeMsg(const TrackID& id)
+{
+ std::stringstream ss;
+ ss << "Track with ID '" << id << "' is empty. \n"
+ "Add a keyframe to track '" << id << "' before updating.";
+ return ss.str();
+}
+
+WrongValueTypeInKeyframe::WrongValueTypeInKeyframe(
+ const TrackID& trackID, const std::type_info& type, const std::type_info& expected) :
+ TrajectoryException(makeMsg(trackID, type, expected))
+{}
+
+std::string WrongValueTypeInKeyframe::makeMsg(
+ const TrackID& id, const std::type_info& type, const std::type_info& expected)
+{
+ std::stringstream ss;
+ ss << "Tried to add keyframe with value type '" << type.name() << "' to non-empty track '"
+ << id << "' containing values of type '" << expected.name() << "'. \n"
+ << "Only one value type per track is allowed.";
+ return ss.str();
+}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h
new file mode 100644
index 0000000000000000000000000000000000000000..6235ef3f737fe97359f4da72c25ec6c870a49da2
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/exceptions.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <sstream>
+#include <stdexcept>
+
+#include "VariantValue.h" // for TrackID
+
+
+namespace armarx::trajectory::error
+{
+
+
+ struct TrajectoryException : public std::logic_error
+ {
+ TrajectoryException(const std::string& msg);
+ };
+
+
+ struct InterpolateDifferentTypesError : public TrajectoryException
+ {
+ InterpolateDifferentTypesError();
+ };
+
+
+ struct NoTrackWithID : public TrajectoryException
+ {
+ NoTrackWithID(const TrackID& id);
+ static std::string makeMsg(const TrackID& id);
+ };
+
+
+ struct EmptyTrack : public TrajectoryException
+ {
+ EmptyTrack(const TrackID& id);
+ static std::string makeMsg(const TrackID& id);
+ };
+
+
+ struct WrongValueTypeInKeyframe : public TrajectoryException
+ {
+ WrongValueTypeInKeyframe(const TrackID& trackID, const std::type_info& type,
+ const std::type_info& expected);
+ static std::string makeMsg(const TrackID& trackID, const std::type_info& type,
+ const std::type_info& expected);
+ };
+
+
+
+}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4b41c638d9ae8f3c5100d9f60f1d47743bcb99d
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.cpp
@@ -0,0 +1,14 @@
+#include "linear.h"
+
+
+namespace armarx::trajectory
+{
+
+ template <>
+ VariantValue interpolate::linear<VariantValue>(float t, const VariantValue& lhs, const VariantValue& rhs)
+ {
+ // dont use boost::get
+ return boost::apply_visitor(Linear {t}, lhs, rhs);
+ }
+
+}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h
new file mode 100644
index 0000000000000000000000000000000000000000..4cf381844ec819550b1a3bc857b28ee713705a5c
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/interpolate/linear.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include "../exceptions.h"
+#include "../VariantValue.h"
+
+
+namespace armarx::trajectory::interpolate
+{
+
+ /**
+ * @brief Linear interpolation visitor: Interpolates between the given values linearly.
+ */
+ class Linear : public boost::static_visitor<>
+ {
+ public:
+
+ using result_type = VariantValue; ///< Exposed result type.
+
+ public:
+
+ /**
+ * @brief Interpolator
+ * @param t in [0, 1], where `t = 0` for `lhs` and `t = 1` for `rhs`.
+ */
+ Linear(float t) : t(t) {}
+
+ template <typename U, typename V>
+ float operator()(const U&, const V&) const
+ {
+ throw error::InterpolateDifferentTypesError();
+ }
+
+ float operator()(const float& lhs, const float& rhs) const
+ {
+ return (1 - t) * lhs + t * rhs;
+ }
+
+ Eigen::MatrixXf operator()(const Eigen::MatrixXf& lhs, const Eigen::MatrixXf& rhs) const
+ {
+ return (1 - t) * lhs + t * rhs;
+ }
+
+ Eigen::Quaternionf operator()(const Eigen::Quaternionf& lhs, const Eigen::Quaternionf& rhs) const
+ {
+ return lhs.slerp(t, rhs);
+ }
+
+
+ private:
+
+ float t;
+
+ };
+
+ template <typename ReturnT>
+ ReturnT linear(float t, const VariantValue& lhs, const VariantValue& rhs)
+ {
+ return boost::get<ReturnT>(boost::apply_visitor(Linear {t}, lhs, rhs));
+ }
+
+ template <>
+ VariantValue linear<VariantValue>(float t, const VariantValue& lhs, const VariantValue& rhs);
+
+}
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/test/CMakeLists.txt b/source/RobotAPI/libraries/SimpleTrajectory/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6d0e31ccf0b662d39fde412ddcd4f070302e7813
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore ${LIB_NAME})
+
+armarx_add_test(${LIB_NAME}TrajectoryTest TrajectoryTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp b/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6f0553c521f68f8a583736d7b12ac59632cffb46
--- /dev/null
+++ b/source/RobotAPI/libraries/SimpleTrajectory/test/TrajectoryTest.cpp
@@ -0,0 +1,266 @@
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXLibraries::RobotAPISimpleTrajectory
+
+#define ARMARX_BOOST_TEST
+
+#include <iostream>
+
+#include <RobotAPI/Test.h>
+#include "../Trajectory.h"
+
+
+using namespace armarx::trajectory;
+
+namespace std
+{
+ std::ostream& operator<<(std::ostream& os, const std::type_info& rhs)
+ {
+ os << "TypeInfo (" << rhs.name() << ", " << rhs.hash_code() << ")";
+ return os;
+ }
+}
+
+
+struct Fixture
+{
+ Trajectory trajectory;
+
+ float float_;
+ Eigen::Vector3f vec;
+ Eigen::MatrixXf mat;
+ Eigen::Quaternionf quat;
+
+ float ftol = 1e-9f;
+};
+
+
+BOOST_FIXTURE_TEST_SUITE(TrajectoryTest, Fixture)
+
+
+BOOST_AUTO_TEST_CASE(testNoTrackWithID)
+{
+ for (auto* id :
+ { "id1", "id2"
+ })
+ {
+ BOOST_CHECK_THROW(trajectory.addKeyframe(id, 0, 0.f), error::NoTrackWithID);
+ BOOST_CHECK_THROW(trajectory[id].addKeyframe(0, 0.f), error::NoTrackWithID);
+ BOOST_CHECK_THROW(trajectory[id][0] = 0.f, error::NoTrackWithID);
+ }
+
+ trajectory.addTrack("id1", updateValue(float_));
+
+ BOOST_CHECK_NO_THROW(trajectory.addKeyframe("id1", 0, 0.f));
+ BOOST_CHECK_NO_THROW(trajectory["id1"].addKeyframe(1, 0.f));
+ BOOST_CHECK_NO_THROW(trajectory["id1"][2] = 0.f);
+
+ BOOST_CHECK_THROW(trajectory.addKeyframe("id2", 0, 0.f), error::NoTrackWithID);
+ BOOST_CHECK_THROW(trajectory["id2"].addKeyframe(0, 0.f), error::NoTrackWithID);
+ BOOST_CHECK_THROW(trajectory["id2"][0] = 0.f, error::NoTrackWithID);
+}
+
+
+BOOST_AUTO_TEST_CASE(testEmptyTrack)
+{
+ trajectory.addTrack("float", updateValue(float_));
+
+ BOOST_CHECK(trajectory["float"].empty());
+
+ BOOST_CHECK_THROW(trajectory["float"].update(0), error::EmptyTrack);
+ BOOST_CHECK_THROW(trajectory.update(0), error::EmptyTrack);
+
+ trajectory.addKeyframe("float", VariantKeyframe {0.f, 1.f});
+
+ BOOST_CHECK_NO_THROW(trajectory["float"].update(0));
+ BOOST_CHECK_NO_THROW(trajectory.update(0));
+ BOOST_CHECK_NO_THROW(trajectory.update(-1));
+ BOOST_CHECK_NO_THROW(trajectory.update(1));
+}
+
+template <typename Derived>
+std::ostream& operator<<(std::ostream& os, const Eigen::MatrixBase<Derived>& rhs)
+{
+ static const Eigen::IOFormat iof(5, 0, " ", "\n", "| ", " |", "");
+ os << rhs.format(iof);
+ return os;
+}
+
+template <typename Derived>
+std::ostream& operator<<(std::ostream& os, const Eigen::QuaternionBase<Derived>& rhs)
+{
+ static const Eigen::IOFormat iof(5, 0, " ", "\n", "| ", " |", "");
+ os << rhs.format(iof);
+ return os;
+}
+
+
+
+
+BOOST_AUTO_TEST_CASE(testDifferentTypes)
+{
+ trajectory.addTrack("float", updateValue(float_));
+
+ std::vector<VariantValue> values
+ {
+ 1.f, Eigen::MatrixXf::Zero(3, 3), Eigen::Quaternionf::Identity()
+ };
+
+ for (std::size_t i = 1; i < values.size(); ++i)
+ {
+ for (std::size_t j = 0; j < i; ++j)
+ {
+ BOOST_CHECK_NE(values[i].type(), values[j].type());
+ }
+ }
+
+ BOOST_CHECK_NO_THROW(trajectory.addKeyframe("float", 0, 0.f));
+ BOOST_CHECK_THROW(trajectory.addKeyframe("float", 1, Eigen::MatrixXf::Zero(3, 3)),
+ error::WrongValueTypeInKeyframe);
+ BOOST_CHECK_THROW(trajectory.addKeyframe("float", 1, Eigen::Quaternionf::Identity()),
+ error::WrongValueTypeInKeyframe);
+}
+
+BOOST_AUTO_TEST_CASE(testInterpolateFloat)
+{
+ float f1 = -2;
+ float f2 = 5;
+
+ VariantValue v1(f1);
+ VariantValue v2(f2);
+
+ BOOST_CHECK_EQUAL(interpolate::linear<float>(0, v1, v2), f1);
+ BOOST_CHECK_EQUAL(interpolate::linear<float>(.5, v1, v2), .5f * (f1 + f2));
+ BOOST_CHECK_EQUAL(interpolate::linear<float>(1, v1, v2), f2);
+}
+
+BOOST_AUTO_TEST_CASE(testInterpolateMatrix)
+{
+ Eigen::MatrixXf m1 = Eigen::MatrixXf::Zero(3, 3);
+ Eigen::MatrixXf m2 = Eigen::MatrixXf::Identity(3, 3);
+
+ VariantValue v1(m1);
+ VariantValue v2(m2);
+
+ Eigen::MatrixXf mi;
+
+ mi = interpolate::linear<Eigen::MatrixXf>(0, v1, v2);
+ BOOST_CHECK(mi.isApprox(m1, ftol));
+
+ mi = interpolate::linear<Eigen::MatrixXf>(0.5, v1, v2);
+ BOOST_CHECK(mi.isApprox(0.5f * (m1 + m2), ftol));
+
+ mi = interpolate::linear<Eigen::MatrixXf>(1, v1, v2);
+ BOOST_CHECK(mi.isApprox(m2, ftol));
+}
+
+BOOST_AUTO_TEST_CASE(testInterpolateQuaternion)
+{
+ Eigen::Quaternionf q1(Eigen::AngleAxisf(0.f, Eigen::Vector3f(1, 0, 0)));
+ Eigen::Quaternionf q2(Eigen::AngleAxisf(2.f, Eigen::Vector3f(0, 1, 0)));;
+
+ VariantValue v1(q1);
+ VariantValue v2(q2);
+
+ Eigen::Quaternionf mi;
+
+ mi = interpolate::linear<Eigen::Quaternionf>(0, v1, v2);
+ BOOST_CHECK(mi.isApprox(q1, ftol));
+
+ mi = interpolate::linear<Eigen::Quaternionf>(0.5, v1, v2);
+ BOOST_CHECK(mi.isApprox(q1.slerp(.5, q2), ftol));
+
+ mi = interpolate::linear<Eigen::Quaternionf>(1, v1, v2);
+ BOOST_CHECK(mi.isApprox(q2, ftol));
+}
+
+BOOST_AUTO_TEST_CASE(testInterpolateVector3)
+{
+ using ValueT = Eigen::Vector3f;
+
+ ValueT vec1 = Eigen::Vector3f::Zero();
+ ValueT vec2 = Eigen::Vector3f::Ones();
+
+ VariantValue v1(vec1);
+ VariantValue v2(vec2);
+
+ ValueT mi;
+
+ mi = interpolate::linear<Eigen::MatrixXf>(0, v1, v2);
+ BOOST_CHECK(mi.isApprox(vec1, ftol));
+
+ mi = interpolate::linear<Eigen::MatrixXf>(0.5, v1, v2);
+ BOOST_CHECK(mi.isApprox(0.5 * (vec1 + vec2), ftol));
+
+ mi = interpolate::linear<Eigen::MatrixXf>(1, v1, v2);
+ BOOST_CHECK(mi.isApprox(vec2, ftol));
+}
+
+
+
+BOOST_AUTO_TEST_CASE(testTrajectoryFloat)
+{
+ trajectory.addTrack("float", updateValue(float_));
+
+ trajectory.addKeyframe("float", 0.f, 0.f);
+ trajectory.addKeyframe("float", VariantKeyframe {1.f, 1.f});
+
+ int num = 10;
+
+ for (float i = 0; i <= num; ++i)
+ {
+ float time = -i / num;
+ trajectory.update(time);
+ BOOST_CHECK_CLOSE(float_, 0, ftol);
+ }
+
+ for (float i = 0; i <= num; ++i)
+ {
+ float time = i / num;
+ trajectory.update(time);
+ BOOST_CHECK_CLOSE(float_, time, ftol);
+ }
+
+ for (float i = 0; i <= num; ++i)
+ {
+ float time = 1 + i / num;
+ trajectory.update(time);
+ BOOST_CHECK_CLOSE(float_, 1, ftol);
+ }
+}
+
+BOOST_AUTO_TEST_CASE(testTrajectoryMatrix)
+{
+ trajectory.addTrack("matrix", updateValue(mat));
+
+ trajectory.addKeyframe("matrix", 0.f, Eigen::MatrixXf::Zero(3, 3));
+ trajectory.addKeyframe("matrix", 1.f, Eigen::MatrixXf::Identity(3, 3));
+
+ int num = 10;
+
+ for (float i = 0; i <= num; ++i)
+ {
+ float time = -i / num;
+ trajectory.update(time);
+ BOOST_CHECK_CLOSE(mat(0, 0), 0, ftol);
+ BOOST_CHECK_CLOSE(mat(0, 1), 0, ftol);
+ }
+
+ for (float i = 0; i <= num; ++i)
+ {
+ float time = i / num;
+ trajectory.update(time);
+ BOOST_CHECK_CLOSE(mat(0, 0), time, ftol);
+ BOOST_CHECK_CLOSE(mat(0, 1), 0, ftol);
+ }
+
+ for (float i = 0; i <= num; ++i)
+ {
+ float time = 1 + i / num;
+ trajectory.update(time);
+ BOOST_CHECK_CLOSE(mat(0, 0), 1, ftol);
+ BOOST_CHECK_CLOSE(mat(0, 1), 0, ftol);
+ }
+}
+
+BOOST_AUTO_TEST_SUITE_END()
+