Add OperationSpaceController to docs and tests and implement corresponding action/action_cfg classes

docs/source/api/lab/omni.isaac.lab.controllers.rst

@@ -9,6 +9,8 @@
 
     DifferentialIKController
     DifferentialIKControllerCfg
+    OperationSpaceController
+    OperationSpaceControllerCfg
 
 Differential Inverse Kinematics
 -------------------------------
@@ -23,3 +25,17 @@ Differential Inverse Kinematics
     :inherited-members:
     :show-inheritance:
     :exclude-members: __init__, class_type
+
+Operational Space controllers
+-----------------------------
+
+.. autoclass:: OperationalSpaceController
+    :members:
+    :inherited-members:
+    :show-inheritance:
+
+.. autoclass:: OperationalSpaceControllerCfg
+    :members:
+    :inherited-members:
+    :show-inheritance:
+    :exclude-members: __init__, class_type

docs/source/tutorials/05_controllers/run_osc.rst

@@ -0,0 +1,175 @@
+Using an operational space controller
+=============================
+
+.. currentmodule:: omni.isaac.lab
+
+Sometimes, controlling the end-effector pose of the robot using a differential IK controller is not sufficient.
+For example, we might want to enforce a very specific pose tracking error dynamics in the task space, actuate the robot
+with joint effort/torque commands, or apply a contact force at a specific direction while controlling the motion of
+the other directions (e.g., washing the surface of the table with a cloth). In such tasks, we can use an
+operational space controller (OSC).
+
+Reference for the operational space control:
+- [1] O Khatib. A unified approach for motion and force control of robot manipulators: The operational space
+formulation. IEEE Journal of Robotics and Automation, 3(1):43–53, 1987. URL http://dx.doi.org/10.1109/JRA.1987.1087068.
+- [2] https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf
+
+In this tutorial, we will learn how to use an OSC to control the robot.
+We will use the :class:`controllers.OperationalSpaceController` class to apply a constant force perpendicular to a
+tilted wall surface while tracking a desired end-effector pose in all the other directions.
+
+The Code
+~~~~~~~~
+
+The tutorial corresponds to the ``run_osc.py`` script in the
+``source/standalone/tutorials/05_controllers`` directory.
+
+
+.. dropdown:: Code for run_osc.py
+   :icon: code
+
+   .. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py
+      :language: python
+      :emphasize-lines: 122-134
+      :linenos:
+
+
+Creating an Operational Space Controller
+----------------------------------------
+
+The :class:`~controllers.OperationalSpaceController` class computes the joint
+efforts/torques for a robot to do simultaneous motion and force control in task space.
+
+The reference frame of this task space could be an arbitrary coordinate frame in Euclidean space. By default,
+it is the robot's base frame. However, in certain cases, it could be easier to define target coordinates w.r.t. a
+different frame. In such cases, the pose of this task reference frame, w.r.t. to the robot's base frame, should be
+provided in the set_command method's current_task_frame_pose_b argument. For example, in this tutorial, it makes sense
+to define the target commands w.r.t. a frame that is parallel to the wall surface, as the force control direction
+would be then only nonzero in the z-axis of this frame. The target pose, which is set to have the same orientation
+as the wall surface, is such a candidate and is used as the task frame in this tutorial. Therefore, all the arguments
+to the :class:`~controllers.OperationalSpaceControllerCfg` should be set with this task reference frame in mind.
+
+For the motion control, the task space targets could be given as absolute (i.e., defined w.r.t. the robot base,
+target_types: "pose_abs") or relative the the end-effector's current pose (i.e., target_types: "pose_rel").
+For the force control, the task space targets could be given as absolute (i.e., defined w.r.t. the robot base,
+target_types: "force_abs"). If it is desired to apply pose and force control simultaneously, the target_types
+should be a list such as ["pose_abs", "wrench_abs"] or ["pose_rel", "wrench_abs"].
+
+The axes that the motion and force control will be applied can be specified using the motion_control_axes_task and
+force_control_axes_task arguments, respectively. These lists should consist of 0/1 for all six axes (position and
+rotation) and be complementary to each other (e.g., for the x-axis, if the motion_control_axes_task is 0, the
+force_control_axes_task should be 1).
+
+For the motion control axes, desired stiffness, and damping ratio values can be specified using the
+motion_control_stiffness and motion_damping_ratio_task arguments, which can be a scalar (same value for all axes)
+or a list of six scalars, one value corresponding to each axis. If desired, the stiffness and damping ratio values
+could be a command parameter (e.g., to learn the values using RL or change them on the go). For this,
+impedance_mode should be either "variable_kp" to include the stiffness values within the command or "variable" to
+include both the stiffness and damping ratio values. In these cases, motion_stiffness_limits_task and
+motion_damping_limits_task should be set as well, which puts bounds on the stiffness and damping ratio values.
+
+For contact force control, it is possible to apply an open-loop force control by not setting the
+contact_wrench_stiffness_task, or apply a closed-loop force control (with the feed-forward term) by setting
+the desired stiffness values using the contact_wrench_stiffness_task argument, which can be a scalar or a list
+of six scalars. Please note that, currently, only the linear part of the contact wrench (hence the first three
+elements of the contact_wrench_stiffness_task) is considered in the closed-loop control, as the rotational part
+cannot be measured with the contact sensors.
+
+For the motion control, inertial_dynamics_decoupling should be set to True to use the robot's inertia matrix
+to decouple the desired accelerations in the task space. This is important for the motion control to be accurate,
+especially for rapid movements. This inertial decoupling accounts for the coupling between all the six motion axes.
+If desired, the inertial coupling between the translational and rotational axes could be ignored by setting the
+partial_inertial_dynamics_decoupling to True.
+
+If it is desired to include the gravity compensation in the operational space command, the gravity_compensation
+should be set to True.
+
+The included OSC implementation performs the computation in a batched format and uses PyTorch operations.
+
+In this tutorial, we will use "pose_abs" for controlling the motion in all axes except the z-axis and "wrench_abs"
+for controlling the force in the z-axis. Moreover, we will include the full inertia decoupling in the motion control
+and not include the gravity compensation, as the gravity is disabled from the robot configuration. Finally, we set the
+impedance mode to "variable_kp" to dynamically change the stiffness values (motion_damping_ratio_task is set to 1: the
+kd values adapt according to kp values to maintain a critically damped response).
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py
+   :language: python
+   :start-at: # Create the OSC
+   :end-at: osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device)
+
+Updating the states of the robot
+--------------------------------------------
+
+The OSC implementation is a computation-only class. Thus, it expects the user to provide the necessary information
+about the robot. This includes the robot's Jacobian matrix, mass/inertia matrix, end-effector pose, velocity, and
+contact force, all in the root frame. Moreover, the user should provide gravity compensation vector, if desired.
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py
+   :language: python
+   :start-at: # Update robot states
+   :end-before: return jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b
+
+
+Computing robot command
+-----------------------
+
+The OSC separates the operation of setting the desired command and computing the desired joint positions.
+To set the desired command, the user should provide command vector, which  includes the target commands
+(i.e., in the order they appear in the target_types argument of the OSC configuration),
+and the desired stiffness and damping ratio values if the impedance_mode is set to "variable_kp" or "variable".
+They should be all in the same coordinate frame as the task frame (e.g., indicated with _task subscript) and
+concatanated together.
+
+In this tutorial, the desired wrench is already defined w.r.t. the task frame, and the desired pose is transformed
+to the task frame as the following:
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py
+   :language: python
+   :start-at: # Convert the target commands to the task frame
+   :end-at: return command, task_frame_pose_b
+
+The OSC command is set with the command vector in the task frame, the end-effector pose in the base frame, and the
+task (reference) frame pose in the base frame as the following. This information is needed, as the internal
+computations are done in the base frame.
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py
+   :language: python
+   :start-at: # set the osc command
+   :end-at: osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b)
+
+The joint effort/torque values are computed using the provided robot states and the desired command as the following:
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py
+   :language: python
+   :start-at: # compute the joint commands
+   :end-at: # apply actions
+
+
+The computed joint effort/torque targets can then be applied on the robot.
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py
+   :language: python
+   :start-at: # apply actions
+   :end-at: robot.write_data_to_sim()
+
+
+The Code Execution
+~~~~~~~~~~~~~~~~~~
+
+You can now run the script and see the result:
+
+.. code-block:: bash
+
+   ./isaaclab.sh -p source/standalone/tutorials/05_controllers/run_osc.py --num_envs 128
+
+The script will start a simulation with 128 robots. The robots will be controlled using the OSC.
+The current and desired end-effector poses should be displayed using frame markers in addition to the red tilted wall.
+You should see that the robot reaches the desired pose while applying a constant force perpendicular to the wall
+surface.
+
+.. figure:: ../../_static/tutorials/tutorial_operational_space_controller.jpg
+    :align: center
+    :figwidth: 100%
+    :alt: result of run_osc.py
+
+To stop the simulation, you can either close the window or press ``Ctrl+C`` in the terminal.

docs/source/tutorials/index.rst

@@ -101,3 +101,4 @@ tutorials show you how to use motion generators to control the robots at the tas
     :titlesonly:
 
     05_controllers/run_diff_ik
+    05_controllers/run_osc

source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/__init__.py

@@ -13,3 +13,5 @@
 
 from .differential_ik import DifferentialIKController
 from .differential_ik_cfg import DifferentialIKControllerCfg
+from .operational_space import OperationalSpaceController
+from .operational_space_cfg import OperationalSpaceControllerCfg