Skip to content

Commit

Permalink
Clarified longest_valid_segment, also talked about max_waypoint_dist. (
Browse files Browse the repository at this point in the history
…#165)

* Clarified longest_valid_segment, also talked about max_waypoint_dist.

* Fixed one of the travis warnings.
  • Loading branch information
BryceStevenWilley authored and davetcoleman committed May 5, 2018
1 parent 3747bd7 commit 9450ce2
Show file tree
Hide file tree
Showing 2 changed files with 9 additions and 3 deletions.
10 changes: 8 additions & 2 deletions doc/ompl_interface_tutorial.rst
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,15 @@ Here we review important configuration settings for OMPL. These settings can typ
Longest Valid Segment Fraction
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

The ``longest_valid_segement_fraction`` defines the discretization of robot motions used for collision checking, and greatly affects the performance and reliability of OMPL-based solutions. A ``motion`` in this context can be thought of as an edge between two nodes in a graph, where nodes are discretized robot states that randomly sampled and collision checked for validity. The default motion collision checker in OMPL simply discretizes the edge into a number of sub-states to collision check. Currently no continuous collision checking is available in OMPL/MoveIt!, though this is an area of current `discussion <https://github.com/ros-planning/moveit/issues/29>`_.
The ``longest_valid_segement_fraction`` defines the discretization of robot motions used for collision checking, and greatly affects the performance and reliability of OMPL-based solutions. A ``motion`` in this context can be thought of as an edge between two nodes in a graph, where nodes are waypoints along a trajectory. The default motion collision checker in OMPL simply discretizes the edge into a number of sub-states to collision check. Currently no continuous collision checking is available in OMPL/MoveIt!, though this is an area of current `discussion <https://github.com/ros-planning/moveit/issues/29>`_.

Set ``longest_valid_segement_fraction`` too low, and collision checking / motion planning will be very slow. Set too high and collisions will be missed around small or narrow objects. In addition, a high collision checking resolution will cause the path smoothers to output incomprehensible motions because they are able to "catch" the invalid path and then attempt to repair them by sampling around it, but imperfectly.
Specifically, ``longest_valid_segment_fraction`` is the fraction of the robot's state space that, given the robot isn't currently in collision, we assume the robot can travel while remaining collision free. For example, if ``longest_valid_segment_fraction = 0.01``, then we assume that if an edge between two nodes is less than 1/100th of the state space, then we don't need to explicity check any sub-states along that edge, just the two nodes it connects.

In addition to the ``longest_valid_segment_fraction`` parameter in the ``ompl_planning.yaml`` file, there is also the ``maximum_waypoint_distance``, found in the `dynamic reconfigure file <https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_planners/ompl/ompl_interface/cfg/OMPLDynamicReconfigure.cfg#L9>`_. ``maximum_waypoint_distance`` defines the same discretization of robot motions for collision checking, but it does so at an absolute level instead of using fractions of the state space. For example, if ``maximum_waypoint_distance = 0.1``, then if an edge is shorter than ``0.1`` in state space distance, then we don't explicitly check any sub-states along that edge.

If both ``longest_valid_segment_fraction`` and ``maximum_waypoint_distance`` are set, then the variable that produces the most conservative discretization (the one that would generate the most states to collision check on a given edge) is chosen.

Set ``longest_valid_segement_fraction`` (or ``maximum_waypoint_distance``) too low, and collision checking / motion planning will be very slow. Set too high and collisions will be missed around small or narrow objects. In addition, a high collision checking resolution will cause the path smoothers to output incomprehensible motions because they are able to "catch" the invalid path and then attempt to repair them by sampling around it, but imperfectly.

A quick analysis of the effect of this parameter on two of the MoveIt! tutorial examples is documented `here <https://github.com/ros-planning/moveit/pull/337>`_.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ int main(int argc, char **argv)
// PR2 robot. To solve IK, we will need the following:
//
// * The desired pose of the end-effector (by default, this is the last link in the "right_arm" chain):
// end_effector_state that we computed in the step above.
// end_effector_state that we computed in the step above.
// * The number of attempts to be made at solving IK: 10
// * The timeout for each attempt: 0.1 s
bool found_ik = kinematic_state->setFromIK(joint_model_group, end_effector_state, 10, 0.1);
Expand Down

0 comments on commit 9450ce2

Please sign in to comment.