7. Motion Generation: RRT

7.1. Learning Objectives

This tutorial shows how the Lula RRT class in the Motion Generation extension may be used to produce a collision free path from a starting configuration space (c-space) position to a c-space or task-space target.

7.2. Getting Started

Prerequisites

This tutorial includes multiple variations of a standalone script that loads a Franka robot and has it follow a target. Each of these variations may be saved as a Python file and run as demonstrated in the Standalone Application tutorial.

7.3. Generating a Path Using an RRT Instance

Lula RRT requires three configuration files to specify a specific robot as specified in Lula RRT Configuration. Paths to these configuration files are used to initialize the RRT class along with an end effector name matching a frame in the robot URDF. A simple standalone script is provided below which instantiates RRT for the Franka robot. Every 60 frames, the planner replans to move to the current target position.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
from omni.isaac.kit import SimulationApp

    simulation_app = SimulationApp({"headless": False})

    from omni.isaac.franka.tasks import FollowTarget
    from omni.isaac.motion_generation.lula import RRT
    from omni.isaac.motion_generation import PathPlannerVisualizer
    from omni.isaac.core import World
    from omni.isaac.core import objects

    from omni.isaac.core.utils.extensions import get_extension_path_from_name
    import os
    import numpy as np

    my_world = World(stage_units_in_meters=1.0)
    my_task = FollowTarget(name="follow_target_task")
    my_world.add_task(my_task)
    my_world.reset()
    task_params = my_world.get_task("follow_target_task").get_params()
    franka_name = task_params["robot_name"]["value"]
    target_name = task_params["target_name"]["value"]
    my_franka = my_world.scene.get_object(franka_name)

    # Lula config files for supported robots are stored in the motion_generation extension under
    # "/path_planner_configs" and "motion_policy_configs"
    mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
    rrt_config_dir = os.path.join(mg_extension_path, "path_planner_configs")
    rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")

    # Initialize an RRT object
    rrt = RRT(
        robot_description_path = rmp_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
        urdf_path = rmp_config_dir + "/franka/lula_franka_gen.urdf",
        rrt_config_path = rrt_config_dir + "/franka/rrt/franka_planner_config.yaml",
        end_effector_frame_name = "right_gripper"
    )

    # Use the PathPlannerVisualizer wrapper to generate a trajectory of ArticulationActions
    path_planner_visualizer = PathPlannerVisualizer(my_franka,rrt)

    observations = my_world.get_observations()
    target_pose = observations[target_name]["position"]

    plan = path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist = .01)

    articulation_controller = my_franka.get_articulation_controller()
    while simulation_app.is_running():
        my_world.step(render=True)
        if my_world.is_playing():
            if my_world.current_time_step_index == 0:
                my_world.reset()

            observations = my_world.get_observations()

            # Check every 60 frames whether the end effector moved
            if my_world.current_time_step_index % 60 == 0:
                curr_target_pose = observations[target_name]["position"]

                # If the end effector moved: replan
                if np.linalg.norm(target_pose-curr_target_pose) > .01:
                    target_pose = curr_target_pose
                    rrt.set_end_effector_target(target_pose)
                    plan = path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist = .01)

            if plan:
                actions = plan.pop(0)
                articulation_controller.apply_action(actions)

    simulation_app.close()

As an instance of the PathPlanner interface, RRT can be passed to a Path Planner Visualizer to convert its output to a form that is directly usable by the robot Articulation. On line 63, the path_planner_visualizer is activated to make a new plan every 60 frames. RRT outputs sparse plans that, when linearly interpolated, form a collision-free path to the goal position. The max_cspace_dist argument passed to the path_planner_visualizer interpolates the sparse output with a maximum l2 norm of .01 between any two commanded robot positions. On every frame, one of the actions in the plan is removed from the plan and sent to the robot (lines 66,67).

../_images/isaac_sim_rrt_basic_target.gif

7.3.1. World State

A PathPlanner isn’t worth much without being able to avoid obstacles. Objects from omni.isaac.core can be passed to RRT before generating a plan. These obstacles are assumed to remain static during the planning process.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
    from omni.isaac.kit import SimulationApp

    simulation_app = SimulationApp({"headless": False})

    from omni.isaac.franka.tasks import FollowTarget
    from omni.isaac.motion_generation.lula import RRT
    from omni.isaac.motion_generation import PathPlannerVisualizer
    from omni.isaac.core import World
    from omni.isaac.core import objects

    from omni.isaac.core.utils.extensions import get_extension_path_from_name
    import os
    import numpy as np

    my_world = World(stage_units_in_meters=1.0)
    my_task = FollowTarget(name="follow_target_task")
    my_world.add_task(my_task)
    my_world.reset()
    task_params = my_world.get_task("follow_target_task").get_params()
    franka_name = task_params["robot_name"]["value"]
    target_name = task_params["target_name"]["value"]
    my_franka = my_world.scene.get_object(franka_name)

    # Lula config files for supported robots are stored in the motion_generation extension under
    # "/path_planner_configs" and "motion_policy_configs"
    mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
    rrt_config_dir = os.path.join(mg_extension_path, "path_planner_configs")
    rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")

    # Initialize an RRT object
    rrt = RRT(
        robot_description_path = rmp_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
        urdf_path = rmp_config_dir + "/franka/lula_franka_gen.urdf",
        rrt_config_path = rrt_config_dir + "/franka/rrt/franka_planner_config.yaml",
        end_effector_frame_name = "right_gripper"
    )

    # Use the PathPlannerVisualizer wrapper to generate a trajectory of ArticulationActions
    path_planner_visualizer = PathPlannerVisualizer(my_franka,rrt)

    observations = my_world.get_observations()
    target_pose = observations[target_name]["position"]

    wall_obstacle = objects.cuboid.VisualCuboid("/World/Wall", position = np.array([0,.6,.5]), size = 1.0, scale = np.array([.1,.4,.4]))
    rrt.add_obstacle(wall_obstacle)
    rrt.update_world()

    plan = path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist = .01)

    articulation_controller = my_franka.get_articulation_controller()
    while simulation_app.is_running():
        my_world.step(render=True)
        if my_world.is_playing():
            if my_world.current_time_step_index == 0:
                my_world.reset()

            observations = my_world.get_observations()

            # Check every 60 frames whether the end effector moved
            if my_world.current_time_step_index % 60 == 0:
                curr_target_pose = observations[target_name]["position"]

                # If the end effector moved: replan
                if np.linalg.norm(target_pose-curr_target_pose) > .01:
                    target_pose = curr_target_pose
                    rrt.set_end_effector_target(target_pose)
                    plan = path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist = .01)

            if plan:
                actions = plan.pop(0)
                articulation_controller.apply_action(actions)

    simulation_app.close()

On lines 44-46, we add a wall obstacle to the world and to the RRT algorithm. On line 46, we call RRT.update_world() to cause RRT to query the positions of every known obstacle. This must be called at least once, and should be called again whenever a new plan is generated in a unless the world is known to be static.

../_images/isaac_sim_rrt_obstacle.gif

7.4. Summary

This tutorial reviews using the RRT class in order to generate a collision-free path through an environment from a starting position to a task-space target.

7.4.1. Further Learning

To understand the motivation behind the structure and usage of RRT in Omniverse Isaac Sim, reference the Motion Generation page.