Adds null-space control option within OperationSpaceController (#1557)

# Description

This PR adds an option to enable null-space control within
`OperationSpaceController` (following the discussions in #913).

## Details

Currently, `OperationSpaceController` controls only the task space,
which results in joint movements within the null-space of redundant
robotic manipulators. In general, it is desirable to have some control
over the robot null-space, e.g., to prevent the joints from going near
their limits. Hence, the PR adds the following:

- An optional nullspace (position) control integrated into
`OperationSpaceController`. This controller attracts the robot joints to
provided targets whereas possible, in parallel to the task-space target.
- Test cases for `OperationSpaceController` that use null-space control.
- Updates to the related tutorial script and documentation to use and
describe null-space control.
- Integration of (optional) null-space control to
`OperationSpaceControllerAction` with predefined joint targets, e.g.,
default joint positions, joint centers, zero joint position.
- Integration of null-space control to `"Isaac-Reach-Franka-OSC-v0"`
manager-based environment.

Null-space control utilizes either the dynamically consistent Jacobian
inverse or the Moore–Penrose inverse, depending on whether the full
inertia matrix is available.

## Type of change

- New feature (non-breaking change which adds functionality)
- This change requires a documentation update

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

---------

Signed-off-by: Özhan Özen <[email protected]>
Co-authored-by: Kelly Guo <[email protected]>

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

@@ -86,14 +86,26 @@ If desired, the inertial coupling between the translational and rotational axes
 If it is desired to include the gravity compensation in the operational space command, the ``gravity_compensation``
 should be set to ``True``.
 
+A final consideration regarding the operational space control is what to do with the null-space of redundant robots.
+The null-space is the subspace of the joint space that does not affect the task space coordinates. If nothing is done
+to control the null-space, the robot joints will float without moving the end-effector. This might be undesired (e.g.,
+the robot joints might get close to their limits), and one might want to control the robot behaviour within its
+null-space. One way to do is to set ``nullspace_control`` to ``"position"`` (by default it is ``"none"``) which
+integrates a null-space PD controller to attract the robot joints to desired targets without affecting the task
+space. The behaviour of this null-space controller can be defined using the ``nullspace_stiffness`` and
+``nullspace_damping_ratio`` arguments. Please note that theoretical decoupling of the null-space and task space
+accelerations is only possible when ``inertial_dynamics_decoupling`` is set to ``True`` and
+``partial_inertial_dynamics_decoupling`` is set to ``False``.
+
 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
+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).
+damped response). Finally, ``nullspace_control`` is set to use ``"position"`` where the joint set points are provided
+to be the center of the joint position limits.
 
 .. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py
    :language: python
@@ -104,13 +116,14 @@ 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.
+about the robot. This includes the robot's Jacobian matrix, mass/inertia matrix, end-effector pose, velocity, contact
+force (all in the root frame), and finally, the joint positions and velocities. Moreover, the user should provide
+gravity compensation vector and null-space joint position targets if required.
 
 .. 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
+   :end-before: # Update the target commands
 
 
 Computing robot command

source/extensions/omni.isaac.lab/config/extension.toml

@@ -1,7 +1,7 @@
 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.30.0"
+version = "0.30.1"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/extensions/omni.isaac.lab/docs/CHANGELOG.rst

@@ -1,6 +1,20 @@
 Changelog
 ---------
 
+0.30.1 (2024-12-17)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added null-space (position) control option to :class:`omni.isaac.lab.controllers.OperationalSpaceController`.
+* Added test cases that uses null-space control for :class:`omni.isaac.lab.controllers.OperationalSpaceController`.
+* Added information regarding null-space control to the tutorial script and documentation of
+  :class:`omni.isaac.lab.controllers.OperationalSpaceController`.
+* Added arguments to set specific null-space joint position targets within
+  :class:`omni.isaac.lab.envs.mdp.actions.OperationalSpaceControllerAction` class.
+
+
 0.30.0 (2024-12-16)
 ~~~~~~~~~~~~~~~~~~~
 

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

@@ -77,11 +77,15 @@ def __init__(self, cfg: OperationalSpaceControllerCfg, num_envs: int, device: st
         self._selection_matrix_force_b = torch.zeros_like(self._selection_matrix_force_task)
         # -- commands
         self._task_space_target_task = torch.zeros(self.num_envs, self.target_dim, device=self._device)
-        # -- buffers for motion/force control
+        # -- Placeholders for motion/force control
         self.desired_ee_pose_task = None
         self.desired_ee_pose_b = None
         self.desired_ee_wrench_task = None
         self.desired_ee_wrench_b = None
+        # -- buffer for operational space mass matrix
+        self._os_mass_matrix_b = torch.zeros(self.num_envs, 6, 6, device=self._device)
+        # -- Placeholder for the inverse of joint space mass matrix
+        self._mass_matrix_inv = None
         # -- motion control gains
         self._motion_p_gains_task = torch.diag_embed(
             torch.ones(self.num_envs, 6, device=self._device)
@@ -127,6 +131,14 @@ def __init__(self, cfg: OperationalSpaceControllerCfg, num_envs: int, device: st
         # -- end-effector contact wrench
         self._ee_contact_wrench_b = torch.zeros(self.num_envs, 6, device=self._device)
 
+        # -- buffers for null-space control gains
+        self._nullspace_p_gain = torch.tensor(self.cfg.nullspace_stiffness, dtype=torch.float, device=self._device)
+        self._nullspace_d_gain = (
+            2
+            * torch.sqrt(self._nullspace_p_gain)
+            * torch.tensor(self.cfg.nullspace_damping_ratio, dtype=torch.float, device=self._device)
+        )
+
     """
     Properties.
     """
@@ -338,6 +350,9 @@ def compute(
         current_ee_force_b: torch.Tensor | None = None,
         mass_matrix: torch.Tensor | None = None,
         gravity: torch.Tensor | None = None,
+        current_joint_pos: torch.Tensor | None = None,
+        current_joint_vel: torch.Tensor | None = None,
+        nullspace_joint_pos_target: torch.Tensor | None = None,
     ) -> torch.Tensor:
         """Performs inference with the controller.
 
@@ -348,19 +363,33 @@ def compute(
                 (``num_envs``, 7), which contains the position and quaternion ``(w, x, y, z)``. Defaults to ``None``.
             current_ee_vel_b: The current end-effector velocity in root frame. It is a tensor of shape
                 (``num_envs``, 6), which contains the linear and angular velocities. Defaults to None.
-            current_ee_force_b: The current external force on the end-effector in root frame.
-                It is a tensor of shape (``num_envs``, 3), which contains the linear force. Defaults to ``None``.
+            current_ee_force_b: The current external force on the end-effector in root frame. It is a tensor of
+                shape (``num_envs``, 3), which contains the linear force. Defaults to ``None``.
             mass_matrix: The joint-space mass/inertia matrix. It is a tensor of shape (``num_envs``, ``num_DoF``,
-            ``num_DoF``). Defaults to ``None``.
+                ``num_DoF``). Defaults to ``None``.
             gravity: The joint-space gravity vector. It is a tensor of shape (``num_envs``, ``num_DoF``). Defaults
-            to ``None``.
+                to ``None``.
+            current_joint_pos: The current joint positions. It is a tensor of shape (``num_envs``, ``num_DoF``).
+                Defaults to ``None``.
+            current_joint_vel: The current joint velocities. It is a tensor of shape (``num_envs``, ``num_DoF``).
+                Defaults to ``None``.
+            nullspace_joint_pos_target: The target joint positions the null space controller is trying to enforce, when
+                possible. It is a tensor of shape (``num_envs``, ``num_DoF``).
 
         Raises:
             ValueError: When motion-control is enabled but the current end-effector pose or velocity is not provided.
             ValueError: When inertial dynamics decoupling is enabled but the mass matrix is not provided.
             ValueError: When the current end-effector pose is not provided for the ``pose_rel`` command.
             ValueError: When closed-loop force control is enabled but the current end-effector force is not provided.
             ValueError: When gravity compensation is enabled but the gravity vector is not provided.
+            ValueError: When null-space control is enabled but the system is not redundant.
+            ValueError: When dynamically consistent pseudo-inverse is enabled but the mass matrix inverse is not
+                provided.
+            ValueError: When null-space control is enabled but the current joint positions and velocities are not
+                provided.
+            ValueError: When target joint positions are provided for null-space control but their dimensions do not
+                match the current joint positions.
+            ValueError: When an invalid null-space control method is provided.
 
         Returns:
             Tensor: The joint efforts computed by the controller. It is a tensor of shape (``num_envs``, ``num_DoF``).
@@ -398,23 +427,21 @@ def compute(
                 if mass_matrix is None:
                     raise ValueError("Mass matrix is required for inertial decoupling.")
                 # Compute operational space mass matrix
-                mass_matrix_inv = torch.inverse(mass_matrix)
+                self._mass_matrix_inv = torch.inverse(mass_matrix)
                 if self.cfg.partial_inertial_dynamics_decoupling:
-                    # Create a zero tensor representing the mass matrix, to fill in the non-zero elements later
-                    os_mass_matrix = torch.zeros(self.num_envs, 6, 6, device=self._device)
                     # Fill in the translational and rotational parts of the inertia separately, ignoring their coupling
-                    os_mass_matrix[:, 0:3, 0:3] = torch.inverse(
-                        jacobian_b[:, 0:3] @ mass_matrix_inv @ jacobian_b[:, 0:3].mT
+                    self._os_mass_matrix_b[:, 0:3, 0:3] = torch.inverse(
+                        jacobian_b[:, 0:3] @ self._mass_matrix_inv @ jacobian_b[:, 0:3].mT
                     )
-                    os_mass_matrix[:, 3:6, 3:6] = torch.inverse(
-                        jacobian_b[:, 3:6] @ mass_matrix_inv @ jacobian_b[:, 3:6].mT
+                    self._os_mass_matrix_b[:, 3:6, 3:6] = torch.inverse(
+                        jacobian_b[:, 3:6] @ self._mass_matrix_inv @ jacobian_b[:, 3:6].mT
                     )
                 else:
                     # Calculate the operational space mass matrix fully accounting for the couplings
-                    os_mass_matrix = torch.inverse(jacobian_b @ mass_matrix_inv @ jacobian_b.mT)
+                    self._os_mass_matrix_b[:] = torch.inverse(jacobian_b @ self._mass_matrix_inv @ jacobian_b.mT)
                 # (Generalized) operational space command forces
                 # F = (J M^(-1) J^T)^(-1) * \ddot(x_des) = M_task * \ddot(x_des)
-                os_command_forces_b = os_mass_matrix @ des_ee_acc_b
+                os_command_forces_b = self._os_mass_matrix_b @ des_ee_acc_b
             else:
                 # Task-space impedance control: command forces = \ddot(x_des).
                 # Please note that the definition of task-space impedance control varies in literature.
@@ -455,4 +482,67 @@ def compute(
             # add gravity compensation
             joint_efforts += gravity
 
+        # Add null-space control
+        # -- Free null-space control
+        if self.cfg.nullspace_control == "none":
+            # No additional control is applied in the null space.
+            pass
+        else:
+            # Check if the system is redundant
+            if num_DoF <= 6:
+                raise ValueError("Null-space control is only applicable for redundant manipulators.")
+
+            # Calculate the pseudo-inverse of the Jacobian
+            if self.cfg.inertial_dynamics_decoupling and not self.cfg.partial_inertial_dynamics_decoupling:
+                # Dynamically consistent pseudo-inverse allows decoupling of null space and task space
+                if self._mass_matrix_inv is None or mass_matrix is None:
+                    raise ValueError("Mass matrix inverse is required for dynamically consistent pseudo-inverse")
+                jacobian_pinv_transpose = self._os_mass_matrix_b @ jacobian_b @ self._mass_matrix_inv
+            else:
+                # Moore-Penrose pseudo-inverse if full inertia matrix is not available (e.g., no/partial decoupling)
+                jacobian_pinv_transpose = torch.pinverse(jacobian_b).mT
+
+            # Calculate the null-space projector
+            nullspace_jacobian_transpose = (
+                torch.eye(n=num_DoF, device=self._device) - jacobian_b.mT @ jacobian_pinv_transpose
+            )
+
+            # Null space position control
+            if self.cfg.nullspace_control == "position":
+
+                # Check if the current joint positions and velocities are provided
+                if current_joint_pos is None or current_joint_vel is None:
+                    raise ValueError("Current joint positions and velocities are required for null-space control.")
+
+                # Calculate the joint errors for nullspace position control
+                if nullspace_joint_pos_target is None:
+                    nullspace_joint_pos_target = torch.zeros_like(current_joint_pos)
+                # Check if the dimensions of the target nullspace joint positions match the current joint positions
+                elif nullspace_joint_pos_target.shape != current_joint_pos.shape:
+                    raise ValueError(
+                        f"The target nullspace joint positions shape '{nullspace_joint_pos_target.shape}' does not"
+                        f"match the current joint positions shape '{current_joint_pos.shape}'."
+                    )
+
+                joint_pos_error_nullspace = nullspace_joint_pos_target - current_joint_pos
+                joint_vel_error_nullspace = -current_joint_vel
+
+                # Calculate the desired joint accelerations
+                joint_acc_nullspace = (
+                    self._nullspace_p_gain * joint_pos_error_nullspace
+                    + self._nullspace_d_gain * joint_vel_error_nullspace
+                ).unsqueeze(-1)
+
+                # Calculate the projected torques in null-space
+                if mass_matrix is not None:
+                    tau_null = (nullspace_jacobian_transpose @ mass_matrix @ joint_acc_nullspace).squeeze(-1)
+                else:
+                    tau_null = nullspace_jacobian_transpose @ joint_acc_nullspace
+
+                # Add the null-space joint efforts to the total joint efforts
+                joint_efforts += tau_null
+
+            else:
+                raise ValueError(f"Invalid null-space control method: {self.cfg.nullspace_control}.")
+
         return joint_efforts

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

@@ -75,3 +75,16 @@ class OperationalSpaceControllerCfg:
     Note: since only the linear forces could be measured at the moment,
     only the first three elements are used for the feedback loop.
     """
+
+    nullspace_control: str = "none"
+    """The null space control method for redundant manipulators: ``"none"``, ``"position"``.
+
+    Note: ``"position"`` is used to drive the redundant manipulator to zero configuration by default. If
+    ``target_joint_pos`` is provided in the ``compute()`` method, it will be driven to this configuration.
+    """
+
+    nullspace_stiffness: float = 10.0
+    """The stiffness for null space control."""
+
+    nullspace_damping_ratio: float = 1.0
+    """The damping ratio for null space control."""

source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py

@@ -303,3 +303,10 @@ class OffsetCfg:
 
     damping_ratio_scale: float = 1.0
     """Scale factor for the damping ratio commands. Defaults to 1.0."""
+
+    nullspace_joint_pos_target: str = "none"
+    """The joint targets for the null-space control: ``"none"``, ``"zero"``, ``"default"``, ``"center"``.
+
+    Note: Functional only when ``nullspace_control`` is set to ``"position"`` within the
+        ``OperationalSpaceControllerCfg``.
+    """