Skip to content

fix(franka): approach on controller start #329

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
juelg merged 5 commits into from
Aug 21, 2026
Merged

fix(franka): approach on controller start #329

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
7fc5f08
feat(franka): add policy_rate config option
juelg Aug 12, 2026
74bc8ee
feat(franka): approach before controller start
juelg Aug 12, 2026
e71a9c5
style(franka): fromat cpp
juelg Aug 20, 2026
f17da97
refactor(franka): use thread safe value class
juelg Aug 20, 2026
5713533
fix(examples): added policy rate to for low level pd controller in fr…
juelg Aug 20, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions examples/inference/franka.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
3 changes: 3 additions & 0 deletions examples/teleop/franka.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
194 changes: 145 additions & 49 deletions extensions/rcs_fr3/src/hw/Franka.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::mutex> 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;
Expand All @@ -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);
}
Expand All @@ -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<std::mutex> lock(this->interpolator_mutex);
robot_state = this->curr_state;
robot_state = this->curr_state.load();
}
return GetFlangeInBaseFrame(robot_state);
}
Expand All @@ -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,
Expand Down Expand Up @@ -199,11 +193,14 @@ 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) {
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();
this->joint_interpolator = common::LinearJointPositionTrajInterpolator();
} else if (this->running_controller.load() != Controller::jsc) {
Expand All @@ -216,33 +213,72 @@ 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<common::Vector7d>(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 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<common::Vector7d>(this->curr_state.q.data()), desired_q,
policy_rate, traj_rate, traj_interpolation_time_fraction);
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 (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();
franka::RobotState state = this->curr_state.load();
const common::Vector7d q = Eigen::Map<common::Vector7d>(state.q.data());
const common::Vector7d dq = Eigen::Map<common::Vector7d>(state.dq.data());
const double elapsed = std::chrono::duration<double>(
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() {
std::lock_guard<std::mutex> 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<std::mutex> lock(this->exception_mutex);
this->background_exception = nullptr;
this->background_exception.store(nullptr);
}

void Franka::osc_set_cartesian_position(
Expand All @@ -251,15 +287,15 @@ 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) {
const bool starting_fresh =
this->running_controller.load() == Controller::none;

if (starting_fresh) {
this->controller_time = 0.0;
{
std::lock_guard<std::mutex> lock(this->interpolator_mutex);
this->curr_state = this->robot.readOnce();
}
this->m_active_policy_rate = this->m_cfg.policy_rate;
this->curr_state.store(this->robot.readOnce());
this->traj_interpolator = common::LinearPoseTrajInterpolator();
} else if (this->running_controller.load() != Controller::osc) {
throw std::runtime_error(
Expand All @@ -271,20 +307,80 @@ 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;
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(), 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, 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 = this->curr_state.load();
const common::Vector7d dq = Eigen::Map<common::Vector7d>(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<double>(
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
Expand Down Expand Up @@ -377,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,
Expand Down Expand Up @@ -521,8 +618,7 @@ void Franka::osc() {
return tau_d_rate_limited;
});
} catch (...) {
std::lock_guard<std::mutex> 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
Expand Down Expand Up @@ -568,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();
Expand Down Expand Up @@ -617,8 +714,7 @@ void Franka::joint_controller() {
return tau_d_rate_limited;
});
} catch (...) {
std::lock_guard<std::mutex> lock(this->exception_mutex);
this->background_exception = std::current_exception();
this->background_exception.store(std::current_exception());
}

this->running_controller.store(Controller::none);
Expand Down Expand Up @@ -650,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) {
Expand All @@ -661,8 +758,7 @@ void Franka::zero_torque_controller() {
return franka::Torques({0, 0, 0, 0, 0, 0, 0});
});
} catch (...) {
std::lock_guard<std::mutex> lock(this->exception_mutex);
this->background_exception = std::current_exception();
this->background_exception.store(std::current_exception());
}

this->running_controller.store(Controller::none);
Expand Down
Loading
Loading