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
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.
Generating a Path Using an RRT Instance
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.05
3max_iterations: 50000
4max_sampling: 10000
5distance_metric_weights: [3.0, 2.0, 2.0, 1.5, 1.5, 1.0, 1.0]
6task_space_frame_name: "panda_rightfingertip"
7task_space_limits: [[0.0, 0.7], [-0.6, 0.6], [0.0, 0.8]]
8cuda_tree_params:
9 max_num_nodes: 10000
10 max_buffer_size: 30
11 num_nodes_cpu_gpu_crossover: 3000
12c_space_planning_params:
13 exploration_fraction: 0.5
14task_space_planning_params:
15 translation_target_zone_tolerance: 0.05
16 orientation_target_zone_tolerance: 0.09
17 translation_target_final_tolerance: 1e-4
18 orientation_target_final_tolerance: 0.005
19 translation_gradient_weight: 1.0
20 orientation_gradient_weight: 0.125
21 nn_translation_distance_weight: 1.0
22 nn_orientation_distance_weight: 0.125
23 task_space_exploitation_fraction: 0.4
24 task_space_exploration_fraction: 0.1
25 max_extension_substeps_away_from_target: 6
26 max_extension_substeps_near_target: 50
27 extension_substep_target_region_scale_factor: 2.0
28 unexploited_nodes_culling_scalar: 1.0
29 gradient_substep_size: 0.025
The reader may reference the docstring to the function RRT.set_param() in our API Documentation for a description of each parameter.
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). In this example, the planner does not attempt to plan to the same target multiple times if a failure is encountered. The returned plan will be None and no actions will be taken.
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, quats_to_rot_matrices
11from omni.isaac.core.utils.distance_metrics import rotational_distance_angle
12
13from omni.isaac.motion_generation import PathPlannerVisualizer
14from omni.isaac.motion_generation.lula import RRT
15from omni.isaac.motion_generation import interface_config_loader
16
17class FrankaRrtExample():
18 def __init__(self):
19 self._rrt = None
20 self._path_planner_visualizer = None
21 self._plan = []
22
23 self._articulation = None
24 self._target = None
25 self._target_position = None
26
27 self._frame_counter = 0
28
29 def load_example_assets(self):
30 # Add the Franka and target to the stage
31 # The position in which things are loaded is also the position in which they
32
33 robot_prim_path = "/panda"
34 path_to_robot_usd = get_assets_root_path() + "/Isaac/Robots/Franka/franka.usd"
35
36 add_reference_to_stage(path_to_robot_usd, robot_prim_path)
37 self._articulation = Articulation(robot_prim_path)
38
39 add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", "/World/target")
40 self._target = XFormPrim("/World/target", scale=[.04,.04,.04])
41 self._target.set_default_state(np.array([.45, .5, .7]),euler_angles_to_quats([3*np.pi/4, 0, np.pi]))
42
43 self._obstacle = VisualCuboid("/World/Wall", position = np.array([.3,.6,.6]), size = 1.0, scale = np.array([.1,.4,.4]))
44
45 # Return assets that were added to the stage so that they can be registered with the core.World
46 return self._articulation, self._target
47
48 def setup(self):
49 # Lula config files for supported robots are stored in the motion_generation extension under
50 # "/path_planner_configs" and "/motion_policy_configs"
51 mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
52 rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")
53 rrt_config_dir = os.path.join(mg_extension_path, "path_planner_configs")
54
55 #Initialize an RRT object
56 self._rrt = RRT(
57 robot_description_path = rmp_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
58 urdf_path = rmp_config_dir + "/franka/lula_franka_gen.urdf",
59 rrt_config_path = rrt_config_dir + "/franka/rrt/franka_planner_config.yaml",
60 end_effector_frame_name = "right_gripper"
61 )
62
63 # RRT for supported robots can also be loaded with a simpler equivalent:
64 # rrt_config = interface_config_loader.load_supported_path_planner_config("Franka", "RRT")
65 # self._rrt = RRT(**rrt_confg)
66
67 self._rrt.add_obstacle(self._obstacle)
68
69 # Set the maximum number of iterations of RRT to prevent it from blocking Isaac Sim for
70 # too long.
71 self._rrt.set_max_iterations(5000)
72
73 # Use the PathPlannerVisualizer wrapper to generate a trajectory of ArticulationActions
74 self._path_planner_visualizer = PathPlannerVisualizer(self._articulation, self._rrt)
75
76 self.reset()
77
78 def update(self, step: float):
79 current_target_translation, current_target_orientation = self._target.get_world_pose()
80 current_target_rotation = quats_to_rot_matrices(current_target_orientation)
81
82 translation_distance = np.linalg.norm(self._target_translation - current_target_translation)
83 rotation_distance = rotational_distance_angle(current_target_rotation, self._target_rotation)
84 target_moved = translation_distance > 0.01 or rotation_distance > 0.01
85
86 if (self._frame_counter % 60 == 0 and target_moved):
87 # Replan every 60 frames if the target has moved
88 self._rrt.set_end_effector_target(current_target_translation, current_target_orientation)
89 self._rrt.update_world()
90 self._plan = self._path_planner_visualizer.compute_plan_as_articulation_actions(max_cspace_dist=.01)
91
92 self._target_translation = current_target_translation
93 self._target_rotation = current_target_rotation
94
95 if self._plan:
96 action = self._plan.pop(0)
97 self._articulation.apply_action(action)
98
99 self._frame_counter += 1
100
101 def reset(self):
102 self._target_translation = np.zeros(3)
103 self._target_rotation = np.eye(3)
104 self._frame_counter = 0
105 self._plan = []
The RRT class is initialized on lines 51-61. For supported robots, this can be simplified as on lines 63-65. RRT is made aware of the obstacle it needs to watch on line 67. Any time RRT.update_world() is called (line 89), 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 74).
In this example, RRT replans every second if the target has been moved. The replanning is performed on lines 88-90. 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 92-93).
Current Limitations
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.
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.