trajectory_interface.h
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2013, PAL Robotics S.L.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of PAL Robotics S.L. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00029 
00030 #ifndef TRAJECTORY_INTERFACE_TRAJECTORY_INTERFACE_H
00031 #define TRAJECTORY_INTERFACE_TRAJECTORY_INTERFACE_H
00032 
00033 #include <algorithm>
00034 #include <iterator>
00035 #include <vector>
00036 
00037 // Things to generalize:
00038 // - State specification: What values (pos, vel, acc, ...), and their dimension (4 for quats, 1 for scalars)
00039 // - State might be different for derivatives: Quat == 4, ang vel == 3: Solve doing w=[w0, w1, w2, 0]
00040 // - Sample state vs. sample pos and derivatives separately
00041 // - Manage and find active segments of heretogeneous types
00042 
00043 // NOTES:
00044 // - Segments in original implementation are an open interval on the start time. We are making them closed
00045 // - If an action goal superceeds another and the new trajectory starts in the future, until that time no constraint
00046 //   enforcement will take place. Odd.
00047 
00048 namespace trajectory_interface
00049 {
00050 
00051 namespace internal
00052 {
00053 
00054 // NOTE: Implement as a lambda expression when C++11 becomes mainstream
00055 template<class Time, class Segment>
00056 inline bool isBeforeSegment(const Time& time, const Segment& segment)
00057 {
00058   return time < segment.startTime();
00059 }
00060 
00061 } // namespace
00062 
00079 template<class TrajectoryIterator, class Time>
00080 inline TrajectoryIterator findSegment(TrajectoryIterator first, TrajectoryIterator last, const Time& time)
00081 {
00082   typedef typename std::iterator_traits<TrajectoryIterator>::value_type Segment;
00083   return (first == last || internal::isBeforeSegment(time, *first))
00084          ? last // Optimization when time preceeds all segments, or when an empty range is passed
00085          : --std::upper_bound(first, last, time, internal::isBeforeSegment<Time, Segment>); // Notice decrement operator
00086 }
00087 
00098 template<class Trajectory, class Time>
00099 inline typename Trajectory::const_iterator findSegment(const Trajectory& trajectory, const Time& time)
00100 {
00101   return findSegment(trajectory.begin(), trajectory.end(), time);
00102 }
00103 
00111 template<class Trajectory, class Time>
00112 inline typename Trajectory::iterator findSegment(Trajectory& trajectory, const Time& time)
00113 {
00114   return findSegment(trajectory.begin(), trajectory.end(), time);
00115 }
00116 
00137 template<class Trajectory>
00138 inline typename Trajectory::const_iterator sample(const Trajectory&                             trajectory,
00139                                                   const typename Trajectory::value_type::Time&  time,
00140                                                         typename Trajectory::value_type::State& state)
00141 {
00142   typename Trajectory::const_iterator it = findSegment(trajectory, time);
00143   if (it != trajectory.end())
00144   {
00145     it->sample(time, state); // Segment found at specified time
00146   }
00147   else if (!trajectory.empty())
00148   {
00149     trajectory.front().sample(time, state); // Specified time preceeds trajectory start time
00150   }
00151   return it;
00152 }
00153 
00154 } // namespace
00155 
00156 #endif // header guard


joint_trajectory_controller
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Sat Aug 13 2016 04:20:51