Adds approximate torque calculation for implicit actuator (isaac-sim#364

)

# Description

Adds torque computation to implicit actuators. Previously the reported
torque was always zero.

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## 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
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [ ] 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

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

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

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

@@ -1,6 +1,16 @@
 Changelog
 ---------
 
+0.10.15 (2024-01-29)
+~~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed joint torque computation for implicit actuators. Earlier, the torque was always zero for implicit
+  actuators. Now, it is computed approximately by applying the PD law.
+
+
 0.10.14 (2024-01-22)
 ~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/actuator_base.py

@@ -234,3 +234,14 @@ def _parse_joint_parameter(
             raise ValueError("The parameter value is None and no default value is provided.")
 
         return param
+
+    def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor:
+        """Clip the desired torques based on the motor limits.
+
+        Args:
+            desired_torques: The desired torques to clip.
+
+        Returns:
+            The clipped torques.
+        """
+        return torch.clip(effort, min=-self.effort_limit, max=self.effort_limit)

source/extensions/omni.isaac.orbit/omni/isaac/orbit/actuators/actuator_pd.py

@@ -54,7 +54,13 @@ def reset(self, *args, **kwargs):
     def compute(
         self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor
     ) -> ArticulationActions:
-        # we do not need to do anything here
+        """Compute the aproximmate torques for the actuated joint (physX does not compute this explicitly)."""
+        # store approximate torques for reward computation
+        error_pos = control_action.joint_positions - joint_pos
+        error_vel = control_action.joint_velocities - joint_vel
+        self.computed_effort = self.stiffness * error_pos + self.damping * error_vel + control_action.joint_efforts
+        # clip the torques based on the motor limits
+        self.applied_effort = self._clip_effort(self.computed_effort)
         return control_action
 
 
@@ -115,21 +121,6 @@ def compute(
         control_action.joint_velocities = None
         return control_action
 
-    """
-    Helper functions.
-    """
-
-    def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor:
-        """Clip the desired torques based on the motor limits.
-
-        Args:
-            desired_torques: The desired torques to clip.
-
-        Returns:
-            The clipped torques.
-        """
-        return torch.clip(effort, min=-self.effort_limit, max=self.effort_limit)
-
 
 class DCMotor(IdealPDActuator):
     r"""

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

@@ -685,20 +685,14 @@ def _apply_actuator_model(self):
             if control_action.joint_efforts is not None:
                 self._joint_effort_target_sim[:, actuator.joint_indices] = control_action.joint_efforts
             # update state of the actuator model
-            if isinstance(actuator, ImplicitActuator):
-                # TODO read torque from simulation (#127)
-                pass
-                # self._data.computed_torques[:, actuator.joint_indices] = ???
-                # self._data.applied_torques[:, actuator.joint_indices] = ???
-            else:
-                # -- torques
-                self._data.computed_torque[:, actuator.joint_indices] = actuator.computed_effort
-                self._data.applied_torque[:, actuator.joint_indices] = actuator.applied_effort
-                # -- actuator data
-                self._data.soft_joint_vel_limits[:, actuator.joint_indices] = actuator.velocity_limit
-                # TODO: find a cleaner way to handle gear ratio. Only needed for variable gear ratio actuators.
-                if hasattr(actuator, "gear_ratio"):
-                    self._data.gear_ratio[:, actuator.joint_indices] = actuator.gear_ratio
+            # -- torques
+            self._data.computed_torque[:, actuator.joint_indices] = actuator.computed_effort
+            self._data.applied_torque[:, actuator.joint_indices] = actuator.applied_effort
+            # -- actuator data
+            self._data.soft_joint_vel_limits[:, actuator.joint_indices] = actuator.velocity_limit
+            # TODO: find a cleaner way to handle gear ratio. Only needed for variable gear ratio actuators.
+            if hasattr(actuator, "gear_ratio"):
+                self._data.gear_ratio[:, actuator.joint_indices] = actuator.gear_ratio
 
     """
     Internal helpers -- Debugging.