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 
42 #include <moveit_msgs/RobotTrajectory.h>
43 #include <moveit_msgs/RobotState.h>
44 #include <deque>
45 
47 {
49 
53 {
54 public:
55  RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const std::string& group);
56 
57  RobotTrajectory(const robot_model::RobotModelConstPtr& robot_model, const robot_model::JointModelGroup* group);
58 
59  const robot_model::RobotModelConstPtr& getRobotModel() const
60  {
61  return robot_model_;
62  }
63 
65  {
66  return group_;
67  }
68 
69  const std::string& getGroupName() const;
70 
71  void setGroupName(const std::string& group_name);
72 
73  std::size_t getWayPointCount() const
74  {
75  return waypoints_.size();
76  }
77 
78  const robot_state::RobotState& getWayPoint(std::size_t index) const
79  {
80  return *waypoints_[index];
81  }
82 
84  {
85  return *waypoints_.back();
86  }
87 
89  {
90  return *waypoints_.front();
91  }
92 
93  robot_state::RobotStatePtr& getWayPointPtr(std::size_t index)
94  {
95  return waypoints_[index];
96  }
97 
98  robot_state::RobotStatePtr& getLastWayPointPtr()
99  {
100  return waypoints_.back();
101  }
102 
103  robot_state::RobotStatePtr& getFirstWayPointPtr()
104  {
105  return waypoints_.front();
106  }
107 
108  const std::deque<double>& getWayPointDurations() const
109  {
111  }
112 
117  double getWayPointDurationFromStart(std::size_t index) const;
118 
119  [[deprecated]] double getWaypointDurationFromStart(std::size_t index) const;
120 
121  double getWayPointDurationFromPrevious(std::size_t index) const
122  {
123  if (duration_from_previous_.size() > index)
124  return duration_from_previous_[index];
125  else
126  return 0.0;
127  }
128 
129  void setWayPointDurationFromPrevious(std::size_t index, double value)
130  {
131  if (duration_from_previous_.size() <= index)
132  duration_from_previous_.resize(index + 1, 0.0);
133  duration_from_previous_[index] = value;
134  }
135 
136  bool empty() const
137  {
138  return waypoints_.empty();
139  }
140 
146  void addSuffixWayPoint(const robot_state::RobotState& state, double dt)
147  {
148  addSuffixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
149  }
150 
156  void addSuffixWayPoint(const robot_state::RobotStatePtr& state, double dt)
157  {
158  state->update();
159  waypoints_.push_back(state);
160  duration_from_previous_.push_back(dt);
161  }
162 
163  void addPrefixWayPoint(const robot_state::RobotState& state, double dt)
164  {
165  addPrefixWayPoint(robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
166  }
167 
168  void addPrefixWayPoint(const robot_state::RobotStatePtr& state, double dt)
169  {
170  state->update();
171  waypoints_.push_front(state);
172  duration_from_previous_.push_front(dt);
173  }
174 
175  void insertWayPoint(std::size_t index, const robot_state::RobotState& state, double dt)
176  {
177  insertWayPoint(index, robot_state::RobotStatePtr(new robot_state::RobotState(state)), dt);
178  }
179 
180  void insertWayPoint(std::size_t index, const robot_state::RobotStatePtr& state, double dt)
181  {
182  state->update();
183  waypoints_.insert(waypoints_.begin() + index, state);
184  duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
185  }
186 
197  void append(const RobotTrajectory& source, double dt, size_t start_index = 0,
198  size_t end_index = std::numeric_limits<std::size_t>::max());
199 
201 
202  void clear();
203 
204  double getDuration() const;
205 
206  double getAverageSegmentDuration() const;
207 
208  void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory) const;
209 
216  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
217  const trajectory_msgs::JointTrajectory& trajectory);
218 
225  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
226  const moveit_msgs::RobotTrajectory& trajectory);
227 
235  void setRobotTrajectoryMsg(const robot_state::RobotState& reference_state, const moveit_msgs::RobotState& state,
236  const moveit_msgs::RobotTrajectory& trajectory);
237 
238  void reverse();
239 
240  void unwind();
241  void unwind(const robot_state::RobotState& state);
242 
249  void findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after, double& blend) const;
250 
251  // TODO support visitor function for interpolation, or at least different types.
258  bool getStateAtDurationFromStart(const double request_duration, robot_state::RobotStatePtr& output_state) const;
259 
260 private:
261  robot_model::RobotModelConstPtr robot_model_;
263  std::deque<robot_state::RobotStatePtr> waypoints_;
264  std::deque<double> duration_from_previous_;
265 };
266 }
267 
268 #endif
Core components of MoveIt!
void swap(robot_trajectory::RobotTrajectory &other)
robot_state::RobotStatePtr & getWayPointPtr(std::size_t index)
std::deque< robot_state::RobotStatePtr > waypoints_
void setGroupName(const std::string &group_name)
const robot_model::JointModelGroup * group_
robot_model::RobotModelConstPtr robot_model_
RobotTrajectory(const robot_model::RobotModelConstPtr &robot_model, const std::string &group)
robot_state::RobotStatePtr & getFirstWayPointPtr()
std::size_t getWayPointCount() const
const std::string & getGroupName() const
robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm")
unsigned int index
void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory) const
const robot_model::JointModelGroup * getGroup() 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)
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.
void setWayPointDurationFromPrevious(std::size_t index, double value)
const robot_model::RobotModelConstPtr & getRobotModel() const
const robot_state::RobotState & getLastWayPoint() const
robot_state::RobotStatePtr & getLastWayPointPtr()
void addSuffixWayPoint(const robot_state::RobotStatePtr &state, double dt)
Add a point to the trajectory.
void findWayPointIndicesForDurationAfterStart(const double &duration, int &before, int &after, double &blend) const
Finds the waypoint indicies before and after a duration from start.
const std::deque< double > & getWayPointDurations() const
double getWaypointDurationFromStart(std::size_t index) const
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
void addPrefixWayPoint(const robot_state::RobotStatePtr &state, double dt)
double getWayPointDurationFromPrevious(std::size_t index) const
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
Add a point to the trajectory.
const robot_state::RobotState & getWayPoint(std::size_t index) const
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...
const robot_state::RobotState & getFirstWayPoint() const
std::deque< double > duration_from_previous_
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
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 Mon Dec 2 2019 03:56:59