Lula RRT

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.

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.

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.

 1from omni.isaac.kit import SimulationApp
 2
 3    simulation_app = SimulationApp({"headless": False})
 4
 5    from omni.isaac.franka.tasks import FollowTarget
 6    from omni.isaac.motion_generation.lula import RRT
 7    from omni.isaac.motion_generation import PathPlannerVisualizer
 8    from omni.isaac.core import World
 9    from omni.isaac.core import objects
10
11    from omni.isaac.core.utils.extensions import get_extension_path_from_name
12    import os
13    import numpy as np
14
15    my_world = World(stage_units_in_meters=1.0)
16    my_task = FollowTarget(name="follow_target_task")
17    my_world.add_task(my_task)
18    my_world.reset()
19    task_params = my_world.get_task("follow_target_task").get_params()
20    franka_name = task_params["robot_name"]["value"]
21    target_name = task_params["target_name"]["value"]
22    my_franka = my_world.scene.get_object(franka_name)
23
24    # Lula config files for supported robots are stored in the motion_generation extension under
25    # "/path_planner_configs" and "motion_policy_configs"
26    mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
27    rrt_config_dir = os.path.join(mg_extension_path, "path_planner_configs")
28    rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")
29
30    # Initialize an RRT object
31    rrt = RRT(
32        robot_description_path = rmp_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
33        urdf_path = rmp_config_dir + "/franka/lula_franka_gen.urdf",
34        rrt_config_path = rrt_config_dir + "/franka/rrt/franka_planner_config.yaml",
35        end_effector_frame_name = "right_gripper"
36    )
37
38    # Use the PathPlannerVisualizer wrapper to generate a trajectory of ArticulationActions
39    path_planner_visualizer = PathPlannerVisualizer(my_franka,rrt)
40
41    observations = my_world.get_observations()
42    target_pose = observations[target_name]["position"]
43
44    plan = path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist = .01)
45
46    articulation_controller = my_franka.get_articulation_controller()
47    while simulation_app.is_running():
48        my_world.step(render=True)
49        if my_world.is_playing():
50            if my_world.current_time_step_index == 0:
51                my_world.reset()
52
53            observations = my_world.get_observations()
54
55            # Check every 60 frames whether the end effector moved
56            if my_world.current_time_step_index % 60 == 0:
57                curr_target_pose = observations[target_name]["position"]
58
59                # If the end effector moved: replan
60                if np.linalg.norm(target_pose-curr_target_pose) > .01:
61                    target_pose = curr_target_pose
62                    rrt.set_end_effector_target(target_pose)
63                    plan = path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist = .01)
64
65            if plan:
66                actions = plan.pop(0)
67                articulation_controller.apply_action(actions)
68
69    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

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    from omni.isaac.kit import SimulationApp
 2
 3    simulation_app = SimulationApp({"headless": False})
 4
 5    from omni.isaac.franka.tasks import FollowTarget
 6    from omni.isaac.motion_generation.lula import RRT
 7    from omni.isaac.motion_generation import PathPlannerVisualizer
 8    from omni.isaac.core import World
 9    from omni.isaac.core import objects
10
11    from omni.isaac.core.utils.extensions import get_extension_path_from_name
12    import os
13    import numpy as np
14
15    my_world = World(stage_units_in_meters=1.0)
16    my_task = FollowTarget(name="follow_target_task")
17    my_world.add_task(my_task)
18    my_world.reset()
19    task_params = my_world.get_task("follow_target_task").get_params()
20    franka_name = task_params["robot_name"]["value"]
21    target_name = task_params["target_name"]["value"]
22    my_franka = my_world.scene.get_object(franka_name)
23
24    # Lula config files for supported robots are stored in the motion_generation extension under
25    # "/path_planner_configs" and "motion_policy_configs"
26    mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
27    rrt_config_dir = os.path.join(mg_extension_path, "path_planner_configs")
28    rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")
29
30    # Initialize an RRT object
31    rrt = RRT(
32        robot_description_path = rmp_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
33        urdf_path = rmp_config_dir + "/franka/lula_franka_gen.urdf",
34        rrt_config_path = rrt_config_dir + "/franka/rrt/franka_planner_config.yaml",
35        end_effector_frame_name = "right_gripper"
36    )
37
38    # Use the PathPlannerVisualizer wrapper to generate a trajectory of ArticulationActions
39    path_planner_visualizer = PathPlannerVisualizer(my_franka,rrt)
40
41    observations = my_world.get_observations()
42    target_pose = observations[target_name]["position"]
43
44    wall_obstacle = objects.cuboid.VisualCuboid("/World/Wall", position = np.array([0,.6,.5]), size = 1.0, scale = np.array([.1,.4,.4]))
45    rrt.add_obstacle(wall_obstacle)
46    rrt.update_world()
47
48    plan = path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist = .01)
49
50    articulation_controller = my_franka.get_articulation_controller()
51    while simulation_app.is_running():
52        my_world.step(render=True)
53        if my_world.is_playing():
54            if my_world.current_time_step_index == 0:
55                my_world.reset()
56
57            observations = my_world.get_observations()
58
59            # Check every 60 frames whether the end effector moved
60            if my_world.current_time_step_index % 60 == 0:
61                curr_target_pose = observations[target_name]["position"]
62
63                # If the end effector moved: replan
64                if np.linalg.norm(target_pose-curr_target_pose) > .01:
65                    target_pose = curr_target_pose
66                    rrt.set_end_effector_target(target_pose)
67                    plan = path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist = .01)
68
69            if plan:
70                actions = plan.pop(0)
71                articulation_controller.apply_action(actions)
72
73    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

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.

Further Learning

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