41#include <moveit_msgs/msg/robot_trajectory.hpp>
42#include <moveit_msgs/msg/robot_state.hpp>
47#include <rcl/error_handling.h>
49#include <rclcpp/rclcpp.hpp>
50#include <rclcpp/time.hpp>
51#include <rclcpp/utilities.hpp>
63 explicit RobotTrajectory(
const moveit::core::RobotModelConstPtr& robot_model);
69 RobotTrajectory(
const moveit::core::RobotModelConstPtr& robot_model,
const std::string& group);
102 group_ = robot_model_->getJointModelGroup(group_name);
108 return waypoints_.size();
113 assert(waypoints_.size() == duration_from_previous_.size());
114 return waypoints_.size();
119 return *waypoints_[index];
124 return *waypoints_.back();
129 return *waypoints_.front();
134 return waypoints_[index];
139 return waypoints_.back();
144 return waypoints_.front();
149 return duration_from_previous_;
160 if (duration_from_previous_.size() > index)
162 return duration_from_previous_[index];
172 if (duration_from_previous_.size() <= index)
173 duration_from_previous_.resize(index + 1, 0.0);
174 duration_from_previous_[index] = value;
180 return waypoints_.empty();
201 waypoints_.push_back(state);
202 duration_from_previous_.push_back(dt);
214 waypoints_.push_front(state);
215 duration_from_previous_.push_front(dt);
221 return insertWayPoint(index, std::make_shared<moveit::core::RobotState>(state), dt);
227 waypoints_.insert(waypoints_.begin() + index, state);
228 duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
243 size_t end_index = std::numeric_limits<std::size_t>::max());
253 waypoints_.erase(waypoints_.begin() + index);
254 duration_from_previous_.erase(duration_from_previous_.begin() + index);
261 duration_from_previous_.clear();
270 const std::vector<std::string>& joint_filter = std::vector<std::string>())
const;
279 const trajectory_msgs::msg::JointTrajectory& trajectory);
288 const moveit_msgs::msg::RobotTrajectory& trajectory);
298 const moveit_msgs::msg::RobotState& state,
299 const moveit_msgs::msg::RobotTrajectory& trajectory);
327 std::deque<moveit::core::RobotStatePtr>::iterator waypoint_iterator_;
328 std::deque<double>::iterator duration_iterator_;
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)
338 waypoint_iterator_++;
339 duration_iterator_++;
350 return ((waypoint_iterator_ == other.waypoint_iterator_) && (duration_iterator_ == other.duration_iterator_));
354 return !(*
this == other);
356 std::pair<moveit::core::RobotStatePtr, double>
operator*()
const
358 return std::pair{ *waypoint_iterator_, *duration_iterator_ };
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>;
371 assert(waypoints_.size() == duration_from_previous_.size());
372 return Iterator(waypoints_.begin(), duration_from_previous_.begin());
376 assert(waypoints_.size() == duration_from_previous_.size());
377 return Iterator(waypoints_.end(), duration_from_previous_.end());
392 void print(std::ostream& out, std::vector<int> variable_indexes = std::vector<int>())
const;
395 moveit::core::RobotModelConstPtr robot_model_;
397 std::deque<moveit::core::RobotStatePtr> waypoints_;
398 std::deque<double> duration_from_previous_;
402std::ostream&
operator<<(std::ostream& out,
const RobotTrajectory& trajectory);
410[[nodiscard]]
double pathLength(
const RobotTrajectory& trajectory);
416[[nodiscard]] std::optional<double>
smoothness(
const RobotTrajectory& trajectory);
422[[nodiscard]] std::optional<double>
waypointDensity(
const RobotTrajectory& trajectory);
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 = {});
#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
std::input_iterator_tag iterator_category
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 & reverse()
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
double getAverageSegmentDuration() 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.
RobotTrajectory & unwind()
RobotTrajectory & clear()
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...
std::size_t getWayPointCount() const
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
double getDuration() 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.