From 7fc5f0812f67dbfa5bde26f8ba1b65582989a234 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 12 Aug 2026 11:34:56 -0700 Subject: [PATCH 1/5] feat(franka): add policy_rate config option --- extensions/rcs_fr3/src/hw/Franka.cpp | 10 +++++----- extensions/rcs_fr3/src/hw/Franka.h | 6 ++++++ extensions/rcs_fr3/src/pybind/rcs.cpp | 1 + extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 1 + .../rcs_panda/src/rcs_panda/_core/hw/__init__.pyi | 1 + 5 files changed, 14 insertions(+), 5 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index efdc9d11..32c383bf 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -199,11 +199,11 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { // from deoxys/config/osc-position-controller.yml double traj_interpolation_time_fraction = 1.0; // in s // form deoxys/config/charmander.yml - int policy_rate = 20; int traj_rate = 500; if (this->running_controller.load() == Controller::none) { this->controller_time = 0.0; + this->m_active_policy_rate = this->m_cfg.policy_rate; this->get_joint_position(); this->joint_interpolator = common::LinearJointPositionTrajInterpolator(); } else if (this->running_controller.load() != Controller::jsc) { @@ -219,7 +219,7 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { this->joint_interpolator.reset( this->controller_time, Eigen::Map(this->curr_state.q.data()), desired_q, - policy_rate, traj_rate, traj_interpolation_time_fraction); + this->m_active_policy_rate, traj_rate, traj_interpolation_time_fraction); // if not thread is running, then start if (this->running_controller.load() == Controller::none) { @@ -251,11 +251,11 @@ void Franka::osc_set_cartesian_position( // from deoxys/config/osc-position-controller.yml double traj_interpolation_time_fraction = 1.0; // form deoxys/config/charmander.yml - int policy_rate = 20; int traj_rate = 500; if (this->running_controller.load() == Controller::none) { this->controller_time = 0.0; + this->m_active_policy_rate = this->m_cfg.policy_rate; { std::lock_guard lock(this->interpolator_mutex); this->curr_state = this->robot.readOnce(); @@ -275,8 +275,8 @@ void Franka::osc_set_cartesian_position( this->traj_interpolator.reset( this->controller_time, curr_pose.translation(), curr_pose.quaternion(), desired_pose_EE_in_base_frame.translation(), - desired_pose_EE_in_base_frame.quaternion(), policy_rate, traj_rate, - traj_interpolation_time_fraction); + desired_pose_EE_in_base_frame.quaternion(), this->m_active_policy_rate, + traj_rate, traj_interpolation_time_fraction); // if not thread is running, then start if (this->running_controller.load() == Controller::none) { diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 3906f49f..a7f53691 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -38,6 +38,9 @@ struct FrankaConfig : common::RobotConfig { common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE; IKSolver ik_solver = IKSolver::rcs_ik; double speed_factor = DEFAULT_SPEED_FACTOR; + // Rate (Hz) at which set-point commands are streamed from the Python side. + // Used to size the interpolation window between successive targets. + int policy_rate = 20; // values from deoxys/config/joint-impedance-controller.yml common::Vector7d kp = (common::Vector7d() << 100., 100., 100., 100., 75., 150., 50.).finished(); @@ -94,6 +97,9 @@ class Franka : public common::Robot { std::optional control_thread = std::nullopt; common::LinearPoseTrajInterpolator traj_interpolator; double controller_time = 0.0; + // Snapshot of m_cfg.policy_rate taken when a controller thread is started, so + // the interpolation window stays consistent for the controller's lifetime. + int m_active_policy_rate = 20; common::LinearJointPositionTrajInterpolator joint_interpolator; franka::RobotState curr_state; std::mutex interpolator_mutex; diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 9b2f4ece..e57af814 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -128,6 +128,7 @@ PYBIND11_MODULE(_core, m) { py::class_(hw, "FrankaConfig", robot_config) .def_readwrite("ik_solver", &rcs::hw::FrankaConfig::ik_solver) .def_readwrite("speed_factor", &rcs::hw::FrankaConfig::speed_factor) + .def_readwrite("policy_rate", &rcs::hw::FrankaConfig::policy_rate) .def_readwrite("kp", &rcs::hw::FrankaConfig::kp) .def_readwrite("kd", &rcs::hw::FrankaConfig::kd) .def_readwrite("torque_limit", &rcs::hw::FrankaConfig::torque_limit) 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 af761256..5c48e26d 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -114,6 +114,7 @@ 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 + policy_rate: int speed_factor: float tcp_offset: rcs._core.common.Pose tcp_offset_explicit: bool 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 af761256..5c48e26d 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -114,6 +114,7 @@ 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 + policy_rate: int speed_factor: float tcp_offset: rcs._core.common.Pose tcp_offset_explicit: bool From 74bc8ee15de9ed214e52daf7004bd525e46d6a8e Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Wed, 12 Aug 2026 14:30:15 -0700 Subject: [PATCH 2/5] feat(franka): approach before controller start new config option that moves the robot to the inital action with the new controller parameters if a new controller is started --- extensions/rcs_fr3/src/hw/Franka.cpp | 127 +++++++++++++++++- extensions/rcs_fr3/src/hw/Franka.h | 18 +++ extensions/rcs_fr3/src/pybind/rcs.cpp | 8 ++ .../rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi | 4 + .../src/rcs_panda/_core/hw/__init__.pyi | 4 + include/rcs/LinearPoseTrajInterpolator.h | 20 ++- 6 files changed, 171 insertions(+), 10 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 32c383bf..f455a1bf 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -201,7 +201,10 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { // form deoxys/config/charmander.yml int traj_rate = 500; - if (this->running_controller.load() == Controller::none) { + const bool starting_fresh = + this->running_controller.load() == Controller::none; + + if (starting_fresh) { this->controller_time = 0.0; this->m_active_policy_rate = this->m_cfg.policy_rate; this->get_joint_position(); @@ -216,18 +219,64 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { this->interpolator_mutex.lock(); } + const bool approach_on_start = + starting_fresh && this->m_cfg.blocking_move_on_start; + double approach_time = -1.0; + if (approach_on_start) { + const double kMinApproachTime = 0.3; // s + const double kMaxApproachTime = 5.0; // s + const common::Vector7d q_now = + Eigen::Map(this->curr_state.q.data()); + const double max_gap = (desired_q - q_now).cwiseAbs().maxCoeff(); + const double speed = std::max(this->m_cfg.approach_joint_speed, 1e-6); + approach_time = + std::clamp(max_gap / speed, kMinApproachTime, kMaxApproachTime); + } + this->joint_interpolator.reset( this->controller_time, Eigen::Map(this->curr_state.q.data()), desired_q, - this->m_active_policy_rate, traj_rate, traj_interpolation_time_fraction); + this->m_active_policy_rate, traj_rate, traj_interpolation_time_fraction, + approach_time); // if not thread is running, then start - if (this->running_controller.load() == Controller::none) { + if (starting_fresh) { this->running_controller.store(Controller::jsc); this->control_thread = std::thread(&Franka::joint_controller, this); } else { this->interpolator_mutex.unlock(); } + + if (approach_on_start) { + // Block until the controller has driven the robot to desired_q (or a + // safety timeout elapses). + const double pos_tol = 0.02; // rad + const double vel_tol = 0.05; // rad/s + const double timeout = approach_time + 2.0; + const auto start = std::chrono::steady_clock::now(); + while (true) { + this->check_for_background_errors(); + common::Vector7d q; + common::Vector7d dq; + { + std::lock_guard lock(this->interpolator_mutex); + q = Eigen::Map(this->curr_state.q.data()); + dq = Eigen::Map(this->curr_state.dq.data()); + } + const double elapsed = + std::chrono::duration(std::chrono::steady_clock::now() - start) + .count(); + const double pos_err = (q - desired_q).cwiseAbs().maxCoeff(); + const double vel = dq.cwiseAbs().maxCoeff(); + if (elapsed >= approach_time && pos_err < pos_tol && vel < vel_tol) { + break; + } + if (elapsed > timeout) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } } void Franka::check_for_background_errors() { @@ -253,7 +302,10 @@ void Franka::osc_set_cartesian_position( // form deoxys/config/charmander.yml int traj_rate = 500; - if (this->running_controller.load() == Controller::none) { + const bool starting_fresh = + this->running_controller.load() == Controller::none; + + if (starting_fresh) { this->controller_time = 0.0; this->m_active_policy_rate = this->m_cfg.policy_rate; { @@ -272,19 +324,82 @@ void Franka::osc_set_cartesian_position( common::Pose curr_pose = GetTCPInBaseFrame(this->curr_state, this->m_cfg.tcp_offset); + + const bool approach_on_start = + starting_fresh && this->m_cfg.blocking_move_on_start; + double approach_time = -1.0; + if (approach_on_start) { + const double kMinApproachTime = 0.3; // s + const double kMaxApproachTime = 5.0; // s + const double trans_gap = + (desired_pose_EE_in_base_frame.translation() - curr_pose.translation()) + .norm(); + double dot = std::abs(curr_pose.quaternion().normalized().dot( + desired_pose_EE_in_base_frame.quaternion().normalized())); + dot = std::min(1.0, dot); + const double rot_gap = 2.0 * std::acos(dot); + const double trans_speed = std::max(this->m_cfg.approach_cartesian_speed, 1e-6); + const double rot_speed = std::max(this->m_cfg.approach_rotation_speed, 1e-6); + approach_time = std::clamp( + std::max(trans_gap / trans_speed, rot_gap / rot_speed), kMinApproachTime, + kMaxApproachTime); + } + this->traj_interpolator.reset( this->controller_time, curr_pose.translation(), curr_pose.quaternion(), desired_pose_EE_in_base_frame.translation(), desired_pose_EE_in_base_frame.quaternion(), this->m_active_policy_rate, - traj_rate, traj_interpolation_time_fraction); + traj_rate, traj_interpolation_time_fraction, approach_time); // if not thread is running, then start - if (this->running_controller.load() == Controller::none) { + if (starting_fresh) { this->running_controller.store(Controller::osc); this->control_thread = std::thread(&Franka::osc, this); } else { this->interpolator_mutex.unlock(); } + + if (approach_on_start) { + // Block until the controller has driven the robot to the desired pose (or a + // safety timeout elapses). + const double pos_tol = 0.005; // m + const double ori_tol = 0.02; // rad + const double vel_tol = 0.05; // rad/s (joint-space proxy for "stopped") + const double timeout = approach_time + 2.0; + const auto start = std::chrono::steady_clock::now(); + const Eigen::Vector3d target_p = desired_pose_EE_in_base_frame.translation(); + const Eigen::Quaterniond target_q = + desired_pose_EE_in_base_frame.quaternion().normalized(); + while (true) { + this->check_for_background_errors(); + franka::RobotState state; + common::Vector7d dq; + { + std::lock_guard lock(this->interpolator_mutex); + state = this->curr_state; + dq = Eigen::Map(this->curr_state.dq.data()); + } + const common::Pose meas_pose = + GetTCPInBaseFrame(state, this->m_cfg.tcp_offset); + const double pos_err = (target_p - meas_pose.translation()).norm(); + double dot = + std::abs(meas_pose.quaternion().normalized().dot(target_q)); + dot = std::min(1.0, dot); + const double ori_err = 2.0 * std::acos(dot); + const double vel = dq.cwiseAbs().maxCoeff(); + const double elapsed = + std::chrono::duration(std::chrono::steady_clock::now() - start) + .count(); + if (elapsed >= approach_time && pos_err < pos_tol && ori_err < ori_tol && + vel < vel_tol) { + break; + } + if (elapsed > timeout) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } } // method to stop thread diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index a7f53691..982377e0 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -57,6 +57,24 @@ struct FrankaConfig : common::RobotConfig { // Indicates that Cartesian control uses tcp_offset. bool tcp_offset_explicit = false; bool async_control = false; + // When true, on every (re)start of the joint controller the controller first + // holds the current measured position (~zero error) and ramps its target to + // the first commanded target over a gap-scaled window, blocking until it has + // converged before streaming resumes. Avoids a torque/velocity jump when + // (re)starting far from the target (e.g. after a PD-gain switch). + bool blocking_move_on_start = false; + // Max joint speed (rad/s) used to size the blocking approach window on + // (re)start of the joint controller: + // approach_time = max|q_target - q_now| / approach_joint_speed (clamped). + // Only used when blocking_move_on_start is true. + double approach_joint_speed = 0.4; + // Max Cartesian translation (m/s) and rotation (rad/s) speeds used to size the + // blocking approach window on (re)start of the OSC controller: + // approach_time = max(trans_gap / approach_cartesian_speed, + // rot_gap / approach_rotation_speed) (clamped). + // Only used when blocking_move_on_start is true. + double approach_cartesian_speed = 0.1; + double approach_rotation_speed = 0.5; 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 e57af814..fc3ddc18 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -149,6 +149,14 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("tcp_offset_explicit", &rcs::hw::FrankaConfig::tcp_offset_explicit) .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control) + .def_readwrite("blocking_move_on_start", + &rcs::hw::FrankaConfig::blocking_move_on_start) + .def_readwrite("approach_joint_speed", + &rcs::hw::FrankaConfig::approach_joint_speed) + .def_readwrite("approach_cartesian_speed", + &rcs::hw::FrankaConfig::approach_cartesian_speed) + .def_readwrite("approach_rotation_speed", + &rcs::hw::FrankaConfig::approach_rotation_speed) .def_readwrite("ignore_realtime", &rcs::hw::FrankaConfig::ignore_realtime) .def_readwrite("ip", &rcs::hw::FrankaConfig::ip); 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 5c48e26d..5b9ab3eb 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -105,7 +105,11 @@ class Franka(rcs._core.common.Robot): class FrankaConfig(rcs._core.common.RobotConfig): allow_high_collision: bool + approach_cartesian_speed: float + approach_joint_speed: float + approach_rotation_speed: float async_control: bool + blocking_move_on_start: bool ignore_realtime: bool ik_solver: IKSolver ip: str 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 5c48e26d..5b9ab3eb 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -105,7 +105,11 @@ class Franka(rcs._core.common.Robot): class FrankaConfig(rcs._core.common.RobotConfig): allow_high_collision: bool + approach_cartesian_speed: float + approach_joint_speed: float + approach_rotation_speed: float async_control: bool + blocking_move_on_start: bool ignore_realtime: bool ik_solver: IKSolver ip: str diff --git a/include/rcs/LinearPoseTrajInterpolator.h b/include/rcs/LinearPoseTrajInterpolator.h index cecaa754..ed126f42 100644 --- a/include/rcs/LinearPoseTrajInterpolator.h +++ b/include/rcs/LinearPoseTrajInterpolator.h @@ -40,11 +40,17 @@ class LinearPoseTrajInterpolator { const Eigen::Vector3d &p_goal, const Eigen::Quaterniond &q_goal, const int &policy_rate, const int &rate, - const double &traj_interpolator_time_fraction) { + const double &traj_interpolator_time_fraction, + const double &explicit_max_time = -1.0) { dt_ = 1. / static_cast(rate); last_time_ = time_sec; + // A positive explicit_max_time overrides the default (1 / policy_rate) + // interpolation window; used for a longer, gap-scaled approach on (re)start. max_time_ = - 1. / static_cast(policy_rate) * traj_interpolator_time_fraction; + explicit_max_time > 0.0 + ? explicit_max_time + : 1. / static_cast(policy_rate) * + traj_interpolator_time_fraction; start_time_ = time_sec; start_ = false; @@ -131,12 +137,18 @@ class LinearJointPositionTrajInterpolator { const Eigen::Matrix &q_start, const Eigen::Matrix &q_goal, const int &policy_rate, const int &rate, - const double &traj_interpolator_time_fraction) { + const double &traj_interpolator_time_fraction, + const double &explicit_max_time = -1.0) { dt_ = 1. / static_cast(rate); last_time_ = time_sec; + // A positive explicit_max_time overrides the default (1 / policy_rate) + // interpolation window; used for a longer, gap-scaled approach on (re)start. max_time_ = - 1. / static_cast(policy_rate) * traj_interpolator_time_fraction; + explicit_max_time > 0.0 + ? explicit_max_time + : 1. / static_cast(policy_rate) * + traj_interpolator_time_fraction; start_time_ = time_sec; start_ = false; From e71a9c5fef82ee011d8fe668b9a3070b7ce19fa5 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Thu, 20 Aug 2026 15:07:19 -0700 Subject: [PATCH 3/5] style(franka): fromat cpp --- extensions/rcs_fr3/src/hw/Franka.cpp | 26 ++++++++++++++------------ extensions/rcs_fr3/src/hw/Franka.h | 4 ++-- 2 files changed, 16 insertions(+), 14 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index f455a1bf..c6546c45 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -263,8 +263,8 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { q = Eigen::Map(this->curr_state.q.data()); dq = Eigen::Map(this->curr_state.dq.data()); } - const double elapsed = - std::chrono::duration(std::chrono::steady_clock::now() - start) + const double elapsed = std::chrono::duration( + std::chrono::steady_clock::now() - start) .count(); const double pos_err = (q - desired_q).cwiseAbs().maxCoeff(); const double vel = dq.cwiseAbs().maxCoeff(); @@ -338,11 +338,13 @@ void Franka::osc_set_cartesian_position( desired_pose_EE_in_base_frame.quaternion().normalized())); dot = std::min(1.0, dot); const double rot_gap = 2.0 * std::acos(dot); - const double trans_speed = std::max(this->m_cfg.approach_cartesian_speed, 1e-6); - const double rot_speed = std::max(this->m_cfg.approach_rotation_speed, 1e-6); - approach_time = std::clamp( - std::max(trans_gap / trans_speed, rot_gap / rot_speed), kMinApproachTime, - kMaxApproachTime); + const double trans_speed = + std::max(this->m_cfg.approach_cartesian_speed, 1e-6); + const double rot_speed = + std::max(this->m_cfg.approach_rotation_speed, 1e-6); + approach_time = + std::clamp(std::max(trans_gap / trans_speed, rot_gap / rot_speed), + kMinApproachTime, kMaxApproachTime); } this->traj_interpolator.reset( @@ -367,7 +369,8 @@ void Franka::osc_set_cartesian_position( const double vel_tol = 0.05; // rad/s (joint-space proxy for "stopped") const double timeout = approach_time + 2.0; const auto start = std::chrono::steady_clock::now(); - const Eigen::Vector3d target_p = desired_pose_EE_in_base_frame.translation(); + const Eigen::Vector3d target_p = + desired_pose_EE_in_base_frame.translation(); const Eigen::Quaterniond target_q = desired_pose_EE_in_base_frame.quaternion().normalized(); while (true) { @@ -382,13 +385,12 @@ void Franka::osc_set_cartesian_position( const common::Pose meas_pose = GetTCPInBaseFrame(state, this->m_cfg.tcp_offset); const double pos_err = (target_p - meas_pose.translation()).norm(); - double dot = - std::abs(meas_pose.quaternion().normalized().dot(target_q)); + double dot = std::abs(meas_pose.quaternion().normalized().dot(target_q)); dot = std::min(1.0, dot); const double ori_err = 2.0 * std::acos(dot); const double vel = dq.cwiseAbs().maxCoeff(); - const double elapsed = - std::chrono::duration(std::chrono::steady_clock::now() - start) + const double elapsed = std::chrono::duration( + std::chrono::steady_clock::now() - start) .count(); if (elapsed >= approach_time && pos_err < pos_tol && ori_err < ori_tol && vel < vel_tol) { diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 982377e0..7d405587 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -68,8 +68,8 @@ struct FrankaConfig : common::RobotConfig { // approach_time = max|q_target - q_now| / approach_joint_speed (clamped). // Only used when blocking_move_on_start is true. double approach_joint_speed = 0.4; - // Max Cartesian translation (m/s) and rotation (rad/s) speeds used to size the - // blocking approach window on (re)start of the OSC controller: + // Max Cartesian translation (m/s) and rotation (rad/s) speeds used to size + // the blocking approach window on (re)start of the OSC controller: // approach_time = max(trans_gap / approach_cartesian_speed, // rot_gap / approach_rotation_speed) (clamped). // Only used when blocking_move_on_start is true. From f17da977fb6b5ccfb2046c0ea44335b58ec8ebd7 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Thu, 20 Aug 2026 15:30:32 -0700 Subject: [PATCH 4/5] refactor(franka): use thread safe value class --- extensions/rcs_fr3/src/hw/Franka.cpp | 99 +++++++++++----------------- extensions/rcs_fr3/src/hw/Franka.h | 5 +- include/rcs/utils.h | 82 +++++++++++++++++++++++ 3 files changed, 123 insertions(+), 63 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index c6546c45..ca34468d 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -86,8 +86,7 @@ FrankaState* Franka::get_state() { if (this->running_controller.load() == Controller::none) { current_robot_state = this->robot.readOnce(); } else { - std::lock_guard lock(this->interpolator_mutex); - current_robot_state = this->curr_state; + current_robot_state = this->curr_state.load(); } auto* state = new FrankaState(); state->robot_state = current_robot_state; @@ -111,12 +110,10 @@ common::Pose Franka::get_cartesian_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; + robot_state = this->robot.readOnce(); + this->curr_state.store(robot_state); } else { - this->interpolator_mutex.lock(); - robot_state = this->curr_state; - this->interpolator_mutex.unlock(); + robot_state = this->curr_state.load(); } return GetTCPInBaseFrame(robot_state, this->m_cfg.tcp_offset); } @@ -125,11 +122,10 @@ 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; + robot_state = this->robot.readOnce(); + this->curr_state.store(robot_state); } else { - std::lock_guard lock(this->interpolator_mutex); - robot_state = this->curr_state; + robot_state = this->curr_state.load(); } return GetFlangeInBaseFrame(robot_state); } @@ -146,16 +142,14 @@ void Franka::set_joint_position(const common::VectorXd& q) { common::VectorXd Franka::get_joint_position() { this->check_for_background_errors(); - common::Vector7d joints; + franka::RobotState robot_state; if (this->running_controller.load() == Controller::none) { - this->curr_state = this->robot.readOnce(); - joints = common::Vector7d(this->curr_state.q.data()); + robot_state = this->robot.readOnce(); + this->curr_state.store(robot_state); } else { - this->interpolator_mutex.lock(); - joints = common::Vector7d(this->curr_state.q.data()); - this->interpolator_mutex.unlock(); + robot_state = this->curr_state.load(); } - return joints; + return common::Vector7d(robot_state.q.data()); } void Franka::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch, @@ -219,14 +213,16 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { this->interpolator_mutex.lock(); } + franka::RobotState state_now = this->curr_state.load(); + const common::Vector7d q_now = + Eigen::Map(state_now.q.data()); + const bool approach_on_start = starting_fresh && this->m_cfg.blocking_move_on_start; double approach_time = -1.0; if (approach_on_start) { const double kMinApproachTime = 0.3; // s const double kMaxApproachTime = 5.0; // s - const common::Vector7d q_now = - Eigen::Map(this->curr_state.q.data()); const double max_gap = (desired_q - q_now).cwiseAbs().maxCoeff(); const double speed = std::max(this->m_cfg.approach_joint_speed, 1e-6); approach_time = @@ -234,10 +230,8 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { } this->joint_interpolator.reset( - this->controller_time, - Eigen::Map(this->curr_state.q.data()), desired_q, - this->m_active_policy_rate, traj_rate, traj_interpolation_time_fraction, - approach_time); + this->controller_time, q_now, desired_q, this->m_active_policy_rate, + traj_rate, traj_interpolation_time_fraction, approach_time); // if not thread is running, then start if (starting_fresh) { @@ -256,16 +250,12 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { const auto start = std::chrono::steady_clock::now(); while (true) { this->check_for_background_errors(); - common::Vector7d q; - common::Vector7d dq; - { - std::lock_guard lock(this->interpolator_mutex); - q = Eigen::Map(this->curr_state.q.data()); - dq = Eigen::Map(this->curr_state.dq.data()); - } + franka::RobotState state = this->curr_state.load(); + const common::Vector7d q = Eigen::Map(state.q.data()); + const common::Vector7d dq = Eigen::Map(state.dq.data()); const double elapsed = std::chrono::duration( std::chrono::steady_clock::now() - start) - .count(); + .count(); const double pos_err = (q - desired_q).cwiseAbs().maxCoeff(); const double vel = dq.cwiseAbs().maxCoeff(); if (elapsed >= approach_time && pos_err < pos_tol && vel < vel_tol) { @@ -280,18 +270,15 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { } void Franka::check_for_background_errors() { - std::lock_guard lock(this->exception_mutex); - if (this->background_exception) { + std::exception_ptr ex = this->background_exception.load_and_clear(); + if (ex) { this->stop_control_thread(); - std::exception_ptr ex = this->background_exception; - this->background_exception = nullptr; std::rethrow_exception(ex); } } void Franka::clear_background_error() { - std::lock_guard lock(this->exception_mutex); - this->background_exception = nullptr; + this->background_exception.store(nullptr); } void Franka::osc_set_cartesian_position( @@ -308,10 +295,7 @@ void Franka::osc_set_cartesian_position( if (starting_fresh) { this->controller_time = 0.0; this->m_active_policy_rate = this->m_cfg.policy_rate; - { - std::lock_guard lock(this->interpolator_mutex); - this->curr_state = this->robot.readOnce(); - } + this->curr_state.store(this->robot.readOnce()); this->traj_interpolator = common::LinearPoseTrajInterpolator(); } else if (this->running_controller.load() != Controller::osc) { throw std::runtime_error( @@ -323,7 +307,7 @@ void Franka::osc_set_cartesian_position( } common::Pose curr_pose = - GetTCPInBaseFrame(this->curr_state, this->m_cfg.tcp_offset); + GetTCPInBaseFrame(this->curr_state.load(), this->m_cfg.tcp_offset); const bool approach_on_start = starting_fresh && this->m_cfg.blocking_move_on_start; @@ -375,13 +359,8 @@ void Franka::osc_set_cartesian_position( desired_pose_EE_in_base_frame.quaternion().normalized(); while (true) { this->check_for_background_errors(); - franka::RobotState state; - common::Vector7d dq; - { - std::lock_guard lock(this->interpolator_mutex); - state = this->curr_state; - dq = Eigen::Map(this->curr_state.dq.data()); - } + franka::RobotState state = this->curr_state.load(); + const common::Vector7d dq = Eigen::Map(state.dq.data()); const common::Pose meas_pose = GetTCPInBaseFrame(state, this->m_cfg.tcp_offset); const double pos_err = (target_p - meas_pose.translation()).norm(); @@ -391,7 +370,7 @@ void Franka::osc_set_cartesian_position( const double vel = dq.cwiseAbs().maxCoeff(); const double elapsed = std::chrono::duration( std::chrono::steady_clock::now() - start) - .count(); + .count(); if (elapsed >= approach_time && pos_err < pos_tol && ori_err < ori_tol && vel < vel_tol) { break; @@ -494,8 +473,9 @@ void Franka::osc() { int policy_rate = 20; int traj_rate = 500; + this->curr_state.store(robot_state); + this->interpolator_mutex.lock(); - this->curr_state = robot_state; this->controller_time += period.toSec(); this->traj_interpolator.next_step(this->controller_time, desired_pos_EE_in_base_frame, @@ -638,8 +618,7 @@ void Franka::osc() { return tau_d_rate_limited; }); } catch (...) { - std::lock_guard lock(this->exception_mutex); - this->background_exception = std::current_exception(); + this->background_exception.store(std::current_exception()); } // Ensure we mark the controller as stopped so we can restart later @@ -685,8 +664,9 @@ void Franka::joint_controller() { common::Vector7d desired_q; + this->curr_state.store(robot_state); + this->interpolator_mutex.lock(); - this->curr_state = robot_state; this->controller_time += period.toSec(); this->joint_interpolator.next_step(this->controller_time, desired_q); this->interpolator_mutex.unlock(); @@ -734,8 +714,7 @@ void Franka::joint_controller() { return tau_d_rate_limited; }); } catch (...) { - std::lock_guard lock(this->exception_mutex); - this->background_exception = std::current_exception(); + this->background_exception.store(std::current_exception()); } this->running_controller.store(Controller::none); @@ -767,8 +746,9 @@ void Franka::zero_torque_controller() { try { this->robot.control([&](const franka::RobotState& robot_state, franka::Duration period) -> franka::Torques { + this->curr_state.store(robot_state); + this->interpolator_mutex.lock(); - this->curr_state = robot_state; this->controller_time += period.toSec(); this->interpolator_mutex.unlock(); if (this->running_controller.load() == Controller::none) { @@ -778,8 +758,7 @@ void Franka::zero_torque_controller() { return franka::Torques({0, 0, 0, 0, 0, 0, 0}); }); } catch (...) { - std::lock_guard lock(this->exception_mutex); - this->background_exception = std::current_exception(); + this->background_exception.store(std::current_exception()); } this->running_controller.store(Controller::none); diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 7d405587..410ffe8c 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -119,11 +119,10 @@ class Franka : public common::Robot { // the interpolation window stays consistent for the controller's lifetime. int m_active_policy_rate = 20; common::LinearJointPositionTrajInterpolator joint_interpolator; - franka::RobotState curr_state; + common::ThreadSafeValue curr_state; std::mutex interpolator_mutex; std::atomic running_controller{Controller::none}; - std::exception_ptr background_exception = nullptr; - std::mutex exception_mutex; + common::ThreadSafeValue background_exception; void osc(); void joint_controller(); void zero_torque_controller(); diff --git a/include/rcs/utils.h b/include/rcs/utils.h index 5ebc8cce..2baf0e08 100644 --- a/include/rcs/utils.h +++ b/include/rcs/utils.h @@ -2,7 +2,12 @@ #define RCS_UTIL_H #include +#include +#include #include +#include +#include +#include namespace rcs { namespace common { @@ -39,6 +44,83 @@ void bootstrap_egl(std::uintptr_t fn_addr, std::uintptr_t display, std::uintptr_t context); void ensure_current(); +/*** + * @brief thread safe holder for a single value, e.g. to hand data from a + * non-realtime thread to a control loop + */ +template +class ThreadSafeValue { + private: + T value_; + mutable std::mutex mutex_; + + public: + ThreadSafeValue() = default; + explicit ThreadSafeValue(const T& value) : value_(value) {} + + void store(const T& value) { + std::lock_guard lock(mutex_); + value_ = value; + } + + T load() const { + std::lock_guard lock(mutex_); + return value_; + } + + /*** + * @brief atomically read the value and reset it to a default constructed one + */ + T load_and_clear() { + std::lock_guard lock(mutex_); + return std::exchange(value_, T{}); + } +}; + +/*** + * @brief thread safe ring buffer of fixed maximum size, drops the oldest + * element once the buffer is full + */ +template +class ThreadSafeFixedBuffer { + private: + std::deque deque_; + size_t max_size_; + mutable std::mutex mutex_; + + public: + explicit ThreadSafeFixedBuffer(size_t max_size) : max_size_(max_size) {} + + void push_back(const T& value) { + std::lock_guard lock(mutex_); + + deque_.push_back(value); + if (deque_.size() > max_size_) { + deque_.pop_front(); + } + } + + T get(size_t index) const { + std::lock_guard lock(mutex_); + return deque_[index]; + } + + size_t size() const { + std::lock_guard lock(mutex_); + return deque_.size(); + } + + void clear() { + std::lock_guard lock(mutex_); + deque_.clear(); + } + + std::vector to_vector() const { + std::lock_guard lock(mutex_); + return std::vector(deque_.begin(), deque_.end()); + } +}; + } // namespace common } // namespace rcs From 5713533937f900afab048e2e96c4b49d53a8feb7 Mon Sep 17 00:00:00 2001 From: Tobias Juelg Date: Thu, 20 Aug 2026 16:37:15 -0700 Subject: [PATCH 5/5] fix(examples): added policy rate to for low level pd controller in franka examples --- examples/inference/franka.py | 3 +++ examples/teleop/franka.py | 3 +++ 2 files changed, 6 insertions(+) diff --git a/examples/inference/franka.py b/examples/inference/franka.py index 4652fb77..c21052c0 100644 --- a/examples/inference/franka.py +++ b/examples/inference/franka.py @@ -390,6 +390,9 @@ def get_env(cfg: InferenceConfig) -> gym.Env: hw_cfg.robot_cfgs["right"].ignore_realtime = True hw_cfg.robot_cfgs["left"].speed_factor = 0.1 hw_cfg.robot_cfgs["right"].speed_factor = 0.1 + # interpolation window of the controllers must match the rate at which we stream actions + hw_cfg.robot_cfgs["left"].policy_rate = cfg.fps + hw_cfg.robot_cfgs["right"].policy_rate = cfg.fps hw_cfg.gripper_cfgs["left"].serial_number = ROBOTIQ_SERIAL["left"] hw_cfg.gripper_cfgs["right"].serial_number = ROBOTIQ_SERIAL["right"] env_rel = env_creator.create_env(hw_cfg) diff --git a/examples/teleop/franka.py b/examples/teleop/franka.py index 8c539fb7..7c76a211 100644 --- a/examples/teleop/franka.py +++ b/examples/teleop/franka.py @@ -156,6 +156,9 @@ def get_env(): hw_cfg.robot_cfgs["right"].ignore_realtime = True hw_cfg.robot_cfgs["left"].speed_factor = 0.3 hw_cfg.robot_cfgs["right"].speed_factor = 0.3 + # interpolation window of the controllers must match the rate at which we stream actions + hw_cfg.robot_cfgs["left"].policy_rate = RECORD_FPS + hw_cfg.robot_cfgs["right"].policy_rate = RECORD_FPS hw_cfg.gripper_cfgs["left"].serial_number = ROBOTIQ_SERIAL["left"] # type: ignore hw_cfg.gripper_cfgs["right"].serial_number = ROBOTIQ_SERIAL["right"] # type: ignore env_rel = env_creator.create_env(hw_cfg)