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 
198  void append(const RobotTrajectory& source, double dt, size_t start_index = 0,
199  size_t end_index = std::numeric_limits<std::size_t>::max());
200 
202 
203  void clear();
204 
205  double getAverageSegmentDuration() const;
206 
207  void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const;
208 
215  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
216  const trajectory_msgs::JointTrajectory& trajectory);
217 
224  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
225  const moveit_msgs::RobotTrajectory& trajectory);
226 
234  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, const moveit_msgs::RobotState& state,
235  const moveit_msgs::RobotTrajectory& trajectory);
236 
237  void reverse();
238 
239  void unwind();
240  void unwind(const robot_state::RobotState& state);
241 
248  void findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after, double& blend) const;
249 
250  // TODO support visitor function for interpolation, or at least different types.
257  bool getStateAtDurationFromStart(const double request_duration, robot_state::RobotStatePtr& output_state) const;
258 
259 private:
260  robot_model::RobotModelConstPtr robot_model_;
262  std::deque<robot_state::RobotStatePtr> waypoints_;
263  std::deque<double> duration_from_previous_;
264 };
265 }
266 
267 #endif
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_
void append(const RobotTrajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
Add a specified part of a trajectory to the end of the current trajectory. The default (when start_in...
MOVEIT_CLASS_FORWARD(RobotTrajectory)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33