Add SO-101 support in Isaac Lab

CONTRIBUTORS.md

@@ -110,6 +110,7 @@ Guidelines for modifications:
 * Jingzhou Liu
 * Jinqi Wei
 * Jinyeob Kim
+* Jiwen Cai
 * Johnson Sun
 * Juana Du
 * Kaixi Bao

source/isaaclab/changelog.d/jiwenc-so-101.minor.rst

@@ -0,0 +1,25 @@
+Added
+^^^^^
+
+* Added an ``"adaptive_dls"`` ``ik_method`` to :class:`~isaaclab.controllers.DifferentialIKController`:
+  a manipulability-aware damped least squares whose damping ramps from ``lambda_min`` toward
+  ``lambda_max`` as the smallest task-Jacobian singular value drops below ``sigma_thresh``
+  (Maciejewski-Klein style), keeping low-DOF / near-singular arms well-conditioned.
+* Added an optional per-axis :attr:`~isaaclab.controllers.differential_ik_cfg.DifferentialIKControllerCfg.orientation_weight`
+  (scalar or ``(wx, wy, wz)``) that soft-weights the orientation rows of a ``"pose"`` task, so an
+  arm that cannot serve a full 6-DOF pose degrades gracefully instead of leaking orientation error
+  into position.
+* Added null-space joint-limit avoidance to :class:`~isaaclab.controllers.DifferentialIKController`
+  via :attr:`~isaaclab.controllers.differential_ik_cfg.DifferentialIKControllerCfg.joint_limit_avoidance_gain` /
+  :attr:`~isaaclab.controllers.differential_ik_cfg.DifferentialIKControllerCfg.joint_limit_avoidance_margin` and
+  :meth:`~isaaclab.controllers.DifferentialIKController.set_joint_pos_limits`. When enabled, a
+  center-seeking bias is projected into the null space of the position rows so it never perturbs
+  the commanded end-effector position; :class:`~isaaclab.envs.mdp.actions.task_space_actions.DifferentialInverseKinematicsAction`
+  injects the joint limits automatically.
+
+Changed
+^^^^^^^
+
+* Changed :meth:`~isaaclab.controllers.DifferentialIKController.set_command` to renormalize the
+  commanded quaternion for absolute ``"pose"`` commands, hardening the controller against slightly
+  non-unit quaternion inputs. Existing unit-quaternion callers are unaffected.

source/isaaclab/isaaclab/controllers/differential_ik.py

@@ -69,6 +69,20 @@ def __init__(self, cfg: DifferentialIKControllerCfg, num_envs: int, device: str)
         self.ee_quat_des = torch.zeros(self.num_envs, 4, device=self._device)
         # -- input command
         self._command = torch.zeros(self.num_envs, self.action_dim, device=self._device)
+        # -- optional per-axis orientation task weights (used for "pose" command types only)
+        if self.cfg.orientation_weight is None:
+            self._orientation_weight = None
+        else:
+            ori_weight = self.cfg.orientation_weight
+            weight_tuple = (
+                (float(ori_weight),) * 3
+                if isinstance(ori_weight, (int, float))
+                else tuple(float(value) for value in ori_weight)
+            )
+            self._orientation_weight = torch.tensor(weight_tuple, device=self._device)
+        # -- optional joint position limits for null-space joint-limit avoidance (set externally)
+        self._joint_pos_lower = None
+        self._joint_pos_upper = None
 
     """
     Properties.
@@ -144,7 +158,24 @@ def set_command(
                 self.ee_pos_des, self.ee_quat_des = apply_delta_pose(ee_pos, ee_quat, self._command)
             else:
                 self.ee_pos_des = self._command[:, 0:3]
-                self.ee_quat_des = self._command[:, 3:7]
+                # renormalize the commanded quaternion (callers may pass a slightly non-unit quat)
+                quat = self._command[:, 3:7]
+                self.ee_quat_des = quat / torch.linalg.norm(quat, dim=-1, keepdim=True)
+
+    def set_joint_pos_limits(self, lower: torch.Tensor, upper: torch.Tensor) -> None:
+        """Provide the controlled joints' position limits for null-space joint-limit avoidance.
+
+        Only used when
+        :attr:`~isaaclab.controllers.differential_ik_cfg.DifferentialIKControllerCfg.joint_limit_avoidance_gain`
+        is positive. The IK action term injects these automatically on its first step; call this
+        manually only when using the controller standalone.
+
+        Args:
+            lower: Lower joint-position limits in shape (num_joints,).
+            upper: Upper joint-position limits in shape (num_joints,).
+        """
+        self._joint_pos_lower = lower.to(self._device)
+        self._joint_pos_upper = upper.to(self._device)
 
     def compute(
         self, ee_pos: torch.Tensor, ee_quat: torch.Tensor, jacobian: torch.Tensor, joint_pos: torch.Tensor
@@ -160,17 +191,16 @@ def compute(
         Returns:
             The target joint positions commands in shape (N, num_joints).
         """
-        # compute the delta in joint-space
+        # assemble the task Jacobian and task-space error
         if "position" in self.cfg.command_type:
-            position_error = self.ee_pos_des - ee_pos
-            jacobian_pos = jacobian[:, 0:3]
-            delta_joint_pos = self._compute_delta_joint_pos(delta_pose=position_error, jacobian=jacobian_pos)
+            task_jacobian = jacobian[:, 0:3]
+            task_error = self.ee_pos_des - ee_pos
         else:
-            position_error, axis_angle_error = compute_pose_error(
-                ee_pos, ee_quat, self.ee_pos_des, self.ee_quat_des, rot_error_type="axis_angle"
-            )
-            pose_error = torch.cat((position_error, axis_angle_error), dim=1)
-            delta_joint_pos = self._compute_delta_joint_pos(delta_pose=pose_error, jacobian=jacobian)
+            task_jacobian, task_error = self._compute_pose_task(ee_pos, ee_quat, jacobian)
+        # compute the delta in joint-space
+        delta_joint_pos = self._compute_delta_joint_pos(delta_pose=task_error, jacobian=task_jacobian)
+        # add an optional null-space joint-limit-avoidance bias (a no-op when joint_limit_avoidance_gain == 0)
+        delta_joint_pos = delta_joint_pos + self._joint_limit_avoidance(joint_pos, task_jacobian)
         # return the desired joint positions
         return joint_pos + delta_joint_pos
 
@@ -235,7 +265,87 @@ def _compute_delta_joint_pos(self, delta_pose: torch.Tensor, jacobian: torch.Ten
                 jacobian_T @ torch.inverse(jacobian @ jacobian_T + lambda_matrix) @ delta_pose.unsqueeze(-1)
             )
             delta_joint_pos = delta_joint_pos.squeeze(-1)
+        elif self.cfg.ik_method == "adaptive_dls":  # manipulability-aware damped least squares
+            # parameters
+            lambda_min = self.cfg.ik_params["lambda_min"]
+            lambda_max = self.cfg.ik_params["lambda_max"]
+            sigma_thresh = self.cfg.ik_params["sigma_thresh"]
+            # per-environment squared damping: lambda_min^2 away from singularities, ramping
+            # quadratically up to lambda_max^2 as the smallest task-Jacobian singular value -> 0
+            # (Maciejewski-Klein). Keying off the full task Jacobian damps both position and
+            # orientation rank-loss configurations.
+            sigma_min = torch.linalg.svdvals(jacobian)[:, -1]  # (N,)
+            ratio = (sigma_min / sigma_thresh).clamp(max=1.0)
+            lambda_sq = lambda_min**2 + (1.0 - ratio**2) * (lambda_max**2 - lambda_min**2)  # (N,)
+            jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2)
+            lambda_matrix = lambda_sq.view(-1, 1, 1) * torch.eye(n=jacobian.shape[1], device=self._device)
+            delta_joint_pos = torch.bmm(
+                jacobian_T,
+                torch.linalg.solve(torch.bmm(jacobian, jacobian_T) + lambda_matrix, delta_pose.unsqueeze(-1)),
+            ).squeeze(-1)
         else:
             raise ValueError(f"Unsupported inverse-kinematics method: {self.cfg.ik_method}")
 
         return delta_joint_pos
+
+    def _compute_pose_task(
+        self, ee_pos: torch.Tensor, ee_quat: torch.Tensor, jacobian: torch.Tensor
+    ) -> tuple[torch.Tensor, torch.Tensor]:
+        """Assemble the (optionally orientation-weighted) pose task Jacobian and error.
+
+        The orientation error is the axis-angle of ``q_des * q_cur^-1`` from
+        :func:`~isaaclab.utils.math.compute_pose_error`. When
+        :attr:`~isaaclab.controllers.differential_ik_cfg.DifferentialIKControllerCfg.orientation_weight`
+        is set, the 3 orientation rows of both the Jacobian and the error are scaled per
+        base-frame axis (a weight of 0 drops that axis from the solve). Subclasses may override
+        this to further shape the task (e.g. masking which joints serve orientation).
+
+        Args:
+            ee_pos: Current end-effector position in shape (N, 3).
+            ee_quat: Current end-effector orientation (x, y, z, w) in shape (N, 4).
+            jacobian: The geometric Jacobian in shape (N, 6, num_joints).
+
+        Returns:
+            A tuple ``(task_jacobian, task_error)`` with the (N, 6, num_joints) task Jacobian and
+            the (N, 6) task-space error.
+        """
+        position_error, axis_angle_error = compute_pose_error(
+            ee_pos, ee_quat, self.ee_pos_des, self.ee_quat_des, rot_error_type="axis_angle"
+        )
+        task_jacobian = jacobian
+        if self._orientation_weight is not None:
+            weight = self._orientation_weight
+            task_jacobian = torch.cat([jacobian[:, 0:3, :], jacobian[:, 3:6, :] * weight.view(1, 3, 1)], dim=1)
+            axis_angle_error = axis_angle_error * weight.view(1, 3)
+        task_error = torch.cat((position_error, axis_angle_error), dim=1)
+        return task_jacobian, task_error
+
+    def _joint_limit_avoidance(self, joint_pos: torch.Tensor, task_jacobian: torch.Tensor) -> torch.Tensor:
+        """Null-space joint-centering bias that keeps joints off their position limits.
+
+        Projects a center-seeking joint velocity (active only within
+        :attr:`~isaaclab.controllers.differential_ik_cfg.DifferentialIKControllerCfg.joint_limit_avoidance_margin`
+        of a limit) into the null space of the position (linear) task rows, so it never perturbs
+        the commanded end-effector position. Returns zeros when disabled (``joint_limit_avoidance_gain == 0``) or
+        before joint limits are provided via :meth:`set_joint_pos_limits`.
+
+        Args:
+            joint_pos: Current joint positions in shape (N, num_joints).
+            task_jacobian: The task Jacobian in shape (N, T, num_joints); rows 0-2 are the
+                position (linear) rows.
+
+        Returns:
+            The joint-space correction in shape (N, num_joints).
+        """
+        if self.cfg.joint_limit_avoidance_gain <= 0.0 or self._joint_pos_lower is None:
+            return torch.zeros_like(joint_pos)
+        lower, upper = self._joint_pos_lower, self._joint_pos_upper
+        q_mid = 0.5 * (lower + upper)
+        dist = torch.minimum(joint_pos - lower, upper - joint_pos)  # margin to nearest limit
+        activation = 1.0 - (dist / self.cfg.joint_limit_avoidance_margin).clamp(0.0, 1.0)  # 1 at the limit, 0 mid-range
+        dq_center = -self.cfg.joint_limit_avoidance_gain * activation * (joint_pos - q_mid)  # toward the joint center
+        j_pos = task_jacobian[:, :3, :]
+        j_pos_pinv = torch.linalg.pinv(j_pos)
+        num_joints = task_jacobian.shape[2]
+        null_proj = torch.eye(num_joints, device=self._device) - torch.bmm(j_pos_pinv, j_pos)
+        return torch.bmm(null_proj, dq_center.unsqueeze(-1)).squeeze(-1)

source/isaaclab/isaaclab/controllers/differential_ik_cfg.py

@@ -35,7 +35,7 @@ class DifferentialIKControllerCfg:
     Otherwise, the controller treats the input command as the absolute position/pose.
     """
 
-    ik_method: Literal["pinv", "svd", "trans", "dls"] = MISSING
+    ik_method: Literal["pinv", "svd", "trans", "dls", "adaptive_dls"] = MISSING
     """Method for computing inverse of Jacobian."""
 
     ik_params: dict[str, float] | None = None
@@ -51,23 +51,79 @@ class DifferentialIKControllerCfg:
         - "k_val": Scaling of computed delta-joint positions (default: 1.0).
     - Damped Moore-Penrose pseudo-inverse ("dls"):
         - "lambda_val": Damping coefficient (default: 0.01).
+    - Manipulability-aware damped least squares ("adaptive_dls"):
+        - "lambda_min": Baseline damping coefficient used away from singularities (default: 0.05).
+        - "lambda_max": Maximum damping coefficient, reached as the smallest task-Jacobian
+          singular value approaches zero (default: 0.2).
+        - "sigma_thresh": Smallest-singular-value threshold below which the damping ramps
+          quadratically from ``lambda_min`` toward ``lambda_max`` (Maciejewski-Klein style)
+          (default: 0.02).
     """
 
+    orientation_weight: float | tuple[float, float, float] | None = None
+    """Soft weight on the orientation task rows for ``"pose"`` command types. Defaults to ``None``
+    (the orientation rows keep weight 1, i.e. unchanged behavior).
+
+    A scalar weights all three orientation rows equally; a per-axis ``(wx, wy, wz)`` weights the
+    base-frame orientation axes independently. Scaling an orientation row (and its error) by a
+    weight de-emphasises -- or, at weight 0, drops -- that rotation DOF in the solve without
+    changing the task dimensionality. This is useful for arms that cannot serve a full 6-DOF pose
+    (e.g. a 5-DOF arm) so the unreachable orientation DOF degrades gracefully instead of leaking
+    error into the position rows. Ignored for ``"position"`` command types.
+    """
+
+    joint_limit_avoidance_gain: float = 0.0
+    """Gain for the null-space joint-limit-avoidance bias. ``0`` disables it (default).
+
+    When positive, a center-seeking joint velocity (active only within
+    :attr:`joint_limit_avoidance_margin` of a limit) is projected into the null space of the
+    position task rows, so it keeps joints off their limits without perturbing the commanded
+    end-effector position. Active only once joint limits are provided via
+    :meth:`~isaaclab.controllers.differential_ik.DifferentialIKController.set_joint_pos_limits`
+    (the IK action term injects them automatically when ``joint_limit_avoidance_gain > 0``).
+    """
+
+    joint_limit_avoidance_margin: float = 0.3
+    """Joint-range margin within which the joint-limit-avoidance bias activates (1 at the limit,
+    ramping to 0 at ``joint_limit_avoidance_margin`` away from it). Units match the joints
+    (e.g. [rad] for revolute joints)."""
+
     def __post_init__(self):
         # check valid input
         if self.command_type not in ["position", "pose"]:
             raise ValueError(f"Unsupported inverse-kinematics command: {self.command_type}.")
-        if self.ik_method not in ["pinv", "svd", "trans", "dls"]:
+        if self.ik_method not in ["pinv", "svd", "trans", "dls", "adaptive_dls"]:
             raise ValueError(f"Unsupported inverse-kinematics method: {self.ik_method}.")
         # default parameters for different inverse kinematics approaches.
         default_ik_params = {
             "pinv": {"k_val": 1.0},
             "svd": {"k_val": 1.0, "min_singular_value": 1e-5},
             "trans": {"k_val": 1.0},
             "dls": {"lambda_val": 0.01},
+            "adaptive_dls": {"lambda_min": 0.05, "lambda_max": 0.2, "sigma_thresh": 0.02},
         }
         # update parameters for IK-method if not provided
         ik_params = default_ik_params[self.ik_method].copy()
         if self.ik_params is not None:
             ik_params.update(self.ik_params)
         self.ik_params = ik_params
+        # validate adaptive_dls parameters
+        if self.ik_method == "adaptive_dls":
+            if self.ik_params["sigma_thresh"] <= 0.0:
+                raise ValueError(f"adaptive_dls sigma_thresh must be > 0, got {self.ik_params['sigma_thresh']}.")
+            if self.ik_params["lambda_min"] > self.ik_params["lambda_max"]:
+                raise ValueError(
+                    f"adaptive_dls lambda_min ({self.ik_params['lambda_min']}) must be <= "
+                    f"lambda_max ({self.ik_params['lambda_max']})."
+                )
+        # validate optional orientation weighting / joint-limit-avoidance settings
+        if self.orientation_weight is not None and not isinstance(self.orientation_weight, (int, float)):
+            if len(self.orientation_weight) != 3:
+                raise ValueError(
+                    "orientation_weight must be a scalar or a length-3 (wx, wy, wz) tuple, got "
+                    f"{self.orientation_weight}."
+                )
+        if self.joint_limit_avoidance_gain < 0.0:
+            raise ValueError(f"joint_limit_avoidance_gain must be >= 0, got {self.joint_limit_avoidance_gain}.")
+        if self.joint_limit_avoidance_margin <= 0.0:
+            raise ValueError(f"joint_limit_avoidance_margin must be > 0, got {self.joint_limit_avoidance_margin}.")

source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py

@@ -90,6 +90,9 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env:
         self._ik_controller = DifferentialIKController(
             cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device
         )
+        # joint limits are injected lazily on the first apply (asset data is populated by then) so
+        # the controller can do null-space joint-limit avoidance; only needed when joint_limit_avoidance_gain > 0.
+        self._limits_injected = False
 
         # create tensors for raw and processed actions
         self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device)
@@ -201,6 +204,12 @@ def apply_actions(self):
         # obtain quantities from simulation
         ee_pos_curr, ee_quat_curr = self._compute_frame_pose()
         joint_pos = self._asset.data.joint_pos.torch[:, self._joint_ids]
+        # lazily provide joint limits to the controller for null-space joint-limit avoidance
+        # (limits are uniform across envs for these articulations; env 0 is representative)
+        if not self._limits_injected and getattr(self.cfg.controller, "joint_limit_avoidance_gain", 0.0) > 0.0:
+            limits = self._asset.data.soft_joint_pos_limits.torch[0, self._joint_ids, :]
+            self._ik_controller.set_joint_pos_limits(limits[:, 0].clone(), limits[:, 1].clone())
+            self._limits_injected = True
         # compute the delta in joint-space
         if ee_quat_curr.norm() != 0:
             jacobian = self._compute_frame_jacobian()