From b98b1e65dbd7ccdeece019ca8867d2744a67716d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 10:31:49 -0700 Subject: [PATCH 01/14] feat(cli): camera episode video export --- python/rcs/__main__.py | 41 ++++++++++++++++++- python/rcs/utils.py | 91 ++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 127 insertions(+), 5 deletions(-) diff --git a/python/rcs/__main__.py b/python/rcs/__main__.py index 4f97eae0..0df6232f 100644 --- a/python/rcs/__main__.py +++ b/python/rcs/__main__.py @@ -21,7 +21,7 @@ run_conversion, ) from rcs.sim.replayer import replay as replay_dataset -from rcs.utils import export_episode_videos +from rcs.utils import export_camera_episode_videos, export_episode_videos app = typer.Typer() @@ -218,5 +218,44 @@ def episode_videos( export_episode_videos(dataset=dataset, output=output, fps=fps, n=n) +@app.command("camera-episode-videos") +def camera_episode_videos( + dataset: Annotated[ + Path, + typer.Argument( + exists=True, + help="Parquet dataset file or directory with parquet parts.", + ), + ], + output: Annotated[ + Path, + typer.Argument( + exists=False, + help="Output directory for camera episode mp4 files.", + ), + ], + fps: Annotated[int, typer.Option(help="Video frames per second.")] = DEFAULT_FPS, + camera: Annotated[ + str | None, + typer.Option(help="Only export this camera. By default, exports every camera."), + ] = None, + episode: Annotated[ + int | None, + typer.Option(help="Only export this zero-based, recording-order episode."), + ] = None, +): + """Export a simple raw-frame MP4 for every camera in every episode.""" + try: + export_camera_episode_videos( + dataset=dataset, + output=output, + fps=fps, + camera=camera, + episode=episode, + ) + except ValueError as error: + raise typer.BadParameter(str(error)) from error + + if __name__ == "__main__": app() diff --git a/python/rcs/utils.py b/python/rcs/utils.py index 5f5aa372..57a80b6e 100644 --- a/python/rcs/utils.py +++ b/python/rcs/utils.py @@ -133,6 +133,24 @@ def _render_action_panel( return image +def _episode_starts(conn: duckdb.DuckDBPyConnection, source_escaped: str) -> list[tuple[str, float]]: + """Return episodes in recording order, with a stable tie-breaker.""" + return conn.execute( + f""" + SELECT uuid, MIN(timestamp) AS start_timestamp + FROM read_parquet('{source_escaped}') + GROUP BY uuid + ORDER BY start_timestamp, uuid + """ + ).fetchall() + + +def _episode_filename(timestamp: float, episode_number: int, camera_name: str | None = None) -> str: + timestamp_text = datetime.datetime.fromtimestamp(timestamp).strftime("%Y-%m-%d-%H-%M-%S") + filename = f"{timestamp_text}_episode-{episode_number:06d}" + return f"{filename}_{camera_name}.mp4" if camera_name is not None else f"{filename}.mp4" + + def export_episode_videos( dataset: str | Path, output: str | Path, @@ -162,8 +180,8 @@ def export_episode_videos( for robot, robot_struct in action_struct.children } - uuids = conn.execute(f"SELECT DISTINCT uuid FROM read_parquet('{source_escaped}') ORDER BY uuid").fetchall() - for index, (episode_id,) in enumerate(uuids): + episodes = _episode_starts(conn, source_escaped) + for index, (episode_id, _) in enumerate(episodes): if n != -1 and index >= n: break @@ -204,7 +222,6 @@ def export_episode_videos( if not rows: continue - timestamp = datetime.datetime.fromtimestamp(float(rows[0][0])).strftime("%Y-%m-%d-%H-%M-%S") frames = [] joint_history = { robot: np.asarray([row[1 + len(camera_names) + robot_idx] for row in rows], dtype=np.float32) @@ -233,4 +250,70 @@ def export_episode_videos( tiled[top : top + height, left : left + width] = image frames.append(tiled) - _write_mp4(frames, output / f"{timestamp}.mp4", fps=fps) + _write_mp4(frames, output / _episode_filename(float(rows[0][0]), index), fps=fps) + + +def export_camera_episode_videos( + dataset: str | Path, + output: str | Path, + fps: int = 30, + camera: str | None = None, + episode: int | None = None, +) -> None: + """Export raw camera frames as one MP4 for every selected camera and episode. + + ``episode`` is the zero-based recording-order index used in the filenames. + """ + import torch + from torchvision.io import decode_jpeg + + dataset = Path(dataset) + output = Path(output) + output.mkdir(parents=True, exist_ok=True) + + source = str(dataset / "*.parquet") if dataset.is_dir() else str(dataset) + source_escaped = source.replace("'", "''") + conn = duckdb.connect() + relation = conn.sql(f"SELECT * FROM read_parquet('{source_escaped}')") + frame_struct = relation.select("obs.frames").types[0] + camera_names = [name for name, _ in frame_struct.children] + if camera is not None: + if camera not in camera_names: + available = ", ".join(camera_names) + message = f"Unknown camera {camera!r}. Available cameras: {available}" + raise ValueError(message) + camera_names = [camera] + + episodes = _episode_starts(conn, source_escaped) + if episode is not None: + if episode < 0 or episode >= len(episodes): + message = f"Episode {episode} is out of range (dataset has {len(episodes)} episodes)." + raise ValueError(message) + selected_episodes = [(episode, episodes[episode])] + else: + selected_episodes = list(enumerate(episodes)) + + for episode_number, (episode_id, _) in selected_episodes: + for camera_name in camera_names: + rows = conn.execute( + f""" + SELECT timestamp, obs.frames.{camera_name}.rgb.data + FROM read_parquet('{source_escaped}') + WHERE uuid = ? + AND obs.frames.{camera_name}.rgb.data IS NOT NULL + ORDER BY step + """, + [episode_id], + ).fetchall() + if not rows: + continue + + frames = [ + decode_jpeg(torch.frombuffer(bytearray(image_bytes), dtype=torch.uint8)).permute(1, 2, 0).cpu().numpy() + for _, image_bytes in rows + ] + _write_mp4( + frames, + output / _episode_filename(float(rows[0][0]), episode_number, camera_name), + fps=fps, + ) From c9e6f9653f5df5554733be7eb6ed85c533672219 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 10:50:49 -0700 Subject: [PATCH 02/14] feat(fr3/panda): add max torque values to config --- extensions/rcs_fr3/src/hw/Franka.cpp | 8 +++----- extensions/rcs_fr3/src/hw/Franka.h | 1 + extensions/rcs_fr3/src/pybind/rcs.cpp | 17 +++++++++++------ 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 4ea7ccda..e1054b2f 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -164,7 +164,7 @@ void PInverse(const Eigen::MatrixXd& M, Eigen::MatrixXd& M_inv, } void TorqueSafetyGuardFn(std::array& tau_d_array, - const std::array& torque_limit) { + const common::Vector7d& torque_limit) { for (size_t i = 0; i < tau_d_array.size(); i++) { if (tau_d_array[i] < -torque_limit[i]) { tau_d_array[i] = -torque_limit[i]; @@ -284,6 +284,7 @@ void Franka::osc() { franka::Model model = this->robot.loadModel(); const Eigen::Vector3d kp_p_cfg = this->m_cfg.kp_p; const double kp_r_cfg = this->m_cfg.kp_r; + const common::Vector7d torque_limit = this->m_cfg.torque_limit; this->controller_time = 0.0; @@ -495,8 +496,6 @@ void Franka::osc() { std::array tau_d_rate_limited = franka::limitRate( franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); - // deoxys/config/control_config.yml - std::array torque_limit = {5, 5, 5, 5, 5, 5, 5}; TorqueSafetyGuardFn(tau_d_rate_limited, torque_limit); return tau_d_rate_limited; @@ -514,6 +513,7 @@ void Franka::joint_controller() { franka::Model model = this->robot.loadModel(); const common::Vector7d Kp = this->m_cfg.kp; const common::Vector7d Kd = this->m_cfg.kd; + const common::Vector7d torque_limit = this->m_cfg.torque_limit; this->controller_time = 0.0; // conservative collision and impedance behavior @@ -589,8 +589,6 @@ void Franka::joint_controller() { std::array tau_d_rate_limited = franka::limitRate( franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); - // deoxys/config/control_config.yml - std::array torque_limit = {5, 5, 5, 5, 5, 5, 5}; TorqueSafetyGuardFn(tau_d_rate_limited, torque_limit); return tau_d_rate_limited; diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 6c66ba38..ada988eb 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -43,6 +43,7 @@ struct FrankaConfig : common::RobotConfig { (common::Vector7d() << 100., 100., 100., 100., 75., 150., 50.).finished(); common::Vector7d kd = (common::Vector7d() << 20., 20., 20., 20., 7.5, 15.0, 5.0).finished(); + common::Vector7d torque_limit = common::Vector7d::Constant(5.0); // values from deoxys/config/osc-position-controller.yml Eigen::Vector3d kp_p = (Eigen::Vector3d() << 150., 150., 150.).finished(); double kp_r = 250.0; diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 51c8a4a0..413c1047 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -130,6 +130,7 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("speed_factor", &rcs::hw::FrankaConfig::speed_factor) .def_readwrite("kp", &rcs::hw::FrankaConfig::kp) .def_readwrite("kd", &rcs::hw::FrankaConfig::kd) + .def_readwrite("torque_limit", &rcs::hw::FrankaConfig::torque_limit) .def_readwrite("kp_p", &rcs::hw::FrankaConfig::kp_p) .def_readwrite("kp_r", &rcs::hw::FrankaConfig::kp_r) .def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters) @@ -154,13 +155,14 @@ PYBIND11_MODULE(_core, m) { std::optional world_to_robot, bool async_control, bool tcp_offset_configured_in_desk, bool ignore_realtime, rcs::common::Pose tcp_offset, - std::string attachment_site, - std::string kinematic_model_path) { + std::string attachment_site, std::string kinematic_model_path, + const rcs::common::Vector7d& torque_limit) { rcs::hw::FR3Config cfg; cfg.ik_solver = ik_solver; cfg.speed_factor = speed_factor; cfg.kp = kp; cfg.kd = kd; + cfg.torque_limit = torque_limit; cfg.kp_p = kp_p; cfg.kp_r = kp_r; cfg.load_parameters = load_parameters; @@ -193,7 +195,8 @@ PYBIND11_MODULE(_core, m) { py::arg("tcp_offset") = default_fr3_config.tcp_offset, py::arg("attachment_site") = default_fr3_config.attachment_site, py::arg("kinematic_model_path") = - default_fr3_config.kinematic_model_path); + default_fr3_config.kinematic_model_path, + py::arg("torque_limit") = default_fr3_config.torque_limit); rcs::hw::PandaConfig default_panda_config; py::class_(hw, "PandaConfig") .def(py::init( @@ -206,13 +209,14 @@ PYBIND11_MODULE(_core, m) { std::optional world_to_robot, bool async_control, bool tcp_offset_configured_in_desk, bool ignore_realtime, rcs::common::Pose tcp_offset, - std::string attachment_site, - std::string kinematic_model_path) { + std::string attachment_site, std::string kinematic_model_path, + const rcs::common::Vector7d& torque_limit) { rcs::hw::PandaConfig cfg; cfg.ik_solver = ik_solver; cfg.speed_factor = speed_factor; cfg.kp = kp; cfg.kd = kd; + cfg.torque_limit = torque_limit; cfg.kp_p = kp_p; cfg.kp_r = kp_r; cfg.load_parameters = load_parameters; @@ -245,7 +249,8 @@ PYBIND11_MODULE(_core, m) { py::arg("tcp_offset") = default_panda_config.tcp_offset, py::arg("attachment_site") = default_panda_config.attachment_site, py::arg("kinematic_model_path") = - default_panda_config.kinematic_model_path); + default_panda_config.kinematic_model_path, + py::arg("torque_limit") = default_panda_config.torque_limit); py::object gripper_config = (py::object)py::module_::import("rcs").attr("common").attr( From e7bf1d2997253bb9fb130390af294296eb6c3f64 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 10:52:31 -0700 Subject: [PATCH 03/14] feat(fr3/panda): config for collision values --- extensions/rcs_fr3/src/hw/Franka.cpp | 45 ++++++++++++++++----------- extensions/rcs_fr3/src/hw/Franka.h | 1 + extensions/rcs_fr3/src/pybind/rcs.cpp | 18 ++++++++--- 3 files changed, 42 insertions(+), 22 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index e1054b2f..2cec907e 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -285,18 +285,21 @@ void Franka::osc() { const Eigen::Vector3d kp_p_cfg = this->m_cfg.kp_p; const double kp_r_cfg = this->m_cfg.kp_r; const common::Vector7d torque_limit = this->m_cfg.torque_limit; + const bool allow_high_collision = this->m_cfg.allow_high_collision; this->controller_time = 0.0; // conservative collision and impedance behavior this->set_default_robot_behavior(); - // high collision threshold values for high impedance - this->robot.setCollisionBehavior( - {{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}}); + if (allow_high_collision) { + // High collision threshold values for high impedance. + this->robot.setCollisionBehavior( + {{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}}); + } // from bench mark // ([150.0, 150.0, 60.0], 250.0), // kp_translation, kp_rotation @@ -514,17 +517,20 @@ void Franka::joint_controller() { const common::Vector7d Kp = this->m_cfg.kp; const common::Vector7d Kd = this->m_cfg.kd; const common::Vector7d torque_limit = this->m_cfg.torque_limit; + const bool allow_high_collision = this->m_cfg.allow_high_collision; this->controller_time = 0.0; // conservative collision and impedance behavior this->set_default_robot_behavior(); - // high collision threshold values for high impedance - this->robot.setCollisionBehavior( - {{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}}); + if (allow_high_collision) { + // High collision threshold values for high impedance. + this->robot.setCollisionBehavior( + {{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}}); + } Eigen::Array joint_max_; Eigen::Array joint_min_; @@ -613,12 +619,15 @@ void Franka::zero_torque_guiding() { } void Franka::zero_torque_controller() { - // high collision threshold values for high impedance - robot.setCollisionBehavior( - {{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}}); + this->set_default_robot_behavior(); + if (this->m_cfg.allow_high_collision) { + // High collision threshold values for high impedance. + robot.setCollisionBehavior( + {{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}}); + } this->controller_time = 0.0; try { diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index ada988eb..ea0d6bd6 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -44,6 +44,7 @@ struct FrankaConfig : common::RobotConfig { common::Vector7d kd = (common::Vector7d() << 20., 20., 20., 20., 7.5, 15.0, 5.0).finished(); common::Vector7d torque_limit = common::Vector7d::Constant(5.0); + bool allow_high_collision = false; // values from deoxys/config/osc-position-controller.yml Eigen::Vector3d kp_p = (Eigen::Vector3d() << 150., 150., 150.).finished(); double kp_r = 250.0; diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 413c1047..f0833a36 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -131,6 +131,8 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("kp", &rcs::hw::FrankaConfig::kp) .def_readwrite("kd", &rcs::hw::FrankaConfig::kd) .def_readwrite("torque_limit", &rcs::hw::FrankaConfig::torque_limit) + .def_readwrite("allow_high_collision", + &rcs::hw::FrankaConfig::allow_high_collision) .def_readwrite("kp_p", &rcs::hw::FrankaConfig::kp_p) .def_readwrite("kp_r", &rcs::hw::FrankaConfig::kp_r) .def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters) @@ -156,13 +158,15 @@ PYBIND11_MODULE(_core, m) { bool async_control, bool tcp_offset_configured_in_desk, bool ignore_realtime, rcs::common::Pose tcp_offset, std::string attachment_site, std::string kinematic_model_path, - const rcs::common::Vector7d& torque_limit) { + const rcs::common::Vector7d& torque_limit, + bool allow_high_collision) { rcs::hw::FR3Config cfg; cfg.ik_solver = ik_solver; cfg.speed_factor = speed_factor; cfg.kp = kp; cfg.kd = kd; cfg.torque_limit = torque_limit; + cfg.allow_high_collision = allow_high_collision; cfg.kp_p = kp_p; cfg.kp_r = kp_r; cfg.load_parameters = load_parameters; @@ -196,7 +200,9 @@ PYBIND11_MODULE(_core, m) { py::arg("attachment_site") = default_fr3_config.attachment_site, py::arg("kinematic_model_path") = default_fr3_config.kinematic_model_path, - py::arg("torque_limit") = default_fr3_config.torque_limit); + py::arg("torque_limit") = default_fr3_config.torque_limit, + py::arg("allow_high_collision") = + default_fr3_config.allow_high_collision); rcs::hw::PandaConfig default_panda_config; py::class_(hw, "PandaConfig") .def(py::init( @@ -210,13 +216,15 @@ PYBIND11_MODULE(_core, m) { bool async_control, bool tcp_offset_configured_in_desk, bool ignore_realtime, rcs::common::Pose tcp_offset, std::string attachment_site, std::string kinematic_model_path, - const rcs::common::Vector7d& torque_limit) { + const rcs::common::Vector7d& torque_limit, + bool allow_high_collision) { rcs::hw::PandaConfig cfg; cfg.ik_solver = ik_solver; cfg.speed_factor = speed_factor; cfg.kp = kp; cfg.kd = kd; cfg.torque_limit = torque_limit; + cfg.allow_high_collision = allow_high_collision; cfg.kp_p = kp_p; cfg.kp_r = kp_r; cfg.load_parameters = load_parameters; @@ -250,7 +258,9 @@ PYBIND11_MODULE(_core, m) { py::arg("attachment_site") = default_panda_config.attachment_site, py::arg("kinematic_model_path") = default_panda_config.kinematic_model_path, - py::arg("torque_limit") = default_panda_config.torque_limit); + py::arg("torque_limit") = default_panda_config.torque_limit, + py::arg("allow_high_collision") = + default_panda_config.allow_high_collision); py::object gripper_config = (py::object)py::module_::import("rcs").attr("common").attr( From b0a572fb7f55fb2c22d0a7c7282d3767cd6a0fb4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 11:00:41 -0700 Subject: [PATCH 04/14] chore: add stub files --- extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 6 ++++++ extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index 84947baa..b5fc9393 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -103,6 +103,7 @@ class Franka(rcs._core.common.Robot): def zero_torque_guiding(self) -> None: ... class FrankaConfig(rcs._core.common.RobotConfig): + allow_high_collision: bool async_control: bool ignore_realtime: bool ik_solver: IKSolver @@ -115,6 +116,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float tcp_offset_configured_in_desk: bool + torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] world_to_robot: rcs._core.common.Pose | None class FrankaHand(rcs._core.common.Gripper): @@ -315,6 +317,8 @@ class FR3Config(FrankaConfig): tcp_offset: rcs._core.common.Pose = ..., attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., + allow_high_collision: bool = False, ) -> None: ... class PandaConfig(FrankaConfig): @@ -336,6 +340,8 @@ class PandaConfig(FrankaConfig): tcp_offset: rcs._core.common.Pose = ..., attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., + allow_high_collision: bool = False, ) -> None: ... class FrankaState(rcs._core.common.RobotState): diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index 84947baa..b5fc9393 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -103,6 +103,7 @@ class Franka(rcs._core.common.Robot): def zero_torque_guiding(self) -> None: ... class FrankaConfig(rcs._core.common.RobotConfig): + allow_high_collision: bool async_control: bool ignore_realtime: bool ik_solver: IKSolver @@ -115,6 +116,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float tcp_offset_configured_in_desk: bool + torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] world_to_robot: rcs._core.common.Pose | None class FrankaHand(rcs._core.common.Gripper): @@ -315,6 +317,8 @@ class FR3Config(FrankaConfig): tcp_offset: rcs._core.common.Pose = ..., attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., + allow_high_collision: bool = False, ) -> None: ... class PandaConfig(FrankaConfig): @@ -336,6 +340,8 @@ class PandaConfig(FrankaConfig): tcp_offset: rcs._core.common.Pose = ..., attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", + torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., + allow_high_collision: bool = False, ) -> None: ... class FrankaState(rcs._core.common.RobotState): From 6449f37ab30270cb785d6d36a819bb316a2c8398 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 13:16:09 -0700 Subject: [PATCH 05/14] fix(franka): desk ignore realtime control --- extensions/rcs_fr3/src/rcs_fr3/desk.py | 2 ++ extensions/rcs_panda/src/rcs_panda/desk.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/extensions/rcs_fr3/src/rcs_fr3/desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py index 32d6c0ed..10006252 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/desk.py +++ b/extensions/rcs_fr3/src/rcs_fr3/desk.py @@ -51,6 +51,7 @@ def home(ip: str): env_cfg = default_env.config() robot_cfg = env_cfg.robot_cfg robot_cfg.speed_factor = 0.2 + robot_cfg.ignore_realtime = True f = rcs_fr3.hw.Franka(robot_cfg) f.move_home() @@ -72,6 +73,7 @@ def gripper(ip: str, close_gripper: bool): def info(ip: str, include_hand: bool = False): robot_cfg = rcs_fr3.hw.FR3Config(ip=ip) robot_cfg.speed_factor = 0.2 + robot_cfg.ignore_realtime = True f = rcs_fr3.hw.Franka(robot_cfg) print("Robot info:") print("Current cartesian position:") diff --git a/extensions/rcs_panda/src/rcs_panda/desk.py b/extensions/rcs_panda/src/rcs_panda/desk.py index f64f8d96..5ca6bd4d 100644 --- a/extensions/rcs_panda/src/rcs_panda/desk.py +++ b/extensions/rcs_panda/src/rcs_panda/desk.py @@ -51,6 +51,7 @@ def home(ip: str): env_cfg = default_env.config() robot_cfg = env_cfg.robot_cfg robot_cfg.speed_factor = 0.2 + robot_cfg.ignore_realtime = True f = rcs_panda.hw.Franka(robot_cfg) f.move_home() @@ -71,6 +72,7 @@ def gripper(ip: str, close_gripper: bool): def info(ip: str, include_hand: bool = False): robot_cfg = rcs_panda.hw.PandaConfig(ip=ip) robot_cfg.speed_factor = 0.2 + robot_cfg.ignore_realtime = True f = rcs_panda.hw.Franka(robot_cfg) print("Robot info:") print("Current cartesian position:") From 7eadd6f905d59ea43be25f777624506b83747149 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 13:20:38 -0700 Subject: [PATCH 06/14] fix(franka): reset also returns franka state --- extensions/rcs_fr3/src/rcs_fr3/envs.py | 3 ++- extensions/rcs_panda/src/rcs_panda/envs.py | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/extensions/rcs_fr3/src/rcs_fr3/envs.py b/extensions/rcs_fr3/src/rcs_fr3/envs.py index 8b780691..3bd10c58 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/envs.py +++ b/extensions/rcs_fr3/src/rcs_fr3/envs.py @@ -44,7 +44,8 @@ def _rs2dict(self, state: hw.RobotState): def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: - return super().reset(seed=seed, options=options) + obs, info = super().reset(seed=seed, options=options) + return self.get_obs(obs), info def close(self): self.hw_robot.stop_control_thread() diff --git a/extensions/rcs_panda/src/rcs_panda/envs.py b/extensions/rcs_panda/src/rcs_panda/envs.py index 34e8d1b1..2e83794f 100644 --- a/extensions/rcs_panda/src/rcs_panda/envs.py +++ b/extensions/rcs_panda/src/rcs_panda/envs.py @@ -44,7 +44,8 @@ def _rs2dict(self, state: hw.RobotState): def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict[str, Any], dict[str, Any]]: - return super().reset(seed=seed, options=options) + obs, info = super().reset(seed=seed, options=options) + return self.get_obs(obs), info def close(self): self.hw_robot.stop_control_thread() From 1f3a9dc8198d44a33fc5ead3568c9fe3d4f70ff9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 13:23:18 -0700 Subject: [PATCH 07/14] feat(zed): added intrinsics cli command --- extensions/rcs_zed/src/rcs_zed/__main__.py | 46 ++++++++++++++++++++++ extensions/rcs_zed/src/rcs_zed/camera.py | 1 - 2 files changed, 46 insertions(+), 1 deletion(-) diff --git a/extensions/rcs_zed/src/rcs_zed/__main__.py b/extensions/rcs_zed/src/rcs_zed/__main__.py index ba6c3771..7e184b82 100644 --- a/extensions/rcs_zed/src/rcs_zed/__main__.py +++ b/extensions/rcs_zed/src/rcs_zed/__main__.py @@ -62,6 +62,52 @@ def serials(): typer.echo(f" {device.model}: {device.serial} (imu={device.has_imu})") +def _format_intrinsics(name: str, matrix) -> str: + fx, fy, cx, cy = float(matrix[0, 0]), float(matrix[1, 1]), float(matrix[0, 2]), float(matrix[1, 2]) + lines = [ + f"{name}:", + f" fx={fx:.6f} fy={fy:.6f} cx={cx:.6f} cy={cy:.6f}", + f" K=\n{matrix}", + ] + return "\n".join(lines) + + +@zed_app.command() +def intrinsics( + serial: str | None = typer.Argument(None, help="Optional ZED serial number. Uses the first device if omitted."), + width: int = typer.Option(1280, help="Requested capture width."), + height: int = typer.Option(720, help="Requested capture height."), + fps: int = typer.Option(30, help="Requested capture frame rate."), + right: bool = typer.Option(False, "--right", help="Also print right-camera intrinsics."), +): + """Open a ZED camera and print its RGB intrinsics for the requested resolution.""" + serial = _resolve_serial(serial) + config = common.BaseCameraConfig( + identifier=serial, + resolution_width=width, + resolution_height=height, + frame_rate=fps, + ) + try: + handle = ZEDCameraSet.open_camera( + config, + enable_depth=False, + enable_imu=False, + include_right=right, + ) + except Exception as exc: + msg = f"Could not start ZED camera {serial}: {exc}" + raise typer.BadParameter(msg) from exc + + try: + typer.echo(f"serial={handle.device_info.serial} model={handle.device_info.model} resolution={width}x{height}") + typer.echo(_format_intrinsics("left", handle.color_intrinsics)) + if right and handle.right_color_intrinsics is not None: + typer.echo(_format_intrinsics("right", handle.right_color_intrinsics)) + finally: + handle.close() + + @zed_app.command("rgb-view") def rgb_view( serial: str | None = typer.Argument(None, help="Optional ZED serial number. Uses the first device if omitted."), diff --git a/extensions/rcs_zed/src/rcs_zed/camera.py b/extensions/rcs_zed/src/rcs_zed/camera.py index 881a1368..8e86481d 100644 --- a/extensions/rcs_zed/src/rcs_zed/camera.py +++ b/extensions/rcs_zed/src/rcs_zed/camera.py @@ -216,7 +216,6 @@ def open_camera( init.camera_fps = config.frame_rate init.coordinate_units = sl.UNIT.METER init.depth_mode = sl.DEPTH_MODE.NONE if not enable_depth else sl.DEPTH_MODE.QUALITY - init.sdk_verbose = False init.set_from_serial_number(int(config.identifier)) camera = sl.Camera() From ca219e4ef7e9d2437738fcc02ef5279a4ac8780d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 15:13:32 -0700 Subject: [PATCH 08/14] feat(franka): tcp offset defaults to desk --- extensions/rcs_fr3/src/hw/Franka.cpp | 68 +++++++++++-------- extensions/rcs_fr3/src/hw/Franka.h | 4 +- extensions/rcs_fr3/src/pybind/rcs.cpp | 19 ++---- .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 8 +-- extensions/rcs_fr3/src/rcs_fr3/configs.py | 1 - .../src/rcs_panda/_core/hw/__init__.pyi | 8 +-- 6 files changed, 52 insertions(+), 56 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 2cec907e..f973bea5 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -19,6 +19,15 @@ namespace rcs { namespace hw { +common::Pose GetTCPInBaseFrame(const franka::RobotState& robot_state, + const std::optional& tcp_offset) { + if (!tcp_offset.has_value()) { + return common::Pose(robot_state.O_T_EE); + } + return common::Pose(robot_state.O_T_EE) * + common::Pose(robot_state.F_T_EE).inverse() * tcp_offset.value(); +} + Franka::Franka(const FrankaConfig& cfg, std::optional> ik) : m_cfg(cfg), @@ -99,19 +108,16 @@ void Franka::set_default_robot_behavior() { common::Pose Franka::get_cartesian_position() { this->check_for_background_errors(); - common::Pose x; + franka::RobotState robot_state; if (this->running_controller.load() == Controller::none) { this->curr_state = this->robot.readOnce(); - x = common::Pose(this->curr_state.O_T_EE); + robot_state = this->curr_state; } else { this->interpolator_mutex.lock(); - x = common::Pose(this->curr_state.O_T_EE); + robot_state = this->curr_state; this->interpolator_mutex.unlock(); } - if (!this->m_cfg.tcp_offset_configured_in_desk) { - return x * this->m_cfg.tcp_offset; - } - return x; + return GetTCPInBaseFrame(robot_state, this->m_cfg.tcp_offset); } void Franka::set_joint_position(const common::VectorXd& q) { @@ -250,10 +256,8 @@ void Franka::osc_set_cartesian_position( this->interpolator_mutex.lock(); } - common::Pose curr_pose(this->curr_state.O_T_EE); - if (!this->m_cfg.tcp_offset_configured_in_desk) { - curr_pose = curr_pose * this->m_cfg.tcp_offset; - } + common::Pose curr_pose = + GetTCPInBaseFrame(this->curr_state, this->m_cfg.tcp_offset); this->traj_interpolator.reset( this->controller_time, curr_pose.translation(), curr_pose.quaternion(), desired_pose_EE_in_base_frame.translation(), @@ -385,8 +389,15 @@ void Franka::osc() { Eigen::Map> gravity( gravity_array.data()); - std::array jacobian_array = - model.zeroJacobian(franka::Frame::kEndEffector, robot_state); + std::array jacobian_array; + if (this->m_cfg.tcp_offset.has_value()) { + jacobian_array = model.zeroJacobian( + franka::Frame::kEndEffector, robot_state.q, + this->m_cfg.tcp_offset->affine_array(), robot_state.EE_T_K); + } else { + jacobian_array = + model.zeroJacobian(franka::Frame::kEndEffector, robot_state); + } Eigen::Map> jacobian( jacobian_array.data()); @@ -398,9 +409,7 @@ void Franka::osc() { // Express OSC feedback in the same TCP frame exposed by the public // Cartesian API. common::Pose T_EE_in_base_frame_pose = - this->m_cfg.tcp_offset_configured_in_desk - ? common::Pose(robot_state.O_T_EE) - : common::Pose(robot_state.O_T_EE) * this->m_cfg.tcp_offset; + GetTCPInBaseFrame(robot_state, this->m_cfg.tcp_offset); Eigen::Affine3d T_EE_in_base_frame = T_EE_in_base_frame_pose.affine_matrix(); @@ -737,12 +746,8 @@ std::optional> Franka::get_ik() { void Franka::set_cartesian_position(const common::Pose& x) { // pose is assumed to be in the robots coordinate frame - common::Pose target_pose = x; - if (!this->m_cfg.tcp_offset_configured_in_desk) { - target_pose = target_pose * this->m_cfg.tcp_offset.inverse(); - } if (this->m_cfg.async_control) { - this->osc_set_cartesian_position(target_pose); + this->osc_set_cartesian_position(x); return; } // TODO: this should handled with tcp offset config @@ -757,8 +762,13 @@ void Franka::set_cartesian_position(const common::Pose& x) { // takes care of the default franka hand offset lets add a franka hand offset if (this->m_cfg.ik_solver == IKSolver::franka_ik) { - // if gripper is attached the tcp offset will automatically be applied - // by libfranka + const franka::RobotState robot_state = this->robot.readOnce(); + common::Pose target_pose = x; + if (this->m_cfg.tcp_offset.has_value()) { + target_pose = x * this->m_cfg.tcp_offset->inverse() * + common::Pose(robot_state.F_T_NE) * + nominal_end_effector_frame_value; + } this->robot.setEE(nominal_end_effector_frame_value.affine_array()); this->set_cartesian_position_internal(target_pose, 1.0, std::nullopt, std::nullopt); @@ -774,8 +784,11 @@ void Franka::set_cartesian_position_ik(const common::Pose& pose) { "No inverse kinematics was provided. Cannot use IK to set cartesian " "position."); } - auto joints = this->m_ik.value()->inverse(pose, this->get_joint_position(), - this->m_cfg.tcp_offset); + const franka::RobotState robot_state = this->robot.readOnce(); + const common::Pose tcp_offset = + this->m_cfg.tcp_offset.value_or(common::Pose(robot_state.F_T_EE)); + auto joints = + this->m_ik.value()->inverse(pose, this->get_joint_position(), tcp_offset); if (joints.has_value()) { this->set_joint_position(joints.value()); @@ -821,10 +834,7 @@ void Franka::set_cartesian_position_internal(const common::Pose& pose, if (time == 0) { initial_elbow = state.elbow_c; - initial_pose = - this->m_cfg.tcp_offset_configured_in_desk - ? common::Pose(state.O_T_EE) - : common::Pose(state.O_T_EE) * this->m_cfg.tcp_offset; + initial_pose = common::Pose(state.O_T_EE); } auto new_elbow = initial_elbow; const double progress = time / max_time; diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index ea0d6bd6..ab230eb8 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -38,7 +38,7 @@ struct FrankaConfig : common::RobotConfig { common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE; IKSolver ik_solver = IKSolver::rcs_ik; double speed_factor = DEFAULT_SPEED_FACTOR; - // deoxys/config/joint-impedance-controller.yml + // values from deoxys/config/joint-impedance-controller.yml common::Vector7d kp = (common::Vector7d() << 100., 100., 100., 100., 75., 150., 50.).finished(); common::Vector7d kd = @@ -51,8 +51,8 @@ struct FrankaConfig : common::RobotConfig { std::optional load_parameters = std::nullopt; std::optional nominal_end_effector_frame = std::nullopt; std::optional world_to_robot = std::nullopt; + std::optional tcp_offset = std::nullopt; bool async_control = false; - bool tcp_offset_configured_in_desk = true; bool ignore_realtime = false; size_t dof = 7; Eigen::Matrix joint_limits = diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index f0833a36..97dfbad6 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -139,8 +139,7 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("nominal_end_effector_frame", &rcs::hw::FrankaConfig::nominal_end_effector_frame) .def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot) - .def_readwrite("tcp_offset_configured_in_desk", - &rcs::hw::FrankaConfig::tcp_offset_configured_in_desk) + .def_readwrite("tcp_offset", &rcs::hw::FrankaConfig::tcp_offset) .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control) .def_readwrite("ignore_realtime", &rcs::hw::FrankaConfig::ignore_realtime) .def_readwrite("ip", &rcs::hw::FrankaConfig::ip); @@ -155,8 +154,8 @@ PYBIND11_MODULE(_core, m) { std::optional load_parameters, std::optional nominal_end_effector_frame, std::optional world_to_robot, - bool async_control, bool tcp_offset_configured_in_desk, - bool ignore_realtime, rcs::common::Pose tcp_offset, + bool async_control, bool ignore_realtime, + std::optional tcp_offset, std::string attachment_site, std::string kinematic_model_path, const rcs::common::Vector7d& torque_limit, bool allow_high_collision) { @@ -173,8 +172,6 @@ PYBIND11_MODULE(_core, m) { cfg.nominal_end_effector_frame = nominal_end_effector_frame; cfg.world_to_robot = world_to_robot; cfg.async_control = async_control; - cfg.tcp_offset_configured_in_desk = - tcp_offset_configured_in_desk; cfg.ignore_realtime = ignore_realtime; cfg.ip = ip; cfg.tcp_offset = tcp_offset; @@ -193,8 +190,6 @@ PYBIND11_MODULE(_core, m) { default_fr3_config.nominal_end_effector_frame, py::arg("world_to_robot") = default_fr3_config.world_to_robot, py::arg("async_control") = default_fr3_config.async_control, - py::arg("tcp_offset_configured_in_desk") = - default_fr3_config.tcp_offset_configured_in_desk, py::arg("ignore_realtime") = default_fr3_config.ignore_realtime, py::arg("tcp_offset") = default_fr3_config.tcp_offset, py::arg("attachment_site") = default_fr3_config.attachment_site, @@ -213,8 +208,8 @@ PYBIND11_MODULE(_core, m) { std::optional load_parameters, std::optional nominal_end_effector_frame, std::optional world_to_robot, - bool async_control, bool tcp_offset_configured_in_desk, - bool ignore_realtime, rcs::common::Pose tcp_offset, + bool async_control, bool ignore_realtime, + std::optional tcp_offset, std::string attachment_site, std::string kinematic_model_path, const rcs::common::Vector7d& torque_limit, bool allow_high_collision) { @@ -231,8 +226,6 @@ PYBIND11_MODULE(_core, m) { cfg.nominal_end_effector_frame = nominal_end_effector_frame; cfg.world_to_robot = world_to_robot; cfg.async_control = async_control; - cfg.tcp_offset_configured_in_desk = - tcp_offset_configured_in_desk; cfg.ignore_realtime = ignore_realtime; cfg.ip = ip; cfg.tcp_offset = tcp_offset; @@ -251,8 +244,6 @@ PYBIND11_MODULE(_core, m) { default_panda_config.nominal_end_effector_frame, py::arg("world_to_robot") = default_panda_config.world_to_robot, py::arg("async_control") = default_panda_config.async_control, - py::arg("tcp_offset_configured_in_desk") = - default_panda_config.tcp_offset_configured_in_desk, py::arg("ignore_realtime") = default_panda_config.ignore_realtime, py::arg("tcp_offset") = default_panda_config.tcp_offset, py::arg("attachment_site") = default_panda_config.attachment_site, diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index b5fc9393..3eb54522 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -115,7 +115,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): load_parameters: FrankaLoad | None nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float - tcp_offset_configured_in_desk: bool + tcp_offset: rcs._core.common.Pose | None torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] world_to_robot: rcs._core.common.Pose | None @@ -312,9 +312,8 @@ class FR3Config(FrankaConfig): nominal_end_effector_frame: rcs._core.common.Pose | None = None, world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, - tcp_offset_configured_in_desk: bool = True, ignore_realtime: bool = False, - tcp_offset: rcs._core.common.Pose = ..., + tcp_offset: rcs._core.common.Pose | None = None, attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., @@ -335,9 +334,8 @@ class PandaConfig(FrankaConfig): nominal_end_effector_frame: rcs._core.common.Pose | None = None, world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, - tcp_offset_configured_in_desk: bool = True, ignore_realtime: bool = False, - tcp_offset: rcs._core.common.Pose = ..., + tcp_offset: rcs._core.common.Pose | None = None, attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., diff --git a/extensions/rcs_fr3/src/rcs_fr3/configs.py b/extensions/rcs_fr3/src/rcs_fr3/configs.py index 2a25d056..c989bc2c 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/configs.py +++ b/extensions/rcs_fr3/src/rcs_fr3/configs.py @@ -25,7 +25,6 @@ def config(self) -> FR3HardwareEnvCreatorConfig: ik_solver=hw.IKSolver.rcs_ik, speed_factor=0.1, async_control=False, - tcp_offset_configured_in_desk=True, ignore_realtime=False, tcp_offset=common.Pose(common.FrankaHandTCPOffset()), attachment_site="attachment_site", diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index b5fc9393..3eb54522 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -115,7 +115,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): load_parameters: FrankaLoad | None nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float - tcp_offset_configured_in_desk: bool + tcp_offset: rcs._core.common.Pose | None torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] world_to_robot: rcs._core.common.Pose | None @@ -312,9 +312,8 @@ class FR3Config(FrankaConfig): nominal_end_effector_frame: rcs._core.common.Pose | None = None, world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, - tcp_offset_configured_in_desk: bool = True, ignore_realtime: bool = False, - tcp_offset: rcs._core.common.Pose = ..., + tcp_offset: rcs._core.common.Pose | None = None, attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., @@ -335,9 +334,8 @@ class PandaConfig(FrankaConfig): nominal_end_effector_frame: rcs._core.common.Pose | None = None, world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, - tcp_offset_configured_in_desk: bool = True, ignore_realtime: bool = False, - tcp_offset: rcs._core.common.Pose = ..., + tcp_offset: rcs._core.common.Pose | None = None, attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., From ce4c2f9825175d4ef668f7e5b09d387a20e3a4a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 15:15:12 -0700 Subject: [PATCH 09/14] chore(franka): removed unused nominal_end_effector_frame_value --- extensions/rcs_fr3/src/hw/Franka.cpp | 15 +- extensions/rcs_fr3/src/hw/Franka.h | 1 - extensions/rcs_fr3/src/pybind/rcs.cpp | 134 ++++++++---------- .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 3 - .../src/rcs_panda/_core/hw/__init__.pyi | 3 - 5 files changed, 63 insertions(+), 93 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index f973bea5..21ff3c81 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -750,26 +750,13 @@ void Franka::set_cartesian_position(const common::Pose& x) { this->osc_set_cartesian_position(x); return; } - // TODO: this should handled with tcp offset config - common::Pose nominal_end_effector_frame_value; - if (this->m_cfg.nominal_end_effector_frame.has_value()) { - nominal_end_effector_frame_value = - this->m_cfg.nominal_end_effector_frame.value(); - } else { - nominal_end_effector_frame_value = common::Pose::Identity(); - } - // nominal end effector frame should be on top of tcp offset as franka already - // takes care of the default franka hand offset lets add a franka hand offset - if (this->m_cfg.ik_solver == IKSolver::franka_ik) { const franka::RobotState robot_state = this->robot.readOnce(); common::Pose target_pose = x; if (this->m_cfg.tcp_offset.has_value()) { target_pose = x * this->m_cfg.tcp_offset->inverse() * - common::Pose(robot_state.F_T_NE) * - nominal_end_effector_frame_value; + common::Pose(robot_state.F_T_EE); } - this->robot.setEE(nominal_end_effector_frame_value.affine_array()); this->set_cartesian_position_internal(target_pose, 1.0, std::nullopt, std::nullopt); diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index ab230eb8..e6184333 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -49,7 +49,6 @@ struct FrankaConfig : common::RobotConfig { Eigen::Vector3d kp_p = (Eigen::Vector3d() << 150., 150., 150.).finished(); double kp_r = 250.0; std::optional load_parameters = std::nullopt; - std::optional nominal_end_effector_frame = std::nullopt; std::optional world_to_robot = std::nullopt; std::optional tcp_offset = std::nullopt; bool async_control = false; diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 97dfbad6..86b85005 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -136,8 +136,6 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("kp_p", &rcs::hw::FrankaConfig::kp_p) .def_readwrite("kp_r", &rcs::hw::FrankaConfig::kp_r) .def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters) - .def_readwrite("nominal_end_effector_frame", - &rcs::hw::FrankaConfig::nominal_end_effector_frame) .def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot) .def_readwrite("tcp_offset", &rcs::hw::FrankaConfig::tcp_offset) .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control) @@ -146,39 +144,37 @@ PYBIND11_MODULE(_core, m) { rcs::hw::FR3Config default_fr3_config; py::class_(hw, "FR3Config") - .def(py::init( - [](const std::string& ip, rcs::hw::IKSolver ik_solver, - double speed_factor, const rcs::common::Vector7d& kp, - const rcs::common::Vector7d& kd, const Eigen::Vector3d& kp_p, - double kp_r, - std::optional load_parameters, - std::optional nominal_end_effector_frame, - std::optional world_to_robot, - bool async_control, bool ignore_realtime, - std::optional tcp_offset, - std::string attachment_site, std::string kinematic_model_path, - const rcs::common::Vector7d& torque_limit, - bool allow_high_collision) { - rcs::hw::FR3Config cfg; - cfg.ik_solver = ik_solver; - cfg.speed_factor = speed_factor; - cfg.kp = kp; - cfg.kd = kd; - cfg.torque_limit = torque_limit; - cfg.allow_high_collision = allow_high_collision; - cfg.kp_p = kp_p; - cfg.kp_r = kp_r; - cfg.load_parameters = load_parameters; - cfg.nominal_end_effector_frame = nominal_end_effector_frame; - cfg.world_to_robot = world_to_robot; - cfg.async_control = async_control; - cfg.ignore_realtime = ignore_realtime; - cfg.ip = ip; - cfg.tcp_offset = tcp_offset; - cfg.attachment_site = attachment_site; - cfg.kinematic_model_path = kinematic_model_path; - return cfg; - }), + .def(py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver, + double speed_factor, const rcs::common::Vector7d& kp, + const rcs::common::Vector7d& kd, + const Eigen::Vector3d& kp_p, double kp_r, + std::optional load_parameters, + std::optional world_to_robot, + bool async_control, bool ignore_realtime, + std::optional tcp_offset, + std::string attachment_site, + std::string kinematic_model_path, + const rcs::common::Vector7d& torque_limit, + bool allow_high_collision) { + rcs::hw::FR3Config cfg; + cfg.ik_solver = ik_solver; + cfg.speed_factor = speed_factor; + cfg.kp = kp; + cfg.kd = kd; + cfg.torque_limit = torque_limit; + cfg.allow_high_collision = allow_high_collision; + cfg.kp_p = kp_p; + cfg.kp_r = kp_r; + cfg.load_parameters = load_parameters; + cfg.world_to_robot = world_to_robot; + cfg.async_control = async_control; + cfg.ignore_realtime = ignore_realtime; + cfg.ip = ip; + cfg.tcp_offset = tcp_offset; + cfg.attachment_site = attachment_site; + cfg.kinematic_model_path = kinematic_model_path; + return cfg; + }), py::arg("ip"), py::arg("ik_solver") = default_fr3_config.ik_solver, py::arg("speed_factor") = default_fr3_config.speed_factor, py::arg("kp") = default_fr3_config.kp, @@ -186,8 +182,6 @@ PYBIND11_MODULE(_core, m) { py::arg("kp_p") = default_fr3_config.kp_p, py::arg("kp_r") = default_fr3_config.kp_r, py::arg("load_parameters") = default_fr3_config.load_parameters, - py::arg("nominal_end_effector_frame") = - default_fr3_config.nominal_end_effector_frame, py::arg("world_to_robot") = default_fr3_config.world_to_robot, py::arg("async_control") = default_fr3_config.async_control, py::arg("ignore_realtime") = default_fr3_config.ignore_realtime, @@ -200,39 +194,37 @@ PYBIND11_MODULE(_core, m) { default_fr3_config.allow_high_collision); rcs::hw::PandaConfig default_panda_config; py::class_(hw, "PandaConfig") - .def(py::init( - [](const std::string& ip, rcs::hw::IKSolver ik_solver, - double speed_factor, const rcs::common::Vector7d& kp, - const rcs::common::Vector7d& kd, const Eigen::Vector3d& kp_p, - double kp_r, - std::optional load_parameters, - std::optional nominal_end_effector_frame, - std::optional world_to_robot, - bool async_control, bool ignore_realtime, - std::optional tcp_offset, - std::string attachment_site, std::string kinematic_model_path, - const rcs::common::Vector7d& torque_limit, - bool allow_high_collision) { - rcs::hw::PandaConfig cfg; - cfg.ik_solver = ik_solver; - cfg.speed_factor = speed_factor; - cfg.kp = kp; - cfg.kd = kd; - cfg.torque_limit = torque_limit; - cfg.allow_high_collision = allow_high_collision; - cfg.kp_p = kp_p; - cfg.kp_r = kp_r; - cfg.load_parameters = load_parameters; - cfg.nominal_end_effector_frame = nominal_end_effector_frame; - cfg.world_to_robot = world_to_robot; - cfg.async_control = async_control; - cfg.ignore_realtime = ignore_realtime; - cfg.ip = ip; - cfg.tcp_offset = tcp_offset; - cfg.attachment_site = attachment_site; - cfg.kinematic_model_path = kinematic_model_path; - return cfg; - }), + .def(py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver, + double speed_factor, const rcs::common::Vector7d& kp, + const rcs::common::Vector7d& kd, + const Eigen::Vector3d& kp_p, double kp_r, + std::optional load_parameters, + std::optional world_to_robot, + bool async_control, bool ignore_realtime, + std::optional tcp_offset, + std::string attachment_site, + std::string kinematic_model_path, + const rcs::common::Vector7d& torque_limit, + bool allow_high_collision) { + rcs::hw::PandaConfig cfg; + cfg.ik_solver = ik_solver; + cfg.speed_factor = speed_factor; + cfg.kp = kp; + cfg.kd = kd; + cfg.torque_limit = torque_limit; + cfg.allow_high_collision = allow_high_collision; + cfg.kp_p = kp_p; + cfg.kp_r = kp_r; + cfg.load_parameters = load_parameters; + cfg.world_to_robot = world_to_robot; + cfg.async_control = async_control; + cfg.ignore_realtime = ignore_realtime; + cfg.ip = ip; + cfg.tcp_offset = tcp_offset; + cfg.attachment_site = attachment_site; + cfg.kinematic_model_path = kinematic_model_path; + return cfg; + }), py::arg("ip"), py::arg("ik_solver") = default_panda_config.ik_solver, py::arg("speed_factor") = default_panda_config.speed_factor, py::arg("kp") = default_panda_config.kp, @@ -240,8 +232,6 @@ PYBIND11_MODULE(_core, m) { py::arg("kp_p") = default_panda_config.kp_p, py::arg("kp_r") = default_panda_config.kp_r, py::arg("load_parameters") = default_panda_config.load_parameters, - py::arg("nominal_end_effector_frame") = - default_panda_config.nominal_end_effector_frame, py::arg("world_to_robot") = default_panda_config.world_to_robot, py::arg("async_control") = default_panda_config.async_control, py::arg("ignore_realtime") = default_panda_config.ignore_realtime, diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index 3eb54522..b002e3dd 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -113,7 +113,6 @@ class FrankaConfig(rcs._core.common.RobotConfig): kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] kp_r: float load_parameters: FrankaLoad | None - nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float tcp_offset: rcs._core.common.Pose | None torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] @@ -309,7 +308,6 @@ class FR3Config(FrankaConfig): kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] = ..., kp_r: float = 250.0, load_parameters: FrankaLoad | None = None, - nominal_end_effector_frame: rcs._core.common.Pose | None = None, world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, ignore_realtime: bool = False, @@ -331,7 +329,6 @@ class PandaConfig(FrankaConfig): kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] = ..., kp_r: float = 250.0, load_parameters: FrankaLoad | None = None, - nominal_end_effector_frame: rcs._core.common.Pose | None = None, world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, ignore_realtime: bool = False, diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index 3eb54522..b002e3dd 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -113,7 +113,6 @@ class FrankaConfig(rcs._core.common.RobotConfig): kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] kp_r: float load_parameters: FrankaLoad | None - nominal_end_effector_frame: rcs._core.common.Pose | None speed_factor: float tcp_offset: rcs._core.common.Pose | None torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] @@ -309,7 +308,6 @@ class FR3Config(FrankaConfig): kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] = ..., kp_r: float = 250.0, load_parameters: FrankaLoad | None = None, - nominal_end_effector_frame: rcs._core.common.Pose | None = None, world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, ignore_realtime: bool = False, @@ -331,7 +329,6 @@ class PandaConfig(FrankaConfig): kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] = ..., kp_r: float = 250.0, load_parameters: FrankaLoad | None = None, - nominal_end_effector_frame: rcs._core.common.Pose | None = None, world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, ignore_realtime: bool = False, From c01979c569f75b0804f29718101a942b28b2ab9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 15:17:32 -0700 Subject: [PATCH 10/14] feat(franka): add get_cartesian_flange_position method --- extensions/rcs_fr3/src/hw/Franka.cpp | 21 +++++++++++++++++-- extensions/rcs_fr3/src/hw/Franka.h | 2 ++ extensions/rcs_fr3/src/pybind/rcs.cpp | 2 ++ .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 1 + .../src/rcs_panda/_core/hw/__init__.pyi | 1 + 5 files changed, 25 insertions(+), 2 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 21ff3c81..de50c6f0 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -19,13 +19,17 @@ namespace rcs { namespace hw { +common::Pose GetFlangeInBaseFrame(const franka::RobotState& robot_state) { + return common::Pose(robot_state.O_T_EE) * + common::Pose(robot_state.F_T_EE).inverse(); +} + common::Pose GetTCPInBaseFrame(const franka::RobotState& robot_state, const std::optional& tcp_offset) { if (!tcp_offset.has_value()) { return common::Pose(robot_state.O_T_EE); } - return common::Pose(robot_state.O_T_EE) * - common::Pose(robot_state.F_T_EE).inverse() * tcp_offset.value(); + return GetFlangeInBaseFrame(robot_state) * tcp_offset.value(); } Franka::Franka(const FrankaConfig& cfg, @@ -120,6 +124,19 @@ common::Pose Franka::get_cartesian_position() { return GetTCPInBaseFrame(robot_state, this->m_cfg.tcp_offset); } +common::Pose Franka::get_cartesian_flange_position() { + this->check_for_background_errors(); + franka::RobotState robot_state; + if (this->running_controller.load() == Controller::none) { + this->curr_state = this->robot.readOnce(); + robot_state = this->curr_state; + } else { + std::lock_guard lock(this->interpolator_mutex); + robot_state = this->curr_state; + } + return GetFlangeInBaseFrame(robot_state); +} + void Franka::set_joint_position(const common::VectorXd& q) { if (this->m_cfg.async_control) { this->controller_set_joint_position(q); diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index e6184333..b3726d47 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -119,6 +119,8 @@ class Franka : public common::Robot { common::Pose get_cartesian_position() override; + common::Pose get_cartesian_flange_position(); + void set_joint_position(const common::VectorXd& q) override; common::VectorXd get_joint_position() override; diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 86b85005..fb9dab7f 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -301,6 +301,8 @@ PYBIND11_MODULE(_core, m) { .def("set_config", &rcs::hw::Franka::set_config, py::arg("cfg")) .def("get_config", &rcs::hw::Franka::get_config) .def("get_state", &rcs::hw::Franka::get_state) + .def("get_cartesian_flange_position", + &rcs::hw::Franka::get_cartesian_flange_position) .def("set_default_robot_behavior", &rcs::hw::Franka::set_default_robot_behavior) .def("set_guiding_mode", &rcs::hw::Franka::set_guiding_mode, diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index b002e3dd..f2d32702 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -80,6 +80,7 @@ class Franka(rcs._core.common.Robot): self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] ) -> None: ... def double_tap_robot_to_continue(self) -> None: ... + def get_cartesian_flange_position(self) -> rcs._core.common.Pose: ... def get_config(self) -> FrankaConfig: ... def get_state(self) -> FrankaState: ... def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ... diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index b002e3dd..f2d32702 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -80,6 +80,7 @@ class Franka(rcs._core.common.Robot): self, desired_q: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] ) -> None: ... def double_tap_robot_to_continue(self) -> None: ... + def get_cartesian_flange_position(self) -> rcs._core.common.Pose: ... def get_config(self) -> FrankaConfig: ... def get_state(self) -> FrankaState: ... def osc_set_cartesian_position(self, desired_pos_EE_in_base_frame: rcs._core.common.Pose) -> None: ... From 3cb449c91fd632c773270d4ab76da46de3a4cd47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 15:22:28 -0700 Subject: [PATCH 11/14] feat(interface): add get_cartesian_flange_position --- extensions/rcs_fr3/src/hw/Franka.cpp | 35 +-- extensions/rcs_fr3/src/hw/Franka.h | 6 +- extensions/rcs_fr3/src/pybind/rcs.cpp | 205 +++++++++--------- .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 7 +- .../src/rcs_panda/_core/hw/__init__.pyi | 7 +- extensions/rcs_so101/src/rcs_so101/hw.py | 3 + extensions/rcs_ur5e/src/rcs_ur5e/hw.py | 3 + extensions/rcs_xarm7/src/rcs_xarm7/hw.py | 3 + extensions/rcs_yam/src/rcs_yam/hw.py | 6 +- include/rcs/Robot.h | 4 + python/rcs/_core/common.pyi | 1 + src/pybind/rcs.cpp | 7 + src/sim/SimRobot.cpp | 4 + src/sim/SimRobot.h | 1 + 14 files changed, 160 insertions(+), 132 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index de50c6f0..efdc9d11 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -25,11 +25,8 @@ common::Pose GetFlangeInBaseFrame(const franka::RobotState& robot_state) { } common::Pose GetTCPInBaseFrame(const franka::RobotState& robot_state, - const std::optional& tcp_offset) { - if (!tcp_offset.has_value()) { - return common::Pose(robot_state.O_T_EE); - } - return GetFlangeInBaseFrame(robot_state) * tcp_offset.value(); + const common::Pose& tcp_offset) { + return GetFlangeInBaseFrame(robot_state) * tcp_offset; } Franka::Franka(const FrankaConfig& cfg, @@ -406,15 +403,9 @@ void Franka::osc() { Eigen::Map> gravity( gravity_array.data()); - std::array jacobian_array; - if (this->m_cfg.tcp_offset.has_value()) { - jacobian_array = model.zeroJacobian( - franka::Frame::kEndEffector, robot_state.q, - this->m_cfg.tcp_offset->affine_array(), robot_state.EE_T_K); - } else { - jacobian_array = - model.zeroJacobian(franka::Frame::kEndEffector, robot_state); - } + std::array jacobian_array = model.zeroJacobian( + franka::Frame::kEndEffector, robot_state.q, + this->m_cfg.tcp_offset.affine_array(), robot_state.EE_T_K); Eigen::Map> jacobian( jacobian_array.data()); @@ -769,16 +760,13 @@ void Franka::set_cartesian_position(const common::Pose& x) { } if (this->m_cfg.ik_solver == IKSolver::franka_ik) { const franka::RobotState robot_state = this->robot.readOnce(); - common::Pose target_pose = x; - if (this->m_cfg.tcp_offset.has_value()) { - target_pose = x * this->m_cfg.tcp_offset->inverse() * - common::Pose(robot_state.F_T_EE); - } + const common::Pose target_pose = + x * this->m_cfg.tcp_offset.inverse() * common::Pose(robot_state.F_T_EE); this->set_cartesian_position_internal(target_pose, 1.0, std::nullopt, std::nullopt); } else if (this->m_cfg.ik_solver == IKSolver::rcs_ik) { - this->set_cartesian_position_ik(target_pose); + this->set_cartesian_position_ik(x); } } @@ -788,11 +776,8 @@ void Franka::set_cartesian_position_ik(const common::Pose& pose) { "No inverse kinematics was provided. Cannot use IK to set cartesian " "position."); } - const franka::RobotState robot_state = this->robot.readOnce(); - const common::Pose tcp_offset = - this->m_cfg.tcp_offset.value_or(common::Pose(robot_state.F_T_EE)); - auto joints = - this->m_ik.value()->inverse(pose, this->get_joint_position(), tcp_offset); + auto joints = this->m_ik.value()->inverse(pose, this->get_joint_position(), + this->m_cfg.tcp_offset); if (joints.has_value()) { this->set_joint_position(joints.value()); diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index b3726d47..3906f49f 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -50,7 +50,9 @@ struct FrankaConfig : common::RobotConfig { double kp_r = 250.0; std::optional load_parameters = std::nullopt; std::optional world_to_robot = std::nullopt; - std::optional tcp_offset = std::nullopt; + common::Pose tcp_offset = common::Pose::Identity(); + // Indicates that Cartesian control uses tcp_offset. + bool tcp_offset_explicit = false; bool async_control = false; bool ignore_realtime = false; size_t dof = 7; @@ -119,7 +121,7 @@ class Franka : public common::Robot { common::Pose get_cartesian_position() override; - common::Pose get_cartesian_flange_position(); + common::Pose get_cartesian_flange_position() override; void set_joint_position(const common::VectorXd& q) override; diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index fb9dab7f..9b2f4ece 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -137,111 +137,122 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("kp_r", &rcs::hw::FrankaConfig::kp_r) .def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters) .def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot) - .def_readwrite("tcp_offset", &rcs::hw::FrankaConfig::tcp_offset) + .def_property( + "tcp_offset", + [](const rcs::hw::FrankaConfig& config) { return config.tcp_offset; }, + [](rcs::hw::FrankaConfig& config, + const rcs::common::Pose& tcp_offset) { + config.tcp_offset = tcp_offset; + config.tcp_offset_explicit = true; + }) + .def_readwrite("tcp_offset_explicit", + &rcs::hw::FrankaConfig::tcp_offset_explicit) .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control) .def_readwrite("ignore_realtime", &rcs::hw::FrankaConfig::ignore_realtime) .def_readwrite("ip", &rcs::hw::FrankaConfig::ip); rcs::hw::FR3Config default_fr3_config; py::class_(hw, "FR3Config") - .def(py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver, - double speed_factor, const rcs::common::Vector7d& kp, - const rcs::common::Vector7d& kd, - const Eigen::Vector3d& kp_p, double kp_r, - std::optional load_parameters, - std::optional world_to_robot, - bool async_control, bool ignore_realtime, - std::optional tcp_offset, - std::string attachment_site, - std::string kinematic_model_path, - const rcs::common::Vector7d& torque_limit, - bool allow_high_collision) { - rcs::hw::FR3Config cfg; - cfg.ik_solver = ik_solver; - cfg.speed_factor = speed_factor; - cfg.kp = kp; - cfg.kd = kd; - cfg.torque_limit = torque_limit; - cfg.allow_high_collision = allow_high_collision; - cfg.kp_p = kp_p; - cfg.kp_r = kp_r; - cfg.load_parameters = load_parameters; - cfg.world_to_robot = world_to_robot; - cfg.async_control = async_control; - cfg.ignore_realtime = ignore_realtime; - cfg.ip = ip; - cfg.tcp_offset = tcp_offset; - cfg.attachment_site = attachment_site; - cfg.kinematic_model_path = kinematic_model_path; - return cfg; - }), - py::arg("ip"), py::arg("ik_solver") = default_fr3_config.ik_solver, - py::arg("speed_factor") = default_fr3_config.speed_factor, - py::arg("kp") = default_fr3_config.kp, - py::arg("kd") = default_fr3_config.kd, - py::arg("kp_p") = default_fr3_config.kp_p, - py::arg("kp_r") = default_fr3_config.kp_r, - py::arg("load_parameters") = default_fr3_config.load_parameters, - py::arg("world_to_robot") = default_fr3_config.world_to_robot, - py::arg("async_control") = default_fr3_config.async_control, - py::arg("ignore_realtime") = default_fr3_config.ignore_realtime, - py::arg("tcp_offset") = default_fr3_config.tcp_offset, - py::arg("attachment_site") = default_fr3_config.attachment_site, - py::arg("kinematic_model_path") = - default_fr3_config.kinematic_model_path, - py::arg("torque_limit") = default_fr3_config.torque_limit, - py::arg("allow_high_collision") = - default_fr3_config.allow_high_collision); + .def( + py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver, + double speed_factor, const rcs::common::Vector7d& kp, + const rcs::common::Vector7d& kd, + const Eigen::Vector3d& kp_p, double kp_r, + std::optional load_parameters, + std::optional world_to_robot, + bool async_control, bool ignore_realtime, + rcs::common::Pose tcp_offset, std::string attachment_site, + std::string kinematic_model_path, + const rcs::common::Vector7d& torque_limit, + bool allow_high_collision) { + rcs::hw::FR3Config cfg; + cfg.ik_solver = ik_solver; + cfg.speed_factor = speed_factor; + cfg.kp = kp; + cfg.kd = kd; + cfg.torque_limit = torque_limit; + cfg.allow_high_collision = allow_high_collision; + cfg.kp_p = kp_p; + cfg.kp_r = kp_r; + cfg.load_parameters = load_parameters; + cfg.world_to_robot = world_to_robot; + cfg.async_control = async_control; + cfg.ignore_realtime = ignore_realtime; + cfg.ip = ip; + cfg.tcp_offset = tcp_offset; + cfg.tcp_offset_explicit = true; + cfg.attachment_site = attachment_site; + cfg.kinematic_model_path = kinematic_model_path; + return cfg; + }), + py::arg("ip"), py::arg("ik_solver") = default_fr3_config.ik_solver, + py::arg("speed_factor") = default_fr3_config.speed_factor, + py::arg("kp") = default_fr3_config.kp, + py::arg("kd") = default_fr3_config.kd, + py::arg("kp_p") = default_fr3_config.kp_p, + py::arg("kp_r") = default_fr3_config.kp_r, + py::arg("load_parameters") = default_fr3_config.load_parameters, + py::arg("world_to_robot") = default_fr3_config.world_to_robot, + py::arg("async_control") = default_fr3_config.async_control, + py::arg("ignore_realtime") = default_fr3_config.ignore_realtime, + py::arg("tcp_offset") = default_fr3_config.tcp_offset, + py::arg("attachment_site") = default_fr3_config.attachment_site, + py::arg("kinematic_model_path") = + default_fr3_config.kinematic_model_path, + py::arg("torque_limit") = default_fr3_config.torque_limit, + py::arg("allow_high_collision") = + default_fr3_config.allow_high_collision); rcs::hw::PandaConfig default_panda_config; py::class_(hw, "PandaConfig") - .def(py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver, - double speed_factor, const rcs::common::Vector7d& kp, - const rcs::common::Vector7d& kd, - const Eigen::Vector3d& kp_p, double kp_r, - std::optional load_parameters, - std::optional world_to_robot, - bool async_control, bool ignore_realtime, - std::optional tcp_offset, - std::string attachment_site, - std::string kinematic_model_path, - const rcs::common::Vector7d& torque_limit, - bool allow_high_collision) { - rcs::hw::PandaConfig cfg; - cfg.ik_solver = ik_solver; - cfg.speed_factor = speed_factor; - cfg.kp = kp; - cfg.kd = kd; - cfg.torque_limit = torque_limit; - cfg.allow_high_collision = allow_high_collision; - cfg.kp_p = kp_p; - cfg.kp_r = kp_r; - cfg.load_parameters = load_parameters; - cfg.world_to_robot = world_to_robot; - cfg.async_control = async_control; - cfg.ignore_realtime = ignore_realtime; - cfg.ip = ip; - cfg.tcp_offset = tcp_offset; - cfg.attachment_site = attachment_site; - cfg.kinematic_model_path = kinematic_model_path; - return cfg; - }), - py::arg("ip"), py::arg("ik_solver") = default_panda_config.ik_solver, - py::arg("speed_factor") = default_panda_config.speed_factor, - py::arg("kp") = default_panda_config.kp, - py::arg("kd") = default_panda_config.kd, - py::arg("kp_p") = default_panda_config.kp_p, - py::arg("kp_r") = default_panda_config.kp_r, - py::arg("load_parameters") = default_panda_config.load_parameters, - py::arg("world_to_robot") = default_panda_config.world_to_robot, - py::arg("async_control") = default_panda_config.async_control, - py::arg("ignore_realtime") = default_panda_config.ignore_realtime, - py::arg("tcp_offset") = default_panda_config.tcp_offset, - py::arg("attachment_site") = default_panda_config.attachment_site, - py::arg("kinematic_model_path") = - default_panda_config.kinematic_model_path, - py::arg("torque_limit") = default_panda_config.torque_limit, - py::arg("allow_high_collision") = - default_panda_config.allow_high_collision); + .def( + py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver, + double speed_factor, const rcs::common::Vector7d& kp, + const rcs::common::Vector7d& kd, + const Eigen::Vector3d& kp_p, double kp_r, + std::optional load_parameters, + std::optional world_to_robot, + bool async_control, bool ignore_realtime, + rcs::common::Pose tcp_offset, std::string attachment_site, + std::string kinematic_model_path, + const rcs::common::Vector7d& torque_limit, + bool allow_high_collision) { + rcs::hw::PandaConfig cfg; + cfg.ik_solver = ik_solver; + cfg.speed_factor = speed_factor; + cfg.kp = kp; + cfg.kd = kd; + cfg.torque_limit = torque_limit; + cfg.allow_high_collision = allow_high_collision; + cfg.kp_p = kp_p; + cfg.kp_r = kp_r; + cfg.load_parameters = load_parameters; + cfg.world_to_robot = world_to_robot; + cfg.async_control = async_control; + cfg.ignore_realtime = ignore_realtime; + cfg.ip = ip; + cfg.tcp_offset = tcp_offset; + cfg.tcp_offset_explicit = true; + cfg.attachment_site = attachment_site; + cfg.kinematic_model_path = kinematic_model_path; + return cfg; + }), + py::arg("ip"), py::arg("ik_solver") = default_panda_config.ik_solver, + py::arg("speed_factor") = default_panda_config.speed_factor, + py::arg("kp") = default_panda_config.kp, + py::arg("kd") = default_panda_config.kd, + py::arg("kp_p") = default_panda_config.kp_p, + py::arg("kp_r") = default_panda_config.kp_r, + py::arg("load_parameters") = default_panda_config.load_parameters, + py::arg("world_to_robot") = default_panda_config.world_to_robot, + py::arg("async_control") = default_panda_config.async_control, + py::arg("ignore_realtime") = default_panda_config.ignore_realtime, + py::arg("tcp_offset") = default_panda_config.tcp_offset, + py::arg("attachment_site") = default_panda_config.attachment_site, + py::arg("kinematic_model_path") = + default_panda_config.kinematic_model_path, + py::arg("torque_limit") = default_panda_config.torque_limit, + py::arg("allow_high_collision") = + default_panda_config.allow_high_collision); py::object gripper_config = (py::object)py::module_::import("rcs").attr("common").attr( diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index f2d32702..af761256 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -115,7 +115,8 @@ class FrankaConfig(rcs._core.common.RobotConfig): kp_r: float load_parameters: FrankaLoad | None speed_factor: float - tcp_offset: rcs._core.common.Pose | None + tcp_offset: rcs._core.common.Pose + tcp_offset_explicit: bool torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] world_to_robot: rcs._core.common.Pose | None @@ -312,7 +313,7 @@ class FR3Config(FrankaConfig): world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, ignore_realtime: bool = False, - tcp_offset: rcs._core.common.Pose | None = None, + tcp_offset: rcs._core.common.Pose = ..., attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., @@ -333,7 +334,7 @@ class PandaConfig(FrankaConfig): world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, ignore_realtime: bool = False, - tcp_offset: rcs._core.common.Pose | None = None, + tcp_offset: rcs._core.common.Pose = ..., attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index f2d32702..af761256 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -115,7 +115,8 @@ class FrankaConfig(rcs._core.common.RobotConfig): kp_r: float load_parameters: FrankaLoad | None speed_factor: float - tcp_offset: rcs._core.common.Pose | None + tcp_offset: rcs._core.common.Pose + tcp_offset_explicit: bool torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] world_to_robot: rcs._core.common.Pose | None @@ -312,7 +313,7 @@ class FR3Config(FrankaConfig): world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, ignore_realtime: bool = False, - tcp_offset: rcs._core.common.Pose | None = None, + tcp_offset: rcs._core.common.Pose = ..., attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., @@ -333,7 +334,7 @@ class PandaConfig(FrankaConfig): world_to_robot: rcs._core.common.Pose | None = None, async_control: bool = False, ignore_realtime: bool = False, - tcp_offset: rcs._core.common.Pose | None = None, + tcp_offset: rcs._core.common.Pose = ..., attachment_site: str = "attachment_site", kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml", torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ..., diff --git a/extensions/rcs_so101/src/rcs_so101/hw.py b/extensions/rcs_so101/src/rcs_so101/hw.py index d3e50718..3c44dee6 100644 --- a/extensions/rcs_so101/src/rcs_so101/hw.py +++ b/extensions/rcs_so101/src/rcs_so101/hw.py @@ -50,6 +50,9 @@ def __init__(self, cfg: SO101Config, ik: common.Kinematics): def get_cartesian_position(self) -> common.Pose: return self.ik.forward(self.get_joint_position()) + def get_cartesian_flange_position(self) -> common.Pose: + return self.get_cartesian_position() + def get_ik(self) -> common.Kinematics | None: return self.ik diff --git a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py index 56b371e2..9fa9530a 100644 --- a/extensions/rcs_ur5e/src/rcs_ur5e/hw.py +++ b/extensions/rcs_ur5e/src/rcs_ur5e/hw.py @@ -269,6 +269,9 @@ def get_cartesian_position(self) -> common.Pose: common.Pose(rpy_vector=np.array([0, 0, np.deg2rad(180)]), translation=np.array([0, 0, 0])).inverse() * pose # type: ignore ) + def get_cartesian_flange_position(self) -> common.Pose: + return self.get_cartesian_position() + def get_ik(self) -> common.Kinematics | None: return self.ik diff --git a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py index d5ece995..dd48f56d 100644 --- a/extensions/rcs_xarm7/src/rcs_xarm7/hw.py +++ b/extensions/rcs_xarm7/src/rcs_xarm7/hw.py @@ -60,6 +60,9 @@ def get_cartesian_position(self) -> common.Pose: return common.Pose(rpy_vector=rpy, translation=translation_meter) # type: ignore + def get_cartesian_flange_position(self) -> common.Pose: + return self.get_cartesian_position() + def get_ik(self) -> common.Kinematics | None: return self.ik diff --git a/extensions/rcs_yam/src/rcs_yam/hw.py b/extensions/rcs_yam/src/rcs_yam/hw.py index 76cd0e5d..24063733 100644 --- a/extensions/rcs_yam/src/rcs_yam/hw.py +++ b/extensions/rcs_yam/src/rcs_yam/hw.py @@ -123,8 +123,10 @@ def get_gripper_width(self) -> float: def get_cartesian_position(self) -> common.Pose: # `Kinematics.forward` applies the inverse of the offset it is handed, so the TCP is composed # here instead, to match the pose `SimRobot::get_cartesian_position` reports in simulation. - flange = self.ik.forward(self.get_joint_position(), common.Pose()) - return flange * self._config.tcp_offset + return self.get_cartesian_flange_position() * self._config.tcp_offset + + def get_cartesian_flange_position(self) -> common.Pose: + return self.ik.forward(self.get_joint_position(), common.Pose()) def set_cartesian_position(self, pose: common.Pose) -> None: q = self.ik.inverse(pose, self.get_joint_position(), self._config.tcp_offset) diff --git a/include/rcs/Robot.h b/include/rcs/Robot.h index 9a94cf4a..e7b680c7 100644 --- a/include/rcs/Robot.h +++ b/include/rcs/Robot.h @@ -116,6 +116,10 @@ class Robot { virtual Pose get_cartesian_position() = 0; + virtual Pose get_cartesian_flange_position() { + return get_cartesian_position(); + } + virtual void set_joint_position(const VectorXd& q) = 0; virtual VectorXd get_joint_position() = 0; diff --git a/python/rcs/_core/common.pyi b/python/rcs/_core/common.pyi index b2749517..95ec05fd 100644 --- a/python/rcs/_core/common.pyi +++ b/python/rcs/_core/common.pyi @@ -226,6 +226,7 @@ class Robot: def __init__(self) -> None: ... def close(self) -> None: ... def get_base_pose_in_world_coordinates(self) -> Pose: ... + def get_cartesian_flange_position(self) -> Pose: ... def get_cartesian_position(self) -> Pose: ... def get_config(self) -> RobotConfig: ... def get_ik(self) -> Kinematics | None: ... diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 9be15edf..1f172b81 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -73,6 +73,11 @@ class PyRobot : public rcs::common::Robot { get_cartesian_position, ); } + rcs::common::Pose get_cartesian_flange_position() override { + PYBIND11_OVERRIDE(rcs::common::Pose, rcs::common::Robot, + get_cartesian_flange_position, ); + } + void set_joint_position(const rcs::common::VectorXd& q) override { PYBIND11_OVERRIDE_PURE(void, rcs::common::Robot, set_joint_position, q); } @@ -461,6 +466,8 @@ PYBIND11_MODULE(_core, m) { .def("get_state", &rcs::common::Robot::get_state) .def("get_cartesian_position", &rcs::common::Robot::get_cartesian_position) + .def("get_cartesian_flange_position", + &rcs::common::Robot::get_cartesian_flange_position) .def("set_joint_position", &rcs::common::Robot::set_joint_position, py::arg("q"), py::call_guard()) .def("get_joint_position", &rcs::common::Robot::get_joint_position) diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index c1263580..68949960 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -125,6 +125,10 @@ common::Pose SimRobot::get_cartesian_position() { return this->to_pose_in_robot_coordinates(attachment_site) * cfg.tcp_offset; } +common::Pose SimRobot::get_cartesian_flange_position() { + return this->get_cartesian_position(); +} + void SimRobot::set_joint_position(const common::VectorXd& q) { this->state.target_angles = q; this->state.previous_angles = this->get_joint_position(); diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index 99b23184..5270bd67 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -66,6 +66,7 @@ class SimRobot : public common::Robot { SimRobotConfig* get_config() override; SimRobotState* get_state() override; common::Pose get_cartesian_position() override; + common::Pose get_cartesian_flange_position() override; void set_joint_position(const common::VectorXd& q) override; common::VectorXd get_joint_position() override; void move_home() override; From fcde881aa5eb97f7250e083624c362da6eb51ce2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 17:32:08 -0700 Subject: [PATCH 12/14] docs: mention yam --- README.md | 6 +++--- docs/apps/index.md | 7 ++++--- docs/extensions/rcs_yam.md | 6 ++++++ 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 47156100..9bd70c7f 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ **Robot Control Stack (RCS)** is a flexible, native [Gymnasium](https://gymnasium.farama.org/) wrapper-based robot control interface designed specifically for modern robot learning and Vision-Language-Action (VLA) models. -It completely unifies **MuJoCo simulation** and real-world physical robot control into a single, seamless API. Currently, RCS natively supports four robots out-of-the-box: **Franka FR3/Panda, xArm7, UR5e, and SO101.** +It completely unifies **MuJoCo simulation** and real-world physical robot control into a single, seamless API. Currently, RCS natively supports five robots out-of-the-box: **Franka FR3/Panda, xArm7, UR5e, SO101, and I2RT YAM.** ![RCS Demo](https://raw.githubusercontent.com/RobotControlStack/robotcontrolstack.github.io/refs/heads/master/static/videos/grid.webp) @@ -26,7 +26,7 @@ Traditional robotics middleware (like ROS/ROS2) and complex motion planning pipe * **Zero ROS Overhead:** No complex message-passing, middleware, or network configuration required. Run natively in Python with a lightweight C++ backend. * **Frictionless Sim-to-Real:** Train your Reinforcement Learning or VLA policies in our MuJoCo Gymnasium wrapper, and deploy the *exact same code* directly to physical hardware. * **Synchronous Execution:** Optimized specifically for the highly parallelized, synchronous data collection required by modern ML workflows. -* **Ready-to-Use Apps:** Ships with pre-built applications for data collection via teleoperation and remote model inference via [vlagents](https://github.com/RobotControlStack/vlagents). See [examples/teleop/README.md](examples/teleop/README.md) and [examples/inference/README.md](examples/inference/README.md). +* **Ready-to-Use Apps:** Ships with pre-built applications for data collection via teleoperation and remote model inference via [vlagents](https://github.com/RobotControlStack/vlagents). See the [teleoperation guide](examples/teleop/README.md), and [inference guide](examples/inference/README.md). ## 🧩 Wrapper-Based Architecture @@ -168,7 +168,7 @@ export RCS_PREFIX=/path/to/rcs-assets ## 🦾 Hardware Extensions -RCS supports various hardware extensions to seamlessly connect your policies to the real world (e.g., FR3, xArm7, RealSense). These are located in the `extensions` directory. +RCS supports various hardware extensions to seamlessly connect your policies to the real world (e.g., FR3, xArm7, YAM, RealSense). These are located in the `extensions` directory. To install a specific robot extension (example for Franka FR3): diff --git a/docs/apps/index.md b/docs/apps/index.md index 799132db..a1cb72bc 100644 --- a/docs/apps/index.md +++ b/docs/apps/index.md @@ -4,12 +4,13 @@ RCS ships with ready-to-use applications for common operator workflows such as r ## Teleoperation -Use the Franka teleoperation app when you want to collect demonstrations or directly control a robot from an operator interface. +Use the Meta Quest 3 teleoperation examples when you want to collect demonstrations or directly control a robot from an operator interface. RCS provides examples for Franka and I2RT YAM arms. - Example guide: [examples/teleop/README.md](../../examples/teleop/README.md) -- Main script: [examples/teleop/franka.py](../../examples/teleop/franka.py) +- Franka example: [examples/teleop/franka.py](../../examples/teleop/franka.py) +- YAM example: [examples/teleop/yam.py](../../examples/teleop/yam.py) -The current example focuses on Franka teleoperation with Meta Quest 3 and GELLO-based setups. +The Franka example supports Meta Quest 3 and GELLO-based setups. The YAM example supports dual-arm Meta Quest 3 teleoperation in simulation or on hardware; install the [YAM extension](../extensions/rcs_yam.md) before using real YAM hardware. ## Inference diff --git a/docs/extensions/rcs_yam.md b/docs/extensions/rcs_yam.md index d8599e12..dee5b74e 100644 --- a/docs/extensions/rcs_yam.md +++ b/docs/extensions/rcs_yam.md @@ -58,3 +58,9 @@ See `extensions/rcs_yam/README.md` for the full extension documentation and `extensions/rcs_yam/src/rcs_yam/scripts/test_robot.py` for a bring-up script. For a maintained example, see `examples/yam/yam_env_cartesian_control.py`, which moves the TCP forward and backward in synchronous Cartesian mode in simulation or on hardware. + +## Teleoperation example + +Use [examples/teleop/yam.py](../../examples/teleop/yam.py) to teleoperate YAM arms with a Meta Quest +3. The example runs against simulation or hardware, and can optionally record RealSense cameras. See +the [teleoperation README](../../examples/teleop/README.md) for setup instructions. From 381ed09bc404e2672c7c559031b077035892a03d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 17:32:40 -0700 Subject: [PATCH 13/14] feat: add flange in sim --- src/sim/SimRobot.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index 68949960..646a2387 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -117,16 +117,16 @@ SimRobotState* SimRobot::get_state() { } common::Pose SimRobot::get_cartesian_position() { + return this->get_cartesian_flange_position() * cfg.tcp_offset; +} + +common::Pose SimRobot::get_cartesian_flange_position() { Eigen::Matrix rotation( this->sim->d->site_xmat + 9 * this->ids.attachment_site); Eigen::Vector3d translation(this->sim->d->site_xpos + 3 * this->ids.attachment_site); common::Pose attachment_site(Eigen::Matrix3d(rotation), translation); - return this->to_pose_in_robot_coordinates(attachment_site) * cfg.tcp_offset; -} - -common::Pose SimRobot::get_cartesian_flange_position() { - return this->get_cartesian_position(); + return this->to_pose_in_robot_coordinates(attachment_site); } void SimRobot::set_joint_position(const common::VectorXd& q) { From 2a038897f66d7e7ecf16a7e7a88f62c64fcab2e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 10 Aug 2026 17:33:00 -0700 Subject: [PATCH 14/14] feat: add tquat_flange to robot observation --- python/rcs/envs/base.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/python/rcs/envs/base.py b/python/rcs/envs/base.py index c5d8da0e..62bc897f 100644 --- a/python/rcs/envs/base.py +++ b/python/rcs/envs/base.py @@ -167,7 +167,15 @@ class CameraDictType(RCSpaceType): # joining works with inheritance but need to inherit from protocol again -class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): ... +class ArmObsType(TQuatDictType, JointsDictType, TRPYDictType): + tquat_flange: Annotated[ + Vec7Type, + gym.spaces.Box( + low=np.array([-0.855, -0.855, -1] + [-1] + [-np.inf] * 3), + high=np.array([0.855, 0.855, 1.188] + [1] + [np.inf] * 3), + dtype=np.float64, + ), + ] CartOrJointContType: TypeAlias = TQuatDictType | JointsDictType | TRPYDictType @@ -346,6 +354,12 @@ def get_robot_obs(self) -> ArmObsType: ), joints=self.robot.get_joint_position(), xyzrpy=self.robot.get_cartesian_position().xyzrpy(), + tquat_flange=np.concatenate( + [ + self.robot.get_cartesian_flange_position().translation(), + self.robot.get_cartesian_flange_position().rotation_q(), + ] + ), ) def action(self, action: dict[str, Any]) -> dict[str, Any]: