#include <robot_trajectory.h>
Public Member Functions | |
void | addPrefixWayPoint (const robot_state::RobotState &state, double dt) |
void | addPrefixWayPoint (const robot_state::RobotStatePtr &state, double dt) |
void | addSuffixWayPoint (const robot_state::RobotState &state, double dt) |
void | addSuffixWayPoint (const robot_state::RobotStatePtr &state, double dt) |
void | append (const RobotTrajectory &source, double dt) |
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. | |
double | getAverageSegmentDuration () const |
const robot_state::RobotState & | getFirstWayPoint () const |
robot_state::RobotStatePtr & | getFirstWayPointPtr () |
const robot_model::JointModelGroup * | getGroup () const |
const std::string & | getGroupName () const |
const robot_state::RobotState & | getLastWayPoint () const |
robot_state::RobotStatePtr & | getLastWayPointPtr () |
const robot_model::RobotModelConstPtr & | getRobotModel () const |
void | getRobotTrajectoryMsg (moveit_msgs::RobotTrajectory &trajectory) const |
bool | getStateAtDurationFromStart (const double request_duration, robot_state::RobotStatePtr &output_state) const |
Gets a robot state corresponding to a supplied duration from start for the trajectory, using linear time interpolation. | |
const robot_state::RobotState & | getWayPoint (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. | |
const std::deque< double > & | getWayPointDurations () const |
robot_state::RobotStatePtr & | getWayPointPtr (std::size_t index) |
void | insertWayPoint (std::size_t index, const robot_state::RobotState &state, double dt) |
void | insertWayPoint (std::size_t index, const robot_state::RobotStatePtr &state, double dt) |
void | reverse () |
RobotTrajectory (const robot_model::RobotModelConstPtr &kmodel, const std::string &group) | |
void | setGroupName (const std::string &group_name) |
void | setRobotTrajectoryMsg (const robot_state::RobotState &reference_state, const moveit_msgs::RobotTrajectory &trajectory) |
void | setRobotTrajectoryMsg (const robot_state::RobotState &reference_state, const moveit_msgs::RobotState &state, const moveit_msgs::RobotTrajectory &trajectory) |
void | setWayPointDurationFromPrevious (std::size_t index, double value) |
void | swap (robot_trajectory::RobotTrajectory &other) |
void | unwind () |
void | unwind (const robot_state::RobotState &state) |
Private Attributes | |
std::deque< double > | duration_from_previous_ |
const robot_model::JointModelGroup * | group_ |
robot_model::RobotModelConstPtr | kmodel_ |
std::deque < robot_state::RobotStatePtr > | waypoints_ |
Definition at line 48 of file robot_trajectory.h.
robot_trajectory::RobotTrajectory::RobotTrajectory | ( | const robot_model::RobotModelConstPtr & | kmodel, |
const std::string & | group | ||
) |
Definition at line 43 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::addPrefixWayPoint | ( | const robot_state::RobotState & | state, |
double | dt | ||
) | [inline] |
Definition at line 144 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::addPrefixWayPoint | ( | const robot_state::RobotStatePtr & | state, |
double | dt | ||
) | [inline] |
Definition at line 149 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::addSuffixWayPoint | ( | const robot_state::RobotState & | state, |
double | dt | ||
) | [inline] |
Definition at line 133 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::addSuffixWayPoint | ( | const robot_state::RobotStatePtr & | state, |
double | dt | ||
) | [inline] |
Definition at line 138 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::append | ( | const RobotTrajectory & | source, |
double | dt | ||
) |
Definition at line 78 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::clear | ( | void | ) |
Definition at line 178 of file robot_trajectory.cpp.
bool robot_trajectory::RobotTrajectory::empty | ( | ) | const [inline] |
Definition at line 128 of file robot_trajectory.h.
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.
The | duration from start. |
The | waypoint index before the supplied duration. |
The | waypoint index after (or equal to) the supplied duration. |
The | progress (0 to 1) between the two waypoints, based on time (not based on joint distances). |
Definition at line 312 of file robot_trajectory.cpp.
double robot_trajectory::RobotTrajectory::getAverageSegmentDuration | ( | ) | const |
Definition at line 62 of file robot_trajectory.cpp.
const robot_state::RobotState& robot_trajectory::RobotTrajectory::getFirstWayPoint | ( | ) | const [inline] |
Definition at line 82 of file robot_trajectory.h.
Definition at line 97 of file robot_trajectory.h.
const robot_model::JointModelGroup* robot_trajectory::RobotTrajectory::getGroup | ( | ) | const [inline] |
Definition at line 58 of file robot_trajectory.h.
const std::string & robot_trajectory::RobotTrajectory::getGroupName | ( | ) | const |
Definition at line 54 of file robot_trajectory.cpp.
const robot_state::RobotState& robot_trajectory::RobotTrajectory::getLastWayPoint | ( | ) | const [inline] |
Definition at line 77 of file robot_trajectory.h.
Definition at line 92 of file robot_trajectory.h.
const robot_model::RobotModelConstPtr& robot_trajectory::RobotTrajectory::getRobotModel | ( | ) | const [inline] |
Definition at line 53 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::getRobotTrajectoryMsg | ( | moveit_msgs::RobotTrajectory & | trajectory | ) | const |
Definition at line 184 of file robot_trajectory.cpp.
bool robot_trajectory::RobotTrajectory::getStateAtDurationFromStart | ( | const double | request_duration, |
robot_state::RobotStatePtr & | output_state | ||
) | const |
Gets a robot state corresponding to a supplied duration from start for the trajectory, using linear time interpolation.
The | duration from start. |
The | resulting robot state. |
Definition at line 354 of file robot_trajectory.cpp.
const robot_state::RobotState& robot_trajectory::RobotTrajectory::getWayPoint | ( | std::size_t | index | ) | const [inline] |
Definition at line 72 of file robot_trajectory.h.
std::size_t robot_trajectory::RobotTrajectory::getWayPointCount | ( | ) | const [inline] |
Definition at line 67 of file robot_trajectory.h.
double robot_trajectory::RobotTrajectory::getWayPointDurationFromPrevious | ( | std::size_t | index | ) | const [inline] |
Definition at line 113 of file robot_trajectory.h.
double robot_trajectory::RobotTrajectory::getWaypointDurationFromStart | ( | std::size_t | index | ) | const |
Returns the duration after start that a waypoint will be reached.
The | waypoint index. |
Definition at line 343 of file robot_trajectory.cpp.
const std::deque<double>& robot_trajectory::RobotTrajectory::getWayPointDurations | ( | ) | const [inline] |
Definition at line 102 of file robot_trajectory.h.
robot_state::RobotStatePtr& robot_trajectory::RobotTrajectory::getWayPointPtr | ( | std::size_t | index | ) | [inline] |
Definition at line 87 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::insertWayPoint | ( | std::size_t | index, |
const robot_state::RobotState & | state, | ||
double | dt | ||
) | [inline] |
Definition at line 155 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::insertWayPoint | ( | std::size_t | index, |
const robot_state::RobotStatePtr & | state, | ||
double | dt | ||
) | [inline] |
Definition at line 160 of file robot_trajectory.h.
Definition at line 87 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::setGroupName | ( | const std::string & | group_name | ) |
Definition at line 49 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg | ( | const robot_state::RobotState & | reference_state, |
const moveit_msgs::RobotTrajectory & | trajectory | ||
) |
Definition at line 264 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg | ( | const robot_state::RobotState & | reference_state, |
const moveit_msgs::RobotState & | state, | ||
const moveit_msgs::RobotTrajectory & | trajectory | ||
) |
Definition at line 304 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::setWayPointDurationFromPrevious | ( | std::size_t | index, |
double | value | ||
) | [inline] |
Definition at line 121 of file robot_trajectory.h.
Definition at line 70 of file robot_trajectory.cpp.
Definition at line 98 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::unwind | ( | const robot_state::RobotState & | state | ) |
Definition at line 132 of file robot_trajectory.cpp.
std::deque<double> robot_trajectory::RobotTrajectory::duration_from_previous_ [private] |
Definition at line 209 of file robot_trajectory.h.
const robot_model::JointModelGroup* robot_trajectory::RobotTrajectory::group_ [private] |
Definition at line 207 of file robot_trajectory.h.
Definition at line 206 of file robot_trajectory.h.
std::deque<robot_state::RobotStatePtr> robot_trajectory::RobotTrajectory::waypoints_ [private] |
Definition at line 208 of file robot_trajectory.h.