11. Lula Trajectory Generator

11.1. Learning Objectives

This tutorial shows how the Lula Trajectory Generator in the Motion Generation extension can be used to create both task-space and c-space trajectories that can be easily applied to a simulated robot Articulation.

11.2. Getting Started

Prerequisites

This tutorial includes multiple variations of a standalone script that loads a UR10 robot and has it follow a trajectory. Each of these variations may be saved as a Python file and run as demonstrated in the Standalone Application tutorial.

11.3. Generating a C-Space Trajectory

The LulaCSpaceTrajectoryGenerator class is able to generate a trajectory that connects a provided set of c-space waypoints. The code snippet below demonstrates how, given appropriate config files, the LulaCSpaceTrajectoryGenerator class can be initialized and used to create a sequence of ArticulationAction that can be set on each frame to produce the desired trajectory.

 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
from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.motion_generation.lula import LulaCSpaceTrajectoryGenerator, LulaKinematicsSolver
from omni.isaac.motion_generation import ArticulationTrajectory
from omni.isaac.core import World
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.utils.extensions import get_extension_path_from_name
from omni.isaac.core.utils.numpy.rotations import rot_matrices_to_quats
from omni.isaac.core.prims import XFormPrim
import os
import numpy as np

my_world = World(stage_units_in_meters=1.0)

robot_prim_path = "/ur10"
usd_path = get_assets_root_path() + "/Isaac/Robots/UniversalRobots/ur10/ur10.usd"
end_effector_name = "ee_link"

add_reference_to_stage(usd_path, robot_prim_path)
my_robot = Articulation(robot_prim_path)

my_world.reset()
my_robot.initialize()

# Lula config files for supported robots are stored in the motion_generation extension under "/motion_policy_configs"
mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")

#Initialize a LulaCSpaceTrajectoryGenerator object
c_space_trajectory_generator = LulaCSpaceTrajectoryGenerator(
    robot_description_path = rmp_config_dir + "/universal_robots/ur10/rmpflow/ur10_robot_description.yaml",
    urdf_path = rmp_config_dir + "/universal_robots/ur10/ur10_robot.urdf"
)

# 6 DOF c_space points selected from within the UR10's joint limits
c_space_points = np.array([
    [-0.41 , 0.5 , -2.36 , -1.28 , 5.13 , -4.71 , ],
    [-1.43 , 1.0 , -2.58 , -1.53 , 6.14 , -4.74 , ],
    [-2.83 , 0.34 , -2.11 , -1.38 , 1.26 , -4.71 , ]
    ])
trajectory = c_space_trajectory_generator.compute_c_space_trajectory(c_space_points)
if trajectory is None:
    print("No trajectory could be generated!")
    exit(0)

# Use the ArticulationTrajectory wrapper to run Trajectory on UR10 robot Articulation
physics_dt = 1/60. # Physics steps are fixed at 60 fps when running a standalone script
articulation_trajectory = ArticulationTrajectory(my_robot,trajectory,physics_dt)

# Returns a list of ArticulationAction meant to be set on subsequent physics steps
action_sequence = articulation_trajectory.get_action_sequence()


#Create frames to visualize the c_space targets of the UR10
lula_kinematics_solver = LulaKinematicsSolver(
    robot_description_path = rmp_config_dir + "/universal_robots/ur10/rmpflow/ur10_robot_description.yaml",
    urdf_path = rmp_config_dir + "/universal_robots/ur10/ur10_robot.urdf"
)
for i,point in enumerate(c_space_points):
    position,rotation = lula_kinematics_solver.compute_forward_kinematics(end_effector_name, point)
    add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", f"/target_{i}")
    frame = XFormPrim(f"/target_{i}",scale=[.04,.04,.04])
    frame.set_world_pose(position,rot_matrices_to_quats(rotation))



#Run the action sequence on a loop
articulation_controller = my_robot.get_articulation_controller()
action_index = 0
while simulation_app.is_running():
    my_world.step(render=True)
    if my_world.is_playing():
        if action_index == 0:
            #Teleport robot to the initial state of the trajectory
            my_robot.set_joint_positions(c_space_points[0])
            my_robot.set_joint_velocities(np.zeros_like(c_space_points[0]))

        articulation_controller.apply_action(action_sequence[action_index])

        action_index += 1
        action_index %= len(action_sequence)

simulation_app.close()

On lines 33-37, the LulaCSpaceTrajectoryGenerator class is initialized. It is then used to generate an instance of the Trajectory Interface that connects a series of c_space waypoints (see lines 40-48). This Trajectory object stores the desired trajectory, but it needs to be wrapped in an Articulation Trajectory to interface with the robot Articulation (lines 50-52). The ArticulationTrajectory is used on line 52 to generate a sequence of ArticulationAction that can be directly passed to the robot Articulation on subsequent frames.

If no trajectory can be computed that connects the c-space waypoints, the trajectory returned by LulaCSpaceTrajectoryGenerator.compute_c_space_trajectory will be None. This can occur when one of the specified c-space waypoints is not reachable or is very close to a joint limit. On lines 46-48, the output trajectory is checked, and if it does not exist, the simulation is closed.

One lines 59-67, a visualization of the original c_space_points is created by converting them to task-space points. This code is not functional, but it helps to verify that the robot is hitting every target.

In the main loop on lines 71-85, the trajectory is rolled out on loop forever. Each action in the action_sequence is commanded for exactly one frame, and when the trajectory is on its first frame, the robot Articulation is teleported to the initial state of the trajectory.

../_images/isaac_sim_c_space_trajectory.gif

11.4. Generating a Task-Space Trajectory

11.4.1. Simple Case: Linearly Connecting Waypoints

Generating a task-space trajectory is similar to generating a c-space trajectory. In the simplest use-case, the user may pass in a set of task-space position and quaternion orientation targets, which will be linearly interpolated in task-space to produce the resulting trajectory. An example is provided in the code snippet below.

 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
from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.motion_generation.lula import LulaTaskSpaceTrajectoryGenerator
from omni.isaac.motion_generation import ArticulationTrajectory
from omni.isaac.core import World
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.utils.extensions import get_extension_path_from_name
from omni.isaac.core.prims import XFormPrim
import os
import numpy as np

my_world = World(stage_units_in_meters=1.0)

robot_prim_path = "/ur10"
usd_path = get_assets_root_path() + "/Isaac/Robots/UniversalRobots/ur10/ur10.usd"
end_effector_name = "ee_link"

add_reference_to_stage(usd_path, robot_prim_path)
my_robot = Articulation(robot_prim_path)

my_world.reset()
my_robot.initialize()

# Lula config files for supported robots are stored in the motion_generation extension under "/motion_policy_configs"
mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")

#Initialize a LulaCSpaceTrajectoryGenerator object
task_space_trajectory_generator = LulaTaskSpaceTrajectoryGenerator(
    robot_description_path = rmp_config_dir + "/universal_robots/ur10/rmpflow/ur10_robot_description.yaml",
    urdf_path = rmp_config_dir + "/universal_robots/ur10/ur10_robot.urdf"
)

# Choose reachable position and orientation targets
task_space_position_targets = np.array([
    [0.3, -0.3, 0.1],
    [0.3, 0.3, 0.1],
    [0.3, 0.3, 0.5],
    [0.3, -0.3, 0.5],
    [0.3, -0.3, 0.1]
    ])
task_space_orientation_targets = np.tile(np.array([0,1,0,0]),(5,1))
trajectory = task_space_trajectory_generator.compute_task_space_trajectory_from_points(
    task_space_position_targets, task_space_orientation_targets, end_effector_name
)
if trajectory is None:
    print("No trajectory could be generated!")
    exit(0)

# Use the ArticulationTrajectory wrapper to run Trajectory on UR10 robot Articulation
physics_dt = 1/60.
articulation_trajectory = ArticulationTrajectory(my_robot,trajectory,physics_dt)

# Returns a list of ArticulationAction meant to be set on subsequent physics steps
action_sequence = articulation_trajectory.get_action_sequence()


#Create frames to visualize the task_space targets of the UR10
for i in range(len(task_space_position_targets)):
    add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", f"/target_{i}")
    frame = XFormPrim(f"/target_{i}",scale=[.04,.04,.04])
    position = task_space_position_targets[i]
    orientation = task_space_orientation_targets[i]
    frame.set_world_pose(position,orientation)


#Run the action sequence on a loop
articulation_controller = my_robot.get_articulation_controller()
action_index = 0
while simulation_app.is_running():
    my_world.step(render=True)
    if my_world.is_playing():
        if action_index == 0:
            #Teleport robot to the initial state of the trajectory
            initial_state = action_sequence[0]
            my_robot.set_joint_positions(initial_state.joint_positions)
            my_robot.set_joint_velocities(initial_state.joint_velocities)

        articulation_controller.apply_action(action_sequence[action_index])

        action_index += 1
        action_index %= len(action_sequence)

simulation_app.close()

In moving to the task-space trajectory generator, there are few code changes required. The initialization is nearly the same on line 33 as for the c-space trajectory generator. The main changes are on lines 38-49 where a task-space trajectory is specified. When using the function LulaTaskSpaceTrajectoryGenerator.compute_task_space_trajectory_from_points, a position and orientation target must be specified for each task-space waypoint. Additionally, a frame from the robot URDF must be specified as the end effector frame. If the waypoints cannot be connected to form a trajectory, the compute_task_space_trajectory_from_points function will return None. This case is checked on lines 50-52.

There are minor changes in the rest of the code snippet from the c-space trajectory case, but they are not significant enough to discuss in this tutorial.

../_images/isaac_sim_task_space_trajectory.gif

11.4.2. Defining Complicated Trajectories

The LulaTaskSpaceTrajectoryGenerator can be used to create paths with more complicated specifications than to connect a set of task-space targets linearly. Using the class lula.TaskSpacePathSpec, the user may define paths with arcs and circles with multiple options for orientation targets. The code snippet below demonstrates creating a lula.TaskSpacePathSpec and gives an example of each available function for adding to a task-space path.

  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
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.motion_generation.lula import LulaTaskSpaceTrajectoryGenerator
from omni.isaac.motion_generation import ArticulationTrajectory
from omni.isaac.core import World
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.utils.extensions import get_extension_path_from_name
from omni.isaac.core.prims import XFormPrim
import os
import numpy as np
import lula

my_world = World(stage_units_in_meters=1.0)

robot_prim_path = "/ur10"
usd_path = get_assets_root_path() + "/Isaac/Robots/UniversalRobots/ur10/ur10.usd"
end_effector_name = "ee_link"

add_reference_to_stage(usd_path, robot_prim_path)
my_robot = Articulation(robot_prim_path)

my_world.reset()
my_robot.initialize()

# Lula config files for supported robots are stored in the motion_generation extension under "/motion_policy_configs"
mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")

#Initialize a LulaCSpaceTrajectoryGenerator object
task_space_trajectory_generator = LulaTaskSpaceTrajectoryGenerator(
    robot_description_path = rmp_config_dir + "/universal_robots/ur10/rmpflow/ur10_robot_description.yaml",
    urdf_path = rmp_config_dir + "/universal_robots/ur10/ur10_robot.urdf"
)


# The following code demonstrates how to specify a complicated task-space path
# using the lula.TaskSpacePathSpec object

#Lula has its own classes for Rotations and 6 DOF poses: Rotation3 and Pose3
r0 = lula.Rotation3(np.pi, np.array([1.0, 0.0, 0.0]))
t0 = np.array([.3,-.3,.3])
spec = lula.create_task_space_path_spec(lula.Pose3(r0,t0))

# Add path linearly interpolating between r0,r1 and t0,t1
t1 = np.array([.3,-.3,.5])
r1 = lula.Rotation3(np.pi/2,np.array([1,0,0]))
spec.add_linear_path(lula.Pose3(r1, t1))

# Add pure translation.  Constant rotation is assumed
spec.add_translation(t0)

# Add pure rotation.
spec.add_rotation(r0)

# Add three-point arc with constant orientation.
t2 = np.array([.3,.3,.3,])
midpoint = np.array([.3,0,.5])
spec.add_three_point_arc(t2, midpoint, constant_orientation = True)

# Add three-point arc with tangent orientation.
spec.add_three_point_arc(t0, midpoint, constant_orientation = False)

# Add three-point arc with orientation target.
spec.add_three_point_arc_with_orientation_target(lula.Pose3(r1, t2), midpoint)

# Add tangent arc with constant orientation. Tangent arcs are circles that connect two points
spec.add_tangent_arc(t0, constant_orientation = True)

# Add tangent arc with tangent orientation.
spec.add_tangent_arc(t2, constant_orientation=False)

# Add tangent arc with orientation target.
spec.add_tangent_arc_with_orientation_target(
    lula.Pose3(r0, t0))

trajectory = task_space_trajectory_generator.compute_task_space_trajectory_from_path_spec(
    spec, end_effector_name
)
if trajectory is None:
    print("No trajectory could be generated!")
    exit(0)

# Use the ArticulationTrajectory wrapper to run Trajectory on UR10 robot Articulation
physics_dt = 1/60.
articulation_trajectory = ArticulationTrajectory(my_robot,trajectory,physics_dt)

# Returns a list of ArticulationAction meant to be set on subsequent physics steps
action_sequence = articulation_trajectory.get_action_sequence()


#Run the action sequence on a loop
articulation_controller = my_robot.get_articulation_controller()
action_index = 0
while simulation_app.is_running():
    my_world.step(render=True)
    if my_world.is_playing():
        if action_index == 0:
            #Teleport robot to the initial state of the trajectory
            initial_state = action_sequence[0]
            my_robot.set_joint_positions(initial_state.joint_positions)
            my_robot.set_joint_velocities(initial_state.joint_velocities)

        articulation_controller.apply_action(action_sequence[action_index])

        action_index += 1
        action_index %= len(action_sequence)

simulation_app.close()

The code snippet above moves mainly between three translations: t0, t1, t2 with possible rotations r0, r1. The lula.TaskSpacePathSpec object is created with an initial position on line 46. Each following add function that is called adds a path between the last position in the path_spec so far and a new position.

The basic possibilities are:

Linearly interpolate translation to a new point while keeping rotation fixed (line 54)

Linearly interpolate rotation to a new point while keeping translation fixed (line 57)

Linearly interpolate both rotation and translation to a new 6 DOF point (line 51)

The lula.TaskSpacePathSpec also makes it easy to define various arcs and circular paths that connect points in space. A three-point arc can be defined that moves through a midpoint to a translation target. There are three options for the orientation of the robot while moving along the path:

Keep rotation constant (line 62)

Always stay oriented tangent to the arc (line 65)

Linearly interpolate rotation to a rotation target (line 68)

Finally, a circular path may be specified without defining a midpoint as on lines 71, 74, and 77. The same three options for specifying orientation are available.

../_images/isaac_sim_task_space_path_spec.gif

11.5. Summary

This tutorial shows how to use the Lula Trajectory Generator to generate c-space and task-space trajectories for a robot. Task-space trajectories may be specified simply using a series of task-space waypoints that will be connected linearly, or they may be defined piecewise with many different options for connecting each pair of points in space.

11.5.1. Further Learning

Reference the Motion Generation page for a complete description of trajectories in Omniverse Isaac Sim.