feat: added desk and cli tool to panda

- added desk to panda
- added cli tool to panda
- refactored name of credential loading function: load_creds_fr3_desk to load_creds_franka_desk

Author: juelg

examples/fr3/fr3_direct_control.py

@@ -5,7 +5,7 @@
 from rcs._core.sim import CameraType
 from rcs.camera.sim import SimCameraConfig, SimCameraSet
 from rcs_fr3._core import hw
-from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_fr3_desk
+from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_franka_desk
 
 import rcs
 from rcs import sim
@@ -26,8 +26,8 @@
 
 
 Create a .env file in the same directory as this file with the following content:
-FR3_USERNAME=<username on franka desk>
-FR3_PASSWORD=<password on franka desk>
+DESK_USERNAME=<username on franka desk>
+DESK_PASSWORD=<password on franka desk>
 
 When you use a real FR3 you first need to unlock its joints using the following cli script:
 
@@ -50,7 +50,7 @@
 def main():
     context_manger: FCI | ContextManager
     if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
-        user, pw = load_creds_fr3_desk()
+        user, pw = load_creds_franka_desk()
         context_manger = FCI(Desk(ROBOT_IP, user, pw), unlock=False, lock_when_done=False)
     else:
         context_manger = ContextManager()
@@ -100,7 +100,7 @@ def main():
                 mjcf_path,
                 "attachment_site_0",
             )
-            robot = hw.FR3(ROBOT_IP, ik)
+            robot = hw.Franka(ROBOT_IP, ik)
             robot_cfg = hw.FR3Config()
             robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
             robot_cfg.ik_solver = hw.IKSolver.rcs_ik

extensions/rcs_fr3/src/rcs_fr3/__main__.py

@@ -4,7 +4,7 @@
 
 import rcs_fr3
 import typer
-from rcs_fr3.desk import load_creds_fr3_desk
+from rcs_fr3.desk import load_creds_franka_desk
 
 logger = logging.getLogger(__name__)
 
@@ -25,7 +25,7 @@ def home(
     unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False,
 ):
     """Moves the FR3 to home position"""
-    user, pw = load_creds_fr3_desk()
+    user, pw = load_creds_franka_desk()
     rcs_fr3.desk.home(ip, user, pw, shut, unlock)
 
 
@@ -35,7 +35,7 @@ def info(
     include_gripper: Annotated[bool, typer.Option("-g", help="includes gripper")] = False,
 ):
     """Prints info about the robots current joint position and end effector pose, optionally also the gripper."""
-    user, pw = load_creds_fr3_desk()
+    user, pw = load_creds_franka_desk()
     rcs_fr3.desk.info(ip, user, pw, include_gripper)
 
 
@@ -44,7 +44,7 @@ def lock(
     ip: Annotated[str, typer.Argument(help="IP of the robot")],
 ):
     """Locks the robot."""
-    user, pw = load_creds_fr3_desk()
+    user, pw = load_creds_franka_desk()
     rcs_fr3.desk.lock(ip, user, pw)
 
 
@@ -53,7 +53,7 @@ def unlock(
     ip: Annotated[str, typer.Argument(help="IP of the robot")],
 ):
     """Prepares the robot by unlocking the joints and putting the robot into the FCI mode."""
-    user, pw = load_creds_fr3_desk()
+    user, pw = load_creds_franka_desk()
     rcs_fr3.desk.unlock(ip, user, pw)
     with rcs_fr3.desk.Desk(ip, user, pw) as d:
         d.activate_fci()
@@ -66,7 +66,7 @@ def fci(
     shutdown: Annotated[bool, typer.Option("-s", help="After ctrl+c shuts the robot down")] = False,
 ):
     """Puts the robot into FCI mode, optionally unlocks the robot. Waits for ctrl+c to exit."""
-    user, pw = load_creds_fr3_desk()
+    user, pw = load_creds_franka_desk()
     try:
         with rcs_fr3.desk.FCI(rcs_fr3.desk.Desk(ip, user, pw), unlock=unlock, lock_when_done=False):
             while True:
@@ -83,7 +83,7 @@ def guiding_mode(
     unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False,
 ):
     """Enables or disables guiding mode."""
-    user, pw = load_creds_fr3_desk()
+    user, pw = load_creds_franka_desk()
     rcs_fr3.desk.guiding_mode(ip, user, pw, disable, unlock)
 
 
@@ -92,7 +92,7 @@ def shutdown(
     ip: Annotated[str, typer.Argument(help="IP of the robot")],
 ):
     """Shuts the robot down"""
-    user, pw = load_creds_fr3_desk()
+    user, pw = load_creds_franka_desk()
     rcs_fr3.desk.shutdown(ip, user, pw)
 
 

extensions/rcs_fr3/src/rcs_fr3/desk.py

@@ -28,19 +28,28 @@
 """
 
 
-def load_creds_fr3_desk() -> tuple[str, str]:
+def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]:
+    """Loads the FR3 Desk credentials from a .env file.
+
+    The keys in the .env file are expected to be DESK_USERNAME and DESK_PASSWORD.
+
+    If you have multiple robots with multiple credentials, you can specify a postfix
+    which is appended to the keys, e.g. for postfix "1" the keys would be DESK_USERNAME_1 and DESK_PASSWORD_1.
+    """
     load_dotenv()
-    assert "DESK_USERNAME" in os.environ, "DESK_USERNAME not set in .env file or environment var."
-    assert "DESK_PASSWORD" in os.environ, "DESK_PASSWORD not set in .env file or environment var."
-    return os.environ["DESK_USERNAME"], os.environ["DESK_PASSWORD"]
+    username_key = f"DESK_USERNAME_{postfix}" if postfix else "DESK_USERNAME"
+    password_key = f"DESK_PASSWORD_{postfix}" if postfix else "DESK_PASSWORD"
+
+    assert username_key in os.environ, f"{username_key} not set in .env file or environment var."
+    assert password_key in os.environ, f"{password_key} not set in .env file or environment var."
+    return os.environ[username_key], os.environ[password_key]
 
 
 def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False):
     with Desk.fci(ip, username, password, unlock=unlock):
         f = rcs_fr3.hw.Franka(ip)
         config = rcs_fr3.hw.FR3Config()
         config.speed_factor = 0.2
-        config.ik_solver = rcs_fr3.hw.IKSolver.franka_ik
         f.set_config(config)
         config_hand = rcs_fr3.hw.FHConfig()
         g = rcs_fr3.hw.FrankaHand(ip, config_hand)

extensions/rcs_panda/src/rcs_panda/__main__.py

@@ -0,0 +1,100 @@
+import logging
+from time import sleep
+from typing import Annotated
+
+import rcs_panda
+import typer
+from rcs_panda.desk import load_creds_franka_desk
+
+logger = logging.getLogger(__name__)
+
+
+# panda CLI
+panda_app = typer.Typer(
+    help=(
+        "Commands to control a Franka Panda in RCS. "
+        "This includes tools that you would usually do with Franka's Desk interface."
+    ),
+)
+
+
+@panda_app.command()
+def home(
+    ip: Annotated[str, typer.Argument(help="IP of the robot")],
+    shut: Annotated[bool, typer.Option("-s", help="Should the robot be shut down")] = False,
+    unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False,
+):
+    """Moves the panda to home position"""
+    user, pw = load_creds_franka_desk()
+    rcs_panda.desk.home(ip, user, pw, shut, unlock)
+
+
+@panda_app.command()
+def info(
+    ip: Annotated[str, typer.Argument(help="IP of the robot")],
+    include_gripper: Annotated[bool, typer.Option("-g", help="includes gripper")] = False,
+):
+    """Prints info about the robots current joint position and end effector pose, optionally also the gripper."""
+    user, pw = load_creds_franka_desk()
+    rcs_panda.desk.info(ip, user, pw, include_gripper)
+
+
+@panda_app.command()
+def lock(
+    ip: Annotated[str, typer.Argument(help="IP of the robot")],
+):
+    """Locks the robot."""
+    user, pw = load_creds_franka_desk()
+    rcs_panda.desk.lock(ip, user, pw)
+
+
+@panda_app.command()
+def unlock(
+    ip: Annotated[str, typer.Argument(help="IP of the robot")],
+):
+    """Prepares the robot by unlocking the joints and putting the robot into the FCI mode."""
+    user, pw = load_creds_franka_desk()
+    rcs_panda.desk.unlock(ip, user, pw)
+    with rcs_panda.desk.Desk(ip, user, pw) as d:
+        d.activate_fci()
+
+
+@panda_app.command()
+def fci(
+    ip: Annotated[str, typer.Argument(help="IP of the robot")],
+    unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False,
+    shutdown: Annotated[bool, typer.Option("-s", help="After ctrl+c shuts the robot down")] = False,
+):
+    """Puts the robot into FCI mode, optionally unlocks the robot. Waits for ctrl+c to exit."""
+    user, pw = load_creds_franka_desk()
+    try:
+        with rcs_panda.desk.FCI(rcs_panda.desk.Desk(ip, user, pw), unlock=unlock, lock_when_done=False):
+            while True:
+                sleep(1)
+    except KeyboardInterrupt:
+        if shutdown:
+            rcs_panda.desk.shutdown(ip, user, pw)
+
+
+@panda_app.command()
+def guiding_mode(
+    ip: Annotated[str, typer.Argument(help="IP of the robot")],
+    disable: Annotated[bool, typer.Option("-d", help="Disable guiding mode")] = False,
+    unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False,
+):
+    """Enables or disables guiding mode."""
+    user, pw = load_creds_franka_desk()
+    rcs_panda.desk.guiding_mode(ip, user, pw, disable, unlock)
+
+
+@panda_app.command()
+def shutdown(
+    ip: Annotated[str, typer.Argument(help="IP of the robot")],
+):
+    """Shuts the robot down"""
+    user, pw = load_creds_franka_desk()
+    rcs_panda.desk.shutdown(ip, user, pw)
+
+
+if __name__ == "__main__":
+    panda_app()