moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
robot_trajectory.hpp
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/msg/robot_trajectory.hpp>
42#include <moveit_msgs/msg/robot_state.hpp>
43#include <deque>
44#include <memory>
45#include <optional>
46
47#include <rcl/error_handling.h>
48#include <rcl/time.h>
49#include <rclcpp/rclcpp.hpp>
50#include <rclcpp/time.hpp>
51#include <rclcpp/utilities.hpp>
52
54{
55MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr, WeakPtr... etc
56
60{
61public:
63 explicit RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model);
64
69 RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group);
70
77 RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group);
78
81
86 RobotTrajectory(const RobotTrajectory& other, bool deepcopy = false);
87
88 const moveit::core::RobotModelConstPtr& getRobotModel() const
89 {
90 return robot_model_;
91 }
92
94 {
95 return group_;
96 }
97
98 const std::string& getGroupName() const;
99
100 RobotTrajectory& setGroupName(const std::string& group_name)
101 {
102 group_ = robot_model_->getJointModelGroup(group_name);
103 return *this;
104 }
105
106 std::size_t getWayPointCount() const
107 {
108 return waypoints_.size();
109 }
110
111 std::size_t size() const
112 {
113 assert(waypoints_.size() == duration_from_previous_.size());
114 return waypoints_.size();
115 }
116
117 const moveit::core::RobotState& getWayPoint(std::size_t index) const
118 {
119 return *waypoints_[index];
120 }
121
123 {
124 return *waypoints_.back();
125 }
126
128 {
129 return *waypoints_.front();
130 }
131
132 moveit::core::RobotStatePtr& getWayPointPtr(std::size_t index)
133 {
134 return waypoints_[index];
135 }
136
137 moveit::core::RobotStatePtr& getLastWayPointPtr()
138 {
139 return waypoints_.back();
140 }
141
142 moveit::core::RobotStatePtr& getFirstWayPointPtr()
143 {
144 return waypoints_.front();
145 }
146
147 const std::deque<double>& getWayPointDurations() const
148 {
149 return duration_from_previous_;
150 }
151
156 double getWayPointDurationFromStart(std::size_t index) const;
157
158 double getWayPointDurationFromPrevious(std::size_t index) const
159 {
160 if (duration_from_previous_.size() > index)
161 {
162 return duration_from_previous_[index];
163 }
164 else
165 {
166 return 0.0;
167 }
168 }
169
170 RobotTrajectory& setWayPointDurationFromPrevious(std::size_t index, double value)
171 {
172 if (duration_from_previous_.size() <= index)
173 duration_from_previous_.resize(index + 1, 0.0);
174 duration_from_previous_[index] = value;
175 return *this;
176 }
177
178 bool empty() const
179 {
180 return waypoints_.empty();
181 }
182
189 {
190 return addSuffixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
191 }
192
198 RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
199 {
200 state->update();
201 waypoints_.push_back(state);
202 duration_from_previous_.push_back(dt);
203 return *this;
204 }
205
207 {
208 return addPrefixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
209 }
210
211 RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
212 {
213 state->update();
214 waypoints_.push_front(state);
215 duration_from_previous_.push_front(dt);
216 return *this;
217 }
218
219 RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt)
220 {
221 return insertWayPoint(index, std::make_shared<moveit::core::RobotState>(state), dt);
222 }
223
224 RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt)
225 {
226 state->update();
227 waypoints_.insert(waypoints_.begin() + index, state);
228 duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
229 return *this;
230 }
231
242 RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0,
243 size_t end_index = std::numeric_limits<std::size_t>::max());
244
245 void swap(robot_trajectory::RobotTrajectory& other) noexcept;
246
251 RobotTrajectory& removeWayPoint(std::size_t index)
252 {
253 waypoints_.erase(waypoints_.begin() + index);
254 duration_from_previous_.erase(duration_from_previous_.begin() + index);
255 return *this;
256 }
257
259 {
260 waypoints_.clear();
261 duration_from_previous_.clear();
262 return *this;
263 }
264
265 double getDuration() const;
266
267 double getAverageSegmentDuration() const;
268
269 void getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory,
270 const std::vector<std::string>& joint_filter = std::vector<std::string>()) const;
271
279 const trajectory_msgs::msg::JointTrajectory& trajectory);
280
288 const moveit_msgs::msg::RobotTrajectory& trajectory);
289
298 const moveit_msgs::msg::RobotState& state,
299 const moveit_msgs::msg::RobotTrajectory& trajectory);
300
302
304
307
314 void findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const;
315
316 // TODO support visitor function for interpolation, or at least different types.
323 bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const;
324
326 {
327 std::deque<moveit::core::RobotStatePtr>::iterator waypoint_iterator_;
328 std::deque<double>::iterator duration_iterator_;
329
330 public:
331 explicit Iterator(const std::deque<moveit::core::RobotStatePtr>::iterator& waypoint_iterator,
332 const std::deque<double>::iterator& duration_iterator)
333 : waypoint_iterator_(waypoint_iterator), duration_iterator_(duration_iterator)
334 {
335 }
337 {
338 waypoint_iterator_++;
339 duration_iterator_++;
340 return *this;
341 }
343 {
344 Iterator retval = *this;
345 ++(*this);
346 return retval;
347 }
348 bool operator==(const Iterator& other) const
349 {
350 return ((waypoint_iterator_ == other.waypoint_iterator_) && (duration_iterator_ == other.duration_iterator_));
351 }
352 bool operator!=(const Iterator& other) const
353 {
354 return !(*this == other);
355 }
356 std::pair<moveit::core::RobotStatePtr, double> operator*() const
357 {
358 return std::pair{ *waypoint_iterator_, *duration_iterator_ };
359 }
360
361 // iterator traits
362 using difference_type = long;
363 using value_type = std::pair<moveit::core::RobotStatePtr, double>;
364 using pointer = const std::pair<moveit::core::RobotStatePtr, double>*;
365 using reference = std::pair<moveit::core::RobotStatePtr, double>;
366 using iterator_category = std::input_iterator_tag;
367 };
368
370 {
371 assert(waypoints_.size() == duration_from_previous_.size());
372 return Iterator(waypoints_.begin(), duration_from_previous_.begin());
373 }
375 {
376 assert(waypoints_.size() == duration_from_previous_.size());
377 return Iterator(waypoints_.end(), duration_from_previous_.end());
378 }
392 void print(std::ostream& out, std::vector<int> variable_indexes = std::vector<int>()) const;
393
394private:
395 moveit::core::RobotModelConstPtr robot_model_;
396 const moveit::core::JointModelGroup* group_;
397 std::deque<moveit::core::RobotStatePtr> waypoints_;
398 std::deque<double> duration_from_previous_;
399};
400
402std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory);
403
410[[nodiscard]] double pathLength(const RobotTrajectory& trajectory);
411
416[[nodiscard]] std::optional<double> smoothness(const RobotTrajectory& trajectory);
417
422[[nodiscard]] std::optional<double> waypointDensity(const RobotTrajectory& trajectory);
423
425// \param[in] trajectory Given robot trajectory
426// \param[in] include_mdof_joints Treat Multi-DOF variables as joints, e.g. position/x position/y position/theta
427// \param[in] joint_filter Exclude joints with the provided names
428// \return JointTrajectory message including all waypoints
429// or nullopt if the provided RobotTrajectory or RobotModel is empty
430[[nodiscard]] std::optional<trajectory_msgs::msg::JointTrajectory>
431toJointTrajectory(const RobotTrajectory& trajectory, bool include_mdof_joints = false,
432 const std::vector<std::string>& joint_filter = {});
433} // namespace robot_trajectory
#define MOVEIT_CLASS_FORWARD(C)
Representation of a robot's state. This includes position, velocity, acceleration and effort.
const std::pair< moveit::core::RobotStatePtr, double > * pointer
Iterator(const std::deque< moveit::core::RobotStatePtr >::iterator &waypoint_iterator, const std::deque< double >::iterator &duration_iterator)
std::pair< moveit::core::RobotStatePtr, double > reference
std::pair< moveit::core::RobotStatePtr, double > value_type
std::pair< moveit::core::RobotStatePtr, double > operator*() const
bool operator!=(const Iterator &other) const
bool operator==(const Iterator &other) const
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & addPrefixWayPoint(const moveit::core::RobotState &state, double dt)
const std::string & getGroupName() const
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
void findWayPointIndicesForDurationAfterStart(double duration, int &before, int &after, double &blend) const
Finds the waypoint indices before and after a duration from start.
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
void swap(robot_trajectory::RobotTrajectory &other) noexcept
RobotTrajectory::Iterator end()
const moveit::core::RobotState & getLastWayPoint() const
moveit::core::RobotStatePtr & getWayPointPtr(std::size_t index)
const moveit::core::RobotModelConstPtr & getRobotModel() const
void print(std::ostream &out, std::vector< int > variable_indexes=std::vector< int >()) const
Print information about the trajectory.
RobotTrajectory & removeWayPoint(std::size_t index)
Remove a point from the trajectory.
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,...
RobotTrajectory & setWayPointDurationFromPrevious(std::size_t index, double value)
moveit::core::RobotStatePtr & getLastWayPointPtr()
void getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory &trajectory, const std::vector< std::string > &joint_filter=std::vector< std::string >()) const
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotStatePtr &state, double dt)
Add a point to the trajectory.
const moveit::core::JointModelGroup * getGroup() const
moveit::core::RobotStatePtr & getFirstWayPointPtr()
RobotTrajectory & setRobotTrajectoryMsg(const moveit::core::RobotState &reference_state, const trajectory_msgs::msg::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
RobotTrajectory::Iterator begin()
RobotTrajectory & operator=(const RobotTrajectory &)=default
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...
RobotTrajectory & addPrefixWayPoint(const moveit::core::RobotStatePtr &state, double dt)
double getWayPointDurationFromPrevious(std::size_t index) const
RobotTrajectory & insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr &state, double dt)
RobotTrajectory & setGroupName(const std::string &group_name)
const moveit::core::RobotState & getWayPoint(std::size_t index) const
const std::deque< double > & getWayPointDurations() const
RobotTrajectory & insertWayPoint(std::size_t index, const moveit::core::RobotState &state, double dt)
const moveit::core::RobotState & getFirstWayPoint() const
std::optional< trajectory_msgs::msg::JointTrajectory > toJointTrajectory(const RobotTrajectory &trajectory, bool include_mdof_joints=false, const std::vector< std::string > &joint_filter={})
Converts a RobotTrajectory to a JointTrajectory message.
std::optional< double > waypointDensity(const RobotTrajectory &trajectory)
Calculate the waypoint density of a trajectory.
std::optional< double > smoothness(const RobotTrajectory &trajectory)
Calculate the smoothness of a given trajectory.
double pathLength(const RobotTrajectory &trajectory)
Calculate the path length of a given trajectory based on the accumulated robot state distances....
std::ostream & operator<<(std::ostream &out, const RobotTrajectory &trajectory)
Operator overload for printing trajectory to a stream.