Motion Planning using RViz

In this exercise, we will (finally) learn how to use the MoveIt! RViz plugin to plan and execute motion on a simulated robot. We will explore the different options and constraints associated with both MoveIt! and the RViz plugin.

Launch the Planning Environment

  1. Source your ROS2 workspace.

  2. Bring up the planning environment, with simulated execution:

    ros2 launch myworkcell_moveit_config myworkcell_planning_execution.launch.py

Plugin Display Options

  1. Find and test the following display options in the Displays panel, Motion Planning display

    • Scene Robot -> Show Robot Visual

    • Scene Robot -> Show Robot Collision

    • Planning Request -> Query Start State

    • Planning Request -> Query Goal State

  2. For now, enable display of the Show Robot Visual and Query Goal State, leaving Show Robot Collision and Query Start State disabled

  3. Use the Panels dropdown next to File to display a trajectory preview slider by checking the Motion Planning - Trajectory Slider box

    • this slider allows for detailed review of the last planned trajectory

    • Check the Context tab in the motion planning panel to ensure that RRTConnect is selected.

    • If you are struggling to find a motion plan, try increasing the planning time and/or planning attempts.

Basic Motion

  1. In the Motion Planning panel, select the Planning tab

  2. Under the Query section, expand the Select Goal State section

    • select <random valid>

    • you may need to repeat this a few times to get a position that is not in collision. The robot model will be shaded RED if MoveIt detects a collision. “Random Valid” is supposed to find a collision-free target position, but quits trying after 100 random-sampling attempts.

    • observe the goal position in the graphics window

  3. Click Plan to see the robot motion generated by the MoveIt! planning libraries

    • select Displays -> Motion Planning -> Planned Path -> Loop Animation to show the planned path in a cyclic display.

    • select Displays -> Motion Planning -> Planned Path -> Show Trail to show the planned path in a swept display.

  4. Click Execute to run the motion on the simulated robot

    • observe that the multi-colored scene robot display updates to show that the robot has “moved” to the goal position

  5. Repeat steps 2-5 a few more times

    • try using the interactive marker to manually move the robot to a desired position

    • try using a named pose (e.g. “straight up”)

Beyond the Basics

  1. Experiment with different Planning Algorithms

    • select Context tab, choose a Planning Algorithm (drop-down box below “OMPL”)

    • the RRTConnect algorithm is one of our favorites.

  2. Environment Obstacles

    • Adjust the Goal State to move the robot into collision with an obstacle (e.g. the table)

      • note the colliding links are colored red

      • depending on the “Collision-Aware IK” setting on the Planning tab, the robot may try to search through different IK solutions to find a non-colliding solution.

    • Try to plan a path through the obstacle

      • It may help to have “Collision-Aware IK” disabled when moving the Goal State

      • If the robot fails to plan, check the error log and try repeating the plan request

      • Because the default planners are sampling-based, they may produce different results on each execution

      • You can also try increasing the planning time to allow a successful plan to be created

      • Try different planning algorithms in this, more complex, planning task

    • Try adding a new obstacle to the scene:

      • Under the Scene Objects tab, Add a Box object (0.1 x 1.0 x 0.1)

      • Move the Box into an interesting position, using the manipulation handles

      • Press Publish Scene, to push the updated position to MoveIt

      • Try to plan around the obstacle