From b369f13e93b1b94cdd25def15e992d96df7d63a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 4 May 2026 20:28:14 +0200 Subject: [PATCH 1/8] feat(teleop): include depth option, duo and optimizations --- examples/teleop/franka.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/examples/teleop/franka.py b/examples/teleop/franka.py index 09824148..2f0d6ede 100644 --- a/examples/teleop/franka.py +++ b/examples/teleop/franka.py @@ -43,6 +43,7 @@ "zed": "19928076", } MQ3_ADDR = "10.42.0.1" +INCLUDE_DEPTH = False # DIGIT_DICT = { # "digit_right_left": "D21182", @@ -69,7 +70,7 @@ mq3_addr=MQ3_ADDR, simulation=ROBOT_INSTANCE == RobotPlatform.SIMULATION, switched_left_right=False, - display_cameras=True, + display_cameras=False, ) # config = GelloConfig( # arms={ @@ -82,10 +83,10 @@ def get_env(): if ROBOT_INSTANCE == RobotPlatform.HARDWARE: - from rcs_fr3.configs import DefaultFR3MultiHardwareEnv + from rcs_fr3.configs import FrankaDuoEnv from rcs_fr3.creators import HardwareCameraCreatorConfig - env_creator = DefaultFR3MultiHardwareEnv() + env_creator = FrankaDuoEnv() env_creator.left_ip = ROBOT2IP["left"] env_creator.right_ip = ROBOT2IP["right"] hw_cfg = env_creator.config() @@ -135,15 +136,13 @@ def get_env(): ) hw_cfg.camera_cfgs = camera_cfgs or None hw_cfg.control_mode = config.operator_class.control_mode[0] + hw_cfg.wrapper_cfg.include_depth = INCLUDE_DEPTH hw_cfg.max_relative_movement = ( 0.5 if config.operator_class.control_mode[0] == ControlMode.JOINTS else (0.5, np.deg2rad(90)) ) hw_cfg.relative_to = config.operator_class.control_mode[1] hw_cfg.robot_to_shared_base_frame = robot2world env_rel = env_creator.create_env(hw_cfg) - # env_rel = StorageWrapper( - # env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 - # ) operator = GelloOperator(config) if isinstance(config, GelloConfig) else QuestOperator(config) else: # FR3 @@ -154,19 +153,21 @@ def get_env(): async_control=True, realtime=True, frequency=RECORD_FPS, max_convergence_steps=500 ) sim_cfg_data.relative_to = RelativeTo.CONFIGURED_ORIGIN + sim_cfg_data.wrapper_cfg.include_depth = INCLUDE_DEPTH if sim_cfg_data.root_frame_objects is None: sim_cfg_data.root_frame_objects = {} # cfg.root_frame_objects["green_cube"] = (rcs.OBJECT_PATHS["green_cube"], Pose(translation=[0.5, 0, 0.5], quaternion=[0, 0, 0, 1])) sim_cfg_data.task_cfg = PickTaskConfig(robot_name="right") env_rel = scene.create_env(sim_cfg_data) - env_rel = StorageWrapper( - env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 - ) sim = env_rel.get_wrapper_attr("sim") MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3))) operator = GelloOperator(config, sim) if isinstance(config, GelloConfig) else QuestOperator(config, sim) + + env_rel = StorageWrapper( + env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000 + ) return env_rel, operator From b7baa88fea060a3876836ca28166cdb15f3a7162 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 4 May 2026 20:28:53 +0200 Subject: [PATCH 2/8] fix(fr3:desk): home command possible without franka hand --- extensions/rcs_fr3/src/rcs_fr3/__main__.py | 3 ++- extensions/rcs_fr3/src/rcs_fr3/desk.py | 17 +++++++++-------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/extensions/rcs_fr3/src/rcs_fr3/__main__.py b/extensions/rcs_fr3/src/rcs_fr3/__main__.py index 540e2409..8382efff 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/__main__.py +++ b/extensions/rcs_fr3/src/rcs_fr3/__main__.py @@ -23,10 +23,11 @@ def home( ip: Annotated[str, typer.Argument(help="IP of the robot")], shut: Annotated[bool, typer.Option("-s", help="Should the robot be shut down")] = False, unlock: Annotated[bool, typer.Option("-u", help="unlocks the robot")] = False, + fh: Annotated[bool, typer.Option("-h", help="franka hand open")] = False, ): """Moves the FR3 to home position""" user, pw = load_creds_franka_desk() - rcs_fr3.desk.home(ip, user, pw, shut, unlock) + rcs_fr3.desk.home(ip, user, pw, shut, unlock, fh) @fr3_app.command() diff --git a/extensions/rcs_fr3/src/rcs_fr3/desk.py b/extensions/rcs_fr3/src/rcs_fr3/desk.py index aca08c2d..5636a9f2 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/desk.py +++ b/extensions/rcs_fr3/src/rcs_fr3/desk.py @@ -45,7 +45,7 @@ def load_creds_franka_desk(postfix: str = "") -> tuple[str, str]: return os.environ[username_key], os.environ[password_key] -def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False): +def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False, fh: bool = False): with Desk.fci(ip, username, password, unlock=unlock): default_env = DefaultFR3HardwareEnv() default_env.ip = ip @@ -53,13 +53,14 @@ def home(ip: str, username: str, password: str, shut: bool, unlock: bool = False robot_cfg = env_cfg.robot_cfg robot_cfg.speed_factor = 0.2 f = rcs_fr3.hw.Franka(robot_cfg) - config_hand = env_cfg.gripper_cfg - assert isinstance(config_hand, rcs_fr3.hw.FHConfig) - g = rcs_fr3.hw.FrankaHand(config_hand) - if shut: - g.shut() - else: - g.open() + if fh: + config_hand = env_cfg.gripper_cfg + assert isinstance(config_hand, rcs_fr3.hw.FHConfig) + g = rcs_fr3.hw.FrankaHand(config_hand) + if shut: + g.shut() + else: + g.open() f.move_home() From b53fceed7ebdcaa6e5e14d4f13a43b69639121f0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 4 May 2026 20:32:22 +0200 Subject: [PATCH 3/8] fix(fr3): duo config, gipper offset and copy - added gripper offsets - added home positions for duo - added serial numbers for utn robotiq - fixed not working copy of config --- extensions/rcs_fr3/src/rcs_fr3/configs.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/extensions/rcs_fr3/src/rcs_fr3/configs.py b/extensions/rcs_fr3/src/rcs_fr3/configs.py index 1d85089f..3eaa8b0a 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/configs.py +++ b/extensions/rcs_fr3/src/rcs_fr3/configs.py @@ -125,12 +125,12 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig: return FR3MultiHardwareEnvCreatorConfig( control_mode=ControlMode.CARTESIAN_TRPY, robot_cfgs={ - "left": copy.deepcopy(left_cfg.robot_cfg), - "right": copy.deepcopy(right_cfg.robot_cfg), + "left": left_cfg.robot_cfg, + "right": right_cfg.robot_cfg, }, gripper_cfgs={ - "left": copy.deepcopy(left_cfg.gripper_cfg), - "right": copy.deepcopy(right_cfg.gripper_cfg), + "left": left_cfg.gripper_cfg, + "right": right_cfg.gripper_cfg, }, camera_cfgs=copy.deepcopy(left_cfg.camera_cfgs), max_relative_movement=(0.5, np.deg2rad(90)), @@ -152,8 +152,8 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig: class FrankaDuoEnv(DefaultFR3MultiHardwareEnv): - left_gripper_serial_number = "LEFT_GRIPPER_SERIAL_NUMBER" - right_gripper_serial_number = "RIGHT_GRIPPER_SERIAL_NUMBER" + left_gripper_serial_number = "DAAQMJHX" + right_gripper_serial_number = "DAAQMPDC" def config(self) -> FR3MultiHardwareEnvCreatorConfig: try: @@ -164,6 +164,10 @@ def config(self) -> FR3MultiHardwareEnvCreatorConfig: cfg = super().config() cfg.camera_cfgs = None + cfg.robot_cfgs["left"].tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfgs["right"].tcp_offset = rcs.GRIPPER_OFFSETS[common.GripperType("Robotiq2F85")] + cfg.robot_cfgs["left"].q_home = rcs.HOME_POSITIONS["FR3_DUO_LEFT"] + cfg.robot_cfgs["right"].q_home = rcs.HOME_POSITIONS["FR3_DUO_RIGHT"] cfg.gripper_cfgs = { "left": RobotiQ2F85GripperConfig( serial_number=self.left_gripper_serial_number, From 84782395e19fc48a5aa6339ebfd677ab4ce1c604 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 4 May 2026 20:33:04 +0200 Subject: [PATCH 4/8] fix(robotiq): gripper type --- extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py index 9fbe047c..a5ed5736 100644 --- a/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py +++ b/extensions/rcs_robotiq2f85/src/rcs_robotiq2f85/hw.py @@ -4,6 +4,8 @@ from rcs.common_typing import GripperConfigKwargs from Robotiq2F85Driver.Robotiq2F85Driver import GripperStatus, Robotiq2F85Driver +import rcs + class RobotiQ2F85GripperConfig(GripperConfig): @@ -27,6 +29,7 @@ def __init__( self.speed = speed self.force = force self.async_control = async_control + self.gripper_type = rcs.common.GripperType("Robotiq2F85") class RobotiQ2F85GripperState(GripperState): From 38a1ba91d37d6bf1a08b3f47139f949b05917ec4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Mon, 4 May 2026 20:37:06 +0200 Subject: [PATCH 5/8] fix(franka): random jumping induced by uninitialized matrixes - fixed shared access on enum by using atomic - initialize curr_state in osc by calling robot readOnce - fix uninitialized K matrixes --- extensions/rcs_fr3/src/hw/Franka.cpp | 60 +++++++++++++++++----------- extensions/rcs_fr3/src/hw/Franka.h | 3 +- 2 files changed, 38 insertions(+), 25 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index f2e04ce2..9e4f81a0 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -73,7 +73,7 @@ FrankaConfig* Franka::get_config() { FrankaState* Franka::get_state() { franka::RobotState current_robot_state; - if (this->running_controller == Controller::none) { + if (this->running_controller.load() == Controller::none) { current_robot_state = this->robot.readOnce(); } else { std::lock_guard lock(this->interpolator_mutex); @@ -100,7 +100,7 @@ void Franka::set_default_robot_behavior() { common::Pose Franka::get_cartesian_position() { this->check_for_background_errors(); common::Pose x; - if (this->running_controller == Controller::none) { + if (this->running_controller.load() == Controller::none) { this->curr_state = this->robot.readOnce(); x = common::Pose(this->curr_state.O_T_EE); } else { @@ -127,7 +127,7 @@ void Franka::set_joint_position(const common::VectorXd& q) { common::VectorXd Franka::get_joint_position() { this->check_for_background_errors(); common::Vector7d joints; - if (this->running_controller == Controller::none) { + if (this->running_controller.load() == Controller::none) { this->curr_state = this->robot.readOnce(); joints = common::Vector7d(this->curr_state.q.data()); } else { @@ -182,15 +182,15 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { int policy_rate = 20; int traj_rate = 500; - if (this->running_controller == Controller::none) { + if (this->running_controller.load() == Controller::none) { this->controller_time = 0.0; this->get_joint_position(); this->joint_interpolator = common::LinearJointPositionTrajInterpolator(); - } else if (this->running_controller != Controller::jsc) { + } else if (this->running_controller.load() != Controller::jsc) { // runtime error throw std::runtime_error( "Controller type must but joint space but is " + - std::to_string(this->running_controller) + + std::to_string(static_cast(this->running_controller.load())) + ". To change controller type stop the current controller first."); } else { this->interpolator_mutex.lock(); @@ -202,8 +202,8 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { policy_rate, traj_rate, traj_interpolation_time_fraction); // if not thread is running, then start - if (this->running_controller == Controller::none) { - this->running_controller = Controller::jsc; + if (this->running_controller.load() == Controller::none) { + this->running_controller.store(Controller::jsc); this->control_thread = std::thread(&Franka::joint_controller, this); } else { this->interpolator_mutex.unlock(); @@ -229,13 +229,17 @@ void Franka::osc_set_cartesian_position( int policy_rate = 20; int traj_rate = 500; - if (this->running_controller == Controller::none) { + if (this->running_controller.load() == Controller::none) { this->controller_time = 0.0; + { + std::lock_guard lock(this->interpolator_mutex); + this->curr_state = this->robot.readOnce(); + } this->traj_interpolator = common::LinearPoseTrajInterpolator(); - } else if (this->running_controller != Controller::osc) { + } else if (this->running_controller.load() != Controller::osc) { throw std::runtime_error( "Controller type must but osc but is " + - std::to_string(this->running_controller) + + std::to_string(static_cast(this->running_controller.load())) + ". To change controller type stop the current controller first."); } else { this->interpolator_mutex.lock(); @@ -249,8 +253,8 @@ void Franka::osc_set_cartesian_position( traj_interpolation_time_fraction); // if not thread is running, then start - if (this->running_controller == Controller::none) { - this->running_controller = Controller::osc; + if (this->running_controller.load() == Controller::none) { + this->running_controller.store(Controller::osc); this->control_thread = std::thread(&Franka::osc, this); } else { this->interpolator_mutex.unlock(); @@ -259,10 +263,11 @@ void Franka::osc_set_cartesian_position( // method to stop thread void Franka::stop_control_thread() { - if (this->control_thread.has_value() && - this->running_controller != Controller::none) { - this->running_controller = Controller::none; - this->control_thread->join(); + if (this->control_thread.has_value()) { + this->running_controller.store(Controller::none); + if (this->control_thread->joinable()) { + this->control_thread->join(); + } this->control_thread.reset(); } } @@ -291,7 +296,10 @@ void Franka::osc() { // translation: [150.0, 150.0, 150.0] // rotation: 250.0 - Eigen::Matrix Kp_p, Kp_r, Kd_p, Kd_r; + Eigen::Matrix Kp_p = Eigen::Matrix3d::Zero(); + Eigen::Matrix Kp_r = Eigen::Matrix3d::Zero(); + Eigen::Matrix Kd_p = Eigen::Matrix3d::Zero(); + Eigen::Matrix Kd_r = Eigen::Matrix3d::Zero(); Eigen::Matrix static_q_task_; Eigen::Matrix residual_mass_vec_; Eigen::Array joint_max_; @@ -323,7 +331,7 @@ void Franka::osc() { std::chrono::high_resolution_clock::now(); // torques handler - if (this->running_controller == Controller::none) { + if (this->running_controller.load() == Controller::none) { franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; return franka::MotionFinished(zero_torques); } @@ -487,7 +495,7 @@ void Franka::osc() { } // Ensure we mark the controller as stopped so we can restart later - this->running_controller = Controller::none; + this->running_controller.store(Controller::none); } void Franka::joint_controller() { @@ -524,7 +532,7 @@ void Franka::joint_controller() { std::chrono::high_resolution_clock::now(); // torques handler - if (this->running_controller == Controller::none) { + if (this->running_controller.load() == Controller::none) { // TODO: test if this also works when the robot is moving franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; return franka::MotionFinished(zero_torques); @@ -588,16 +596,18 @@ void Franka::joint_controller() { std::lock_guard lock(this->exception_mutex); this->background_exception = std::current_exception(); } + + this->running_controller.store(Controller::none); } void Franka::zero_torque_guiding() { this->check_for_background_errors(); - if (this->running_controller != Controller::none) { + if (this->running_controller.load() != Controller::none) { throw std::runtime_error( "A controller is currently running. Please stop it first."); } this->controller_time = 0.0; - this->running_controller = Controller::ztc; + this->running_controller.store(Controller::ztc); this->control_thread = std::thread(&Franka::zero_torque_controller, this); } @@ -617,7 +627,7 @@ void Franka::zero_torque_controller() { this->curr_state = robot_state; this->controller_time += period.toSec(); this->interpolator_mutex.unlock(); - if (this->running_controller == Controller::none) { + if (this->running_controller.load() == Controller::none) { // stop return franka::MotionFinished(franka::Torques({0, 0, 0, 0, 0, 0, 0})); } @@ -627,6 +637,8 @@ void Franka::zero_torque_controller() { std::lock_guard lock(this->exception_mutex); this->background_exception = std::current_exception(); } + + this->running_controller.store(Controller::none); } void Franka::move_home() { diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 72c9db35..944d7804 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -89,7 +90,7 @@ class Franka : public common::Robot { common::LinearJointPositionTrajInterpolator joint_interpolator; franka::RobotState curr_state; std::mutex interpolator_mutex; - Controller running_controller = Controller::none; + std::atomic running_controller{Controller::none}; std::exception_ptr background_exception = nullptr; std::mutex exception_mutex; void osc(); From ebfc9e3c6eaf8955d30780c36c852570967fd588 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 5 May 2026 09:10:25 +0200 Subject: [PATCH 6/8] fix(fr3): tcp in desk applied in controllers --- extensions/rcs_fr3/src/hw/Franka.cpp | 35 ++++++++++++++++++++-------- 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 9e4f81a0..1fbb140d 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -246,6 +246,9 @@ void Franka::osc_set_cartesian_position( } 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; + } this->traj_interpolator.reset( this->controller_time, curr_pose.translation(), curr_pose.quaternion(), desired_pose_EE_in_base_frame.translation(), @@ -343,7 +346,6 @@ void Franka::osc() { Eigen::Vector3d desired_pos_EE_in_base_frame; Eigen::Quaterniond desired_quat_EE_in_base_frame; - common::Pose pose(robot_state.O_T_EE); // form deoxys/config/charmander.yml int policy_rate = 20; int traj_rate = 500; @@ -384,9 +386,15 @@ void Franka::osc() { jacobian_pos << jacobian.block(0, 0, 3, 7); jacobian_ori << jacobian.block(3, 0, 3, 7); - // End effector pose in base frame - Eigen::Affine3d T_EE_in_base_frame( - Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); + // 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; + Eigen::Affine3d T_EE_in_base_frame = + T_EE_in_base_frame_pose.affine_matrix(); + Eigen::Vector3d pos_EE_in_base_frame(T_EE_in_base_frame.translation()); Eigen::Quaterniond quat_EE_in_base_frame(T_EE_in_base_frame.linear()); @@ -539,7 +547,6 @@ void Franka::joint_controller() { } common::Vector7d desired_q; - common::Pose pose(robot_state.O_T_EE); this->interpolator_mutex.lock(); this->curr_state = robot_state; @@ -726,8 +733,12 @@ 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(x); + this->osc_set_cartesian_position(target_pose); return; } // TODO: this should handled with tcp offset config @@ -745,10 +756,11 @@ void Franka::set_cartesian_position(const common::Pose& x) { // if gripper is attached the tcp offset will automatically be applied // by libfranka this->robot.setEE(nominal_end_effector_frame_value.affine_array()); - this->set_cartesian_position_internal(x, 1.0, std::nullopt, std::nullopt); + 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(x); + this->set_cartesian_position_ik(target_pose); } } @@ -798,14 +810,17 @@ void Franka::set_cartesian_position_internal(const common::Pose& pose, this->robot.control( [&force_stop_condition, &initial_elbow, &elbow, &max_force, &time, - &max_time, &initial_pose, &should_stop, + &max_time, &initial_pose, &should_stop, this, &pose](const franka::RobotState& state, franka::Duration time_step) -> franka::CartesianPose { time += time_step.toSec(); if (time == 0) { initial_elbow = state.elbow_c; - initial_pose = common::Pose(state.O_T_EE); + 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; } auto new_elbow = initial_elbow; const double progress = time / max_time; From 943004964af8ee685281aebe6bb7b6eb5e85e2d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 5 May 2026 09:23:20 +0200 Subject: [PATCH 7/8] fix(franka): redefine q_home --- extensions/rcs_fr3/src/hw/Franka.h | 4 ---- 1 file changed, 4 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 944d7804..6054b924 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -44,10 +44,6 @@ struct FrankaConfig : common::RobotConfig { bool async_control = false; bool tcp_offset_configured_in_desk = true; bool ignore_realtime = false; - std::optional q_home = - (common::VectorXd(7) << 0.0, -M_PI_4, 0.0, -3.0 * M_PI_4, 0.0, M_PI_2, - M_PI_4) - .finished(); size_t dof = 7; Eigen::Matrix joint_limits = (Eigen::Matrix(2, 7) << From 5876a988f1350b47c1583b146c589e6ee469002c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 5 May 2026 09:28:11 +0200 Subject: [PATCH 8/8] fix(robotiq): tcp of open pose --- python/rcs/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/rcs/__init__.py b/python/rcs/__init__.py index d4ab267a..2cd762ce 100644 --- a/python/rcs/__init__.py +++ b/python/rcs/__init__.py @@ -127,7 +127,7 @@ class RobotMetaConfig: GRIPPER_OFFSETS: dict[common.GripperType, common.Pose] = { common.GripperType.FrankaHand: common.Pose(pose_matrix=common.FrankaHandTCPOffset()), - common.GripperType("Robotiq2F85"): common.Pose(translation=np.array([0, 0.0, 0.1628])), + common.GripperType("Robotiq2F85"): common.Pose(translation=np.array([0, 0.0, 0.1493])), } SCENE_PATHS: dict[str, str] = {"empty_world": "assets/scenes/empty_world/scene.xml"}