feat: add collision behavior and realtime parameters (#8)

* chore(deps): update panda-py

* feat: add parameters for collision and realtime behavior

* feat: implement collision behavior and realtime parameters

* fix: use factories for mutable dataclass defaults

* fix: import future annotations

* chore: remove unused import

* chore: formatting

pyproject.toml

@@ -26,7 +26,7 @@ dependencies = [
   "dm-robotics-geometry>=0.6.0",
   "dm-robotics-transformations>=0.6.0",
   "dm_env",
-  "panda-python",
+  "panda-python>=0.7.4",
 ]
 license = {file = "LICENSE"}
 readme = "README.md"

src/dm_robotics/panda/arm.py

@@ -8,7 +8,6 @@
 import numpy as np
 from dm_control import mjcf
 from dm_control.composer.observation import observable
-from dm_env import specs
 from dm_robotics.geometry import geometry, mujoco_physics
 from dm_robotics.moma import effector, robot, sensor
 from dm_robotics.moma.effectors import (arm_effector,
@@ -359,7 +358,7 @@ def set_control(self, physics: mjcf.Physics, command: np.ndarray) -> None:
 
 class ArmEffector(arm_effector.ArmEffector):
   """Robot arm effector for the Panda MoMa model.
-  
+
   Takes :py:class:`dm_robotics.panda.parameters.RobotParams`
   and changes the joint stiffness and damping of the robot arm. Otherwise behaves
   like :py:class:`dm_robotics.moma.effectors.arm_effector.ArmEffector`.
@@ -467,7 +466,7 @@ def _vel_control(self, physics: mjcf.Physics) -> np.ndarray:
 
 class RobotArmSensor(robot_arm_sensor.RobotArmSensor):
   """Behaves like :py:class:`dm_robotics.moma.sensors.robot_arm_sensor.RobotArmSensor`.
-  
+
   Except that the joint torque signal does not include passive forces.
   This is done so as to model the external torque signal provided by the Panda robot.
   """

src/dm_robotics/panda/hardware.py

@@ -211,13 +211,16 @@ def set_control(self, physics: mjcf.Physics, command: np.ndarray) -> None:
 def build_robot(robot_params: params.RobotParams,
                 control_timestep: float = 0.1) -> robot.Robot:
   """Builds a MoMa robot model of the Panda with hardware in the loop."""
-  hardware_panda = panda_py.Panda(robot_params.robot_ip)
+  hardware_panda = panda_py.Panda(
+      robot_params.robot_ip,
+      realtime_config=libfranka.RealtimeConfig.kEnforce
+      if robot_params.enforce_realtime else libfranka.RealtimeConfig.kIgnore)
   hardware_panda.set_default_behavior()
   hardware_panda.get_robot().set_collision_behavior(
-      [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0],
-      [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0],
-      [100.0, 100.0, 100.0, 100.0, 100.0, 100.0],
-      [100.0, 100.0, 100.0, 100.0, 100.0, 100.0])
+      robot_params.collision_behavior.lower_torque_thresholds,
+      robot_params.collision_behavior.upper_torque_thresholds,
+      robot_params.collision_behavior.lower_force_thresholds,
+      robot_params.collision_behavior.upper_force_thresholds)
 
   robot_sensors = []
   panda = arm_module.Panda(actuation=robot_params.actuation,

src/dm_robotics/panda/parameters.py

@@ -1,4 +1,6 @@
 """Contains dataclasses holding parameter configurations."""
+from __future__ import annotations
+
 import dataclasses
 from typing import Optional, Sequence
 
@@ -17,6 +19,19 @@ class GripperParams:
   sensors: Optional[Sequence[sensor.Sensor]] = None
 
 
+@dataclasses.dataclass
+class CollisionBehavior:
+  """Parameters to define the collision behavior of the real robot."""
+  lower_torque_thresholds: list[float] = dataclasses.field(
+      default_factory=lambda: [50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0])
+  upper_torque_thresholds: list[float] = dataclasses.field(
+      default_factory=lambda: [50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0])
+  lower_force_thresholds: list[float] = dataclasses.field(
+      default_factory=lambda: [50.0, 50.0, 50.0, 50.0, 50.0, 50.0])
+  upper_force_thresholds: list[float] = dataclasses.field(
+      default_factory=lambda: [50.0, 50.0, 50.0, 50.0, 50.0, 50.0])
+
+
 @dataclasses.dataclass
 class RobotParams:
   """Parameters used for building the Panda robot model.
@@ -35,6 +50,8 @@ class RobotParams:
     robot_ip: Robot IP or hostname. If `None` hardware in the loop is not used.
     joint_stiffness: Joint stiffness of the robot used in actuation.
     joint_damping: Joint damping of the robot used in actuation.
+    collision_behavior: configures the collision behavior of the real robot.
+    enforce_realtime: enforce realtime priority of real robot control threat
   """
   name: str = 'panda'
   pose: Optional[Sequence[float]] = None
@@ -47,3 +64,5 @@ class RobotParams:
   robot_ip: Optional[str] = None
   joint_stiffness: Sequence[float] = (600, 600, 600, 600, 250, 150, 50)
   joint_damping: Sequence[float] = (50, 50, 50, 20, 20, 20, 10)
+  collision_behavior: CollisionBehavior = CollisionBehavior()
+  enforce_realtime: bool = False