Skip to content

fix(franka): controllers #303

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 8 commits into from
May 5, 2026
Merged

fix(franka): controllers #303

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
b369f13
feat(teleop): include depth option, duo and optimizations
juelg May 4, 2026
b7baa88
fix(fr3:desk): home command possible without franka hand
juelg May 4, 2026
b53fcee
fix(fr3): duo config, gipper offset and copy
juelg May 4, 2026
8478239
fix(robotiq): gripper type
juelg May 4, 2026
38a1ba9
fix(franka): random jumping induced by uninitialized matrixes
juelg May 4, 2026
ebfc9e3
fix(fr3): tcp in desk applied in controllers
juelg May 5, 2026
9430049
fix(franka): redefine q_home
juelg May 5, 2026
5876a98
fix(robotiq): tcp of open pose
juelg May 5, 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
19 changes: 10 additions & 9 deletions examples/teleop/franka.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
"zed": "19928076",
}
MQ3_ADDR = "10.42.0.1"
INCLUDE_DEPTH = False

# DIGIT_DICT = {
# "digit_right_left": "D21182",
Expand All @@ -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={
Expand All @@ -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()
Expand Down Expand Up @@ -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
Expand All @@ -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


Expand Down
95 changes: 61 additions & 34 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 @@ -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<std::mutex> lock(this->interpolator_mutex);
Expand All @@ -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 {
Expand All @@ -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 {
Expand Down Expand Up @@ -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<int>(this->running_controller.load())) +
". To change controller type stop the current controller first.");
} else {
this->interpolator_mutex.lock();
Expand All @@ -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();
Expand All @@ -229,28 +229,35 @@ 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<std::mutex> 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<int>(this->running_controller.load())) +
". To change controller type stop the current controller first.");
} else {
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;
}
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);

// 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();
Expand All @@ -259,10 +266,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();
}
}
Expand Down Expand Up @@ -291,7 +299,10 @@ void Franka::osc() {
// translation: [150.0, 150.0, 150.0]
// rotation: 250.0

Eigen::Matrix<double, 3, 3> Kp_p, Kp_r, Kd_p, Kd_r;
Eigen::Matrix<double, 3, 3> Kp_p = Eigen::Matrix3d::Zero();
Eigen::Matrix<double, 3, 3> Kp_r = Eigen::Matrix3d::Zero();
Eigen::Matrix<double, 3, 3> Kd_p = Eigen::Matrix3d::Zero();
Eigen::Matrix<double, 3, 3> Kd_r = Eigen::Matrix3d::Zero();
Eigen::Matrix<double, 7, 1> static_q_task_;
Eigen::Matrix<double, 7, 1> residual_mass_vec_;
Eigen::Array<double, 7, 1> joint_max_;
Expand Down Expand Up @@ -323,7 +334,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);
}
Expand All @@ -335,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;
Expand Down Expand Up @@ -376,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());

Expand Down Expand Up @@ -487,7 +503,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() {
Expand Down Expand Up @@ -524,14 +540,13 @@ 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);
}

common::Vector7d desired_q;
common::Pose pose(robot_state.O_T_EE);

this->interpolator_mutex.lock();
this->curr_state = robot_state;
Expand Down Expand Up @@ -588,16 +603,18 @@ void Franka::joint_controller() {
std::lock_guard<std::mutex> 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);
}

Expand All @@ -617,7 +634,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}));
}
Expand All @@ -627,6 +644,8 @@ void Franka::zero_torque_controller() {
std::lock_guard<std::mutex> lock(this->exception_mutex);
this->background_exception = std::current_exception();
}

this->running_controller.store(Controller::none);
}

void Franka::move_home() {
Expand Down Expand Up @@ -714,8 +733,12 @@ std::optional<std::shared_ptr<common::Kinematics>> 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
Expand All @@ -733,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);
}
}

Expand Down Expand Up @@ -786,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;
Expand Down
7 changes: 2 additions & 5 deletions extensions/rcs_fr3/src/hw/Franka.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include <franka/robot.h>
#include <franka/robot_state.h>

#include <atomic>
#include <cmath>
#include <memory>
#include <mutex>
Expand Down Expand Up @@ -43,10 +44,6 @@ struct FrankaConfig : common::RobotConfig {
bool async_control = false;
bool tcp_offset_configured_in_desk = true;
bool ignore_realtime = false;
std::optional<common::VectorXd> 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<double, 2, Eigen::Dynamic, Eigen::ColMajor> joint_limits =
(Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 7) <<
Expand Down Expand Up @@ -89,7 +86,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<Controller> running_controller{Controller::none};
std::exception_ptr background_exception = nullptr;
std::mutex exception_mutex;
void osc();
Expand Down
3 changes: 2 additions & 1 deletion extensions/rcs_fr3/src/rcs_fr3/__main__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
Loading
Loading