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 #include <memory>
45 
47 {
48 MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr, WeakPtr... etc
49 
52 class RobotTrajectory
53 {
54 public:
56  explicit RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model);
57 
62  RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group);
63 
70  RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group);
71 
73  RobotTrajectory& operator=(const RobotTrajectory&) = default;
74 
79  RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false);
80 
81  const moveit::core::RobotModelConstPtr& getRobotModel() const
82  {
83  return robot_model_;
84  }
85 
87  {
88  return group_;
89  }
90 
91  const std::string& getGroupName() const;
92 
93  RobotTrajectory& setGroupName(const std::string& group_name)
94  {
95  group_ = robot_model_->getJointModelGroup(group_name);
96  return *this;
97  }
98 
99  std::size_t getWayPointCount() const
100  {
101  return waypoints_.size();
102  }
103 
104  const moveit::core::RobotState& getWayPoint(std::size_t index) const
105  {
106  return *waypoints_[index];
107  }
108 
110  {
111  return *waypoints_.back();
112  }
113 
115  {
116  return *waypoints_.front();
117  }
118 
119  moveit::core::RobotStatePtr& getWayPointPtr(std::size_t index)
120  {
121  return waypoints_[index];
122  }
123 
124  moveit::core::RobotStatePtr& getLastWayPointPtr()
125  {
126  return waypoints_.back();
127  }
128 
129  moveit::core::RobotStatePtr& getFirstWayPointPtr()
130  {
131  return waypoints_.front();
132  }
133 
134  const std::deque<double>& getWayPointDurations() const
135  {
137  }
138 
143  double getWayPointDurationFromStart(std::size_t index) const;
144 
145  [[deprecated]] double getWaypointDurationFromStart(std::size_t index) const;
146 
147  double getWayPointDurationFromPrevious(std::size_t index) const
148  {
149  if (duration_from_previous_.size() > index)
151  else
152  return 0.0;
153  }
154 
155  RobotTrajectory& setWayPointDurationFromPrevious(std::size_t index, double value)
156  {
157  if (duration_from_previous_.size() <= index)
158  duration_from_previous_.resize(index + 1, 0.0);
160  return *this;
161  }
162 
163  bool empty() const
164  {
165  return waypoints_.empty();
166  }
167 
174  {
175  return addSuffixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
176  }
177 
183  RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
184  {
185  state->update();
186  waypoints_.push_back(state);
187  duration_from_previous_.push_back(dt);
188  return *this;
189  }
190 
192  {
193  return addPrefixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
194  }
195 
196  RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
197  {
198  state->update();
199  waypoints_.push_front(state);
200  duration_from_previous_.push_front(dt);
201  return *this;
202  }
203 
204  RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt)
205  {
206  return insertWayPoint(index, std::make_shared<moveit::core::RobotState>(state), dt);
207  }
208 
209  RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt)
210  {
211  state->update();
212  waypoints_.insert(waypoints_.begin() + index, state);
213  duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
214  return *this;
215  }
216 
227  RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0,
228  size_t end_index = std::numeric_limits<std::size_t>::max());
229 
231 
233  {
234  waypoints_.clear();
235  duration_from_previous_.clear();
236  return *this;
237  }
238 
239  double getDuration() const;
240 
242 
243  void getRobotTrajectoryMsg(moveit_msgs::RobotTrajectory& trajectory,
244  const std::vector<std::string>& joint_filter = std::vector<std::string>()) const;
245 
253  const trajectory_msgs::JointTrajectory& trajectory);
254 
262  const moveit_msgs::RobotTrajectory& trajectory);
263 
272  const moveit_msgs::RobotState& state,
273  const moveit_msgs::RobotTrajectory& trajectory);
274 
276 
279 
286  void findWayPointIndicesForDurationAfterStart(const double duration, int& before, int& after, double& blend) const;
287 
288  // TODO support visitor function for interpolation, or at least different types.
295  bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const;
296 
297 private:
298  moveit::core::RobotModelConstPtr robot_model_;
300  std::deque<moveit::core::RobotStatePtr> waypoints_;
301  std::deque<double> duration_from_previous_;
302 };
303 
310 [[nodiscard]] double path_length(RobotTrajectory const& trajectory);
311 } // namespace robot_trajectory
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
robot_trajectory::RobotTrajectory::getWaypointDurationFromStart
double getWaypointDurationFromStart(std::size_t index) const
Definition: robot_trajectory.cpp:518
robot_trajectory::RobotTrajectory::getWayPointCount
std::size_t getWayPointCount() const
Definition: robot_trajectory.h:131
robot_trajectory::RobotTrajectory::getGroup
const moveit::core::JointModelGroup * getGroup() const
Definition: robot_trajectory.h:118
duration
std::chrono::system_clock::duration duration
robot_trajectory::RobotTrajectory::getWayPointDurations
const std::deque< double > & getWayPointDurations() const
Definition: robot_trajectory.h:166
robot_trajectory::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(RobotTrajectory)
robot_trajectory::RobotTrajectory::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Definition: robot_trajectory.h:113
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
robot_trajectory::RobotTrajectory::getFirstWayPointPtr
moveit::core::RobotStatePtr & getFirstWayPointPtr()
Definition: robot_trajectory.h:161
robot_trajectory::RobotTrajectory::setWayPointDurationFromPrevious
RobotTrajectory & setWayPointDurationFromPrevious(std::size_t index, double value)
Definition: robot_trajectory.h:187
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:523
robot_trajectory::RobotTrajectory::group_
const moveit::core::JointModelGroup * group_
Definition: robot_trajectory.h:331
robot_trajectory::RobotTrajectory::reverse
RobotTrajectory & reverse()
Definition: robot_trajectory.cpp:152
robot_trajectory::RobotTrajectory::addSuffixWayPoint
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
Definition: robot_trajectory.h:205
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::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:84
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:505
robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg
RobotTrajectory & 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:389
robot_trajectory::RobotTrajectory::operator=
RobotTrajectory & operator=(const RobotTrajectory &)=default
robot_trajectory::RobotTrajectory::waypoints_
std::deque< moveit::core::RobotStatePtr > waypoints_
Definition: robot_trajectory.h:332
robot_trajectory::path_length
double path_length(RobotTrajectory const &trajectory)
Calculate the path length of a given trajectory based on the accumulated robot state distances....
Definition: robot_trajectory.cpp:539
robot_trajectory::RobotTrajectory::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: robot_trajectory.h:330
robot_trajectory::RobotTrajectory::duration_from_previous_
std::deque< double > duration_from_previous_
Definition: robot_trajectory.h:333
robot_trajectory
Definition: robot_trajectory.h:46
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:255
robot_trajectory::RobotTrajectory::setGroupName
RobotTrajectory & setGroupName(const std::string &group_name)
Definition: robot_trajectory.h:125
robot_trajectory::RobotTrajectory::clear
RobotTrajectory & clear()
Definition: robot_trajectory.h:264
robot_trajectory::RobotTrajectory::getLastWayPointPtr
moveit::core::RobotStatePtr & getLastWayPointPtr()
Definition: robot_trajectory.h:156
robot_trajectory::RobotTrajectory::empty
bool empty() const
Definition: robot_trajectory.h:195
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:474
robot_trajectory::RobotTrajectory::getWayPointDurationFromPrevious
double getWayPointDurationFromPrevious(std::size_t index) const
Definition: robot_trajectory.h:179
robot_trajectory::RobotTrajectory::getLastWayPoint
const moveit::core::RobotState & getLastWayPoint() const
Definition: robot_trajectory.h:141
robot_trajectory::RobotTrajectory::getFirstWayPoint
const moveit::core::RobotState & getFirstWayPoint() const
Definition: robot_trajectory.h:146
robot_trajectory::RobotTrajectory::RobotTrajectory
RobotTrajectory(const moveit::core::RobotModelConstPtr &robot_model)
construct a trajectory for the whole robot
Definition: robot_trajectory.cpp:77
robot_trajectory::RobotTrajectory::getAverageSegmentDuration
double getAverageSegmentDuration() const
Definition: robot_trajectory.cpp:119
index
unsigned int index
robot_trajectory::RobotTrajectory::getWayPointPtr
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
Definition: robot_trajectory.h:151
robot_trajectory::RobotTrajectory::getGroupName
const std::string & getGroupName() const
Definition: robot_trajectory.cpp:106
robot_trajectory::RobotTrajectory::unwind
RobotTrajectory & unwind()
Definition: robot_trajectory.cpp:170
robot_trajectory::RobotTrajectory::addPrefixWayPoint
RobotTrajectory & addPrefixWayPoint(const moveit::core::RobotState &state, double dt)
Definition: robot_trajectory.h:223
robot_trajectory::RobotTrajectory::append
RobotTrajectory & 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_state.h
robot_trajectory::RobotTrajectory::swap
void swap(robot_trajectory::RobotTrajectory &other)
Definition: robot_trajectory.cpp:127
robot_trajectory::RobotTrajectory::insertWayPoint
RobotTrajectory & insertWayPoint(std::size_t index, const moveit::core::RobotState &state, double dt)
Definition: robot_trajectory.h:236
robot_trajectory::RobotTrajectory::getWayPoint
const moveit::core::RobotState & getWayPoint(std::size_t index) const
Definition: robot_trajectory.h:136


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Mar 3 2024 03:23:35