trajectory_interface.h

Go to the documentation of this file.

// Copyright (C) 2013, PAL Robotics S.L.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//   * Redistributions of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//   * Redistributions in binary form must reproduce the above copyright
//     notice, this list of conditions and the following disclaimer in the
//     documentation and/or other materials provided with the distribution.
//   * Neither the name of PAL Robotics S.L. nor the names of its
//     contributors may be used to endorse or promote products derived from
//     this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.


#ifndef TRAJECTORY_INTERFACE_TRAJECTORY_INTERFACE_H
#define TRAJECTORY_INTERFACE_TRAJECTORY_INTERFACE_H

#include <algorithm>
#include <iterator>
#include <vector>

// Things to generalize:
// - State specification: What values (pos, vel, acc, ...), and their dimension (4 for quats, 1 for scalars)
// - State might be different for derivatives: Quat == 4, ang vel == 3: Solve doing w=[w0, w1, w2, 0]
// - Sample state vs. sample pos and derivatives separately
// - Manage and find active segments of heretogeneous types

// NOTES:
// - Segments in original implementation are an open interval on the start time. We are making them closed
// - If an action goal superceeds another and the new trajectory starts in the future, until that time no constraint
//   enforcement will take place. Odd.

namespace trajectory_interface
{

namespace internal
{

// NOTE: Implement as a lambda expression when C++11 becomes mainstream
template<class Time, class Segment>
inline bool isBeforeSegment(const Time& time, const Segment& segment)
{
  return time < segment.startTime();
}

} // namespace

template<class TrajectoryIterator, class Time>
inline TrajectoryIterator findSegment(TrajectoryIterator first, TrajectoryIterator last, const Time& time)
{
  typedef typename std::iterator_traits<TrajectoryIterator>::value_type Segment;
  return (first == last || internal::isBeforeSegment(time, *first))
         ? last // Optimization when time preceeds all segments, or when an empty range is passed
         : --std::upper_bound(first, last, time, internal::isBeforeSegment<Time, Segment>); // Notice decrement operator
}

template<class Trajectory, class Time>
inline typename Trajectory::const_iterator findSegment(const Trajectory& trajectory, const Time& time)
{
  return findSegment(trajectory.begin(), trajectory.end(), time);
}

template<class Trajectory, class Time>
inline typename Trajectory::iterator findSegment(Trajectory& trajectory, const Time& time)
{
  return findSegment(trajectory.begin(), trajectory.end(), time);
}

template<class Trajectory>
inline typename Trajectory::const_iterator sample(const Trajectory&                             trajectory,
                                                  const typename Trajectory::value_type::Time&  time,
                                                        typename Trajectory::value_type::State& state)
{
  typename Trajectory::const_iterator it = findSegment(trajectory, time);
  if (it != trajectory.end())
  {
    it->sample(time, state); // Segment found at specified time
  }
  else if (!trajectory.empty())
  {
    trajectory.front().sample(time, state); // Specified time preceeds trajectory start time
  }
  return it;
}

} // namespace

#endif // header guard