Lula RMPflow#

Learning Objectives#

This tutorial shows how the RMPflow class in the Motion Generation may be used to generate smooth motions to reach task-space targets while avoiding dynamic obstacles. This tutorial first shows how RmpFlow can be directly instantiated and used to generate motions, then shows how RmpFlow can be loaded and used simply on supported robots , and finally, demonstrates how built-in debugging features can improve easy of use and integration.

Getting Started#

Prerequisites

In order to follow along with the tutorial, you may download a standalone RMPflow example extension here: RMPflow Tutorial. The provided zip file provides a fully functional example of RMPflow including following a target, world awareness, and a debugging option. The sections of this tutorial build up the file scenario.py from basic functionality to the completed code. To follow along with this tutorial, you may download the provided extension and replace the contents of /RmpFlow_Example_python/scenario.py with the code snippets provided.

Generating Motions with an RMPflow Instance#

RMPflow is used heavily throughout Omniverse Isaac Sim for controlling robot manipulators. As documented in RMPflow Configuration, there are three configuration files needed to directly instantiate the RmpFlow class directly. Once these configuration files are loaded and an end effector target has been specified, actions can be computed to move the robot to the desired target.

 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.prims import XFormPrim
 9from omni.isaac.core.utils.numpy.rotations import euler_angles_to_quats
10
11from omni.isaac.motion_generation import RmpFlow, ArticulationMotionPolicy
12
13class FrankaRmpFlowExample():
14    def __init__(self):
15        self._rmpflow = None
16        self._articulation_rmpflow = None
17
18        self._articulation = None
19        self._target = None
20
21    def load_example_assets(self):
22        # Add the Franka and target to the stage
23        # The position in which things are loaded is also the position in which they
24
25        robot_prim_path = "/panda"
26        path_to_robot_usd = get_assets_root_path() + "/Isaac/Robots/Franka/franka.usd"
27
28        add_reference_to_stage(path_to_robot_usd, robot_prim_path)
29        self._articulation = Articulation(robot_prim_path)
30
31        add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", "/World/target")
32        self._target = XFormPrim("/World/target", scale=[.04,.04,.04])
33
34        # Return assets that were added to the stage so that they can be registered with the core.World
35        return self._articulation, self._target
36
37    def setup(self):
38        # RMPflow config files for supported robots are stored in the motion_generation extension under "/motion_policy_configs"
39        mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
40        rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")
41
42        #Initialize an RmpFlow object
43        self._rmpflow = RmpFlow(
44            robot_description_path = rmp_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
45            urdf_path = rmp_config_dir + "/franka/lula_franka_gen.urdf",
46            rmpflow_config_path = rmp_config_dir + "/franka/rmpflow/franka_rmpflow_common.yaml",
47            end_effector_frame_name = "right_gripper",
48            maximum_substep_size = 0.00334
49        )
50
51        #Use the ArticulationMotionPolicy wrapper object to connect rmpflow to the Franka robot articulation.
52        self._articulation_rmpflow = ArticulationMotionPolicy(self._articulation,self._rmpflow)
53
54        self._target.set_world_pose(np.array([.5,0,.7]),euler_angles_to_quats([0,np.pi,0]))
55
56    def update(self, step: float):
57        # Step is the time elapsed on this frame
58        target_position, target_orientation = self._target.get_world_pose()
59
60        self._rmpflow.set_end_effector_target(
61            target_position, target_orientation
62        )
63
64        action = self._articulation_rmpflow.get_next_articulation_action(step)
65        self._articulation.apply_action(action)
66
67    def reset(self):
68        # Rmpflow is stateless unless it is explicitly told not to be
69
70        self._target.set_world_pose(np.array([.5,0,.7]),euler_angles_to_quats([0,np.pi,0]))

RMPflow is an implementation of the Motion Policy Algorithm interface. Any MotionPolicy can be passed to an Articulation Motion Policy in order to start moving a robot on the USD stage. On line 43, an instance of RmpFlow is instantiated with the required configuration information. The ArticulationMotionPolicy created on line 52 acts as a translational layer between RmpFlow and the simulated Franka robot Articulation. The user may interact with RmpFlow directly to communicate the world state, set an end effector target, or modify internal settings. On each frame, an end effector target is passed directly to the RmpFlow object (line 60). The ArticulationMotionPolicy is used on line 64 to compute an action that can be directly consumed by the Franka Articulation.

Using RMPflow to follow a target.

World State#

As a Motion Policy Algorithm, RmpFlow is capable of dynamic collision avoidance while navigating the end effector to a target. The world state may be changing over time while RmpFlow is navigating to its target. Objects created with the omni.isaac.core.objects package (see Inputs: World State) can be registered with RmpFlow and the policy will automatically avoid collisions with these obstacles. RmpFlow is triggered to query the current state of all tracked objects whenever RmpFlow.update_world() is called.

RmpFlow may also be informed about a change in the robot base pose on a given frame by calling RmpFlow.set_robot_base_pose(). As object positions are queried in world coordinates, it is critical to use this function if the base of the robot is moved within the USD stage.

 1class FrankaRmpFlowExample():
 2    def __init__(self):
 3        self._rmpflow = None
 4        self._articulation_rmpflow = None
 5
 6        self._articulation = None
 7        self._target = None
 8
 9    def load_example_assets(self):
10        # Add the Franka and target to the stage
11        # The position in which things are loaded is also the position in which they
12
13        robot_prim_path = "/panda"
14        path_to_robot_usd = get_assets_root_path() + "/Isaac/Robots/Franka/franka.usd"
15
16        add_reference_to_stage(path_to_robot_usd, robot_prim_path)
17        self._articulation = Articulation(robot_prim_path)
18
19        add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", "/World/target")
20        self._target = XFormPrim("/World/target", scale=[.04,.04,.04])
21
22        self._obstacle = FixedCuboid("/World/obstacle",size=.05,position=np.array([0.4, 0.0, 0.65]),color=np.array([0.,0.,1.]))
23
24        # Return assets that were added to the stage so that they can be registered with the core.World
25        return self._articulation, self._target, self._obstacle
26
27    def setup(self):
28        # RMPflow config files for supported robots are stored in the motion_generation extension under "/motion_policy_configs"
29        mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
30        rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")
31
32        #Initialize an RmpFlow object
33        self._rmpflow = RmpFlow(
34            robot_description_path = rmp_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
35            urdf_path = rmp_config_dir + "/franka/lula_franka_gen.urdf",
36            rmpflow_config_path = rmp_config_dir + "/franka/rmpflow/franka_rmpflow_common.yaml",
37            end_effector_frame_name = "right_gripper",
38            maximum_substep_size = 0.00334
39        )
40        self._rmpflow.add_obstacle(self._obstacle)
41
42        #Use the ArticulationMotionPolicy wrapper object to connect rmpflow to the Franka robot articulation.
43        self._articulation_rmpflow = ArticulationMotionPolicy(self._articulation,self._rmpflow)
44
45        self._target.set_world_pose(np.array([.5,0,.7]),euler_angles_to_quats([0,np.pi,0]))
46
47    def update(self, step: float):
48        # Step is the time elapsed on this frame
49        target_position, target_orientation = self._target.get_world_pose()
50
51        self._rmpflow.set_end_effector_target(
52            target_position, target_orientation
53        )
54
55        # Track any movements of the cube obstacle
56        self._rmpflow.update_world()
57
58        #Track any movements of the robot base
59        robot_base_translation,robot_base_orientation = self._articulation.get_world_pose()
60        self._rmpflow.set_robot_base_pose(robot_base_translation,robot_base_orientation)
61
62        action = self._articulation_rmpflow.get_next_articulation_action(step)
63        self._articulation.apply_action(action)
64
65    def reset(self):
66        # Rmpflow is stateless unless it is explicitly told not to be
67
68        self._target.set_world_pose(np.array([.5,0,.7]),euler_angles_to_quats([0,np.pi,0]))

On lines 22, an obstacle is added to the stage, and on line 40, it is registered as an obstacle with RmpFlow. On each frame, RmpFlow.update_world() is called (line 56). This triggers RmpFlow to query the current position of the cube to account for any movement.

On lines 59-60, the current position of the robot base is queried and passed to RmpFlow. This step is separated from other world state because it is often unnecessary (when the robot base never moves from the origin), or this step may require extra consideration (e.g. RmpFlow is controlling an arm that is mounted on a moving base).

Adding world awareness to RMPflow.

Loading RMPflow for Supported Robots#

In the previous sections, we see that RmpFlow requires five arguments to be initialized. Three of these arguments are file paths to required configuration files. The end_effector_frame_name argument specifies what frame on the robot (from the frames found in the referenced URDF file) should be considered the end effector. The maximum_substep_size argument specifies a maximum step-size when internally performing Euler Integration.

For manipulators in the Omniverse Isaac Sim library, appropriate config information for loading RmpFlow can be found in the omni.isaac.motion_generation extension. This information is indexed by robot name, and can be accessed simply. The following change shows how loading configs for supported robots can be simplified.

 1from omni.isaac.motion_generation.interface_config_loader import (
 2    get_supported_robot_policy_pairs,
 3    load_supported_motion_policy_config,
 4)
 5
 6class FrankaRmpFlowExample():
 7    def __init__(self):
 8        self._rmpflow = None
 9        self._articulation_rmpflow = None
10
11        self._articulation = None
12        self._target = None
13
14    def load_example_assets(self):
15        # Add the Franka and target to the stage
16        # The position in which things are loaded is also the position in which they
17
18        robot_prim_path = "/panda"
19        path_to_robot_usd = get_assets_root_path() + "/Isaac/Robots/Franka/franka.usd"
20
21        add_reference_to_stage(path_to_robot_usd, robot_prim_path)
22        self._articulation = Articulation(robot_prim_path)
23
24        add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", "/World/target")
25        self._target = XFormPrim("/World/target", scale=[.04,.04,.04])
26
27        self._obstacle = FixedCuboid("/World/obstacle",size=.05,position=np.array([0.4, 0.0, 0.65]),color=np.array([0.,0.,1.]))
28
29        # Return assets that were added to the stage so that they can be registered with the core.World
30        return self._articulation, self._target, self._obstacle
31
32    def setup(self):
33        # Loading RMPflow can be done quickly for supported robots
34        print("Supported Robots with a Provided RMPflow Config:", list(get_supported_robot_policy_pairs().keys()))
35        rmp_config = load_supported_motion_policy_config("Franka","RMPflow")
36
37        #Initialize an RmpFlow object
38        self._rmpflow = RmpFlow(**rmp_config)
39        self._rmpflow.add_obstacle(self._obstacle)
40
41        #Use the ArticulationMotionPolicy wrapper object to connect rmpflow to the Franka robot articulation.
42        self._articulation_rmpflow = ArticulationMotionPolicy(self._articulation,self._rmpflow)
43
44        self._target.set_world_pose(np.array([.5,0,.7]),euler_angles_to_quats([0,np.pi,0]))
45
46    def update(self, step: float):
47        # Step is the time elapsed on this frame
48        target_position, target_orientation = self._target.get_world_pose()
49
50        self._rmpflow.set_end_effector_target(
51            target_position, target_orientation
52        )
53
54        # Track any movements of the cube obstacle
55        self._rmpflow.update_world()
56
57        #Track any movements of the robot base
58        robot_base_translation,robot_base_orientation = self._articulation.get_world_pose()
59        self._rmpflow.set_robot_base_pose(robot_base_translation,robot_base_orientation)
60
61        action = self._articulation_rmpflow.get_next_articulation_action(step)
62        self._articulation.apply_action(action)
63
64    def reset(self):
65        # Rmpflow is stateless unless it is explicitly told not to be
66
67        self._target.set_world_pose(np.array([.5,0,.7]),euler_angles_to_quats([0,np.pi,0]))

A supported set of robots can have their RMPflow configs loaded by name. Line 34 prints the names of every supported robot with a provided RMPflow config (at the time of writing this tutorial): [‘Franka’, ‘UR3’, ‘UR3e’, ‘UR5’, ‘UR5e’, ‘UR10’, ‘UR10e’, ‘UR16e’, ‘Rizon4’, ‘Cobotta_Pro_900’, ‘Cobotta_Pro_1300’, ‘RS007L’, ‘RS007N’, ‘RS013N’, ‘RS025N’, ‘RS080N’, ‘FestoCobot’, ‘Techman_TM12’, ‘Kuka_KR210’, ‘Fanuc_CRX10IAL’]

On lines 35,38, the RmpFlow class initializer is simplified to unpacking a dictionary of loaded keyword arguments. The load_supported_motion_policy_config() function is currently the simplest way to load supported robots. In the future, Isaac Sim will provide a centralized registry of robots with Lula robot description files and RMP configuration files stored alongside the robot USD.

Debugging Features#

The RmpFlow class has contains debugging features that are not generally available in the Motion Policy Algorithm interface. These debugging features make it simple to decouple the simulator from the RmpFlow algorithm to help diagnose any undesirable behaviors that are encountered (RMPflow Debugging Features).

RmpFlow uses collision spheres internally to avoid collisions with external objects. These spheres can be visualized with the RmpFlow.visualize_collision_spheres() function. This helps to determine whether RmpFlow has a reasonable representation of the simulated robot.

The visualization can be used alongside a flag RmpFlow.set_ignore_state_updates(True) to ignore state updates from the robot Articulation and instead assume that robot joint targets returned by RmpFlow are always perfectly achieved. This causes RmpFlow to compute a robot path over time that is independent of the simulated robot Articulation. At each timestep, RmpFlow returns joint targets that are passed to the robot Articulation.

 1class FrankaRmpFlowExample():
 2    def __init__(self):
 3        self._rmpflow = None
 4        self._articulation_rmpflow = None
 5
 6        self._articulation = None
 7        self._target = None
 8
 9        self._dbg_mode = True
10
11    def load_example_assets(self):
12        # Add the Franka and target to the stage
13        # The position in which things are loaded is also the position in which they
14
15        robot_prim_path = "/panda"
16        path_to_robot_usd = get_assets_root_path() + "/Isaac/Robots/Franka/franka.usd"
17
18        add_reference_to_stage(path_to_robot_usd, robot_prim_path)
19        self._articulation = Articulation(robot_prim_path)
20
21        add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", "/World/target")
22        self._target = XFormPrim("/World/target", scale=[.04,.04,.04])
23
24        self._obstacle = FixedCuboid("/World/obstacle",size=.05,position=np.array([0.4, 0.0, 0.65]),color=np.array([0.,0.,1.]))
25
26        # Return assets that were added to the stage so that they can be registered with the core.World
27        return self._articulation, self._target, self._obstacle
28
29    def setup(self):
30        # Loading RMPflow can be done quickly for supported robots
31        print("Supported Robots with a Provided RMPflow Config:", list(get_supported_robot_policy_pairs().keys()))
32        rmp_config = load_supported_motion_policy_config("Franka","RMPflow")
33
34        #Initialize an RmpFlow object
35        self._rmpflow = RmpFlow(**rmp_config)
36        self._rmpflow.add_obstacle(self._obstacle)
37
38        if self._dbg_mode:
39            self._rmpflow.set_ignore_state_updates(True)
40            self._rmpflow.visualize_collision_spheres()
41
42            # Set the robot gains to be deliberately poor
43            bad_proportional_gains = self._articulation.get_articulation_controller().get_gains()[0]/50
44            self._articulation.get_articulation_controller().set_gains(kps = bad_proportional_gains)
45
46        #Use the ArticulationMotionPolicy wrapper object to connect rmpflow to the Franka robot articulation.
47        self._articulation_rmpflow = ArticulationMotionPolicy(self._articulation,self._rmpflow)
48
49        self._target.set_world_pose(np.array([.5,0,.7]),euler_angles_to_quats([0,np.pi,0]))
50
51    def update(self, step: float):
52        # Step is the time elapsed on this frame
53        target_position, target_orientation = self._target.get_world_pose()
54
55        self._rmpflow.set_end_effector_target(
56            target_position, target_orientation
57        )
58
59        # Track any movements of the cube obstacle
60        self._rmpflow.update_world()
61
62        #Track any movements of the robot base
63        robot_base_translation,robot_base_orientation = self._articulation.get_world_pose()
64        self._rmpflow.set_robot_base_pose(robot_base_translation,robot_base_orientation)
65
66        action = self._articulation_rmpflow.get_next_articulation_action(step)
67        self._articulation.apply_action(action)
68
69    def reset(self):
70        # Rmpflow is stateless unless it is explicitly told not to be
71        if self._dbg_mode:
72            # RMPflow was set to roll out robot state internally, assuming that all returned joint targets were hit exactly.
73            self._rmpflow.reset()
74            self._rmpflow.visualize_collision_spheres()
75
76        self._target.set_world_pose(np.array([.5,0,.7]),euler_angles_to_quats([0,np.pi,0]))

The collision sphere visualization can be very helpful to distinguish between behaviors that are coming from the simulator, and behaviors that are coming from RmpFlow. In the video below, the franka robot is given weak proportional gains (lines 43-44). Using the debugging visualization, it is easy to determine that RmpFlow is producing reasonable motions, but the simulated robot is simply not able to follow the motions. When RMPflow moves the robot quickly, the Franka robot Articulation lags significantly behind the commanded position.

../_images/isaac_sim_rmpflow_tutorial_3.gif

Summary#

This tutorial reviews using the RmpFlow class in order to generate reactive motions in response to a dynamic environment. The RmpFlow class can be used to generate motions directly alongside an Articulation Motion Policy.

This tutorial reviewed four of the main features of RmpFlow:

  1. Navigating the robot through an environment to a target position and orientation.

  2. Adapting to a dynamic world on every frame.

  3. Adapting to a change in the robot’s position on the USD stage.

  4. Using visualization to decouple the simulated robot Articulation from the RMPflow algorithm for quick and easy debugging.

Further Learning#

To learn how to configure RMPflow for a new robot, first review the basic formalism, and then read the RMPflow tuning guide for practical advice.

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

On this page