Public Member Functions | Private Attributes | List of all members
robot_trajectory::RobotTrajectory Class Reference

Maintain a sequence of waypoints and the time durations between these waypoints. More...

#include <robot_trajectory.h>

Public Member Functions

void addPrefixWayPoint (const moveit::core::RobotState &state, double dt)
 
void addPrefixWayPoint (const moveit::core::RobotStatePtr &state, double dt)
 
void addSuffixWayPoint (const moveit::core::RobotState &state, double dt)
 Add a point to the trajectory. More...
 
void addSuffixWayPoint (const moveit::core::RobotStatePtr &state, double dt)
 Add a point to the trajectory. More...
 
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_index and end_index are omitted) is to add the whole trajectory. More...
 
void clear ()
 
bool empty () const
 
void findWayPointIndicesForDurationAfterStart (const double &duration, int &before, int &after, double &blend) const
 Finds the waypoint indicies before and after a duration from start. More...
 
double getAverageSegmentDuration () const
 
double getDuration () const
 
const moveit::core::RobotStategetFirstWayPoint () const
 
moveit::core::RobotStatePtr & getFirstWayPointPtr ()
 
const moveit::core::JointModelGroupgetGroup () const
 
const std::stringgetGroupName () const
 
const moveit::core::RobotStategetLastWayPoint () const
 
moveit::core::RobotStatePtr & getLastWayPointPtr ()
 
const moveit::core::RobotModelConstPtr & getRobotModel () const
 
void getRobotTrajectoryMsg (moveit_msgs::RobotTrajectory &trajectory, const std::vector< std::string > &joint_filter=std::vector< std::string >()) const
 
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, using linear time interpolation. More...
 
const moveit::core::RobotStategetWayPoint (std::size_t index) const
 
std::size_t getWayPointCount () const
 
double getWayPointDurationFromPrevious (std::size_t index) const
 
double getWayPointDurationFromStart (std::size_t index) const
 Returns the duration after start that a waypoint will be reached. More...
 
double getWaypointDurationFromStart (std::size_t index) const
 
const std::deque< double > & getWayPointDurations () const
 
moveit::core::RobotStatePtr & getWayPointPtr (std::size_t index)
 
void insertWayPoint (std::size_t index, const moveit::core::RobotState &state, double dt)
 
void insertWayPoint (std::size_t index, const moveit::core::RobotStatePtr &state, double dt)
 
RobotTrajectoryoperator= (const RobotTrajectory &)=default
 
void reverse ()
 
 RobotTrajectory (const moveit::core::RobotModelConstPtr &robot_model, const moveit::core::JointModelGroup *group)
 
 RobotTrajectory (const moveit::core::RobotModelConstPtr &robot_model, const std::string &group)
 
 RobotTrajectory (const RobotTrajectory &other, bool deepcopy=false)
 Copy constructor allowing a shallow or deep copy of waypoints. More...
 
void setGroupName (const std::string &group_name)
 
void setRobotTrajectoryMsg (const moveit::core::RobotState &reference_state, const moveit_msgs::RobotState &state, const moveit_msgs::RobotTrajectory &trajectory)
 Copy the content of the trajectory message into this class. The trajectory message itself is not required to contain the values for all joints. For this reason a full starting state must be specified as reference (reference_state). Before use, the reference state is updated using state. Each point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in trajectory. More...
 
void setRobotTrajectoryMsg (const moveit::core::RobotState &reference_state, const moveit_msgs::RobotTrajectory &trajectory)
 Copy the content of the trajectory message into this class. The trajectory message itself is not required to contain the values for all joints. For this reason a full starting state must be specified as reference (reference_state). Each point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in trajectory. More...
 
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 required to contain the values for all joints. For this reason a full starting state must be specified as reference (reference_state). Each point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in trajectory. More...
 
void setWayPointDurationFromPrevious (std::size_t index, double value)
 
void swap (robot_trajectory::RobotTrajectory &other)
 
void unwind ()
 
void unwind (const moveit::core::RobotState &state)
 

Private Attributes

std::deque< double > duration_from_previous_
 
const moveit::core::JointModelGroupgroup_
 
moveit::core::RobotModelConstPtr robot_model_
 
std::deque< moveit::core::RobotStatePtr > waypoints_
 

Detailed Description

Maintain a sequence of waypoints and the time durations between these waypoints.

Definition at line 83 of file robot_trajectory.h.

Constructor & Destructor Documentation

◆ RobotTrajectory() [1/3]

robot_trajectory::RobotTrajectory::RobotTrajectory ( const moveit::core::RobotModelConstPtr &  robot_model,
const std::string group 
)

Definition at line 77 of file robot_trajectory.cpp.

◆ RobotTrajectory() [2/3]

robot_trajectory::RobotTrajectory::RobotTrajectory ( const moveit::core::RobotModelConstPtr &  robot_model,
const moveit::core::JointModelGroup group 
)

Definition at line 82 of file robot_trajectory.cpp.

◆ RobotTrajectory() [3/3]

robot_trajectory::RobotTrajectory::RobotTrajectory ( const RobotTrajectory other,
bool  deepcopy = false 
)

Copy constructor allowing a shallow or deep copy of waypoints.

Parameters
other- RobotTrajectory to copy from
deepcopy- copy waypoints by value (true) or by pointer (false)?

Definition at line 88 of file robot_trajectory.cpp.

Member Function Documentation

◆ addPrefixWayPoint() [1/2]

void robot_trajectory::RobotTrajectory::addPrefixWayPoint ( const moveit::core::RobotState state,
double  dt 
)
inline

Definition at line 203 of file robot_trajectory.h.

◆ addPrefixWayPoint() [2/2]

void robot_trajectory::RobotTrajectory::addPrefixWayPoint ( const moveit::core::RobotStatePtr &  state,
double  dt 
)
inline

Definition at line 208 of file robot_trajectory.h.

◆ addSuffixWayPoint() [1/2]

void robot_trajectory::RobotTrajectory::addSuffixWayPoint ( const moveit::core::RobotState state,
double  dt 
)
inline

Add a point to the trajectory.

Parameters
state- current robot state
dt- duration from previous

Definition at line 186 of file robot_trajectory.h.

◆ addSuffixWayPoint() [2/2]

void robot_trajectory::RobotTrajectory::addSuffixWayPoint ( const moveit::core::RobotStatePtr &  state,
double  dt 
)
inline

Add a point to the trajectory.

Parameters
state- current robot state
dt- duration from previous

Definition at line 196 of file robot_trajectory.h.

◆ append()

void robot_trajectory::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_index and end_index are omitted) is to add the whole trajectory.

Parameters
source- the trajectory containing the part to append to the end of current trajectory
dt- time step between last traj point in current traj and first traj point of append traj
start_index- index of first traj point of the part to append from the source traj, the default is to add from the start of the source traj
end_index- index of last traj point of the part to append from the source traj, the default is to add until the end of the source traj

Definition at line 135 of file robot_trajectory.cpp.

◆ clear()

void robot_trajectory::RobotTrajectory::clear ( )

Definition at line 247 of file robot_trajectory.cpp.

◆ empty()

bool robot_trajectory::RobotTrajectory::empty ( ) const
inline

Definition at line 176 of file robot_trajectory.h.

◆ findWayPointIndicesForDurationAfterStart()

void robot_trajectory::RobotTrajectory::findWayPointIndicesForDurationAfterStart ( const double &  duration,
int &  before,
int &  after,
double &  blend 
) const

Finds the waypoint indicies before and after a duration from start.

Parameters
Theduration from start.
Thewaypoint index before the supplied duration.
Thewaypoint index after (or equal to) the supplied duration.
Theprogress (0 to 1) between the two waypoints, based on time (not based on joint distances).

Definition at line 469 of file robot_trajectory.cpp.

◆ getAverageSegmentDuration()

double robot_trajectory::RobotTrajectory::getAverageSegmentDuration ( ) const

Definition at line 119 of file robot_trajectory.cpp.

◆ getDuration()

double robot_trajectory::RobotTrajectory::getDuration ( ) const

Definition at line 114 of file robot_trajectory.cpp.

◆ getFirstWayPoint()

const moveit::core::RobotState& robot_trajectory::RobotTrajectory::getFirstWayPoint ( ) const
inline

Definition at line 128 of file robot_trajectory.h.

◆ getFirstWayPointPtr()

moveit::core::RobotStatePtr& robot_trajectory::RobotTrajectory::getFirstWayPointPtr ( )
inline

Definition at line 143 of file robot_trajectory.h.

◆ getGroup()

const moveit::core::JointModelGroup* robot_trajectory::RobotTrajectory::getGroup ( ) const
inline

Definition at line 104 of file robot_trajectory.h.

◆ getGroupName()

const std::string & robot_trajectory::RobotTrajectory::getGroupName ( ) const

Definition at line 106 of file robot_trajectory.cpp.

◆ getLastWayPoint()

const moveit::core::RobotState& robot_trajectory::RobotTrajectory::getLastWayPoint ( ) const
inline

Definition at line 123 of file robot_trajectory.h.

◆ getLastWayPointPtr()

moveit::core::RobotStatePtr& robot_trajectory::RobotTrajectory::getLastWayPointPtr ( )
inline

Definition at line 138 of file robot_trajectory.h.

◆ getRobotModel()

const moveit::core::RobotModelConstPtr& robot_trajectory::RobotTrajectory::getRobotModel ( ) const
inline

Definition at line 99 of file robot_trajectory.h.

◆ getRobotTrajectoryMsg()

void robot_trajectory::RobotTrajectory::getRobotTrajectoryMsg ( moveit_msgs::RobotTrajectory &  trajectory,
const std::vector< std::string > &  joint_filter = std::vector<std::string>() 
) const

Definition at line 253 of file robot_trajectory.cpp.

◆ getStateAtDurationFromStart()

bool robot_trajectory::RobotTrajectory::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, using linear time interpolation.

Parameters
Theduration from start.
Theresulting robot state.
Returns
True if state is valid, false otherwise (trajectory is empty).

Definition at line 518 of file robot_trajectory.cpp.

◆ getWayPoint()

const moveit::core::RobotState& robot_trajectory::RobotTrajectory::getWayPoint ( std::size_t  index) const
inline

Definition at line 118 of file robot_trajectory.h.

◆ getWayPointCount()

std::size_t robot_trajectory::RobotTrajectory::getWayPointCount ( ) const
inline

Definition at line 113 of file robot_trajectory.h.

◆ getWayPointDurationFromPrevious()

double robot_trajectory::RobotTrajectory::getWayPointDurationFromPrevious ( std::size_t  index) const
inline

Definition at line 161 of file robot_trajectory.h.

◆ getWayPointDurationFromStart()

double robot_trajectory::RobotTrajectory::getWayPointDurationFromStart ( std::size_t  index) const

Returns the duration after start that a waypoint will be reached.

Parameters
Thewaypoint index.
Returns
The duration from start; returns overall duration if index is out of range.

Definition at line 500 of file robot_trajectory.cpp.

◆ getWaypointDurationFromStart()

double robot_trajectory::RobotTrajectory::getWaypointDurationFromStart ( std::size_t  index) const

Definition at line 513 of file robot_trajectory.cpp.

◆ getWayPointDurations()

const std::deque<double>& robot_trajectory::RobotTrajectory::getWayPointDurations ( ) const
inline

Definition at line 148 of file robot_trajectory.h.

◆ getWayPointPtr()

moveit::core::RobotStatePtr& robot_trajectory::RobotTrajectory::getWayPointPtr ( std::size_t  index)
inline

Definition at line 133 of file robot_trajectory.h.

◆ insertWayPoint() [1/2]

void robot_trajectory::RobotTrajectory::insertWayPoint ( std::size_t  index,
const moveit::core::RobotState state,
double  dt 
)
inline

Definition at line 215 of file robot_trajectory.h.

◆ insertWayPoint() [2/2]

void robot_trajectory::RobotTrajectory::insertWayPoint ( std::size_t  index,
const moveit::core::RobotStatePtr &  state,
double  dt 
)
inline

Definition at line 220 of file robot_trajectory.h.

◆ operator=()

RobotTrajectory& robot_trajectory::RobotTrajectory::operator= ( const RobotTrajectory )
default

Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer

◆ reverse()

void robot_trajectory::RobotTrajectory::reverse ( )

Definition at line 150 of file robot_trajectory.cpp.

◆ setGroupName()

void robot_trajectory::RobotTrajectory::setGroupName ( const std::string group_name)

Definition at line 101 of file robot_trajectory.cpp.

◆ setRobotTrajectoryMsg() [1/3]

void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg ( const moveit::core::RobotState reference_state,
const moveit_msgs::RobotState &  state,
const moveit_msgs::RobotTrajectory &  trajectory 
)

Copy the content of the trajectory message into this class. The trajectory message itself is not required to contain the values for all joints. For this reason a full starting state must be specified as reference (reference_state). Before use, the reference state is updated using state. Each point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in trajectory.

Definition at line 460 of file robot_trajectory.cpp.

◆ setRobotTrajectoryMsg() [2/3]

void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg ( const moveit::core::RobotState reference_state,
const moveit_msgs::RobotTrajectory &  trajectory 
)

Copy the content of the trajectory message into this class. The trajectory message itself is not required to contain the values for all joints. For this reason a full starting state must be specified as reference (reference_state). Each point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in trajectory.

Definition at line 413 of file robot_trajectory.cpp.

◆ setRobotTrajectoryMsg() [3/3]

void robot_trajectory::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 required to contain the values for all joints. For this reason a full starting state must be specified as reference (reference_state). Each point in the trajectory to be constructed internally is obtained by copying the reference state and overwriting the content from a trajectory point in trajectory.

Definition at line 387 of file robot_trajectory.cpp.

◆ setWayPointDurationFromPrevious()

void robot_trajectory::RobotTrajectory::setWayPointDurationFromPrevious ( std::size_t  index,
double  value 
)
inline

Definition at line 169 of file robot_trajectory.h.

◆ swap()

void robot_trajectory::RobotTrajectory::swap ( robot_trajectory::RobotTrajectory other)

Definition at line 127 of file robot_trajectory.cpp.

◆ unwind() [1/2]

void robot_trajectory::RobotTrajectory::unwind ( )

Definition at line 166 of file robot_trajectory.cpp.

◆ unwind() [2/2]

void robot_trajectory::RobotTrajectory::unwind ( const moveit::core::RobotState state)

Definition at line 201 of file robot_trajectory.cpp.

Member Data Documentation

◆ duration_from_previous_

std::deque<double> robot_trajectory::RobotTrajectory::duration_from_previous_
private

Definition at line 305 of file robot_trajectory.h.

◆ group_

const moveit::core::JointModelGroup* robot_trajectory::RobotTrajectory::group_
private

Definition at line 303 of file robot_trajectory.h.

◆ robot_model_

moveit::core::RobotModelConstPtr robot_trajectory::RobotTrajectory::robot_model_
private

Definition at line 302 of file robot_trajectory.h.

◆ waypoints_

std::deque<moveit::core::RobotStatePtr> robot_trajectory::RobotTrajectory::waypoints_
private

Definition at line 304 of file robot_trajectory.h.


The documentation for this class was generated from the following files:


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