Trajectory.cpp

Go to the documentation of this file.

#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(std::get<float>(value));
        };
    }
 
    auto trajectory::toUpdateFunc(std::function<void (const Eigen::MatrixXf&)> func) -> VariantTrack::UpdateFunc
    {
        return [func](VariantValue value)
        {
            func(std::get<Eigen::MatrixXf>(value));
        };
    }
 
    auto trajectory::toUpdateFunc(std::function<void (const Eigen::Quaternionf&)> func) -> VariantTrack::UpdateFunc
    {
        return [func](VariantValue value)
        {
            func(std::get<Eigen::Quaternionf>(value));
        };
    }
 
}