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¶
Source your ROS2 workspace.
Bring up the planning environment, with simulated execution:
ros2 launch myworkcell_moveit_config demo.launch.py
Plugin Display Options¶
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
For now, enable display of the Show Robot Visual and Query Goal State, leaving Show Robot Collision and Query Start State disabled
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¶
In the Motion Planning panel, select the Planning tab
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
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.
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
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¶
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.
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