robot_trajectory.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, Adam Leeper */
36 
37 #ifndef MOVEIT_ROBOT_TRAJECTORY_KINEMATIC_TRAJECTORY_
38 #define MOVEIT_ROBOT_TRAJECTORY_KINEMATIC_TRAJECTORY_
39 
43 #include <moveit_msgs/RobotTrajectory.h>
44 #include <moveit_msgs/RobotState.h>
45 #include <deque>
46 
48 {
50 
54 {
55 public:
56  RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const std::string& group);
57 
58  RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const robot_model::JointModelGroup* group);
59 
60  const robot_model::RobotModelConstPtr& getRobotModel() const
61  {
62  return robot_model_;
63  }
64 
66  {
67  return group_;
68  }
69 
70  const std::string& getGroupName() const;
71 
72  void setGroupName(const std::string& group_name);
73 
74  std::size_t getWayPointCount() const
75  {
76  return waypoints_.size();
77  }
78 
79  const robot_state::RobotState& getWayPoint(std::size_t index) const
80  {
81  return *waypoints_[index];
82  }
83 
85  {
86  return *waypoints_.back();
87  }
88 
90  {
91  return *waypoints_.front();
92  }
93 
94  robot_state::RobotStatePtr& getWayPointPtr(std::size_t index)
95  {
96  return waypoints_[index];
97  }
98 
99  robot_state::RobotStatePtr& getLastWayPointPtr()
100  {
101  return waypoints_.back();
102  }
103 
104  robot_state::RobotStatePtr& getFirstWayPointPtr()
105  {
106  return waypoints_.front();
107  }
108 
109  const std::deque<double>& getWayPointDurations() const
110  {
112  }
113 
118  double getWayPointDurationFromStart(std::size_t index) const;
119 
120  MOVEIT_DEPRECATED double getWaypointDurationFromStart(std::size_t index) const;
121 
122  double getWayPointDurationFromPrevious(std::size_t index) const
123  {
124  if (duration_from_previous_.size() > index)
125  return duration_from_previous_[index];
126  else
127  return 0.0;
128  }
129 
130  void setWayPointDurationFromPrevious(std::size_t index, double value)
131  {
132  if (duration_from_previous_.size() <= index)
133  duration_from_previous_.resize(index + 1, 0.0);
134  duration_from_previous_[index] = value;
135  }
136 
137  bool empty() const
138  {
139  return waypoints_.empty();
140  }
141 
147  void addSuffixWayPoint(const robot_state::RobotState& state, double dt)
148  {
149  addSuffixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
150  }
151 
157  void addSuffixWayPoint(const robot_state::RobotStatePtr& state, double dt)
158  {
159  state->update();
160  waypoints_.push_back(state);
161  duration_from_previous_.push_back(dt);
162  }
163 
164  void addPrefixWayPoint(const robot_state::RobotState& state, double dt)
165  {
166  addPrefixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
167  }
168 
169  void addPrefixWayPoint(const robot_state::RobotStatePtr& state, double dt)
170  {
171  state->update();
172  waypoints_.push_front(state);
173  duration_from_previous_.push_front(dt);
174  }
175 
176  void insertWayPoint(std::size_t index, const robot_state::RobotState& state, double dt)
177  {
178  insertWayPoint(index, robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
179  }
180 
181  void insertWayPoint(std::size_t index, const robot_state::RobotStatePtr& state, double dt)
182  {
183  state->update();
184  waypoints_.insert(waypoints_.begin() + index, state);
185  duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
186  }
187 
193  void append(const RobotTrajectory& source, double dt);
194 
196 
197  void clear();
198 
199  double getAverageSegmentDuration() const;
200 
201  void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const;
202 
209  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
210  const trajectory_msgs::JointTrajectory& trajectory);
211 
218  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
219  const moveit_msgs::RobotTrajectory& trajectory);
220 
228  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, const moveit_msgs::RobotState& state,
229  const moveit_msgs::RobotTrajectory& trajectory);
230 
231  void reverse();
232 
233  void unwind();
234  void unwind(const robot_state::RobotState& state);
235 
242  void findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after, double& blend) const;
243 
244  // TODO support visitor function for interpolation, or at least different types.
251  bool getStateAtDurationFromStart(const double request_duration, robot_state::RobotStatePtr& output_state) const;
252 
253 private:
254  robot_model::RobotModelConstPtr robot_model_;
256  std::deque<robot_state::RobotStatePtr> waypoints_;
257  std::deque<double> duration_from_previous_;
258 };
259 }
260 
261 #endif
void append(const RobotTrajectory &source, double dt)
Add a trajectory to the end of the current trajectory.
double getWayPointDurationFromPrevious(std::size_t index) const
Core components of MoveIt!
void swap(robot_trajectory::RobotTrajectory &other)
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
std::deque< robot_state::RobotStatePtr > waypoints_
const robot_state::RobotState & getLastWayPoint() const
const std::string & getGroupName() const
void setGroupName(const std::string &group_name)
const robot_model::JointModelGroup * group_
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
#define MOVEIT_DEPRECATED
Definition: deprecation.h:48
robot_model::RobotModelConstPtr robot_model_
RobotTrajectory(const robot_model::RobotModelConstPtr &robot_model, const std::string &group)
robot_state::RobotStatePtr & getFirstWayPointPtr()
void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory) const
unsigned int index
const robot_model::JointModelGroup * getGroup() const
bool getStateAtDurationFromStart(const double request_duration, robot_state::RobotStatePtr &output_state) const
Gets a robot state corresponding to a supplied duration from start for the trajectory, using linear time interpolation.
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
const robot_state::RobotState & getFirstWayPoint() const
Maintain a sequence of waypoints and the time durations between these waypoints.
void addPrefixWayPoint(const robot_state::RobotState &state, double dt)
void insertWayPoint(std::size_t index, const robot_state::RobotState &state, double dt)
void setWayPointDurationFromPrevious(std::size_t index, double value)
const robot_state::RobotState & getWayPoint(std::size_t index) const
MOVEIT_DEPRECATED double getWaypointDurationFromStart(std::size_t index) const
std::size_t getWayPointCount() const
robot_state::RobotStatePtr & getLastWayPointPtr()
void addSuffixWayPoint(const robot_state::RobotStatePtr &state, double dt)
Add a point to the trajectory.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
const std::deque< double > & getWayPointDurations() const
const robot_model::RobotModelConstPtr & getRobotModel() const
void addPrefixWayPoint(const robot_state::RobotStatePtr &state, double dt)
void findWayPointIndicesForDurationAfterStart(const double &duration, int &before, int &after, double &blend) const
Finds the waypoint indicies before and after a duration from start.
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
Add a point to the trajectory.
void insertWayPoint(std::size_t index, const robot_state::RobotStatePtr &state, double dt)
void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state, const trajectory_msgs::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
std::deque< double > duration_from_previous_
MOVEIT_CLASS_FORWARD(RobotTrajectory)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Oct 18 2018 02:47:09