5. Configuring RMPflow for a New Manipulator

5.1. Learning Objectives

In this tutorial, we will see how the RMPflow algorithm may be fully configured using the URDF file of a robot as the starting point.

5.2. Getting Started

Prerequisites

This tutorial provides standalone python scripts that may be run using the python.sh python wrapper discussed in the Standalone Application tutorial. In this tutorial, the python wrapper is refered to simply as python.sh.

This tutorial provides a URDF file and USD file describing the Cobotta Pro 900 robot. The USD file was generated from the URDF using the process discussed in Adding a New Manipulator tutorial.

The standalone scripts used in this tutorial are very similar to the scripts reviewed in the Motion Generation: RMPflow tutorial, which has a more detailed and complete description of writing scripts with RMPflow.

5.3. Template Config Files

There are three files to describe the robot and parameterize the RMPflow algorithm:

  1. A URDF (universal robot description file), used for specifying robot kinematics as well as joint and link names. Position limits for each joint are also required. Other properties in the URDF are ignored and may be omitted; these include masses, moments of inertia, visual and collision meshes, etc.

  2. A supplementary robot description file in YAML format. In addition to enumerating the list of actuated joints that define the configuration space (c-space) for the robot, this file also includes sections for specifying the default c-space configuration as well as sets of collision spheres used for collision avoidance. This file may also be used to specify fixed positions for unactuated joints.

  3. A RMPflow configuration file in YAML format, containing parameters for all enabled RMPs.

This tutorial assumes that the user is starting with a URDF file describing their robot. In this tutorial, template files are provided for the remaining RMPflow configuration and robot description files, which are modified in order to match the Cobotta Pro 900 robot.

The reader may download the following Zip file to follow along with the tutorial:

Configuring_RMPflow_Cobotta_Pro

In this tutorial, each figure is proceeded by the command that was used to generate it from the tutorial zip folder.

5.3.1. Template Robot Description YAML File

The robot description file provides additional information on top of the URDF that is required for the RMPflow algorithm to function (./Template_Config_Files/template_robot_descriptor.yaml in the provided tutorial zip).

 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
# The robot descriptor defines the generalized coordinates and how to map those
# to the underlying URDF dofs.

api_version: 1.0

# Defines the generalized coordinates. Each generalized coordinate is assumed
# to have an entry in the URDF.
# RMPflow will only use these joints to control the robot position.
cspace:
    - joint1
    - joint2
    - joint3
    - joint4
    - joint5
    - joint6
    - joint7

# Global frame of the URDF
root_link: base

# The default cspace position of this robot
default_q: [
    0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00
]

# Most dimensions of the cspace have a direct corresponding element
# in the URDF. This list of rules defines how unspecified coordinates
# should be extracted or how values in the URDF should be overwritten.

cspace_to_urdf_rules:
#     - {name: fixed_joint, rule: fixed, value: 0.025}

# RMPflow uses collision spheres to define the robot geometry in order to avoid
# collisions with external obstacles.  If no spheres are specified, RMPflow will
# not be able to avoid obstacles.
# The example spheres specified are translated along the z axis of the link0 frame by
# .05 and .1 m respectively.

# collision_spheres:
  # - link0:
    #     - "center": [0.0, 0.0, 0.05]
    #       "radius": 0.045
    #     - "center": [0.0, 0.0, 0.1]
    #       "radius": 0.045



# This argument is no longer supported, but is required for legacy reasons.
# There is no need to change it.

composite_task_spaces: []

The cspace field serves a very important purpose. From the perspective of RMPflow, every movable joint in the URDF is a potential degree of freedom (DOF) for the robot, but this does not reflect how the RMPflow algorithm should be used. RMPflow provides a way of navigating the robot to a target position while dynamically avoiding obstacles in a changing world. For a robotic arm with an attached manipulator, only the DOFs in the arm are used to move the manipulator into position. The fingers of the manipulator should not be considered as valid DOFs in the task of moving the end effector into position. Thus, it is necessary to reduce the cspace that is available in the URDF to limit the scope in which RMPflow controls the robot.

The root_link field is taken by RMPflow to be the global frame of the robot. This frame name should match the global frame given in the URDF.

The default_q field gives default joint angles for each of the joints in the robot cspace. If given no target, RMPflow will drive the robot joints towards these default positions. When RMPflow is given a target, it will use the default joint positions to resolve nullspace solutions when navigating to the target i.e. out of the possible cspace positions that reach the target, RMPflow will be driven towards a cspace position that is close to the default position.

The collision_spheres argument is required to allow the RMPflow algorithm to avoid external obstacles, but it is also the most labor intensive to fill in correctly. For now we will ommit this argument, and it will be covered more later in this tutorial.

5.3.2. Template Robot Config YAML File

The RMPflow algorithm has over 50 settable parameters, but these parameters tend to generalize between robots with similar kinematic structures and length scales. The values in the template have been tuned specifically for the Franka Emika Panda, but serve as a good starting point for many 6- and 7-dof robot arms. The template file can be found in the provided tutorial zip at ./Template_Config_Files/template_rmpflow_config.yaml.

 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
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
# Artificially limit the robot joints.  For example:
# A joint with range +-pi would be limited to +-(pi-.01)
joint_limit_buffers: [.01, .01, .01, .01, .01, .01, .01]

# RMPflow has many modifiable parameters, but these serve as a great start.
# Most parameters will not need to be modified
rmp_params:
    cspace_target_rmp:
        metric_scalar: 50.
        position_gain: 100.
        damping_gain: 50.
        robust_position_term_thresh: .5
        inertia: 1.
    cspace_trajectory_rmp:
        p_gain: 100.
        d_gain: 10.
        ff_gain: .25
        weight: 50.
    cspace_affine_rmp:
        final_handover_time_std_dev: .25
        weight: 2000.
    joint_limit_rmp:
        metric_scalar: 1000.
        metric_length_scale: .01
        metric_exploder_eps: 1e-3
        metric_velocity_gate_length_scale: .01
        accel_damper_gain: 200.
        accel_potential_gain: 1.
        accel_potential_exploder_length_scale: .1
        accel_potential_exploder_eps: 1e-2
    joint_velocity_cap_rmp:
        max_velocity: 4.
        velocity_damping_region: 1.5
        damping_gain: 1000.0
        metric_weight: 100.
    target_rmp:
        accel_p_gain: 30.
        accel_d_gain: 85.
        accel_norm_eps: .075
        metric_alpha_length_scale: .05
        min_metric_alpha: .01
        max_metric_scalar: 10000
        min_metric_scalar: 2500
        proximity_metric_boost_scalar: 20.
        proximity_metric_boost_length_scale: .02
        xi_estimator_gate_std_dev: 20000.
        accept_user_weights: false
    axis_target_rmp:
        accel_p_gain: 210.
        accel_d_gain: 60.
        metric_scalar: 10
        proximity_metric_boost_scalar: 3000.
        proximity_metric_boost_length_scale: .08
        xi_estimator_gate_std_dev: 20000.
        accept_user_weights: false
    collision_rmp:
        damping_gain: 50.
        damping_std_dev: .04
        damping_robustness_eps: 1e-2
        damping_velocity_gate_length_scale: .01
        repulsion_gain: 800.
        repulsion_std_dev: .01
        metric_modulation_radius: .5
        metric_scalar: 10000.
        metric_exploder_std_dev: .02
        metric_exploder_eps: .001
    damping_rmp:
        accel_d_gain: 30.
        metric_scalar: 50.
        inertia: 100.

canonical_resolve:
    max_acceleration_norm: 50.
    projection_tolerance: .01
    verbose: false


# body_cylinders are used to promote self-collision avoidance between the robot and its base
# The example below defines the robot base to be a capsule defined by the absolute coordinates pt1 and pt2.
# The semantic name provided for each body_cylinder does not need to be present in the robot URDF.
body_cylinders:
     - name: base
       pt1: [0,0,.333]
       pt2: [0,0,0.]
       radius: .05


# body_collision_controllers defines spheres located at specified frames in the robot URDF
# These spheres will not be allowed to collide with the capsules enumerated under body_cylinders
# By design, most frames in industrial robots are kinematically unable to collide with the robot base.
# It is often only necessary to define body_collision_controllers near the end effector
body_collision_controllers:
     - name: end_effector
       radius: .05

This tutorial focuses on three fields in this file: joint_limit_buffers, body_cylinders, and body_collision_controllers.

joint_limit_buffers introduces artificial joint limits around the the joint limits stated in the robot URDF. The shape of the provided joint_limit_buffers must match the cspace given in the robot_description.yaml file. Imagining that our template robot has seven revolute joints, The given buffers of .01 on the 7 cspace joints mean that RMPflow will drive the robot up to .01 radians from the joint limits given in the robot URDF. If the robot has prismatic joints, a value of .01 would be expressed implicitly in meters.

body_cylinders and body_collision_controllers help RMPflow to avoid self-collision between the end effector and the robot base. body_cylinders define an imagined robot base via a set of capsules. body_collision_controllers define collision spheres placed on different frames of the robot URDF. The template code above defines an unmoving capsule in absolute coordinates and a sphere centered around the “end_effector” frame in the robot URDF. RMPflow will not allow a collision between the sphere and capsule. Apart from preventing the end effector from colliding with the base, RMPflow does not directly avoid self-collisions based on collision geometry. For most applications, however, joint limits are sufficient to prevent links in the middle of the kinematic chain from colliding with each other.

5.4. Modifying our Template for the Cobotta Pro 900

The link and joint names in the provided template files are generic, and they must be appropriately renamed to match specific frames in the robot URDF. In this tutorial, we start with the URDF for the Cobotta Pro 900 arm and appropriately modify the template files to reach complete RMPflow functionality.

5.4.1. Doing the Bare Minimum

First we show the minimum changes required to get our Cobotta to be able to use RMPflow to follow a target. In our first pass, it will be possible to move the robot into self-collision, and the robot will not be able to avoid external obstacles.

We’ll start with the robot_description file. In the template, our generic robot has a 7-joint cspace with joint names [“joint1”, …, “joint7”]. Investigating our Cobotta URDF reveals that the Cobotta has 6 revolute joints in the arm named [“joint_1”, …, “joint_6”], and a number of prismatic joints in its manipulator. RMPflow should only be used to control the arm, so we modify the robot_description file to represent the appropriate cspace (./Cobotta_Pro_900_Assets/robot_descriptor_basic.yaml in the provided tutorial zip).

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
# Defines the generalized coordinates. Each generalized coordinate is assumed
# to have an entry in the URDF.
# RMPflow will only use these joints to control the robot position.
cspace:
    - joint_1
    - joint_2
    - joint_3
    - joint_4
    - joint_5
    - joint_6


# Global frame of the URDF
root_link: world


# The default cspace position of this robot
default_q: [
    0.00, 0.00, 0.00, 0.00, 0.00, 0.00
]

In the above code snippet, we change the cspace argument to match the URDF in two ways. First, there are 6 joints rather than 7, and second, the names of the joints in the URDF contain underscores. We also needed to change the number of joints represented in the default_q argument to 6. Finally, we change the root_link argument to reflect the name of the global frame in the Cobotta URDF.

The rmpflow_config file also requires little work to get started (./Cobotta_Pro_900_Assets/rmpflow_config_basic.yaml in the provided tutorial zip):

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
# Artificially limit the robot joints.  For example:
# A joint with range +-pi would be limited to +-(pi-.01)
joint_limit_buffers: [.01, .01, .01, .01, .01, .01]

#Omitting `rmp_params` argument

# body_cylinders are used to promote self-collision avoidance between the robot and its base
# The example below defines the robot base to be a capsule defined by the absolute coordinates pt1 and pt2.
# The semantic name provided for each body_cylinder does not need to be present in the robot URDF.
body_cylinders:
     - name: base
       pt1: [0,0,.333]
       pt2: [0,0,0.]
       radius: .05


# body_collision_controllers defines spheres located at specified frames in the robot URDF
# These spheres will not be allowed to collide with the capsules enumerated under body_cylinders
# By design, most frames in industrial robots are kinematically unable to collide with the robot base.
# It is often only necessary to define body_collision_controllers near the end effector
body_collision_controllers:
     - name: right_inner_finger
       radius: .05

To get our robot moving around, we can ignore the rmp_params argument for now. We modify the joint_limit_buffers argument to represent that our robot only has 6 DOFs rather than the 7 listed in the template. We have to provide body_cylinders, but for now we won’t worry about doing a good job representing the robot base. One change was required to the default body_collision_controllers argument, which is to change the frame at which we place a collision sphere. There is no “end_effector” frame in the Cobotta URDF, so for now we pick a frame that is near the end effector: right_inner_finger.

Now with a simple script, we can load our robot with RMPflow (./basic_follow_target.py in the provided tutorial zip).

 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.motion_generation.lula import RmpFlow
from omni.isaac.motion_generation import ArticulationMotionPolicy
from omni.isaac.core.robots import Robot
from omni.isaac.core.objects import cuboid
from omni.isaac.core import World
from omni.isaac.core.utils.stage import open_stage
import numpy as np
import os
import argparse

#TODO: Fill in tutorial directory with absolute path to this file
TUTORIAL_DIRECTORY = "/path/to/tutorial/"

rmp_config_dir = os.path.join(TUTORIAL_DIRECTORY,"Cobotta_Pro_900_Assets")

parser = argparse.ArgumentParser()
parser.add_argument("--robot_description_path",type=str,default="robot_descriptor_basic.yaml")
parser.add_argument("--urdf_path",type=str,default="cobotta_pro_900.urdf")
parser.add_argument("--rmpflow_config_path",type=str,default="cobotta_rmpflow_config_basic.yaml")
parser.add_argument("--end_effector_frame_name",type=str,default="onrobot_rg6_base_link")
args = parser.parse_args()

open_stage(usd_path=os.path.join(rmp_config_dir,"cobotta_pro_900.usd"))

my_world = World(stage_units_in_meters=1.0)

robot = my_world.scene.add(Robot(prim_path="/cobotta_pro_900", name="Cobotta_Pro_900"))

#Initialize an RmpFlow object
rmpflow = RmpFlow(
    robot_description_path = os.path.join(rmp_config_dir,args.robot_description_path),
    urdf_path = os.path.join(rmp_config_dir,args.urdf_path),
    rmpflow_config_path = os.path.join(rmp_config_dir,args.rmpflow_config_path),
    end_effector_frame_name = args.end_effector_frame_name, #This frame name must be present in the URDF
    evaluations_per_frame = 5
)

#Uncomment this line to visualize the collision spheres in the robot_description YAML file
#rmpflow.visualize_collision_spheres()

physics_dt = 1/60.
articulation_rmpflow = ArticulationMotionPolicy(robot,rmpflow,physics_dt)

articulation_controller = robot.get_articulation_controller()

#Make a target to follow
target_cube = cuboid.VisualCuboid("/World/target",position = np.array([0.,0,1.5]),color=np.array([1.,0,0]))

my_world.reset()
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()

        #Set rmpflow target to be the current position of the target cube.
        rmpflow.set_end_effector_target(
            target_position=target_cube.get_world_pose()[0],
            #target_orientation=target_cube.get_world_pose()[1]
        )

        actions = articulation_rmpflow.get_next_articulation_action()
        articulation_controller.apply_action(actions)

simulation_app.close()

Note that the user must fill in an absolute path to their local tutorial directory on line 16.

Our sample script has four command line arguments that we will be using as we modify and add to our RMPflow config files in this tutorial. The default arguments use the minimal changes that we have made so far. We can simply run:

python.sh basic_follow_target.py

Running this script, we see that our robot is able to follow the target pretty well! With a little work, it is possible to get the robot arm to intersect with the base, but this is to be expected with our current minimal config files.

The reader will also notice that the frame that RMPflow is moving to the target position is not in the center of the gripper, but at its base. On lines 24,38 we set the end effector frame to be “onrobot_rg6_base_link”. The frame we choose must be defined in the robot URDF, and there is not a frame resting in the center of the gripper. Creating such a frame will be covered later in this tutorial.

../_images/isaac_sim_denso_basic_rmpflow.gif

5.4.2. Avoiding External Obstacles: Configuring Collision Spheres

The most difficult part of configuring RMPflow on a new robot is specifying the collision behavior properly. First, we will use the collision_spheres argument to prevent collisions with external obstacles. Finding a good collision-sphere representation for a robot is currently an iterative process that requires investigating both the robot URDF and using the visualization capabilities of Omniverse Isaac Sim.

With the Cobotta loaded into Omniverse Isaac Sim, we can click on frames that are present in the robot URDF and see their orientations. Typically a URDF will align each link with one of the principal axes in the link’s frame. Let’s investigate the frames in the Cobotta using Omniverse Isaac Sim to get a good guess for where the collision spheres should be placed. Running the same script as before:

../_images/isaac_sim_denso_rmpflow_watching_frames.gif

Note in the beginning of the video that we click on the transform controls in the upper left to switch into the local frame of our selected link. This is denoted by an orange circle. In this view, we can observe the principal axes of orientation for each link. They are color coded with RGB corresponding to XYZ respectively.

The first link we click on is “J1”. We can observe that the Z axis is pointing in the direction of the link. We can also observe that the “J1” frame is not actually inside of the “J1” link. We now have enough information to place potential spheres offset along the +Z direction of the “J1” frame.

In our robot_description.yaml, we add collision spheres for the “J1” frame with radius .1, offset along the Z axis.

1
2
3
4
5
6
collision_spheres:
  - J1:
    - "center": [0.0, 0.0, 0.1]
      "radius": 0.1
    - "center": [0.0, 0.0, 0.2]
      "radius": 0.1

We can visualize our spheres using a built-in RMPflow debugging function: visualize_collision_spheres().

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
#Initialize an RmpFlow object
rmpflow = RmpFlow(
    robot_description_path = os.path.join(rmp_config_dir,args.robot_description_path),
    urdf_path = os.path.join(rmp_config_dir,args.urdf_path),
    rmpflow_config_path = os.path.join(rmp_config_dir,args.rmpflow_config_path),
    end_effector_frame_name = args.end_effector_frame_name, #This frame name must be present in the URDF
    evaluations_per_frame = 5
)

#Uncomment this line to visualize the collision spheres in the robot_description YAML file
rmpflow.visualize_collision_spheres()
python.sh basic_follow_target.py --robot_description_path "robot_descriptor_first_link_initial_spheres.yaml"
../_images/isaac_sim_denso_first_collision_spheres.gif

We can see in the visualization that our spheres are in the right ball-park, and that they are a little too big. We can do better with some minor adjustments. When the visualize_collision_spheres() function is called, RMPflow creates sphere prims under the names “/lula/collision_spherei”. We are able to change the size of the visualized spheres, but not the location of the spheres relative to the robot; RMPflow is updating their positions at every frame to keep them where they should be. First, we change the size of the spheres until we get something that looks like a good fit. Then, by duplicating “/lula/collision_sphere2”, we get a copy that we can move around freely. In the video, the duplicate sphere is first moved up to test whether a sphere should be placed with an offset of greater than .2. Since the sphere becomes immediately visible outside of the robot link, we can see that .2 was a good estimate for the placement of the furthermost sphere. In the video, we then move the sphere in the -Z direction to determine how far apart spheres should be in order to get good coverage of the link. We were able to move the duplicate sphere around .05 m in the -Z direction while still getting a good coverage of the link. This is enough information to generate good collision spheres for the first link in the robot.

1
2
3
4
5
6
7
8
collision_spheres:
  - J1:
    - "center": [0.0, 0.0, 0.1]
      "radius": 0.08
    - "center": [0.0, 0.0, 0.15]
      "radius": 0.08
    - "center": [0.0, 0.0, 0.2]
      "radius": 0.08
python.sh basic_follow_target.py --robot_description_path "robot_descriptor_first_link_final_spheres.yaml"
../_images/isaac_sim_denso_first_link_final_spheres.png

The process continues for all the other links in the Cobotta until we have a good collision sphere representation. By fully utilizing the visualization capacity of Omniverse Isaac Sim, we can get spheres right for a link in one to two iterations. We can also use the Cobotta URDF to obtain some supplementary information. For example, by searching for “J2” in the Cobotta URDF, we see

1
2
3
4
5
6
7
8
<joint name="joint_3" type="revolute">
    <parent link="J2"/>
    <child link="J3"/>
    <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.510000"/>
    <axis xyz="-0.000000 1.000000 -0.000000"/>
    <limit effort="1" lower="-2.61799387799149" upper="2.61799387799149" velocity="1"/>
    <dynamics damping="0" friction="0"/>
</joint>

“joint_3” connects the “J2” and “J3” frames in the URDF, and it is offset along the “J2” frame by [0,0,.51]. This gives a number for the length of the “J2” link. But in Omniverse Isaac Sim, we saw that the mesh for “J2” was also offset from the “J2” frame by a small amount along the +Y axis. Using the URDF together with our visualization tools and what we learned from the “J1” link, we can generate a great first guess:

 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
collision_spheres:
  - J1:
    - "center": [0.0, 0.0, 0.1]
      "radius": 0.08
    - "center": [0.0, 0.0, 0.15]
      "radius": 0.08
    - "center": [0.0, 0.0, 0.2]
      "radius": 0.08
  - J2:
    - "center": [0.0, 0.1, 0.0]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.0]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.05]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.1]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.15]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.2]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.25]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.3]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.35]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.4]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.45]
      "radius": 0.08
    - "center": [0.0, 0.2, 0.5]
      "radius": 0.08
    - "center": [0.0, 0.1, 0.5]
      "radius": 0.08
python.sh basic_follow_target.py --robot_description_path "robot_descriptor_second_link_initial_spheres.yaml"
../_images/isaac_sim_denso_second_link_initial_guess.png

Following this process, the rest of the collision spheres for our Cobotta robot can be generated and tuned efficiently. We can confirm that everything is in order by moving our target around and making sure that the spheres stay fixed where they should be relative to the robot links.

python.sh basic_follow_target.py --robot_description_path "robot_descriptor_final.yaml"
../_images/isaac_sim_denso_rmpflow_final_spheres_follow_target.gif

Now that we have our collision sphere representation, our robot is able to avoid obstacles. Modifying our standalone script, we create an obstacle and pass it to RMPflow. Then, on every frame, we tell RMPflow to query the new obstacle position (./avoid_obstacles.py in the provided tutorial zip):

 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.motion_generation.lula import RmpFlow
from omni.isaac.motion_generation import ArticulationMotionPolicy
from omni.isaac.core.robots import Robot
from omni.isaac.core.objects import cuboid
from omni.isaac.core import World
from omni.isaac.core.utils.stage import open_stage
import numpy as np
import os
import argparse

#TODO: Fill in tutorial directory with absolute path to this file
TUTORIAL_DIRECTORY = "/path/to/tutorial/"

rmp_config_dir = os.path.join(TUTORIAL_DIRECTORY,"Cobotta_Pro_900_Assets")

parser = argparse.ArgumentParser()
parser.add_argument("--robot_description_path",type=str,default="robot_descriptor_basic.yaml")
parser.add_argument("--urdf_path",type=str,default="cobotta_pro_900.urdf")
parser.add_argument("--rmpflow_config_path",type=str,default="cobotta_rmpflow_config_basic.yaml")
parser.add_argument("--end_effector_frame_name",type=str,default="onrobot_rg6_base_link")
args = parser.parse_args()

open_stage(usd_path=os.path.join(rmp_config_dir,"cobotta_pro_900.usd"))

my_world = World(stage_units_in_meters=1.0)

robot = my_world.scene.add(Robot(prim_path="/cobotta_pro_900", name="Cobotta_Pro_900"))

#Initialize an RmpFlow object
rmpflow = RmpFlow(
    robot_description_path = os.path.join(rmp_config_dir,args.robot_description_path),
    urdf_path = os.path.join(rmp_config_dir,args.urdf_path),
    rmpflow_config_path = os.path.join(rmp_config_dir,args.rmpflow_config_path),
    end_effector_frame_name = args.end_effector_frame_name, #This frame name must be present in the URDF
    evaluations_per_frame = 5
)

physics_dt = 1/60.
articulation_rmpflow = ArticulationMotionPolicy(robot,rmpflow,physics_dt)

articulation_controller = robot.get_articulation_controller()

#Make a target to follow
target_cube = cuboid.VisualCuboid("/World/target",position = np.array([.5,0,.5]),color = np.array([1.,0,0]))

#Make an obstacle to avoid
obstacle = cuboid.VisualCuboid("/World/obstacle", position = np.array([.8,0,.5]),color = np.array([0,1.,0]), size = .1)
rmpflow.add_obstacle(obstacle)

my_world.reset()
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()

        #Set rmpflow target to be the current position of the target cube.
        rmpflow.set_end_effector_target(
            target_position=target_cube.get_world_pose()[0],
            #target_orientation=target_cube.get_world_pose()[1]
        )

        #Query the current obstacle position
        rmpflow.update_world()

        actions = articulation_rmpflow.get_next_articulation_action()
        articulation_controller.apply_action(actions)

simulation_app.close()

Now we can watch RMPflow generate smooth motions and adapt to a changing environment in order to reach its goal position!

python.sh avoid_obstacles.py --robot_description_path "robot_descriptor_final.yaml"
../_images/isaac_sim_denso_rmpflow_avoid_obstacle.gif

5.4.3. Avoiding Self-Collision: Configuring Body Cylinders and Body Collision Controllers

With Avoiding External Obstacles: Configuring Collision Spheres taken care of, our robot will avoid collisions with external obstacles, but it will not avoid self-collision. Currently, there is limitted tooling available for avoiding self-collision because industrial robot arms typically remove most potential for self-collision with joint limits. However, some exploration is required with a particular robot to see what types of self-collision are possible. With the preliminary configuration of body cylinders and body collision controllers we set in our basic rmpflow_config.yaml file, it is easy to cause collisions between the robot end effector and the robot base:

python.sh basic_follow_target.py --robot_description_path "robot_descriptor_final.yaml"
../_images/isaac_sim_denso_rmpflow_self_collision.gif

body_cylinders define an imagined robot base via a set of capsules. body_collision_controllers define collision spheres placed on different frames of the robot URDF. RMPflow will not allow these spheres and capsules to come into contact with each other. In our basic rmpflow config, we defined the base as the capsule connecting two spheres of radius .05 m at the absolute coordinates ([0,0,0], [0,0,.333]) (see Doing the Bare Minimum), and we define a single body_collision_controller at the right_inner_finger frame. In the video above, we can see that the gripper will not pass directly throught the robot base, but it is easy to facilitate a self-collision with the edge of the robot base, or the base of the second link. The self-collision tooling available in RMPflow does not allow us to avoid all self-collisions without sacrificing some acceptible robot configurations as well. To make self collisions completely impossible for the Cobotta, we would need a very conservative estimate of the robot base; essentially we would not allow the gripper to move close to the base at all. Choosing the best possible configuration is use-case dependent. There is no reason to take away maneuverability around the robot base unless we actually see that the robot is self-colliding.

One potential configuration is given in this tutorial that covers the other frames in the gripper and exaggerates the size of the robot base in order to make it harder for the gripper to intersect with the robot’s second link. The sizes and locations for the capsule and spheres are based on the collision spheres that we’ve already added.

 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
# body_cylinders are used to promote self-collision avoidance between the robot and its base
# The example below defines the robot base to be a capsule defined by the absolute coordinates pt1 and pt2.
# The semantic name provided for each body_cylinder does not need to be present in the robot URDF.
body_cylinders:
     - name: base
       pt1: [0,0,.12]
       pt2: [0,0,0.]
       radius: .08
     - name: second_link
       pt1: [0,0,.12]
       pt2: [0,0,.12]
       radius: .16


# body_collision_controllers defines spheres located at specified frames in the robot URDF
# These spheres will not be allowed to collide with the capsules enumerated under body_cylinders
# By design, most frames in industrial robots are kinematically unable to collide with the robot base.
# It is often only necessary to define body_collision_controllers near the end effector
body_collision_controllers:
     - name: J5
       radius: .05
     - name: J6
       radius: .05
     - name: right_inner_finger
       radius: .02
     - name: left_inner_finger
       radius: .02
     - name: right_inner_knuckle
       radius: .02
     - name: left_inner_knuckle
       radius: .02

We represent the robot base link “J1” with a capsule of radius .08 m, which we observed to be a good size for our collision spheres. We represent the robot’s second link with a large sphere of radius .12. Running our script again, we see that the robot does a much better job avoiding collisions with the first and second link. As expected, it is still possible to cause a self-collision, but the cases are much more limitted.

python.sh avoid_obstacles.py --robot_description_path "robot_descriptor_final.yaml" --rmpflow_config_path "cobotta_rmpflow_config_final.yaml"
../_images/isaac_sim_denso_rmpflow_self_collision_tuned.gif

5.4.4. Creating an End Effector Frame

A detail that the user will likely have noticed when following this tutorial is that our chosen end effector frame “onrobot_rg6_base_link” does not directly represent the position of the robot’s gripper. The frame that RMPflow considers to be the end effector must be present in the robot URDF. In this tutorial, we selected a frame that aligned with the center of the gripper as our best option. To directly control where the center of the gripper is, the user has two options:

  1. The user may manually compute transforms between their desired target and the target they send to RMPflow at runtime.

  2. The user may add a frame to the robot’s URDF.

This tutorial covers the second option by adding a frame to the Cobotta Pro 900 URDF. A common choice of end effector position is in the center of the gripper, with two principal axes aligned with the gripper fingers.

Investigating our Cobotta Pro 900 URDF, we can see how the “right_inner_finger” frame is connected to the robot arm. In the URDF, we see that the “right_inner_finger” joint a grandchild of the “onrobot_rg6_base_link” frame (which is at the gripper base).

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
<joint name="right_inner_finger_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 -0.047334999999999995 0.064495"/>
    <parent link="right_outer_knuckle"/>
    <child link="right_inner_finger"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" lower="-0.872665" upper="0.872665" velocity="2.0"/>
    <mimic joint="finger_joint" multiplier="1" offset="0"/>
  </joint>

<joint name="right_outer_knuckle_joint" type="revolute">
    <origin rpy="0 0 3.141592653589793" xyz="0 0.024112 0.136813"/>
    <parent link="onrobot_rg6_base_link"/>
    <child link="right_outer_knuckle"/>
    <axis xyz="1 0 0"/>
    <limit effort="1000" lower="-0.628319" upper="0.628319" velocity="2.0"/>
    <mimic joint="finger_joint" multiplier="-1" offset="0"/>
  </joint>

This tells us that we can create a frame that is offset from the “onrobot_rg6_base_link” frame by a pure Z offset of .064495+.136813=.2013 to represent a point in the center of the gripper, aligned with the “right_inner_finger_joint” and “left_inner_finger_joint”. To get closer with the tips of the fingers, we increase the Z offset to .24.

We add a link to our URDF called “gripper_center”, whose offset from the parent link “onrobot_rg6_base_link” is defined by the connection “gripper_center_joint”. In the provided tutorial file, the modified URDF is saved as cobotta_pro_900_gripper_frame.urdf

1
2
3
4
5
6
<link name="gripper_center"/>
  <joint name="gripper_center_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.0 .24"/>
    <parent link="onrobot_rg6_base_link"/>
    <child link="gripper_center"/>
  </joint>

We can view the orientation of the gripper frame by including orientation as part of the RMPflow target. In basic_follow_target.py we uncomment a line:

1
2
3
4
5
#Set rmpflow target to be the current position of the target cube.
rmpflow.set_end_effector_target(
    target_position=target_cube.get_world_pose()[0],
    target_orientation=target_cube.get_world_pose()[1]
)
python.sh basic_follow_target.py --robot_description_path "robot_descriptor_final.yaml" --rmpflow_config_path "cobotta_rmpflow_config_final.yaml" --urdf_path "cobotta_pro_900_gripper_frame.urdf" --end_effector_frame_name "gripper_center"
../_images/isaac_sim_denso_rmpflow_adjusted_end_effector.gif

We can see in the video that the Z axis of the target lies along the center of the gripper and that the Y axis of the target is aligned with the gripper plane.

5.4.5. Modifying RMPflow Parameters

There is one remaining piece of the RMPflow config files that we have not yet modified: the RMPflow parameters in rmpflow_config.yaml. Luckily, there is typically not much need to modify parameters from the template. RMPflow terms work well for robots with similar scales. The template RMPflow config was tuned based on the Franka Emika Panda robot.

There is one RMPflow parameter that is robot-specific: joint_velocity_cap_rmp. This term sets a limit on the maximum velocity that is allowed by RMPflow for any joint in the specified configuration space. Investigating the URDF, we can see that each joint in the Cobotta Pro 900 has a velocity limit of 1 rad/s.

1
2
3
4
5
6
7
8
<joint name="joint_6" type="revolute">
    <parent link="J5"/>
    <child link="J6"/>
    <origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.120000 0.160000"/>
    <axis xyz="-0.000000 -0.000000 1.000000"/>
    <limit effort="1" lower="-6.28318530717959" upper="6.28318530717959" velocity="1"/>
    <dynamics damping="0" friction="0"/>
  </joint>

To make sure that RMPflow respects these joint velocity limits, we can modify out template parameters so that RMPflow will start damping the velocity of a joint when it comes within .3 rad/s of the 1 rad/sec limit:

1
2
3
4
5
joint_velocity_cap_rmp:
    max_velocity: 1.
    velocity_damping_region: .3
    damping_gain: 1000.0
    metric_weight: 100.

Note: The PD gains from the provided Cobotta Pro 900 USD file are based off the PD gains that we chose for the Franka Emika Panda of P=10000 N*m and D=1000 N*m*s. These values produced oscilations in the Cobotta Pro 900 when we reduced the max_velocity joint_velocity_cap_rmp term to 1 rad/sec. The USD provided for the Cobotta robot in this tutorial has a proportional gain of 10000 N*m damping gain of 10000 N*m*s.

See RMPflow Tuning Guide for more details about the meaning of each RMPflow parameter with and a detailed description of how to improve RMPflow parameters for new robots.

5.5. Summary

This tutorial shows the complete process for configuring RMPflow on a new robot. We saw how to:

  1. Obtain basic functionality on a new robot with minimal alteration from the provided template files.

  2. Use the Omniverse Isaac Sim visualization tools to iteratively generate a collision-sphere representation of a robot.

  3. Use the derived collision spheres to configure self-collision avoidance behavior.

  4. Create a new end effector frame that can be used by RMPflow.

5.5.1. Further Learning

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