fix(franka): left over ik refactor (#256)

Author: juelg

examples/fr3/fr3_direct_control.py

@@ -6,6 +6,7 @@
 from rcs.camera.sim import SimCameraConfig, SimCameraSet
 from rcs_fr3._core import hw
 from rcs_fr3.desk import FCI, ContextManager, Desk, load_creds_franka_desk
+from rcs_fr3.utils import default_fr3_hw_robot_cfg
 
 import rcs
 from rcs import sim
@@ -60,15 +61,15 @@ def main():
         gripper: rcs.common.Gripper
         if ROBOT_INSTANCE == RobotPlatform.SIMULATION:
             simulation = sim.Sim(rcs.scenes["fr3_empty_world"].mjb)
-            mjcf_path = rcs.scenes["fr3_empty_world"].mjcf_robot
+            robot_cfg = sim.SimRobotConfig()
+            robot_cfg.add_id("0")
+            robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
             ik = rcs.common.Pin(
-                mjcf_path,
-                "attachment_site_0",
+                robot_cfg.kinematic_model_path,
+                robot_cfg.attachment_site,
+                urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
             )
-            cfg = sim.SimRobotConfig()
-            cfg.add_id("0")
-            cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
-            robot = rcs.sim.SimRobot(simulation, ik, cfg)
+            robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
 
             gripper_cfg_sim = sim.SimGripperConfig()
             gripper_cfg_sim.add_id("0")
@@ -95,15 +96,14 @@ def main():
             simulation.open_gui()
 
         else:
-            mjcf_path = rcs.scenes["fr3_empty_world"].mjcf_robot
+            fr3_cfg = default_fr3_hw_robot_cfg()
+            fr3_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
             ik = rcs.common.Pin(
-                mjcf_path,
-                "attachment_site_0",
+                fr3_cfg.kinematic_model_path,
+                fr3_cfg.attachment_site,
+                urdf=fr3_cfg.kinematic_model_path.endswith(".urdf"),
             )
             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
             robot.set_config(robot_cfg)  # type: ignore
 
             gripper_cfg_hw = hw.FHConfig()

extensions/rcs_fr3/src/rcs_fr3/desk.py

@@ -13,10 +13,12 @@
 import rcs_fr3
 import requests
 from dotenv import load_dotenv
-from rcs_fr3.utils import default_fr3_hw_gripper_cfg
+from rcs_fr3.utils import default_fr3_hw_gripper_cfg, default_fr3_hw_robot_cfg
 from requests.packages import urllib3  # type: ignore[attr-defined]
 from websockets.sync.client import connect
 
+import rcs
+
 _logger = logging.getLogger("desk")
 
 TOKEN_PATH = "~/.rcs/token.conf"
@@ -47,10 +49,16 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]:
 
 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
-        f.set_config(config)
+        robot_cfg = default_fr3_hw_robot_cfg()
+        robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+        robot_cfg.speed_factor = 0.2
+        ik = rcs.common.Pin(
+            robot_cfg.kinematic_model_path,
+            robot_cfg.attachment_site,
+            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
+        )
+        f = rcs_fr3.hw.Franka(ip, ik)
+        f.set_config(robot_cfg)
         config_hand = rcs_fr3.hw.FHConfig()
         g = rcs_fr3.hw.FrankaHand(ip, config_hand)
         if shut:
@@ -62,9 +70,16 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False
 
 def info(ip: str, username: str, password: str, include_hand: bool = False):
     with Desk.fci(ip, username, password):
-        f = rcs_fr3.hw.Franka(ip)
-        config = rcs_fr3.hw.FR3Config()
-        f.set_config(config)
+        robot_cfg = default_fr3_hw_robot_cfg()
+        robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+        robot_cfg.speed_factor = 0.2
+        ik = rcs.common.Pin(
+            robot_cfg.kinematic_model_path,
+            robot_cfg.attachment_site,
+            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
+        )
+        f = rcs_fr3.hw.Franka(ip, ik)
+        f.set_config(robot_cfg)
         print("Robot info:")
         print("Current cartesian position:")
         print(f.get_cartesian_position())

extensions/rcs_panda/src/rcs_panda/desk.py

@@ -13,10 +13,12 @@
 import rcs_panda
 import requests
 from dotenv import load_dotenv
-from rcs_panda.utils import default_panda_hw_gripper_cfg
+from rcs_panda.utils import default_panda_hw_gripper_cfg, default_panda_hw_robot_cfg
 from requests.packages import urllib3  # type: ignore[attr-defined]
 from websockets.sync.client import connect
 
+import rcs
+
 _logger = logging.getLogger("desk")
 
 TOKEN_PATH = "~/.rcs/token.conf"
@@ -47,10 +49,16 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]:
 
 def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False):
     with Desk.fci(ip, username, password, unlock=unlock):
-        f = rcs_panda.hw.Franka(ip)
-        config = rcs_panda.hw.PandaConfig()
-        config.speed_factor = 0.2
-        f.set_config(config)
+        robot_cfg = default_panda_hw_robot_cfg()
+        robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+        robot_cfg.speed_factor = 0.2
+        ik = rcs.common.Pin(
+            robot_cfg.kinematic_model_path,
+            robot_cfg.attachment_site,
+            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
+        )
+        f = rcs_panda.hw.Franka(ip, ik)
+        f.set_config(robot_cfg)
         config_hand = rcs_panda.hw.FHConfig()
         g = rcs_panda.hw.FrankaHand(ip, config_hand)
         if shut:
@@ -62,9 +70,16 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False
 
 def info(ip: str, username: str, password: str, include_hand: bool = False):
     with Desk.fci(ip, username, password):
-        f = rcs_panda.hw.Franka(ip)
-        config = rcs_panda.hw.PandaConfig()
-        f.set_config(config)
+        robot_cfg = default_panda_hw_robot_cfg()
+        robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
+        robot_cfg.speed_factor = 0.2
+        ik = rcs.common.Pin(
+            robot_cfg.kinematic_model_path,
+            robot_cfg.attachment_site,
+            urdf=robot_cfg.kinematic_model_path.endswith(".urdf"),
+        )
+        f = rcs_panda.hw.Franka(ip, ik)
+        f.set_config(robot_cfg)
         print("Robot info:")
         print("Current cartesian position:")
         print(f.get_cartesian_position())

requirements.txt

@@ -1,5 +1,6 @@
 build
 wheel
+setuptools
 scikit-build-core>=0.3.3
 pybind11
 cmake