Maintain a sequence of waypoints and the time durations between these waypoints. More...
#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) |
Add a point to the trajectory. More... | |
void | addSuffixWayPoint (const robot_state::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 |
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. More... | |
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. More... | |
MOVEIT_DEPRECATED double | getWaypointDurationFromStart (std::size_t index) const |
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 &robot_model, const std::string &group) | |
RobotTrajectory (const robot_model::RobotModelConstPtr &robot_model, const robot_model::JointModelGroup *group) | |
void | setGroupName (const std::string &group_name) |
void | setRobotTrajectoryMsg (const robot_state::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 | setRobotTrajectoryMsg (const robot_state::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 robot_state::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 | 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 | robot_model_ |
std::deque< robot_state::RobotStatePtr > | waypoints_ |
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition at line 53 of file robot_trajectory.h.
robot_trajectory::RobotTrajectory::RobotTrajectory | ( | const robot_model::RobotModelConstPtr & | robot_model, |
const std::string & | group | ||
) |
Definition at line 45 of file robot_trajectory.cpp.
robot_trajectory::RobotTrajectory::RobotTrajectory | ( | const robot_model::RobotModelConstPtr & | robot_model, |
const robot_model::JointModelGroup * | group | ||
) |
Definition at line 50 of file robot_trajectory.cpp.
|
inline |
Definition at line 164 of file robot_trajectory.h.
|
inline |
Definition at line 169 of file robot_trajectory.h.
|
inline |
Add a point to the trajectory.
state | - current robot state |
dt | - duration from previous |
Definition at line 147 of file robot_trajectory.h.
|
inline |
Add a point to the trajectory.
state | - current robot state |
dt | - duration from previous |
Definition at line 157 of file robot_trajectory.h.
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.
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 86 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::clear | ( | void | ) |
Definition at line 198 of file robot_trajectory.cpp.
|
inline |
Definition at line 137 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 413 of file robot_trajectory.cpp.
double robot_trajectory::RobotTrajectory::getAverageSegmentDuration | ( | ) | const |
Definition at line 69 of file robot_trajectory.cpp.
|
inline |
Definition at line 89 of file robot_trajectory.h.
|
inline |
Definition at line 104 of file robot_trajectory.h.
|
inline |
Definition at line 65 of file robot_trajectory.h.
const std::string & robot_trajectory::RobotTrajectory::getGroupName | ( | ) | const |
Definition at line 61 of file robot_trajectory.cpp.
|
inline |
Definition at line 84 of file robot_trajectory.h.
|
inline |
Definition at line 99 of file robot_trajectory.h.
|
inline |
Definition at line 60 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::getRobotTrajectoryMsg | ( | moveit_msgs::RobotTrajectory & | trajectory | ) | const |
Definition at line 204 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 462 of file robot_trajectory.cpp.
|
inline |
Definition at line 79 of file robot_trajectory.h.
|
inline |
Definition at line 74 of file robot_trajectory.h.
|
inline |
Definition at line 122 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 444 of file robot_trajectory.cpp.
double robot_trajectory::RobotTrajectory::getWaypointDurationFromStart | ( | std::size_t | index | ) | const |
Definition at line 457 of file robot_trajectory.cpp.
|
inline |
Definition at line 109 of file robot_trajectory.h.
|
inline |
Definition at line 94 of file robot_trajectory.h.
|
inline |
Definition at line 176 of file robot_trajectory.h.
|
inline |
Definition at line 181 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::reverse | ( | ) |
Definition at line 101 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::setGroupName | ( | const std::string & | group_name | ) |
Definition at line 56 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg | ( | const robot_state::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 329 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::setRobotTrajectoryMsg | ( | const robot_state::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 355 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 | ||
) |
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 404 of file robot_trajectory.cpp.
|
inline |
Definition at line 130 of file robot_trajectory.h.
void robot_trajectory::RobotTrajectory::swap | ( | robot_trajectory::RobotTrajectory & | other | ) |
Definition at line 78 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::unwind | ( | ) |
Definition at line 117 of file robot_trajectory.cpp.
void robot_trajectory::RobotTrajectory::unwind | ( | const robot_state::RobotState & | state | ) |
Definition at line 152 of file robot_trajectory.cpp.
|
private |
Definition at line 263 of file robot_trajectory.h.
|
private |
Definition at line 261 of file robot_trajectory.h.
|
private |
Definition at line 260 of file robot_trajectory.h.
|
private |
Definition at line 262 of file robot_trajectory.h.