Motion Generation

About

The Motion Generation Extension provides a set of abstract interfaces for adding motion control algorithms to Isaac Sim. The interfaces in the Motion Generation Extension provide two basic utilities:

1. Simplify the integration of new robotics algorithms into Omniverse Isaac Sim.

2. Provide a standard structure with which to compare similar robotics algorithms.

Three interfaces are currently provided in the Motion Generation Extension: Motion Policy, Path Planner, and Kinematics Solver. In Isaac Sim, the robot is specified using a USD file that gets added to the stage. However, we expect that robotics algorithms will have their own way of specifying the robot’s kinematic structure and custom parameters. To avoid interfering with any particular robot description format, the interfaces in the Motion Generation Extension include functions that facilitate the translation between the USD robot and a specific algorithm. Specifically, an algorithm may specify which joints in the robot it cares about, and the order in which it expects those joints to be listed. The helper classes provided in this extension, Articulation Motion Policy, Path Planner Visualizer, and Articulation Kinematics Solver, use the interface functions to appropriately map robot joint states between the USD robot articulation and an interface implementation.

In Omniverse Isaac Sim, we use the word Articulation to refer to the simulated robot represented through USD. The word “Articulation” is used as a prefix in the Motion Generation Extension to indicate utility classes that handle interfacing an algorithm with the simulated robot.

In addition, the Motion Generation extension includes a handful of special-purpose controllers that do not currently leverage MotionPolicy or PathPlanner.

Motion Policy

A Motion Policy is a collision aware algorithm that outputs actions on each frame to navigate a single robot to a single task-space target. The MotionPolicy class is an interface that is designed to be simple to fulfill, but complete enough that an implementation of a MotionPolicy can be used alongside the Articulation Motion Policy class to start moving the simulated robot around with just a few lines of code.

In the current release, a single flexible MotionPolicy is provided based on the implementation of RMPflow in the NVIDIA-developed Lula library. (see RMPflow)

Path Planner

A Path Planner is an algorithm that outputs a series of configuration space waypoints, which when linearly interpolated, produce a collision-free path from a starting c-space pose to a c-space or task-space target pose. The PathPlanner class provides an interface that specifies the necessary functions that must be fulfilled in order to specify a path planning algorithm that can interface with Omniverse Isaac Sim.

An implementation is provided using the NVIDIA-developed Lula library. (see Lula RRT)

Kinematics Solver

Like a Motion Policy, a Kinematics Solver is an interface class with a single provided implementation. A KinematicsSolver is able to compute forward and inverse kinematics. A single implementation is provided using the NVIDIA-developed Lula library. (see Lula Kinematics Solver)

Trajectory Generation

In the Motion Generation extension, a workflow is provided for defining c-space and task-space trajectories. An interface is provided for a Trajectory Interface class

API Documentation

See the API Documentation for usage information.

Kinematics Solver

The KinematicsSolver interface specifies functions for computing both forward and inverse kinematics at any available frame in the robot. Like a Motion Policy, an instance of the KinematicsSolver class is not expected to use the same USD robot representation as Omniverse Isaac Sim. A KinematicsSolver may have its own internal representation of the robot, and there are necessary interface functions for performing the mapping between the internal robot representation and the robot Articulation.

Joint Names

An instance of the KinematicsSolver class must fulfill a function KinematicsSolver.get_joint_names() that specifies the joints of interest to the solver, and the order in which it expects them. Think of a robot arm mounted on a moving base. A KinematicsSolver may use only the URDF for the robot arm without knowing about the robot base. In this case, many of the joints in the robot Articulation would not be recognized by the KinematicsSolver.

When computing forward kinematics, the joint positions that are passed to the solver must correspond to the output of KinematicsSolver.get_joint_names(). Likewise, the output of inverse kinematics will have the same shape as KinematicsSolver.get_joint_names(). A mapping layer between the robot Articulation and the KinematicsSolver is provided in the Articulation Kinematics Solver class.

Frame Names

An instance of the KinematicsSolver class must fulfill a function KinematicsSolver.get_all_frame_names() to provide a list of frames in the robot’s kinematics chain that can have their positions referenced by name when solving either forward or inverse kinematics. The frame names returned by a KinematicsSolver do not have to match the frames present in the robot Articulation. Like joint names, the frame names come from the individual solver’s config file structure.

Robot Base Pose

As with a Motion Policy, a the KinematicsSolver interface includes a function set_robot_base_pose() that allows the caller to specify the location of the robot base. If this function has been called, the KinematicsSolver must apply appropriate transformations when computing forward and inverse kinematics. A KinematicsSolver operates in world coordinates. The solution to the forward kinematics will be translated and rotated according to the robot base pose to return the position of the end effector relative to the world frame, and the input to the inverse kinematics will be provided in the world coordinates and transformed so that it is relative to the robot base frame. If the user prefers that the solver inputs are relative to the robot base frame, they may simply set the robot base pose to the origin.

Collision Awareness

Implementations of the KinematicsSolver class do not need to be collision aware with external objects, but they have the option. A function KinematicsSolver.supports_collision_avoidance() -> bool must be implemented to indicate whether a particular KinematicsSolver supports collision avoidance. If a KinematicsSolver supports collision avoidance, it may fulfill the same set of world functions as a MotionPolicy (Inputs: World State). If a solver is collision aware, it is especially important to specify the robot base pose correctly, as the positions of objects can only be queried relative to the world frame, and it is up to the solver to compute the positions of obstacles relative to the robot.

Articulation Kinematics Solver

The ArticulationKinematicsSolver class exists to handle the mapping between the robot Articulation and an implementation of a Kinematics Solver.

Forward Kinematics

ArticulationKinematicsSolver wraps the forward kinematics function of a KinematicsSolver in order to query the joint positions of the robot Articulation and pass the appropriate joint positions to the KinematicsSolver in the order specified by KinematicsSolver.get_joint_names(). This allows the current position of the simulated robot end effector to be queried easily.

Inverse Kinematics

ArticulationKinematicsSolver wraps the inverse kinematics in order to return the resulting joint positions as an ArticulationAction that can be directly applied to the robot Articulation. The current robot Articulation joint positions at the time this method is called are automatically used as a warm start in the IK calculation.

Lula Kinematics Solver

The LulaKinematicsSolver implements the Kinematics Solver interface. The solver does not support collision avoidance with objects in the world. In addition to the functions in the KinematicsSolver interface, the LulaKinematicsSolver includes getters and setters for changing internal settings such as LulaKinematicsSolver.set_max_iterations() to set the maximum number of iterations before the IK computation returns a failure.

Lula Kinematics Solver Configuration

Two files are necessary to configure Lula Kinematics for use with a new robot:

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 supplemental 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. This file may also be used to specify fixed positions for unactuated joints.

Trajectory Interface

An interface is provided in the Motion Generation extension for defining a robot trajectory. An instance of the Trajectory interface must return robot c-space position as a continuous function of time within a specified time horizon. A Trajectory has four basic accessors:

start_time: The earliest time at which this Trajectory will return a robot c-space position.

end_time: The latest time at which this Trajectory will return a robot c-space position.

active_joints: The names of the joints that this Trajectory is intended to control corresponding to the order the joint targets are returned.

joint_targets(time): Joint position/velocity targets as a function of time between start_time and end_time.

An instance of the Trajectory class can be used to directly control a robot by using it to initialize an Articulation Trajectory.

Articulation Trajectory

The ArticulationTrajectory class is initialized using a robot Articulation and an instance of the Trajectory class. This class handles the mapping from a defined Trajectory to controlling a simulated robot Articulation. The ArticulationTrajectory class has two main functions:

get_action_at_time(time): Return an ArticulationAction at a time that is within the time horizon of the provided Trajectory object.

get_action_sequence(timestep): Return a list of ArticulationAction which interpolates between the provided Trajectory start_time and end_time by the specified timestep. This is a convenience method for when the timestep of the physics simulator is known to be fixed.

As a Trajectory only defines the robot behavior within the provided time horizon, it is necessary to bring the robot Articulation to the initial state of the Trajectory before attempting to follow a sequence of generated ArticultionAction.

Lula Trajectory Generator

We provide a Lula implementation of a trajectory generator which can generate a Trajectory given c-space or task-space waypoints. Two classes are provided: LulaCSpaceTrajectoryGenerator and LulaTaskSpaceTrajectoryGenerator. Both classes share the same required configuration information.

Lula Trajectory Generator Configuration

Two files are necessary to configure Lula Trajectory Generators for a specific robot:

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 supplemental 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, acceleration limits, or jerk limits. This file may also be used to specify fixed positions for unactuated joints.

Lula C-space Trajectory Generator

The LulaCSpaceTrajectoryGenerator class takes in a series of c-space waypoints that correspond to the c-space coordinates listed in the required robot description YAML file. The generator will use spline-based interpolation in order to connect the waypoints with an initial and final velocity of 0. The trajectory is time-optimal – i.e. either the velocity, acceleration, or jerk limits are saturated at any given time to produce as trajectory with as short a duration as possible. The generator will return an instance of the Trajectory interface.

Lula Task-space Trajectory Generator

The LulaTaskSpaceTrajectoryGenerator class takes in a sequence of task-space targets and an end effector frame name (which must be a valid frame in the provided URDF file), and it returns an instance of the Trajectory interface if possible.

Task-space trajectories can be defined as a series of position/orientation targets. In this case, the generated trajectory will linearly interpolate in task-space between the provided targets.

Task-space trajectories can also be defined using the lula.TaskSpacePathSpec class, which provides a set of useful primitives to connect task-space waypoints such as creating an arc, pure rotation, pure translation, etc.

Internally, a task-space trajectory is converted to a c-space trajectory using the Lula Kinematics Solver, and is then passed through the LulaCSpaceTrajectoryGenerator. For this reason, the LulaTaskSpaceTrajectoryGenerator class shares the same set of parameters as the LulaCSpaceTrajectoryGenerator class, with added parameters that affect how the task-space trajectory is converted to c-space.

Path Planner

The PathPlanner interface specifies functions for computing a series of configuration space waypoints, which when linearly interpolated, produce a collision-free path from a starting c-space pose to a c-space or task-space target pose. A PathPlanner uses the same set of functions to interface with the USD world as a Motion Policy. Like a Motion Policy, an instance of the PathPlanner class is not expected to use the same USD robot representation as Omniverse Isaac Sim. A PathPlanner may have its own internal representation of the robot, and there are necessary interface functions for performing the mapping between the internal robot representation and the robot Articulation.

Active and Watched Joints

The robot Articulation in Isaac Sim comes from a loaded USD file. This robot specification is not expected to perfectly match the specification used internally by a PathPlanner. To perform the appropriate mapping, a PathPlanner has two functions it must fulfill: PathPlanner.get_active_joints() and PathPlanner.get_watched_joints(). Both functions return a list of joint names in the order that the PathPlanner expects to receive them. “Active joints” are joints that the PathPlanner is going to directly control to achieve the desired end effector target. “Watched joints” are joints that the PathPlanner observes to plan motions, but will not actively control. These are assumed to remain constant when generating a path.

For example, the Franka robot has 9 degrees of freedom (DOFs): 7 revolute joints for controlling the arm, and 2 prismatic joints for controlling its gripper. The robot Articulation exposes all 9 degrees of freedom, but Lula RRT only cares about the 7 revolute joints when navigating the robot to a position target. It is not appropriate for RRT to take control of the gripper DOFs, because those DOFs may be controlled separately when performing a task such as pick-and-place. RRT.get_active_joints() returns the names of the 7 revolute joints in the Franka robot. RRT.get_watched_joints() returns an empty list because the joint states of the gripper DOFs are irrelevant when navigating the Franka’s hand to a target position. Every time RRT returns joint targets for the Franka, it is returning arrays of length 7. When RRT is passed an argument such as active_joint_positions, it is expecting a vector of 7 numbers that describe the joint positions of the Franka robot in the order specified by RRT.get_active_joints().

Inputs: World State

Omniverse Isaac Sim provides a set of objects in omni.isaac.core.objects that are intended to fully describe the simulated world. Currently, only object primitives such as sphere and cone are supported. More advanced objects defined by meshes and point clouds will be added in a future release. A PathPlanner has an adder for each type of object that exists in omni.isaac.core.objects e.g. PathPlanner.add_sphere(sphere: omni.isaac.core.objects.sphere.*). Objects in omni.isaac.core.objects wrap objects that exist on the USD stage. As objects move around on the stage, their location can be retrieved on each frame using the representative object from omni.isaac.core.objects. This means that once a PathPlanner has been passed an object, it can internally query the position of that object on the stage over time as needed. A PathPlanner queries all relevant obstacle positions from the omni.isaac.core.objects that have been passed in when PathPlanner.update_world() is called, and passes the information to its internal world state.

It is not required that a specific PathPlanner actually implement an adder for every type of object that exists in omni.isaac.core.objects. When a class inherits from PathPlanner, any unimplemented adder functions will throw warnings. For example, Lula RRT currently supports spheres, capsules, and cuboids in its world representation. In environments with cones, RRT will ignore the cone objects, and a warning will be printed for each cone object that gets added.

Inputs: Robot State

There are two methods for specifying robot state in a PathPlanner:

1. The base pose of the robot can be specified to a PathPlanner using PathPlanner.set_robot_base_pose(). If this function is never called, the policy implementation can make a reasonable assumption about the position of the robot. Lula RRT assumes that the robot is at the origin of the stage until it is told otherwise.

2. PathPlanner.compute_path(active_joint_positions, watched_joint_positions) expects robot joint positions and velocities to be passed in using the order specified by PathPlanner.get_active_joints() and PathPlanner.get_watched_joints().

Outputs: Path

PathPlanner.compute_path(active_joint_positions, watched_joint_positions) returns a set of configuration space waypoints that can be linearly interpolated to produce a collision free trajectory to reach a target-pose. The c-space configurations output by a PathPlanner will correspond only to the active joints returned by PathPlanner.get_active_joints(). The path output by a PathPlanner is difficult to use on its own; a linearly interpolated path will have sharp corners in c-space. But, a PathPlanner can be a useful component in generating a high-quality trajectory through difficult environments.

A helper class is provided with the PathPlanner interface to enable easy visualization of planned paths connected by simple linear interpolation in the Path Planner Visualizer class.

Path Planner Visualizer

The PathPlannerVisualizer class is provided to make it easy to visualize the path output by a PathPlanner. This class handles the mapping between controllable DOFs in the robot Articulation and the active joints considered by the PathPlanner.

The main function of the class is PathPlannerVisualizer.compute_plan_as_articulation_actions(max_c-space_dist). Calling this function queries the robot state from the robot Articulation, extracts and arranges the appropriate joints from the joint state in order to use the PathPlanner.compute_path() function, linearly interpolates the result, and then creates a valid list of ArticulationAction that can be passed to the robot Articulation one by one to produce the planned path. The max_c-space_dist function determines the density of the linear interpolation such that the L2 norm between any two c-space positions in the output is less than or equal to max_c-space_dist.

Lula RRT

We provide a Lula implementation of the classic Randomly-Exploring Random Tree (RRT) algorithm in order to fulfill the PathPlanner interface. Specifically, the c-space RRT is using RRT-Connect based on 2, and the task-space RRT is using Jacobian transpose RRT based on 3. The RRT implementation does not currently support orientation targets.

Lula RRT Configuration

Three files are necessary to configure Lula RRT for use with a new robot:

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 supplemental 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. This file may also be used to specify fixed positions for unactuated joints.

3. A configuration file in the YAML format specifying parameters for the RRT algorithm such as termination conditions, exploration weights, and step size. These parameters can be modified programatically with the RRT.set_param() function.

Motion Policy

Broadly defined, a motion policy is a mathematical function that takes the current state of a robot (i.e., position and velocity in generalized coordinates) and returns a quantity representing a desired change in that state. Such a policy may depend implicitly on variables representing one or more objectives or constraints, the state of the environment, etc. The MotionPolicy interface has two forms of state as input: Inputs: World State and Inputs: Robot State. The main output of a MotionPolicy are position and velocity targets for the robot on the next frame. A MotionPolicy is expected to be able to perform an internal world update and compute joint targets in real time (a few ms per frame).

Active and Watched Joints

The robot Articulation in Isaac Sim comes from a loaded USD file. This robot specification is not expected to perfectly match the specification used internally by a MotionPolicy. To perform the appropriate mapping, a MotionPolicy has two functions it must fulfill: MotionPolicy.get_active_joints() and MotionPolicy.get_watched_joints(). Both functions return a list of joint names in the order that the MotionPolicy expects to receive them. “Active joints” are joints that the MotionPolicy is going to directly control to achieve the desired end effector target. “Watched joints” are joints that the MotionPolicy observes to plan motions, but will not actively control.

For example, the Franka robot has 9 degrees of freedom (DOFs): 7 revolute joints for controlling the arm, and 2 prismatic joints for controlling its gripper. The robot Articulation exposes all 9 degrees of freedom, but RMPflow only cares about the 7 revolute joints when navigating the robot to a position target. It is not appropriate for RMPflow to take control of the gripper DOFs, because those DOFs may be controlled separately when performing a task such as pick-and-place. RmpFlow.get_active_joints() returns the names of the 7 revolute joints in the Franka robot. RmpFlow.get_watched_joints() returns an empty list because the joint states of the gripper DOFs are irrelevant when navigating the Franka’s hand to a target position. Every time RmpFlow returns joint targets for the Franka, it is returning arrays of length 7. When RmpFlow is passed an argument such as active_joint_positions, it is expecting a vector of 7 numbers that describe the joint positions of the Franka robot in the order specified by RmpFlow.get_active_joints().

Inputs: World State

Omniverse Isaac Sim provides a set of objects in omni.isaac.core.objects that are intended to fully describe the simulated world. Currently, only object primitives such as sphere and cone are supported. More advanced objects defined by meshes and point clouds will be added in a future release. A MotionPolicy has an adder for each type of object that exists in omni.isaac.core.objects e.g. MotionPolicy.add_sphere(sphere: omni.isaac.core.objects.sphere.*). Objects in omni.isaac.core.objects wrap objects that exist on the USD stage. As objects move around on the stage, their location can be retrieved on each frame using the representative object from omni.isaac.core.objects. This means that once a MotionPolicy has been passed an object, it can internally query the position of that object on the stage over time as needed. A MotionPolicy queries all relevant obstacle positions from the omni.isaac.core.objects that have been passed in when MotionPolicy.update_world() is called, and passes the information to its internal world state.

It is not required that a specific MotionPolicy actually implement an adder for every type of object that exists in omni.isaac.core.objects. When a class inherits from MotionPolicy, any unimplemented adder functions will throw warnings. For example, RMPflow currently supports spheres, capsules, and cuboids in its world representation. In environments with cones, RMPflow will ignore the cone objects, and a warning will be printed for each cone object that gets added.

Inputs: Robot State

There are two methods for specifying robot state in a MotionPolicy:

1. The base pose of the robot can be specified to a MotionPolicy using MotionPolicy.set_robot_base_pose(). If this function is never called, the policy implementation can make a reasonable assumption about the position of the robot. RMPflow assumes that the robot is at the origin of the stage until it is told otherwise.

2. MotionPolicy.compute_joint_targets(active_joint_positions, active_joints_velocities, watched_joint_positions, watched_joint_velocities,…) expects robot joint positions and velocities to be passed in using the order specified by MotionPolicy.get_active_joints() and MotionPolicy.get_watched_joints().

Outputs: Robot Joint Targets

MotionPolicy.compute_joints_targets(active_joint_positions, active_joints_velocities, watched_joint_positions, watched_joint_velocities,…) returns position and velocity targets for the robot Articulation on the next frame. The joint targets are for the active joints, and so they will have the same shape as the active_joint_positions argument. By passing a MotionPolicy to the Articulation Motion Policy helper class, the work of translating the robot state between the robot Articulation and the MotionPolicy is done automatically using the outputs of MotionPolicy.get_active_joints() and MotionPolicy.get_watched_joints(). A MotionPolicy may expect joint targets to be used in a standard PD controller:

kp*(joint_position_targets-joint_positions) + kd*(joint_velocity_targets-joint_velocities)

Both position and velocity targets must always be returned by a MotionPolicy. Omniverse Isaac Sim supports providing only position targets or only velocity targets. To match the default behavior of the Isaac Sim controller when only one target is set, the user may set the joint_velocity_targets to zero for pure damping, and it may set the joint_position_targets to be equal to the current joint_positions to effectively remove the position term from the PD equation.

Articulation Motion Policy

An ArticulationMotionPolicy is initialized using a robot Articulation object that represents the simulated robot, and a MotionPolicy. The purpose of this class is to handle the mapping of joints between the robot articulation and the policy automatically by using the outputs of MotionPolicy.get_active_joints() and MotionPolicy.get_watched_joints(). There is a single important function in this class: ArticulationMotionPolicy.get_next_articulation_action(). Calling this function queries the robot state from the robot Articulation, extracts and arranges the appropriate joints from the joint state in order to use the MotionPolicy.compute_joint_targets() function, and then creates a valid ArticulationAction that can be passed to the robot Articulation in order to generate motions.

In the Franka example discussed in Active and Watched Joints, the robot Articulation that represents the Franka expects 9 DOF joint targets. RmpFlow only controls 7 of the DOFs. The appropriate 7 DOFs are passed to RmpFlow, and 7 DOF joint position and velocity targets are returned. This 7-vector is mapped to a 9-vector, pading with None when no action is supposed to be taken for a particular joint. The ArticulationAction object that is returned contains a 9-vector for position and velocity targets, and this can be applied to the robot Articulation using Articulation.get_articulation_controller().apply_action(articulation_action).

Motion Policy Controller

The MotionPolicyController class wraps a motion policy into an instance of omni.isaac.core.controllers.BaseController. Extensions representing individual robots such as omni.isaac.franka have an instance of a BaseController for moving the robot around. The Franka robot may be moved to a target by importing omni.isaac.franka.controllers.RMPFlowController and using the forward function.

RMPflow

Riemannian Motion Policy (RMP) is a set of motion generation tools that underlies most of our manipulator controls inside Omniverse Isaac Sim. It creates smooth trajectories for the robots with intelligent collision avoidance.

Broadly defined, a motion policy is a mathematical function that takes the current state of a robot (e.g., position and velocity in generalized coordinates) and returns a quantity representing a desired change in that state. Such a policy may depend implicitly on variables representing one or more objectives or constraints, the state of the environment, etc. An acceleration policy is a motion policy where the output is desired acceleration, \(\ddot q = \pi(q, \dot{q})\), resulting in a second-order differential equation. A Riemannian Motion Policy, or RMP, is an acceleration policy accompanied by a matrix \(M(q, \dot{q})\) that is sometimes called an inertia matrix, borrowing terminology from classical mechanics, but is also closely related to the concept of a Riemannian metric.

Leveraging the machinery of Riemannian geometry, RMPflow is a framework for combining RMPs representing multiple (possibly competing) objectives and constraints into a single global acceleration policy. Within this framework, the local RMPs may be defined on any number of intermediate task spaces (including the operational space of the end effector, generalizing operational space control). For complete details, see 1.

For the purpose of controlling a robot via position or velocity control, it is generally convenient to work with motion policies where the output is position or velocity. Such policies may be straightforwardly produced from an acceleration policy via a numerical integration scheme such as Euler integration.

The RMPflow Debugging Features section reviews functions belonging to RMPflow that are not part of the MotionPolicy interface. For a user interacting with RMPflow to control a robot that is already supported, there may be no need to read the other sections. Users who are interested in the internal mechanics of RMPflow, or who want to configure RMPflow on an unsupported robot will be interested in reading the entire RMPflow documentation.

RMPflow Debugging Features

By directly interacting with an RmpFlow instance, the user may access features that are not available in other MotionPolicy implementations. It is common for developers to want to decouple a Motion Policy from the simulated robot Articulation in Omniverse Isaac Sim. For example, When the simulated robot is moving sluggishly, it is important to determine whether the MotionPolicy or the PD gains have been improperly tuned, but this can be difficult when both the PD gains and the MotionPolicy both play a role in driving the robot joints. (see Outputs: Robot Joint Targets)

RMPflow provides visualization functions to clearly show the internal state of the algorithm as part of the stage. RMPflow uses collision spheres internally to avoid hitting obstacles in the world. These spheres can be visualized over time by calling RmpFlow.visualize_collision_spheres(). The visualization will stop when RmpFlow.stop_visualizing_collision_spheres() is called. The nominal end effector position can likewise be visualized with RmpFlow.visualize_end_effector_position() and RmpFlow.stop_visualizing_end_effector().

On their own, the visualization functions can be used to make sure that RMPflow’s internal representation of the robot is reasonable, but it does not help to decouple the simulated robot from the RmpFlow internal representation of the robot. On each frame when RmpFlow.compute_joint_targets(active_joint_positions,…) is called, the visualization is updated to use the active_joint_positions. This behavior can be turned off using RmpFlow.set_ignore_state_updates(True). When RmpFlow is “ignoring state updates”, it starts ignoring the active_joint_positions argument, and instead begins internally tracking the believed state of the robot by assuming that is completely independent of the physical simulation of the robot. When RmpFlow is set to ignore state updates from the simulator, and the visualization functions are used, it becomes simple to determine if an undesirable robot behavior comes from RmpFlow or from the robot Articulation and its PD gains.

RMPflow Configuration

Three files are necessary to configure RMPflow for use with a new robot:

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.

As a general mathematical framework, RMPflow does not prescribe the form that individual RMPs must take. The particular implementation of RMPflow in Lula (and by extension Omniverse Isaac Sim) does however expose a pre-specified set of RMPs that have been constructed and empirically found to produce smooth reactive behaviors for a variety of manipulation tasks. The remainder of this section summarizes the provided RMPs before concluding with some advice on parameter tuning.

C-Space Target RMP (c-space_target_rmp)

Purpose: Specifies a default c-space configuration for the robot, used for redundancy resolution.

Definition: Acceleration for this RMP is given by an equation similar to a PD controller, with a position gain and damping gain, but the magnitude of the position term is capped when the C-space distance exceeds a threshold. This cap avoids excessive forces when the configuration is far away from the target. Defining \(q\) to be the full configuration vector, we have

\[\ddot q = k_p r(q_0 - q) - k_d \dot q\,,\]

where the “robust capping function” \(r(p)\) is given by

\[\begin{split}r(p) = \left \{ \begin{array}{cl} p, & ||p|| < \theta \\ \theta\, p / ||p|| & \textrm{otherwise.} \end{array} \right.\end{split}\]

The inertia matrix is proportional to the identity:

\[M = \mu I\]

Finally, the c-space_target_rmp section of the RMPflow configuration file contains an additional inertia parameter \(m\). When this parameter is nonzero, it results in the introduction of a conceptually separate RMP corresponding to zero c-space acceleration, \(\ddot q = 0\), with inertia matrix given by \(M = mI\).

Parameters:

Units assume revolute joints where \(q\) is expressed in radians. If joints are instead prismatic, robust_position_term_thresh will have units of meters.

Name	Symbol	Units	Meaning
metric_scalar	\(\mu\)	-	Priority weight relative to other RMPs
position_gain	\(k_p\)	s-2	Position gain, determining how strongly configuration is pulled toward target
damping_gain	\(k_d\)	s-1	Damping gain, determining amount of “drag”
robust_position_term_thresh	\(\theta\)	rad	Distance in c-space at which the position correction vector is capped
inertia	\(m\)	-	Additional c-space inertia

Target RMP (target_rmp)

Purpose: Drives end effector toward specified position target.

Definition: Similar to the c-space target RMP, acceleration for this RMP resembles a PD controller, albeit with a slightly different strategy for capping the magnitude of the position correction vector.

\[\ddot x = k_p (x_0 - x) / (||x_0-x|| + \epsilon) - k_d \dot x\]

The inertia matrix blends between a rank-deficient metric \(S = n n^T\), where \(n\) is the direction vector toward the target, and the identity \(I\). Intuitively, \(S\) cares only about the direction toward the target (letting other RMPs such as the obstacle avoidance RMP control the orthogonal directions), and \(I\) cares about all directions. The contribution of \(S\) is larger far from the goal, allowing obstacles to push the system more effectively, while \(I\) dominates near the goal, encouraging faster convergence. The blending is controlled by a radial basis function, specifically a Gaussian, that transitions from a minimum constant value far from the target to 1 near the target.

Near the target, an additional nonlinear “proximity boost” multiplier turns on. This factor again takes the form of a Gaussian. In summary,

\[M = \left[\beta(x) b + (1-\beta(x))\right] \left[\alpha(x) M_\textrm{near} + (1-\alpha(x)) M_\textrm{far} \right]\]

where

\[\begin{split}\begin{array}{l} \alpha(x) = (1-\alpha_\textrm{min})\exp \left(\frac{-||x_0-x||^2}{2 \sigma_a^2}\right) + \alpha_\textrm{min} \\ \beta(x) = \exp \left(-\frac{||x_0 - x||^2}{2 \sigma_b^2}\right) \\ M_\textrm{near} = \mu_\textrm{near} I \\ M_\textrm{far} = \mu_\textrm{far} S = \frac{\mu_\textrm{far}}{||x_0-x||^2} (x_0-x)(x_0-x)^T\,. \end{array}\end{split}\]

Parameters:

Name	Symbol	Units	Meaning
accel_p_gain	\(k_p\)	m/s2	Position gain
accel_d_gain	\(k_d\)	s-1	Damping gain
accel_norm_eps	\(\epsilon\)	m	Length scale controlling transition between constant acceleration region far from target and linear region near target
metric_alpha_length_scale	\(\sigma_a\)	m	Length scale of the Gaussian controlling blending between \(S\) and \(I\)
min_metric_alpha	\(\alpha_\textrm{min}\)	-	Controls the minimum contribution of the isotropic \(M_\textrm{near}\) term to the metric (inertia matrix)
max_metric_scalar	\(\mu_\textrm{near}\)	-	Metric scalar for the isotropic \(M_\textrm{near}\) contribution to the metric (inertia matrix)
min_metric_scalar	\(\mu_\textrm{far}\)	-	Metric scalar for the directional \(M_\textrm{far}\) contribution to the metric (inertia matrix)
proximity_metric_boost_scalar	\(b\)	-	Scale factor controlling the strength of boosting near the target
proximity_metric_boost_length_scale	\(\sigma_b\)	m	Length scale of the Gaussian controlling boosting near the target
xi_estimator_gate_std_dev	-	-	Unused parameter (to be removed in a future release)

Axis Target RMP (axis_target_rmp)

Purpose: Drives x-, y-, or z-axis of end effector frame toward target orientation. This RMP is used for general orientation targets (where an axis target RMP is added for each of the three axes) as well as for “partial pose” targets where only alignment of a single axis is desired.

Note

Partial pose targets are not currently supported by the Motion Generation extension.

Definition:

Similar to the (position) target RMP, the axis target RMP supports “proximity boosting,” but only when a target RMP is active at the same time. In this case, it’s the distance to the position target (\(||x_0-x||\)) that controls the strength of boosting.

The current and desired axis orientations are represented by unit vectors, denoted by \(n\) and \(n_0\) respectively. Acceleration is given by

\[\ddot n = k_p (n_0 - n) - k_d \dot n\,.\]

If a position target (i.e., target RMP) is active, the metric has the form

\[M_\textrm{boosted} = \left[\beta(x) b + (1-\beta(x))\right] \mu I\,,\]

where

\[\beta(x) = \exp \left(-\frac{||x_0 - x||^2}{2 \sigma_b^2}\right)\,.\]

When no position target is active, this simplifies to

\[M = \mu I\,.\]

Parameters:

Name	Symbol	Units	Meaning
accel_p_gain	\(k_p\)	s-2	Position gain
accel_d_gain	\(k_d\)	s-1	Damping gain
metric_scalar	\(\mu\)	-	Priority weight relative to other RMPs
proximity_metric_boost_scalar	\(b\)	-	Scale factor controlling the strength of boosting near the position target
proximity_metric_boost_length_scale	\(\sigma_b\)	m	Length scale of the Gaussian controlling boosting near the position target

Joint Limit RMP (joint_limit_rmp)

Purpose: Avoids joint limits.

Definition: This is a one-dimensional RMP that depends on a single c-space coordinate (joint) and a corresponding upper or lower joint limit as specified in the URDF for the robot. If a robot has \(N\) joints, it follows that a total of \(2N\) joint limit RMPs will be introduced. The joint limits specified in the URDF may be padded (i.e., made more conservative) by entering positive padding values in the joint_limit_buffers array in the RMPflow configuration file. For a given joint, the same padding value is used for both upper and lower limits.

The task space for this RMP consists of a shifted and scaled c-space coordinate, measuring the scaled distance to either the upper or lower joint limit. Without loss of generality, we consider a lower joint limit RMP. If \(q\) is the c-space coordinate for a given joint, and \(q_\textrm{upper}\) and \(q_\textrm{lower}\) are the upper and lower limits for that joint, respectively, we define

\[x = \frac{q - q_\textrm{lower}}{q_\textrm{upper} - q_\textrm{lower}}\,.\]

The acceleration for that coordinate is then given by

\[\ddot x = \frac{k_p}{x^2/\ell_p^2 + \epsilon_p} - k_d \dot x\,.\]

The metric (inertia matrix) is a scalar given by

\[m = \left(1 - \frac{1}{1+\exp(-\dot x/v_m)}\right) \frac{\mu}{x/\ell_m + \epsilon_m}\,.\]

Parameters:

Name	Symbol	Units	Meaning
metric_scalar	\(\mu\)	-	Overall priority weight relative to other RMPs
metric_length_scale	\(\ell_m\)	-	Length scale controlling ramp-up of metric as joint limit is approached
metric_exploder_eps	\(\epsilon_m\)	-	Offset determining \(x\) value at which metric diverges
metric_velocity_gate_length_scale	\(v_m\)	s-1	Scale determining rate at which metric increases with velocity in direction of barrier
accel_damper_gain	\(k_d\)	s-1	Damping gain
accel_potential_gain	\(k_p\)	s-2	Gain multiplying position barrier term
accel_potential_exploder_length_scale	\(\ell_p\)	-	Length scale controlling steepness of position barrier
accel_potential_exploder_eps	\(\epsilon_p\)	-	Offset limiting divergence of position barrier strength

Joint Velocity Limit RMP (joint_velocity_cap_rmp)

Purpose: Limits maximum joint velocity.

Definition: This RMP applies damping when the magnitude of the velocity of a given joint approaches the specified limit.

This is a one-dimensional RMP with acceleration given by

\[\ddot q = -k_d\,\textrm{sgn}(\dot q) \left(|\dot q| - (v_\textrm{max} - v_r)\right)\,.\]

The metric (inertia matrix) is a scalar given by

\[\begin{split}m = \left \{ \begin{array}{cl} 0, & |\dot q| < (v_\textrm{max} - v_r) \\ \frac{\mu}{1 - \left(|\dot q| - (v_\textrm{max} - v_r)\right)^2 / v_r^2} & \textrm{otherwise.} \end{array} \right.\end{split}\]

Note that the metric is zero outside the user-specified damping region, thereby disabling this RMP. In addition, clipping is applied to avoid divergence of the metric as \(\dot q\) approaches \(v_\textrm{max}\).

Parameters:

Units assume revolute joints where \(q\) is expressed in radians. If joints are instead prismatic, max_velocity and velocity_damping_region will have units of m/s.

Name	Symbol	Units	Meaning
max_velocity	\(v_\textrm{max}\)	rad/s	Maximum allowed velocity magnitude
velocity_damping_region	\(v_r\)	rad/s	Defines width of velocity region affect by damping
damping_gain	\(k_d\)	s-1	Damping gain
metric_weight	\(\mu\)	-	Overall priority weight relative to other RMPs

Collision Avoidance RMP (collision_rmp)

Purpose: Avoids collision with obstacles in the environment.

Definition: This is a one-dimensional RMP where the task space consists of a single coordinate measuring distance from a given collision sphere on the robot (specified in the robot description YAML file) to an obstacle in the environment. Denoting that coordinate as \(x\), the acceleration is given by

\[\ddot x = k_p \exp(-x / \ell_p) - k_d \left[1 - \frac{1}{1 + \exp(-\dot x/v_d)} \right] \frac{\dot x}{x/\ell_d + \epsilon_d}\,.\]

The metric (inertia matrix) is a scalar given by

\[m = \left[1 - \frac{1}{1 + \exp(-\dot x/v_d)} \right] g(x) \frac{\mu}{x / \ell_m + \epsilon_m}\,,\]

where \(g(x)\) is a piecewise polynomial that varies smoothly from 1 to 0 as \(x\) varies from 0 to \(r\),

\[\begin{split}g(x) = \left \{ \begin{array}{cl} x^2/r^2 -2s/r + 1, & x\le r \\ 0, & x\gt r \end{array} \right.\end{split}\]

Parameters:

Name	Symbol	Units	Meaning
damping_gain	\(k_d\)	s-1	Damping gain
damping_std_dev	\(\ell_d\)	m	Length scale controlling increase in acceleration as obstacle is approached
damping_robustness_eps	\(\epsilon_d\)	-	Offset determining \(x\) value at which acceleration diverges (before clipping)
damping_velocity_gate_length_scale	\(v_d\)	m/s	Scale determining velocity dependence of “velocity gating” function
repulsion_gain	\(k_p\)	m/s2	Gain for position repulsion term
repulsion_std_dev	\(\ell_p\)	m	Length scale controlling distance dependence of repulsion
metric_modulation_radius	\(r\)	m	Length scale determining distance from obstacle at which RMP is disabled completely
metric_scalar	\(\mu\)	-	Overall priority weight relative to other RMPs
metric_exploder_std_dev	\(\ell_m\)	m	Length scale controlling increase in metric as obstacle is approached
metric_exploder_eps	\(\epsilon_m\)	-	Offset determining \(x\) value at which metric diverges (before clipping)

Damping RMP (damping_rmp)

Purpose: Contributes additional nonlinear damping based on control frame (e.g., end effector) velocity relative to target.

Definition: This is a one-dimensional RMP where the task space consists of a single coordinate \(x\) measuring distance from the origin of the control frame to the target. The acceleration is given by

\[\ddot x = -k_d |\dot x|\dot x\]

and the metric by

\[M = \mu |\dot x| I\,.\]

Finally, the damping_rmp section of the RMPflow configuration file contains an additional inertia parameter \(m\). When this parameter is nonzero, it results in the introduction of a conceptually separate RMP corresponding to zero acceleration, \(\ddot x = 0\), with inertia matrix given by \(M = mI\).

Parameters:

Name	Symbol	Units	Meaning
accel_d_gain	\(k_d\)	m-1	Nonlinear damping gain
metric_scalar	\(\mu\)	(m/s)-1	Priority weight relative to other RMPs
inertia	\(m\)	-	Additional inertia

RMPflow Tuning Guide

Given the number of parameters involved in fully specifying a complete set of RMPs, tuning an RMPflow-based motion policy for a new robot or task can be intimidating. In practice, however, parameters that work well for one robot are likely to work well for other robots with similar morphology. Furthermore, for a given robot, it is generally possible to choose a set of parameters that work well for a wide variety of tasks.

Omniverse Isaac Sim includes example RMPflow configuration files for multiple robot arms, including the 7-DOF Franka Emika Panda and the 6-DOF Universal Robots UR10. When tuning RMPflow for a new manipulator, it’s usually best to start with one of these two files. If the new robot is significantly larger or smaller than the one used as a reference, it may be necessary to rescale any parameters that have units of length; refer to the “Units” column in the parameter tables above. If the number of joints differ, the c-space_target_rmp/robust_position_term_thresh parameter may also have to be adjusted. Often, these steps are sufficient to produce a working motion policy.

If adapting an existing RMPflow configuration fails to produce acceptable results, follow the following procedure to tune a new policy from scratch.

First, turn off all RMPs. Each RMP has a parameter called either metric_weight or metric_scalar. Setting this parameter to zero will disable the corresponding RMP. In the case of the target RMP, set the parameters min_metric_scalar, max_metric_scalar, and min_metric_alpha all to zero.

Also set all inertia terms to zero (viz. c-space_target_rmp/inertia and damping_rmp/inertia).

Next, re-enable RMPs one at a time, in the following suggested order:

1. c-space_target_rmp: The first goal to get the robot moving to a configuration in c-space robustly. The magnitude of the metric scalar should be kept relatively small (e.g., in the range 1 to 100), since this sets the global scale of all RMPs. Remember to set the default configuration in the robot description file (YAML) to a reasonable natural “ready” posture. This will be the default posture that the robot will favor while moving from place to place.

2. target_rmp: The next goal is to get the end effector moving to a target robustly while continuing to use the c-space target RMP for redundancy resolution.

   1. Begin by setting target_rmp/min_metric_alpha to zero and target_rmp/metric_alpha_length_scale to a large value relative to the size of the robot (in meters), such as 100,000. This effectively turns off the directional \(S\) term in the metric, reducing \(M\) to a simpler isotropic metric.

   2. Set target_rmp/proximity_metric_boost_length_scalar to 1 to turn off priority boosting.

   3. Set target_rmp/max_metric_scalar to a large value relative to c-space_target_rmp/metric_scalar so it dominates. This will effectively make the c-space target RMP operate purely in the nullspace of the target RMP.

   4. Tune target_rmp/accel_p_gain, target_rmp/accel_d_gain, and target_rmp/accel_norm_eps until good attractor behavior for the end effector has been achieved.

   5. Experiment with reducing target_rmp/max_metric_scalar to ensure that it’s not too large. As max_metric_scalar is increased toward a suitable value, convergence accuracy should progressively improve. If convergence accuracy saturates at small constant error before the chosen max_metric_scalar value is reached, then it is probably set too high. This will be relevant when re-enabling the directional term in the target RMP metric below, ensuring that it makes a difference when the metric scalar decreases.

3. collision_rmp: Enable the collision avoidance RMP by setting collision_rmp/metric_scalar to a value comparable to target_rmp/max_metric_scalar. It may be useful to plot the formulas for the acceleration and metric in order to gain some understanding of the roles of the various parameters.

4. target_rmp (redux): Once the collision RMP is enabled, the system will probably drag near obstacles more slowly than it usually moves because the target RMP is fighting with the collision RMPs. Turning on the directional term in the metric will correct that effect.

   1. Again, it may be useful to plot the target RMP metric (as a function of distance from target) to build understanding. Try this first without the boosting term, noting how the metric transitions from the reduced-rank far metric to the full-rank near metric.

   2. Set target_rmp/min_metric_alpha to a non-zero value, and reduce the value of target_rmp/metric_alpha_length_scale until good behavior is achieved.

5. axis_target_rmp: If an orientation target is set, the axis target RMP will be used to bring the orientation of the control frame (e.g., end effector) into alignment with the target orientation. Note that this RMP includes a “priority boosting” factor that depends on distance to the current position target, if one is set. This allows the robot to first make progress toward the position target before zeroing in on the desired orientation.

6. joint_limit_rmp: When properly tuned, behavior should be essentially unchanged, except that joint limits will be avoided.

7. damping_rmp: Enable the damping RMP as well as target_rmp/inertia to reduce jerk as necessary.

Throughout this process, referring to an existing RMPflow configuration file is likely to be helpful.

Tutorials & Examples

The following tutorials and examples showcase how to best use this extension:

Tutorials

• Lula RMPflow Tutorial

• Lula RRT Tutorial

• Lula Kinematics Solver Tutorial

Examples

• Follow Target Standalone Example: standalone_examples/api/omni.isaac.franka/follow_target_with_rmpflow.py

• Follow Target Standalone Example: standalone_examples/api/omni.isaac.franka/follow_target_with_ik.py

• Follow Target Example: Isaac Examples > Manipulation > Follow Target

• Follow Target Example: Isaac Examples > Multi-Robot > RoboFactory

• Follow Target Example: Isaac Examples > Multi-Robot > RoboParty

References

1

Cheng, C.A., Mukadam, M., Issac, J., Birchfield, S., Fox, D., Boots, B., Ratliff, N., RMPflow: A computational graph for automatic motion policy generation (2018), https://arxiv.org/abs/1811.07049

2

J. J. Kuffner and S. M. LaValle, “RRT-connect: An efficient approach to single-query path planning,” Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), 2000, pp. 995-1001 vol.2, doi: 10.1109/ROBOT.2000.844730.

3

M. Vande Weghe, D. Ferguson and S. S. Srinivasa, “Randomized path planning for redundant manipulators without inverse kinematics,” 2007 7th IEEE-RAS International Conference on Humanoid Robots, 2007, pp. 477-482, doi: 10.1109/ICHR.2007.4813913.