robot_trajectory.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Adam Leeper */
00036 
00037 #ifndef MOVEIT_ROBOT_TRAJECTORY_KINEMATIC_TRAJECTORY_
00038 #define MOVEIT_ROBOT_TRAJECTORY_KINEMATIC_TRAJECTORY_
00039 
00040 #include <moveit/robot_state/robot_state.h>
00041 #include <moveit_msgs/RobotTrajectory.h>
00042 #include <moveit_msgs/RobotState.h>
00043 #include <deque>
00044 
00045 namespace robot_trajectory
00046 {
00047 
00050 class RobotTrajectory
00051 {
00052 public:
00053   RobotTrajectory(const robot_model::RobotModelConstPtr &kmodel, const std::string &group);
00054 
00055   const robot_model::RobotModelConstPtr& getRobotModel() const
00056   {
00057     return robot_model_;
00058   }
00059 
00060   const robot_model::JointModelGroup* getGroup() const
00061   {
00062     return group_;
00063   }
00064 
00065   const std::string& getGroupName() const;
00066 
00067   void setGroupName(const std::string &group_name);
00068 
00069   std::size_t getWayPointCount() const
00070   {
00071     return waypoints_.size();
00072   }
00073 
00074   const robot_state::RobotState& getWayPoint(std::size_t index) const
00075   {
00076     return *waypoints_[index];
00077   }
00078 
00079   const robot_state::RobotState& getLastWayPoint() const
00080   {
00081     return *waypoints_.back();
00082   }
00083 
00084   const robot_state::RobotState& getFirstWayPoint() const
00085   {
00086     return *waypoints_.front();
00087   }
00088 
00089   robot_state::RobotStatePtr& getWayPointPtr(std::size_t index)
00090   {
00091     return waypoints_[index];
00092   }
00093 
00094   robot_state::RobotStatePtr& getLastWayPointPtr()
00095   {
00096     return waypoints_.back();
00097   }
00098 
00099   robot_state::RobotStatePtr& getFirstWayPointPtr()
00100   {
00101     return waypoints_.front();
00102   }
00103 
00104   const std::deque<double>& getWayPointDurations() const
00105   {
00106     return duration_from_previous_;
00107   }
00108 
00113   double getWaypointDurationFromStart(std::size_t index) const;
00114 
00115   double getWayPointDurationFromPrevious(std::size_t index) const
00116   {
00117     if (duration_from_previous_.size() > index)
00118       return duration_from_previous_[index];
00119     else
00120       return 0.0;
00121   }
00122 
00123   void setWayPointDurationFromPrevious(std::size_t index, double value)
00124   {
00125     if (duration_from_previous_.size() <= index)
00126       duration_from_previous_.resize(index + 1, 0.0);
00127     duration_from_previous_[index] = value;
00128   }
00129 
00130   bool empty() const
00131   {
00132     return waypoints_.empty();
00133   }
00134 
00140   void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
00141   {
00142     addSuffixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
00143   }
00144 
00150   void addSuffixWayPoint(const robot_state::RobotStatePtr &state, double dt)
00151   {
00152     state->update();
00153     waypoints_.push_back(state);
00154     duration_from_previous_.push_back(dt);
00155   }
00156 
00157   void addPrefixWayPoint(const robot_state::RobotState &state, double dt)
00158   {
00159     addPrefixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
00160   }
00161 
00162   void addPrefixWayPoint(const robot_state::RobotStatePtr &state, double dt)
00163   {
00164     state->update();
00165     waypoints_.push_front(state);
00166     duration_from_previous_.push_front(dt);
00167   }
00168 
00169   void insertWayPoint(std::size_t index, const robot_state::RobotState &state, double dt)
00170   {
00171     insertWayPoint(index, robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
00172   }
00173 
00174   void insertWayPoint(std::size_t index, const robot_state::RobotStatePtr &state, double dt)
00175   {
00176     state->update();
00177     waypoints_.insert(waypoints_.begin() + index, state);
00178     duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
00179   }
00180 
00181   void append(const RobotTrajectory &source, double dt);
00182 
00183   void swap(robot_trajectory::RobotTrajectory &other);
00184 
00185   void clear();
00186 
00187   double getAverageSegmentDuration() const;
00188 
00189   void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory) const;
00190 
00194   void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state,
00195                              const trajectory_msgs::JointTrajectory &trajectory);
00196   
00200   void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state,
00201                              const moveit_msgs::RobotTrajectory &trajectory);
00202   
00207   void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state,
00208                              const moveit_msgs::RobotState &state, const moveit_msgs::RobotTrajectory &trajectory);
00209 
00210   void reverse();
00211 
00212   void unwind();
00213   void unwind(const robot_state::RobotState &state);
00214 
00221   void findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after, double &blend) const;
00222 
00223   // TODO support visitor function for interpolation, or at least different types.
00229   bool getStateAtDurationFromStart(const double request_duration,
00230                                    robot_state::RobotStatePtr& output_state) const;
00231 
00232 private:
00233 
00234   robot_model::RobotModelConstPtr robot_model_;
00235   const robot_model::JointModelGroup *group_;
00236   std::deque<robot_state::RobotStatePtr> waypoints_;
00237   std::deque<double> duration_from_previous_;
00238 };
00239 
00240 typedef boost::shared_ptr<RobotTrajectory> RobotTrajectoryPtr;
00241 typedef boost::shared_ptr<const RobotTrajectory> RobotTrajectoryConstPtr;
00242 
00243 }
00244 
00245 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53