Fixes issue with applying actuator model in Articulation (#194)

# Description

* Fixed bugs in actuator model implementation for actuator nets.
Earlier, the DC motor clipping was not working.
* Fixed bug in applying actuator model in the
`omni.isaac.orbit.asset.Articulation` class. The new
implementation caches the outputs from the explicit actuator model into
the `_joint_pos_*_sim` buffer to
  avoid feedback loops in the tensor operation.

## 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
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file

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

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

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

 Changelog
 ---------
 
+0.9.12 (2023-10-18)
+~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed bugs in actuator model implementation for actuator nets. Earlier the DC motor clipping was not working.
+* Fixed bug in applying actuator model in the :class:`omni.isaac.orbit.asset.Articulation` class. The new
+  implementation caches the outputs from explicit actuator model into the ``joint_pos_*_sim`` buffer to
+  avoid feedback loops in the tensor operation.
+
+
 0.9.11 (2023-10-17)
 ~~~~~~~~~~~~~~~~~~~
 

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

@@ -119,13 +119,13 @@ class ActuatorNetMLPCfg(DCMotorCfg):
     network_file: str = MISSING
     """Path to the file containing network weights."""
 
-    pos_scale: float = MISSING  # DOF position input scaling
+    pos_scale: float = MISSING
     """Scaling of the joint position errors input to the network."""
-    vel_scale: float = MISSING  # DOF velocity input scaling
+    vel_scale: float = MISSING
     """Scaling of the joint velocities input to the network."""
-    torque_scale: float = MISSING  # DOF torque output scaling
+    torque_scale: float = MISSING
     """Scaling of the joint efforts output from the network."""
-    input_idx: Iterable[int] = MISSING  # Indices from the actuator history buffer to pass as inputs.
+    input_idx: Iterable[int] = MISSING
     """
     Indices of the actuator history buffer passed as inputs to the network.
 

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

@@ -77,6 +77,8 @@ class ActuatorNetLSTM(DCMotor):
         # compute network inputs
         self.sea_input[:, 0, 0] = (control_action.joint_positions - joint_pos).flatten()
         self.sea_input[:, 0, 1] = joint_vel.flatten()
+        # save current joint vel for dc-motor clipping
+        self._joint_vel[:] = joint_vel
 
         # run network inference
         with torch.inference_mode():
@@ -148,6 +150,8 @@ class ActuatorNetMLP(DCMotor):
         # -- velocity
         self._joint_vel_history = self._joint_vel_history.roll(1, 1)
         self._joint_vel_history[:, 0] = joint_vel
+        # save current joint vel for dc-motor clipping
+        self._joint_vel[:] = joint_vel
 
         # compute network inputs
         # -- positions
@@ -161,7 +165,7 @@ class ActuatorNetMLP(DCMotor):
 
         # run network inference
         torques = self.network(network_input).reshape(self._num_envs, self.num_joints)
-        self.computed_effort = torques.reshape(self._num_envs, self.num_joints)
+        self.computed_effort = torques.reshape(self._num_envs, self.num_joints) * self.cfg.torque_scale
 
         # clip the computed effort based on the motor limits
         self.applied_effort = self._clip_effort(self.computed_effort)

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

@@ -51,10 +51,11 @@ class ImplicitActuator(ActuatorBase):
         # This is a no-op. There is no state to reset for implicit actuators.
         pass
 
-    def compute(self, *args, **kwargs) -> ArticulationActions:
-        raise RuntimeError(
-            "This function should not be called! Implicit actuators are handled directly by the simulation."
-        )
+    def compute(
+        self, control_action: ArticulationActions, joint_pos: torch.Tensor, joint_vel: torch.Tensor
+    ) -> ArticulationActions:
+        # we do not need to do anything here
+        return control_action
 
 
 """
@@ -107,7 +108,7 @@ class IdealPDActuator(ActuatorBase):
         # calculate the desired joint torques
         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.clip(-self.effort_limit, self.effort_limit))
+        self.applied_effort = self._clip_effort(self.computed_effort)
         # set the computed actions back into the control action
         control_action.joint_efforts = self.applied_effort
         control_action.joint_positions = None

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

@@ -116,11 +116,11 @@ class Articulation(RigidObject):
         # write commands
         self._apply_actuator_model()
         # apply actions into simulation
-        self.root_physx_view.set_dof_actuation_forces(self._data.joint_effort_target, self._ALL_INDICES)
+        self.root_physx_view.set_dof_actuation_forces(self._joint_effort_target_sim, self._ALL_INDICES)
         # position and velocity targets only for implicit actuators
         if self._has_implicit_actuators:
-            self.root_physx_view.set_dof_position_targets(self._data.joint_pos_target, self._ALL_INDICES)
-            self.root_physx_view.set_dof_velocity_targets(self._data.joint_vel_target, self._ALL_INDICES)
+            self.root_physx_view.set_dof_position_targets(self._joint_pos_target_sim, self._ALL_INDICES)
+            self.root_physx_view.set_dof_velocity_targets(self._joint_vel_target_sim, self._ALL_INDICES)
 
     def update(self, dt: float | None = None):
         # -- root state (note: we roll the quaternion to match the convention used in Isaac Sim -- wxyz)
@@ -469,6 +469,11 @@ class Articulation(RigidObject):
         self._data.soft_joint_pos_limits[..., 0] = joint_pos_mean - 0.5 * joint_pos_range * soft_limit_factor
         self._data.soft_joint_pos_limits[..., 1] = joint_pos_mean + 0.5 * joint_pos_range * soft_limit_factor
 
+        # create buffers to store processed actions from actuator models
+        self._joint_pos_target_sim = torch.zeros_like(self._data.joint_pos_target)
+        self._joint_vel_target_sim = torch.zeros_like(self._data.joint_pos_target)
+        self._joint_effort_target_sim = torch.zeros_like(self._data.joint_pos_target)
+
     def _process_cfg(self):
         """Post processing of configuration parameters."""
         # default state
@@ -554,12 +559,6 @@ class Articulation(RigidObject):
         """
         # process actions per group
         for actuator in self.actuators.values():
-            # skip implicit actuators (they are handled by the simulation)
-            if isinstance(actuator, ImplicitActuator):
-                # TODO read torque from simulation (#127)
-                # self._data.computed_torques[:, actuator.joint_indices] = ???
-                # self._data.applied_torques[:, actuator.joint_indices] = ???
-                continue
             # prepare input for actuator model based on cached data
             # TODO : A tensor dict would be nice to do the indexing of all tensors together
             control_action = ArticulationActions(
@@ -576,17 +575,23 @@ class Articulation(RigidObject):
             )
             # update targets (these are set into the simulation)
             if control_action.joint_positions is not None:
-                self._data.joint_pos_target[:, actuator.joint_indices] = control_action.joint_positions
+                self._joint_pos_target_sim[:, actuator.joint_indices] = control_action.joint_positions
             if control_action.joint_velocities is not None:
-                self._data.joint_vel_target[:, actuator.joint_indices] = control_action.joint_velocities
+                self._joint_vel_target_sim[:, actuator.joint_indices] = control_action.joint_velocities
             if control_action.joint_efforts is not None:
-                self._data.joint_effort_target[:, actuator.joint_indices] = control_action.joint_efforts
+                self._joint_effort_target_sim[:, actuator.joint_indices] = control_action.joint_efforts
             # update state of the actuator model
-            # -- 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
+            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

source/extensions/omni.isaac.orbit/omni/isaac/orbit/assets/config/ridgeback_franka.py

@@ -40,7 +40,7 @@ RIDGEBACK_FRANKA_PANDA_CFG = ArticulationCfg(
             "panda_joint6": 3.037,
             "panda_joint7": 0.741,
             # tool
-            "panda_finger_joint*": 0.035,
+            "panda_finger_joint.*": 0.035,
         },
         joint_vel={".*": 0.0},
     ),

source/standalone/demo/play_quadrupeds.py

@@ -50,7 +50,9 @@ def main():
     """Main function."""
 
     # Load kit helper
-    sim = SimulationContext(sim_utils.SimulationCfg(device="cpu", use_gpu_pipeline=False, dt=0.005))
+    sim = SimulationContext(
+        sim_utils.SimulationCfg(device="cpu", use_gpu_pipeline=False, dt=0.005, physx=sim_utils.PhysxCfg(use_gpu=False))
+    )
     # Set main camera
     sim.set_camera_view(eye=[3.5, 3.5, 3.5], target=[0.0, 0.0, 0.0])
 

source/standalone/demo/play_ridgeback_franka.py

@@ -59,7 +59,9 @@ def main():
 
     # Load kit helper
     # note: there is a bug in Isaac Sim 2022.2.1 that prevents the use of GPU pipeline
-    sim = SimulationContext(sim_utils.SimulationCfg(device="cpu", use_gpu_pipeline=False))
+    sim = SimulationContext(
+        sim_utils.SimulationCfg(device="cpu", use_gpu_pipeline=False, physx=sim_utils.PhysxCfg(use_gpu=False))
+    )
     # Set main camera
     sim.set_camera_view([1.5, 1.5, 1.5], [0.0, 0.0, 0.0])
 
@@ -89,7 +91,7 @@ def main():
     print("[INFO]: Setup complete...")
 
     # dummy action
-    actions = torch.zeros(robot.root_view.count, robot.num_joints, device=robot.device) + robot.data.default_joint_pos
+    actions = robot.data.default_joint_pos.clone()
 
     # Define simulation stepping
     sim_dt = sim.get_physics_dt()
@@ -103,13 +105,17 @@ def main():
             sim_time = 0.0
             ep_step_count = 0
             # reset dof state
-            joint_pos, joint_vel = robot.data.default_joint_pos, robot.data.default_joint_vel
+            joint_pos, joint_vel = robot.data.default_joint_pos.clone(), robot.data.default_joint_vel.clone()
             robot.write_joint_state_to_sim(joint_pos, joint_vel)
+            # reset default joint target
+            robot.set_joint_position_target(joint_pos)
+            robot.write_data_to_sim()
+            # reset internals
             robot.reset()
             # reset command
             actions = torch.rand_like(robot.data.default_joint_pos) + robot.data.default_joint_pos
             # -- base
-            actions[:, 0::3] = 0.0
+            actions[:, 0:3] = 0.0
             # -- gripper
             actions[:, -2:] = 0.04
             print("[INFO]: Resetting robots state...")