Checks default joint states are configured within the limits (#439)

# Description

Adds checks that the default joint states are within the limits, and
raises an Error for out-of-limit situations.

## Type of change

- New feature (non-breaking change which adds functionality)

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [ ] 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 run all the tests with `./orbit.sh --test` and they pass
- [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

---------
Co-authored-by: Mayank Mittal <mittalma@leggedrobotics.com>

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

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.11.1"
+version = "0.11.2"
 
 # Description
 title = "ORBIT framework for Robot Learning"

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

 Changelog
 ---------
 
+0.11.2 (2024-03-04)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added checks for default joint position and joint velocity in the articulation class. This is to prevent
+  users from configuring values for these quantities that might be outside the valid range from the simulation.
+
+
 0.11.1 (2024-02-29)
 ~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/assets/articulation/articulation.py

@@ -516,6 +516,8 @@ class Articulation(RigidObject):
         # process configuration
         self._process_cfg()
         self._process_actuators_cfg()
+        # validate configuration
+        self._validate_cfg()
         # log joint information
         self._log_articulation_joint_info()
 
@@ -699,6 +701,46 @@ class Articulation(RigidObject):
     Internal helpers -- Debugging.
     """
 
+    def _validate_cfg(self):
+        """Validate the configuration after processing.
+
+        Note:
+            This function should be called only after the configuration has been processed and the buffers have been
+            created. Otherwise, some settings that are altered during processing may not be validated.
+            For instance, the actuator models may change the joint max velocity limits.
+        """
+        # check that the default values are within the limits
+        joint_pos_limits = self.root_physx_view.get_dof_limits()[0].to(self.device)
+        out_of_range = self._data.default_joint_pos[0] < joint_pos_limits[:, 0]
+        out_of_range |= self._data.default_joint_pos[0] > joint_pos_limits[:, 1]
+        violated_indices = torch.nonzero(out_of_range, as_tuple=False).squeeze(-1)
+        # throw error if any of the default joint positions are out of the limits
+        if len(violated_indices) > 0:
+            # prepare message for violated joints
+            msg = "The following joints have default positions out of the limits: \n"
+            for idx in violated_indices:
+                joint_name = self.data.joint_names[idx]
+                joint_limits = joint_pos_limits[idx]
+                joint_pos = self.data.default_joint_pos[0, idx]
+                # add to message
+                msg += f"\t- '{joint_name}': {joint_pos:.3f} not in [{joint_limits[0]:.3f}, {joint_limits[1]:.3f}]\n"
+            raise ValueError(msg)
+
+        # check that the default joint velocities are within the limits
+        joint_max_vel = self.root_physx_view.get_dof_max_velocities()[0].to(self.device)
+        out_of_range = torch.abs(self._data.default_joint_vel[0]) > joint_max_vel
+        violated_indices = torch.nonzero(out_of_range, as_tuple=False).squeeze(-1)
+        if len(violated_indices) > 0:
+            # prepare message for violated joints
+            msg = "The following joints have default velocities out of the limits: \n"
+            for idx in violated_indices:
+                joint_name = self.data.joint_names[idx]
+                joint_limits = [-joint_max_vel[idx], joint_max_vel[idx]]
+                joint_vel = self.data.default_joint_vel[0, idx]
+                # add to message
+                msg += f"\t- '{joint_name}': {joint_vel:.3f} not in [{joint_limits[0]:.3f}, {joint_limits[1]:.3f}]\n"
+            raise ValueError(msg)
+
     def _log_articulation_joint_info(self):
         """Log information about the articulation's simulated joints."""
         # read out all joint parameters from simulation

source/extensions/omni.isaac.orbit/test/assets/test_articulation.py

@@ -185,6 +185,42 @@ class TestArticulation(unittest.TestCase):
             # update robot
             robot.update(self.dt)
 
+    def test_out_of_range_default_joint_pos(self):
+        """Test that the default joint position from configuration is out of range."""
+        # Create articulation
+        robot_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Robot")
+        robot_cfg.init_state.joint_pos = {
+            "panda_joint1": 10.0,
+            "panda_joint[2, 4]": -20.0,
+        }
+        robot = Articulation(robot_cfg)
+
+        # Check that boundedness of articulation is correct
+        self.assertEqual(ctypes.c_long.from_address(id(robot)).value, 1)
+
+        # Play sim
+        self.sim.reset()
+        # Check if robot is initialized
+        self.assertFalse(robot._is_initialized)
+
+    def test_out_of_range_default_joint_vel(self):
+        """Test that the default joint velocity from configuration is out of range."""
+        # Create articulation
+        robot_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Robot")
+        robot_cfg.init_state.joint_vel = {
+            "panda_joint1": 100.0,
+            "panda_joint[2, 4]": -60.0,
+        }
+        robot = Articulation(robot_cfg)
+
+        # Check that boundedness of articulation is correct
+        self.assertEqual(ctypes.c_long.from_address(id(robot)).value, 1)
+
+        # Play sim
+        self.sim.reset()
+        # Check if robot is initialized
+        self.assertFalse(robot._is_initialized)
+
     def test_external_force_on_single_body(self):
         """Test application of external force on the base of the robot."""