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
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
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