Add action sensor to arm effector

The action sensor adds an effector's action to the observation.
This commit also adds a non-empty action space for the haptic actuation
mode as dm-robotics doesn't allow an environment with completely
empty action space.

Author: JeanElsner

src/dm_robotics/panda/arm.py

@@ -17,8 +17,9 @@
 from dm_robotics.moma.models import utils as models_utils
 from dm_robotics.moma.models.end_effectors.robot_hands import robot_hand
 from dm_robotics.moma.models.robots.robot_arms import robot_arm
-from dm_robotics.moma.sensors import (robot_arm_sensor, robot_tcp_sensor,
-                                      site_sensor, wrench_observations)
+from dm_robotics.moma.sensors import (action_sensor, robot_arm_sensor,
+                                      robot_tcp_sensor, site_sensor,
+                                      wrench_observations)
 from dm_robotics.transformations import transformations as tr
 
 from . import arm_constants as consts
@@ -373,10 +374,6 @@ def __init__(self, robot_params: params.RobotParams, arm: robot_arm.RobotArm):
     """
     super().__init__(arm, None, robot_params.name)
     self._robot_params = robot_params
-    self._empty_spec = specs.BoundedArray(shape=(0,),
-                                          dtype=np.float32,
-                                          minimum=0,
-                                          maximum=0)
 
   def after_compile(self, mjcf_model: mjcf.RootElement,
                     physics: mjcf.Physics) -> None:
@@ -394,11 +391,6 @@ def set_control(self, physics: mjcf.Physics, command: np.ndarray) -> None:
       return
     super().set_control(physics, command)
 
-  def action_spec(self, physics: mjcf.Physics) -> specs.BoundedArray:
-    if self._robot_params.actuation == consts.Actuation.HAPTIC:
-      return self._empty_spec
-    return super().action_spec(physics)
-
 
 @enum.unique
 class ControlObservations(enum.Enum):
@@ -519,13 +511,20 @@ def build_robot(robot_params: params.RobotParams,
   if robot_params.actuation in [
       consts.Actuation.JOINT_VELOCITY, consts.Actuation.HAPTIC
   ]:
-    _arm_effector = ArmEffector(robot_params, arm)
+    joint_effector = ArmEffector(robot_params, arm)
+    _arm_effector, spy_sensor = action_sensor.create_sensed_effector(
+        joint_effector)
+    robot_sensors.append(spy_sensor)
   elif robot_params.actuation == consts.Actuation.CARTESIAN_VELOCITY:
     joint_velocity_effector = ArmEffector(robot_params, arm)
-    _arm_effector = Cartesian6dVelocityEffector(robot_params, arm, gripper,
+    cart_effector = Cartesian6dVelocityEffector(robot_params, arm, gripper,
                                                 joint_velocity_effector,
                                                 tcp_sensor, control_timestep)
 
+    _arm_effector, spy_sensor = action_sensor.create_sensed_effector(
+        cart_effector)
+    robot_sensors.append(spy_sensor)
+
   robot.standard_compose(arm, gripper)
   moma_robot = robot.StandardRobot(arm=arm,
                                    arm_base_site_name=arm.base_site.name,

src/dm_robotics/panda/arm_constants.py

@@ -56,5 +56,8 @@ class Actuation(enum.Enum):
 ACTUATION_LIMITS = {
     Actuation.CARTESIAN_VELOCITY: VELOCITY_LIMITS,
     Actuation.JOINT_VELOCITY: VELOCITY_LIMITS,
-    Actuation.HAPTIC: JOINT_LIMITS,
+    Actuation.HAPTIC: {
+        'min': (-1, -1, -1, -1, -1, -1, -1),
+        'max': (1, 1, 1, 1, 1, 1, 1),
+    },
 }

src/dm_robotics/panda/hardware.py

@@ -10,7 +10,7 @@
 from dm_robotics.moma import robot
 from dm_robotics.moma.models.end_effectors.robot_hands import robot_hand
 from dm_robotics.moma.models.robots.robot_arms import robot_arm
-from dm_robotics.moma.sensors import robot_arm_sensor
+from dm_robotics.moma.sensors import action_sensor, robot_arm_sensor
 from panda_py import controllers, libfranka
 
 from . import arm as arm_module
@@ -253,12 +253,18 @@ def build_robot(robot_params: params.RobotParams,
   if robot_params.actuation in [
       arm_constants.Actuation.JOINT_VELOCITY, arm_constants.Actuation.HAPTIC
   ]:
-    _arm_effector = ArmEffector(robot_params, panda, hardware_panda)
+    joint_effector = ArmEffector(robot_params, panda, hardware_panda)
+    _arm_effector, spy_sensor = action_sensor.create_sensed_effector(
+        joint_effector)
+    robot_sensors.append(spy_sensor)
   elif robot_params.actuation == arm_constants.Actuation.CARTESIAN_VELOCITY:
     joint_velocity_effector = ArmEffector(robot_params, panda, hardware_panda)
-    _arm_effector = arm_module.Cartesian6dVelocityEffector(
+    cart_effector = arm_module.Cartesian6dVelocityEffector(
         robot_params, panda, gripper, joint_velocity_effector, tcp_sensor,
         control_timestep)
+    _arm_effector, spy_sensor = action_sensor.create_sensed_effector(
+        cart_effector)
+    robot_sensors.append(spy_sensor)
 
   robot.standard_compose(panda, gripper)
   moma_robot = robot.StandardRobot(arm=panda,