6.3. Lula RRT
6.3.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.
6.3.2. Getting Started
Prerequisites
Please complete the Adding a Manipulator Robot tutorial prior to beginning this tutorial.
You may reference the Lula Robot Description Editor to understand how to generate your own robot_description.yaml file to be able to use RRT on unsupported robots.
Review the Loaded Scenario Extension Template to understand how this tutorial is structured and run.
In order to follow along with the tutorial, you may download a standalone RRT example extension here: RRT Tutorial.
The provided zip file contains a fully functional example of RRT being used to plan to a task-space target.
This tutorial explains the contents of the file /RRT_Example_python/scenario.py, which contains all RRT related code.
6.3.3. Generating a Path Using an RRT Instance
6.3.3.1. Required Configuration Files
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.
One of the required files contains parameters for the RRT algorithm specifically, and is not shared with any other Lula algorithms. This tutorial loads the following RRT config file for the Franka robot:
1seed: 123456
2step_size: 0.1
3max_iterations: 4000
4max_sampling: 10000
5distance_metric_weights: [3.0, 2.0, 2.0, 1.5, 1.5, 1.0, 1.0] # One param per robot DOF
6task_space_frame_name: "right_gripper"
7task_space_limits: [[-0.8, 0.9], [-0.8, 0.8], [0.0, 1.2]]
8c_space_planning_params:
9 exploration_fraction: 0.5
10task_space_planning_params:
11 x_target_zone_tolerance: [0.01, 0.01, 0.01]
12 x_target_final_tolerance: 1e-5
13 task_space_exploitation_fraction: 0.4
14 task_space_exploration_fraction: 0.1
The reader may reference the docstring to the function RRT.set_param() in our API Documentation for a description of each parameter.
6.3.3.2. RRT Example
The file /RRT_Example_python/scenario.py loads the Franka robot and uses RRT to move it around obstacles to a target. Every 60 frames, the planner replans to move to the current target position (if possible).
1import numpy as np
2import os
3
4from omni.isaac.core.utils.extensions import get_extension_path_from_name
5from omni.isaac.core.utils.stage import add_reference_to_stage
6from omni.isaac.core.articulations import Articulation
7from omni.isaac.core.utils.nucleus import get_assets_root_path
8from omni.isaac.core.objects.cuboid import VisualCuboid
9from omni.isaac.core.prims import XFormPrim
10from omni.isaac.core.utils.numpy.rotations import euler_angles_to_quats
11
12from omni.isaac.motion_generation import PathPlannerVisualizer
13from omni.isaac.motion_generation.lula import RRT
14from omni.isaac.motion_generation import interface_config_loader
15
16class FrankaRrtExample():
17 def __init__(self):
18 self._rrt = None
19 self._path_planner_visualizer = None
20 self._plan = []
21
22 self._articulation = None
23 self._target = None
24 self._target_position = None
25
26 self._frame_counter = 0
27
28 def load_example_assets(self):
29 # Add the Franka and target to the stage
30 # The position in which things are loaded is also the position in which they
31
32 robot_prim_path = "/panda"
33 path_to_robot_usd = get_assets_root_path() + "/Isaac/Robots/Franka/franka.usd"
34
35 add_reference_to_stage(path_to_robot_usd, robot_prim_path)
36 self._articulation = Articulation(robot_prim_path)
37
38 add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", "/World/target")
39 self._target = XFormPrim("/World/target", scale=[.04,.04,.04])
40 self._target.set_default_state(np.array([0,.5,.7]),euler_angles_to_quats([0,np.pi,0]))
41
42 self._obstacle = VisualCuboid("/World/Wall", position = np.array([.45,.6,.5]), size = 1.0, scale = np.array([.1,.4,.4]))
43
44 # Return assets that were added to the stage so that they can be registered with the core.World
45 return self._articulation, self._target
46
47 def setup(self):
48 # Lula config files for supported robots are stored in the motion_generation extension under
49 # "/path_planner_configs" and "/motion_policy_configs"
50 mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
51 rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")
52 rrt_config_dir = os.path.join(mg_extension_path, "path_planner_configs")
53
54 #Initialize an RRT object
55 self._rrt = RRT(
56 robot_description_path = rmp_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
57 urdf_path = rmp_config_dir + "/franka/lula_franka_gen.urdf",
58 rrt_config_path = rrt_config_dir + "/franka/rrt/franka_planner_config.yaml",
59 end_effector_frame_name = "right_gripper"
60 )
61
62 # RRT for supported robots can also be loaded with a simpler equivalent:
63 # rrt_config = interface_config_loader.load_supported_path_planner_config("Franka", "RRT")
64 # self._rrt = RRT(**rrt_confg)
65
66 self._rrt.add_obstacle(self._obstacle)
67
68 # Use the PathPlannerVisualizer wrapper to generate a trajectory of ArticulationActions
69 self._path_planner_visualizer = PathPlannerVisualizer(self._articulation, self._rrt)
70
71 self.reset()
72
73 def update(self, step: float):
74 current_target_position, _ = self._target.get_world_pose()
75
76 if self._frame_counter % 60 == 0 and np.linalg.norm(self._target_position-current_target_position) > .01:
77 # Replan every 60 frames if the target has moved
78 self._rrt.set_end_effector_target(current_target_position)
79 self._rrt.update_world()
80 self._plan = self._path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist=.01)
81
82 self._target_position = current_target_position
83
84 if self._plan:
85 action = self._plan.pop(0)
86 self._articulation.apply_action(action)
87
88 self._frame_counter += 1
89
90 def reset(self):
91 self._target_position = np.zeros(3)
92 self._frame_counter = 0
93 self._plan = []
The RRT class is initialized on lines 50-60. For supported robots, this can be simplified as on lines 62-64. RRT is made aware of the obstacle it needs to watch on line 66. Any time RRT.update_world() is called (line 79), it will query the current position of watched obstacles.
RRT outputs sparse plans that, when linearly interpolated, form a collision-free path to the goal position. 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 (line 69).
In this example, RRT replans every second if the target has been moved. The replanning is performed on lines 78-80. First, RRT is informed of the new target position, then it is told to query the position of watched obstacles. Finally, the path_planner_visualizer wrapping RRT is used to generate a plan in the form of a list of ArticulationAction. 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 85-86).
6.3.4. Current Limitations
6.3.4.1. Full Pose Targets
Lula’s RRT implementation currently supports translation-only task-space targets and c-space targets. In order to reach a full-pose target, the user may combine inverse kinematics with the RRT planner in order to generate a c-space target that reaches the desired robot pose. This would require swapping RRT.set_end_effector_target() with RRT.set_cspace_target(). This method is not directly supported as a future release will include support for full-pose RRT targets.
6.3.4.2. Following a Plan with Exactness
The PathPlannerVisualizer class is called a “Visualizer” because it is only meant to give a visualization of an output plan, but it is not likely to be useful beyond this. By densely linearly interpolating an RRT plan, the resulting trajectory is far from time-optimal or smooth. In order to follow a plan in a more theoretically sound way, the output of RRT may be combined with the LulaTrajectoryGenerator. This is demonstrated in the Omniverse Isaac Sim Path Planning Example in the “Isaac Examples” menu.
6.3.5. 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.
6.3.5.1. Further Learning
To understand the motivation behind the structure and usage of RRT in Omniverse Isaac Sim, reference the Motion Generation page.