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
+    OperationalSpaceController
+    OperationalSpaceControllerCfg
 
 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,178 @@
+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).
+
+.. rubric:: References 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. Robot Dynamics Lecture Notes by Marco Hutter (ETH Zurich). URL 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
+      :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: )
+
+
+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.png
+    :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/docs/CHANGELOG.rst

@@ -1,6 +1,23 @@
 Changelog
 ---------
 
+## [Unreleased]
+~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed errors within the calculations of :class:`omni.isaac.lab.controllers.OperationalSpaceController`.
+
+Added
+^^^^^
+
+* Added :class:`omni.isaac.lab.controllers.OperationalSpaceController` to API documentation.
+* Added test cases for :class:`omni.isaac.lab.controllers.OperationalSpaceController`.
+* Added a tutorial for :class:`omni.isaac.lab.controllers.OperationalSpaceController`.
+* Added the implementation of :class:`omni.isaac.lab.envs.mdp.actions.OperationalSpaceControllerAction` class.
+
+
 0.27.23 (2024-12-06)
 ~~~~~~~~~~~~~~~~~~~~
 

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