From 5b4935042082ee5e577c552510ed03377cfce7a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Wed, 11 Feb 2026 15:36:05 +0100 Subject: [PATCH] fix(franka): left over ik refactor --- examples/fr3/fr3_direct_control.py | 26 +++++++++--------- extensions/rcs_fr3/src/rcs_fr3/desk.py | 31 ++++++++++++++++------ extensions/rcs_panda/src/rcs_panda/desk.py | 31 ++++++++++++++++------ requirements.txt | 1 + 4 files changed, 60 insertions(+), 29 deletions(-) diff --git a/examples/fr3/fr3_direct_control.py b/examples/fr3/fr3_direct_control.py index 42a3a49c..e9cafb31 100644 --- a/examples/fr3/fr3_direct_control.py +++ b/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() diff --git a/extensions/rcs_fr3/src/rcs_fr3/desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py index 4ec9c780..4b5abc84 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/desk.py +++ b/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()) diff --git a/extensions/rcs_panda/src/rcs_panda/desk.py b/extensions/rcs_panda/src/rcs_panda/desk.py index a8d8a77b..2af7f20d 100644 --- a/extensions/rcs_panda/src/rcs_panda/desk.py +++ b/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()) diff --git a/requirements.txt b/requirements.txt index 7474dbd7..3566c18b 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,5 +1,6 @@ build wheel +setuptools scikit-build-core>=0.3.3 pybind11 cmake