feat: teleop with or without force feedback

Author: danielsanjosepro

crisp_gym/teleop/teleop_robot.py

@@ -48,10 +48,15 @@ def prepare_for_teleop(self, home: bool = True, blocking: bool = True):
         if home:
             self.robot.home(blocking=blocking)
 
-        self.robot.cartesian_controller_parameters_client.load_param_config(
-            file_path=self.config.gravity_compensation_controller
-        )
-        self.robot.controller_switcher_client.switch_controller("cartesian_impedance_controller")
+        if self.config.use_force_feedback:
+            self.robot.controller_switcher_client.switch_controller("torque_feedback_controller")
+        else:
+            self.robot.cartesian_controller_parameters_client.load_param_config(
+                file_path=self.config.gravity_compensation_controller
+            )
+            self.robot.controller_switcher_client.switch_controller(
+                "cartesian_impedance_controller"
+            )
 
         if self.gripper is not None and not self.config.disable_gripper_torque:
             self.gripper.disable_torque()

crisp_gym/teleop/teleop_robot_config.py

@@ -40,6 +40,7 @@ class TeleopRobotConfig(ABC):
 
     use_gripper: bool = True
     disable_gripper_torque: bool = True
+    use_force_feedback: bool = False
 
     @classmethod
     def from_yaml(cls, yaml_path: Path, **overrides) -> "TeleopRobotConfig":  # noqa: ANN003