Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00048 class RobotTrajectory
00049 {
00050 public:
00051 RobotTrajectory(const robot_model::RobotModelConstPtr &kmodel, const std::string &group);
00052
00053 const robot_model::RobotModelConstPtr& getRobotModel() const
00054 {
00055 return kmodel_;
00056 }
00057
00058 const robot_model::JointModelGroup* getGroup() const
00059 {
00060 return group_;
00061 }
00062
00063 const std::string& getGroupName() const;
00064
00065 void setGroupName(const std::string &group_name);
00066
00067 std::size_t getWayPointCount() const
00068 {
00069 return waypoints_.size();
00070 }
00071
00072 const robot_state::RobotState& getWayPoint(std::size_t index) const
00073 {
00074 return *waypoints_[index];
00075 }
00076
00077 const robot_state::RobotState& getLastWayPoint() const
00078 {
00079 return *waypoints_.back();
00080 }
00081
00082 const robot_state::RobotState& getFirstWayPoint() const
00083 {
00084 return *waypoints_.front();
00085 }
00086
00087 robot_state::RobotStatePtr& getWayPointPtr(std::size_t index)
00088 {
00089 return waypoints_[index];
00090 }
00091
00092 robot_state::RobotStatePtr& getLastWayPointPtr()
00093 {
00094 return waypoints_.back();
00095 }
00096
00097 robot_state::RobotStatePtr& getFirstWayPointPtr()
00098 {
00099 return waypoints_.front();
00100 }
00101
00102 const std::deque<double>& getWayPointDurations() const
00103 {
00104 return duration_from_previous_;
00105 }
00106
00111 double getWaypointDurationFromStart(std::size_t index) const;
00112
00113 double getWayPointDurationFromPrevious(std::size_t index) const
00114 {
00115 if (duration_from_previous_.size() > index)
00116 return duration_from_previous_[index];
00117 else
00118 return 0.0;
00119 }
00120
00121 void setWayPointDurationFromPrevious(std::size_t index, double value)
00122 {
00123 if (duration_from_previous_.size() <= index)
00124 duration_from_previous_.resize(index + 1, 0.0);
00125 duration_from_previous_[index] = value;
00126 }
00127
00128 bool empty() const
00129 {
00130 return waypoints_.empty();
00131 }
00132
00133 void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
00134 {
00135 addSuffixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
00136 }
00137
00138 void addSuffixWayPoint(const robot_state::RobotStatePtr &state, double dt)
00139 {
00140 waypoints_.push_back(state);
00141 duration_from_previous_.push_back(dt);
00142 }
00143
00144 void addPrefixWayPoint(const robot_state::RobotState &state, double dt)
00145 {
00146 addPrefixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
00147 }
00148
00149 void addPrefixWayPoint(const robot_state::RobotStatePtr &state, double dt)
00150 {
00151 waypoints_.push_front(state);
00152 duration_from_previous_.push_front(dt);
00153 }
00154
00155 void insertWayPoint(std::size_t index, const robot_state::RobotState &state, double dt)
00156 {
00157 insertWayPoint(index, robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
00158 }
00159
00160 void insertWayPoint(std::size_t index, const robot_state::RobotStatePtr &state, double dt)
00161 {
00162 waypoints_.insert(waypoints_.begin() + index, state);
00163 duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
00164 }
00165
00166 void append(const RobotTrajectory &source, double dt);
00167
00168 void swap(robot_trajectory::RobotTrajectory &other);
00169
00170 void clear();
00171
00172 double getAverageSegmentDuration() const;
00173
00174 void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory) const;
00175
00176 void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state,
00177 const moveit_msgs::RobotTrajectory &trajectory);
00178 void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state,
00179 const moveit_msgs::RobotState &state, const moveit_msgs::RobotTrajectory &trajectory);
00180
00181
00182 void reverse();
00183
00184 void unwind();
00185 void unwind(const robot_state::RobotState &state);
00186
00193 void findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after, double &blend) const;
00194
00195
00201 bool getStateAtDurationFromStart(const double request_duration,
00202 robot_state::RobotStatePtr& output_state) const;
00203
00204 private:
00205
00206 robot_model::RobotModelConstPtr kmodel_;
00207 const robot_model::JointModelGroup *group_;
00208 std::deque<robot_state::RobotStatePtr> waypoints_;
00209 std::deque<double> duration_from_previous_;
00210 };
00211
00212 typedef boost::shared_ptr<RobotTrajectory> RobotTrajectoryPtr;
00213 typedef boost::shared_ptr<const RobotTrajectory> RobotTrajectoryConstPtr;
00214
00215 }
00216
00217 #endif