# 7. Configuring RMPflow for a New Manipulator¶

## 7.1. Learning Objectives¶

In this tutorial, we will see how the RMPflow algorithm may be fully configured following the creation of a Robot Description File.

## 7.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 Lula RMPflow tutorial, which has a more detailed and complete description of writing scripts with RMPflow.

## 7.3. Template RmpFlow Config File¶

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. This file can be generated using the Lula Robot Description Editor UI tool.

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 and has created a Robot Description File using the Robot Description Editor. In this tutorial, a template files is provided for the remaining RMPflow configuration, which is modified in order to match the Cobotta Pro 900 robot. The provided tutorial zip file contains a completed robot_description.yaml for the Cobotta Pro 900.

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.

### 7.3.1. Template RmpFlow 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 c-space given in the robot_description.yaml file. Imagining that our template robot has seven revolute joints, The given buffers of .01 on the 7 c-space 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.

## 7.4. Modifying our Template for the Cobotta Pro 900¶

### 7.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.

The rmpflow_config file 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 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("--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,"robot_description.yaml"), 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 maximum_substep_size = .0034 ) #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]),size = .1) 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 three 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 23,37 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.

Because collision spheres are defined in the provided Robot Description File, RmpFlow is also able to avoid obstacles in the world. 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 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("--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,"robot_description.yaml"), 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 maximum_substep_size = .0034 ) 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]),size = .1) #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


### 7.4.2. Avoiding Self-Collision: Configuring Body Cylinders and Body Collision Controllers¶

With a completed Robot Description File 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 cobotta_rmpflow_config_basic.yaml file, it is easy to cause collisions between the robot end effector and the robot base:

python.sh avoid_obstacle.py


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 matches the size of the collision spheres in near the base of the robot. 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 --rmpflow_config_path "cobotta_rmpflow_config_final.yaml"


### 7.4.3. 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  

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  

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"


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.

### 7.4.4. 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  

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.

## 7.5. Summary¶

This tutorial builds on the Lula Robot Description Editor tutorial to complete the process of configuring RMPflow on a new robot. We saw how to:

1. Modify a template rmpflow_config.yaml file to fit a specific robot.

2. Tune self-collision avoidance behavior

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

### 7.5.1. Further Learning¶

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