4. Lula Kinematics Solver

4.1. Learning Objectives

This tutorial shows how the Lula Kinematics Solver class is used to compute forward and inverse kinematics on a robot in Omniverse Isaac Sim. First, this tutorial shows how a LulaKinematicsSolver can be instantiated and used alongside the Articulation Kinematics Solver helper class to compute forward and inverse kinematics and apply them to the robot Articulation. Next, this tutorial shows how the implementation can be simplified on supported robots.

4.2. Getting Started

Prerequisites

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

4.3. Using the LulaKinematicsSolver to Compute Forward and Inverse Kinematics

The Lula Kinematics Solver is able to calculate forward and inverse kinematics for a robot that is defined by two configuration files (see Lula Kinematics Solver Configuration). The LulaKinematicsSolver may be paired with an Articulation Kinematics Solver to compute kinematics in a way that can be directly applied to the robot Articulation.

The following python script uses forward kinematics to visualize the position of the “panda_hand” frame in the Franka robot (blue cuboid), and it uses inverse kinematics to bring the “panda_hand” frame to a target position (red cuboid).

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

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

from omni.isaac.franka.tasks import FollowTarget
from omni.isaac.motion_generation import LulaKinematicsSolver, ArticulationKinematicsSolver
from omni.isaac.core import World
import carb
from omni.isaac.core.utils.extensions import get_extension_path_from_name
from omni.isaac.core.objects.cuboid import VisualCuboid
from omni.isaac.core.utils.numpy.rotations import rot_matrices_to_quats
import os
import numpy as np

my_world = World(stage_units_in_meters=1.0)
my_task = FollowTarget(name="follow_target_task")
my_world.add_task(my_task)
my_world.reset()
task_params = my_world.get_task("follow_target_task").get_params()
franka_name = task_params["robot_name"]["value"]
target_name = task_params["target_name"]["value"]
my_franka = my_world.scene.get_object(franka_name)

# LulaKinematicsSolver config files for supported robots use the same config files as rmpflow
mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
kinematics_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")

lula_kinematics_solver = LulaKinematicsSolver(
    robot_description_path = kinematics_config_dir + "/franka/rmpflow/robot_descriptor.yaml",
    urdf_path = kinematics_config_dir + "/franka/lula_franka_gen.urdf"
)

print("Valid frame names at which to compute kinematics:", lula_kinematics_solver.get_all_frame_names())

end_effector_name = "panda_hand"
articulation_kinematics_solver = ArticulationKinematicsSolver(my_franka,lula_kinematics_solver,end_effector_name)

#Query the position of the "panda_hand" frame
ee_position,ee_rot_mat = articulation_kinematics_solver.compute_end_effector_pose()
ee_orientation = rot_matrices_to_quats(ee_rot_mat)

#Create a cuboid to visualize where the "panda_hand" frame is according to the kinematics"
panda_hand_visualization = VisualCuboid("/panda_hand",position=ee_position,orientation=ee_orientation,size=np.array([.05,.1,.05]),color=np.array([0,0,1]))

articulation_controller = my_franka.get_articulation_controller()
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()

        #Track any movements of the robot base
        robot_base_translation,robot_base_orientation = my_franka.get_world_pose()
        lula_kinematics_solver.set_robot_base_pose(robot_base_translation,robot_base_orientation)

        #Use forward kinematics to update the position of the panda_hand_visualization
        ee_position,ee_rot_mat = articulation_kinematics_solver.compute_end_effector_pose()
        ee_orientation = rot_matrices_to_quats(ee_rot_mat)
        panda_hand_visualization.set_world_pose(ee_position,ee_orientation)

        observations = my_world.get_observations()
        actions, success = articulation_kinematics_solver.compute_inverse_kinematics(
            target_position=observations[target_name]["position"],
            target_orientation=observations[target_name]["orientation"],
        )
        if success:
            articulation_controller.apply_action(actions)
        else:
            carb.log_warn("IK did not converge to a solution.  No action is being taken.")

simulation_app.close()

The LulaKinematicsSolver is instantiated on lines 25-31 using file paths to the appropriate configuration files. The LulaKinematicsSolver uses the same robot description files as the Lula-based RMPflow Motion Policy. The LulaKinematicsSolver can solver forward and inverse kinematics at any frame that exists in the robot URDF file (line 30). On line 33, the complete list of recognized frames in the Franka robot is printed:

Valid frame names at which to compute kinematics:
['base_link', 'panda_link0', 'panda_link1', 'panda_link2', 'panda_link3', 'panda_link4', 'panda_forearm_end_pt', 'panda_forearm_mid_pt',
 'panda_forearm_mid_pt_shifted', 'panda_link5', 'panda_forearm_distal', 'panda_link6', 'panda_link7', 'panda_link8', 'panda_hand',
 'camera_bottom_screw_frame', 'camera_link', 'camera_depth_frame', 'camera_color_frame', 'camera_color_optical_frame', 'camera_depth_optical_frame',
 'camera_left_ir_frame', 'camera_left_ir_optical_frame', 'camera_right_ir_frame', 'camera_right_ir_optical_frame', 'panda_face_back_left',
 'panda_face_back_right', 'panda_face_left', 'panda_face_right', 'panda_leftfinger', 'panda_leftfingertip', 'panda_rightfinger', 'panda_rightfingertip', 'right_gripper', 'panda_wrist_end_pt']

On line 36, an Articulation Kinematics Solver is instantiated with the franka robot Articulation, the LulaKinematicsSolver instance, and the end effector name. The ArticulationKinematicsSolver class allows the user to compute the end effector position and orientation for the robot Articulation in a single line (lines 39 and 57). This is done in the script to update the position of a blue cuboid that tracks the position of the “panda_hand” frame.

The ArticulationKinematicsSolver also allows the user to compute inverse kinematics. The current position of the robot Articulation is used as a warm start in the IK calculation, and the result is returned as an ArticulationAction that can be consumed by the robot Articulation to move the specified end effector frame to a target position.

The LulaKinematicsSolver returns a flag marking the success or failure of the inverse kinematics computation. On line 67, the script applies the inverse kinematics solution to the robot Articulation only if the kinematics converged successfully to a solution, otherwise no new action is sent to the robot, and a warning is thrown. The LulaKinematicsSolver exposes settings that allow the user to specify how quickly it should terminate its search. These settings are outside the scope of this tutorial.

The LulaKinematicsSolver assumes that the robot base is positions at the origin unless another location is specified. On lines 53-54, the LulaKinematicsSolver is given the current position of the robot base on every frame. This allows the forward and inverse kinematics to operate using world coordinates. For example, the position of the target is queried in world coordinates and passed to the LulaKinematicsSolver, which internally performs the necessary transformation to compute accurate inverse kinematics. Similarly, the end effector position is returned in world coordinates, allowing the panda_hand_visualization cuboid to be directly moved to the output position.

The LulaKinematicsSolver may be used on its own to compute forward kinematics at any position and to compute inverse kinematics with any warm start. A robot Articulation does not need to be present on the USD stage. Using the LulaKinematicsSolver in this way is outside the scope of this tutorial. See Kinematics Solver for more details.

../_images/isaac_sim_kinematics.gif

4.4. Simplifying the Implementation on Supported Robots

On supported robots (Franka, UR10, and Dofbot), an ArticulationKinematicsSolver may be loaded in a single line with an appropriately configured LulaKinematicsSolver. The following python script simplifies the use of a LulaKinematicsSolver on the Franka robot by using a preconfigured, importable class.

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

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

from omni.isaac.franka.tasks import FollowTarget
from omni.isaac.core import World
from omni.isaac.franka import KinematicsSolver as FrankaKinematicsSolver
from omni.isaac.core.objects.cuboid import VisualCuboid
from omni.isaac.core.utils.numpy.rotations import rot_matrices_to_quats
import numpy as np

my_world = World(stage_units_in_meters=1.0)
my_task = FollowTarget(name="follow_target_task")
my_world.add_task(my_task)
my_world.reset()
task_params = my_world.get_task("follow_target_task").get_params()
franka_name = task_params["robot_name"]["value"]
target_name = task_params["target_name"]["value"]
my_franka = my_world.scene.get_object(franka_name)

#If not specified, the FrankaKinematicsSolver will select the "right_gripper" frame at the end of the robot
end_effector_name = "panda_hand"
articulation_kinematics_solver = FrankaKinematicsSolver(my_franka, end_effector_frame_name=end_effector_name)

#The LulaKinematicsSolver can be accessed easily
lula_kinematics_solver = articulation_kinematics_solver.get_kinematics_solver()

#Query the position of the "panda_hand" frame
ee_position,ee_rot_mat = articulation_kinematics_solver.compute_end_effector_pose()
ee_orientation = rot_matrices_to_quats(ee_rot_mat)

#Create a cuboid to visualize where the "panda_hand" frame is according to the kinematics"
panda_hand_visualization = VisualCuboid("/panda_hand",position=ee_position,orientation=ee_orientation,size=np.array([.05,.1,.05]),color=np.array([0,0,1]))

articulation_controller = my_franka.get_articulation_controller()
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()

        #Track any movements of the robot base
        robot_base_translation,robot_base_orientation = my_franka.get_world_pose()
        lula_kinematics_solver.set_robot_base_pose(robot_base_translation,robot_base_orientation)

        #Use forward kinematics to update the position of the panda_hand_visualization
        ee_position,ee_rot_mat = articulation_kinematics_solver.compute_end_effector_pose()
        ee_orientation = rot_matrices_to_quats(ee_rot_mat)
        panda_hand_visualization.set_world_pose(ee_position,ee_orientation)

        observations = my_world.get_observations()
        actions, success = articulation_kinematics_solver.compute_inverse_kinematics(
            target_position=observations[target_name]["position"],
            target_orientation=observations[target_name]["orientation"],
        )
        articulation_controller.apply_action(actions)

simulation_app.close()

The omni.isaac.franka.KinematicsSolver class (imported on line 7) inherits from ArticulationKinematicsSolver and takes care of internally loading an appropriately configured LulaKinematicsSolver. It is instantiated simply on line 23, and the inernal LulaKinematicsSolver object is accessed for use on line 26. This code is identical in behavior to the code in the previous section.

4.5. Summary

This tutorial reviews how to load the LulaKinematicsSolver class and use it alongside the ArticulationKinematicsSolver helper class to visualize the position of a specific frame in the Franka robot using forward kinematics and to bring that frame to a target position with inverse kinematics. The tutorial first reviews how a LulaKinematicsSolver may be instantiated directly using paths to appropriate configuration files, and then shows how it may be loaded automatically on supported robots.

4.5.1. Further Learning

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