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 #pragma once
38 
41 #include <moveit_msgs/RobotTrajectory.h>
42 #include <moveit_msgs/RobotState.h>
43 #include <deque>
44 
46 {
47 MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr, WeakPtr... etc
48 
51 class RobotTrajectory
52 {
53 public:
54  RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group);
55 
56  RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group);
57 
59  RobotTrajectory& operator=(const RobotTrajectory&) = default;
60 
65  RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false);
66 
67  const moveit::core::RobotModelConstPtr& getRobotModel() const
68  {
69  return robot_model_;
70  }
71 
73  {
74  return group_;
75  }
76 
77  const std::string& getGroupName() const;
78 
79  void setGroupName(const std::string& group_name);
80 
81  std::size_t getWayPointCount() const
82  {
83  return waypoints_.size();
84  }
85 
86  const moveit::core::RobotState& getWayPoint(std::size_t index) const
87  {
88  return *waypoints_[index];
89  }
90 
92  {
93  return *waypoints_.back();
94  }
95 
97  {
98  return *waypoints_.front();
99  }
100 
101  moveit::core::RobotStatePtr& getWayPointPtr(std::size_t index)
102  {
103  return waypoints_[index];
104  }
105 
106  moveit::core::RobotStatePtr& getLastWayPointPtr()
107  {
108  return waypoints_.back();
109  }
110 
111  moveit::core::RobotStatePtr& getFirstWayPointPtr()
112  {
113  return waypoints_.front();
114  }
115 
116  const std::deque<double>& getWayPointDurations() const
117  {
119  }
120 
125  double getWayPointDurationFromStart(std::size_t index) const;
126 
127  [[deprecated]] double getWaypointDurationFromStart(std::size_t index) const;
128 
129  double getWayPointDurationFromPrevious(std::size_t index) const
130  {
131  if (duration_from_previous_.size() > index)
133  else
134  return 0.0;
135  }
136 
137  void setWayPointDurationFromPrevious(std::size_t index, double value)
138  {
139  if (duration_from_previous_.size() <= index)
140  duration_from_previous_.resize(index + 1, 0.0);
142  }
143 
144  bool empty() const
145  {
146  return waypoints_.empty();
147  }
148 
154  void addSuffixWayPoint(const moveit::core::RobotState& state, double dt)
155  {
156  addSuffixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
157  }
158 
164  void addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
165  {
166  state->update();
167  waypoints_.push_back(state);
168  duration_from_previous_.push_back(dt);
169  }
170 
171  void addPrefixWayPoint(const moveit::core::RobotState& state, double dt)
172  {
173  addPrefixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
174  }
175 
176  void addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
177  {
178  state->update();
179  waypoints_.push_front(state);
180  duration_from_previous_.push_front(dt);
181  }
182 
183  void insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt)
184  {
185  insertWayPoint(index, moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
186  }
187 
188  void insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt)
189  {
190  state->update();
191  waypoints_.insert(waypoints_.begin() + index, state);
193  }
194 
205  void append(const RobotTrajectory& source, double dt, size_t start_index = 0,
206  size_t end_index = std::numeric_limits<std::size_t>::max());
207 
209 
210  void clear();
211 
212  double getDuration() const;
213 
214  double getAverageSegmentDuration() const;
215 
216  void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory,
217  const std::vector<std::string>& joint_filter = std::vector<std::string>()) const;
218 
225  void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
226  const trajectory_msgs::JointTrajectory& trajectory);
227 
234  void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
235  const moveit_msgs::RobotTrajectory& trajectory);
236 
244  void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::RobotState& state,
245  const moveit_msgs::RobotTrajectory& trajectory);
246 
247  void reverse();
248 
249  void unwind();
250  void unwind(const moveit::core::RobotState& state);
251 
258  void findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after, double& blend) const;
259 
260  // TODO support visitor function for interpolation, or at least different types.
267  bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const;
268 
269 private:
270  moveit::core::RobotModelConstPtr robot_model_;
272  std::deque<moveit::core::RobotStatePtr> waypoints_;
273  std::deque<double> duration_from_previous_;
274 };
275 } // namespace robot_trajectory
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:81
robot_trajectory::RobotTrajectory::reverse
void reverse()
Definition: robot_trajectory.cpp:150
robot_trajectory::RobotTrajectory::getWaypointDurationFromStart
double getWaypointDurationFromStart(std::size_t index) const
Definition: robot_trajectory.cpp:513
robot_trajectory::RobotTrajectory::getWayPointCount
std::size_t getWayPointCount() const
Definition: robot_trajectory.h:113
robot_trajectory::RobotTrajectory::getGroup
const moveit::core::JointModelGroup * getGroup() const
Definition: robot_trajectory.h:104
robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg
void setRobotTrajectoryMsg(const moveit::core::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...
Definition: robot_trajectory.cpp:387
robot_trajectory::RobotTrajectory::setGroupName
void setGroupName(const std::string &group_name)
Definition: robot_trajectory.cpp:101
duration
std::chrono::system_clock::duration duration
robot_trajectory::RobotTrajectory::getWayPointDurations
const std::deque< double > & getWayPointDurations() const
Definition: robot_trajectory.h:148
robot_trajectory::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(RobotTrajectory)
robot_trajectory::RobotTrajectory::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: robot_trajectory.h:99
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
robot_trajectory::RobotTrajectory::getFirstWayPointPtr
moveit::core::RobotStatePtr & getFirstWayPointPtr()
Definition: robot_trajectory.h:143
robot_trajectory::RobotTrajectory::getStateAtDurationFromStart
bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr &output_state) const
Gets a robot state corresponding to a supplied duration from start for the trajectory,...
Definition: robot_trajectory.cpp:518
robot_trajectory::RobotTrajectory::group_
const moveit::core::JointModelGroup * group_
Definition: robot_trajectory.h:303
class_forward.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
robot_trajectory::RobotTrajectory::addPrefixWayPoint
void addPrefixWayPoint(const moveit::core::RobotState &state, double dt)
Definition: robot_trajectory.h:203
robot_trajectory::RobotTrajectory::getDuration
double getDuration() const
Definition: robot_trajectory.cpp:114
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:83
robot_trajectory::RobotTrajectory::findWayPointIndicesForDurationAfterStart
void findWayPointIndicesForDurationAfterStart(const double &duration, int &before, int &after, double &blend) const
Finds the waypoint indicies before and after a duration from start.
Definition: robot_trajectory.cpp:469
robot_trajectory::RobotTrajectory::getWayPointDurationFromStart
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
Definition: robot_trajectory.cpp:500
robot_trajectory::RobotTrajectory::append
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...
Definition: robot_trajectory.cpp:135
robot_trajectory::RobotTrajectory::setWayPointDurationFromPrevious
void setWayPointDurationFromPrevious(std::size_t index, double value)
Definition: robot_trajectory.h:169
robot_trajectory::RobotTrajectory::operator=
RobotTrajectory & operator=(const RobotTrajectory &)=default
robot_trajectory::RobotTrajectory::waypoints_
std::deque< moveit::core::RobotStatePtr > waypoints_
Definition: robot_trajectory.h:304
robot_trajectory::RobotTrajectory::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: robot_trajectory.h:302
robot_trajectory::RobotTrajectory::duration_from_previous_
std::deque< double > duration_from_previous_
Definition: robot_trajectory.h:305
robot_trajectory
Definition: robot_trajectory.h:45
robot_trajectory::RobotTrajectory::getRobotTrajectoryMsg
void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &joint_filter=std::vector< std::string >()) const
Definition: robot_trajectory.cpp:253
robot_trajectory::RobotTrajectory::clear
void clear()
Definition: robot_trajectory.cpp:247
robot_trajectory::RobotTrajectory::getLastWayPointPtr
moveit::core::RobotStatePtr & getLastWayPointPtr()
Definition: robot_trajectory.h:138
robot_trajectory::RobotTrajectory::empty
bool empty() const
Definition: robot_trajectory.h:176
robot_trajectory::RobotTrajectory::getWayPointDurationFromPrevious
double getWayPointDurationFromPrevious(std::size_t index) const
Definition: robot_trajectory.h:161
robot_trajectory::RobotTrajectory::getLastWayPoint
const moveit::core::RobotState & getLastWayPoint() const
Definition: robot_trajectory.h:123
robot_trajectory::RobotTrajectory::getFirstWayPoint
const moveit::core::RobotState & getFirstWayPoint() const
Definition: robot_trajectory.h:128
robot_trajectory::RobotTrajectory::insertWayPoint
void insertWayPoint(std::size_t index, const moveit::core::RobotState &state, double dt)
Definition: robot_trajectory.h:215
robot_trajectory::RobotTrajectory::getAverageSegmentDuration
double getAverageSegmentDuration() const
Definition: robot_trajectory.cpp:119
robot_trajectory::RobotTrajectory::addSuffixWayPoint
void addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
Definition: robot_trajectory.h:186
index
unsigned int index
robot_trajectory::RobotTrajectory::getWayPointPtr
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
Definition: robot_trajectory.h:133
robot_trajectory::RobotTrajectory::getGroupName
const std::string & getGroupName() const
Definition: robot_trajectory.cpp:106
robot_trajectory::RobotTrajectory::unwind
void unwind()
Definition: robot_trajectory.cpp:166
robot_trajectory::RobotTrajectory::RobotTrajectory
RobotTrajectory(const moveit::core::RobotModelConstPtr &robot_model, const std::string &group)
Definition: robot_trajectory.cpp:77
robot_state.h
robot_trajectory::RobotTrajectory::swap
void swap(robot_trajectory::RobotTrajectory &other)
Definition: robot_trajectory.cpp:127
robot_trajectory::RobotTrajectory::getWayPoint
const moveit::core::RobotState & getWayPoint(std::size_t index) const
Definition: robot_trajectory.h:118


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 12 2020 03:25:45