Skip to content

refactor!: device ids are now passed through the config #276

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 4 commits into from
Apr 13, 2026
Merged

refactor!: device ids are now passed through the config #276

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
6c9c740
refactor!: device ids are now passed through the config
juelg Apr 13, 2026
be422ff
feat: add constructors to cpp config structs
juelg Apr 13, 2026
7e7446c
fix(config): corrected config extension form cpp base
juelg Apr 13, 2026
3512142
refactor!: wrapper parameters into config objects
juelg Apr 13, 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
8 changes: 5 additions & 3 deletions Makefile
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,11 @@ stubgen:
find ./python -not -path "./python/rcs/_core/*" -name '*.pyi' -delete
find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[typing\.Literal\[\([0-9]\+\)\], typing\.Literal\[1\]\]/tuple\[typing\.Literal[\1]\]/g'
find ./python/rcs/_core -name '*.pyi' -print | xargs sed -i 's/tuple\[\([M|N]\), typing\.Literal\[1\]\]/tuple\[\1\]/g'
ruff check --fix python/rcs/_core
isort python/rcs/_core
black python/rcs/_core
sed -i 's/ q_home: numpy\.ndarray\[tuple\[M\], numpy\.dtype\[numpy\.float64\]\] | None/ q_home: numpy.ndarray | None/' python/rcs/_core/common.pyi
python scripts/generate_common_typing.py
ruff check --fix python/rcs/_core python/rcs/common_typing.py
isort python/rcs/_core python/rcs/common_typing.py
black python/rcs/_core python/rcs/common_typing.py

# Python
pycheckformat:
Expand Down
9 changes: 4 additions & 5 deletions examples/fr3/fr3_direct_control.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,21 +97,20 @@ def main():
simulation.open_gui()

else:
fr3_cfg = default_fr3_hw_robot_cfg()
fr3_cfg = default_fr3_hw_robot_cfg(ROBOT_IP)
fr3_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset())
ik = rcs.common.Pin(
fr3_cfg.kinematic_model_path,
fr3_cfg.attachment_site,
urdf=fr3_cfg.kinematic_model_path.endswith(".urdf"),
)
robot = hw.Franka(ROBOT_IP, ik)
robot.set_config(fr3_cfg) # type: ignore
robot = hw.Franka(fr3_cfg, ik)

gripper_cfg_hw = hw.FHConfig()
gripper_cfg_hw = hw.FHConfig(ip=ROBOT_IP)
gripper_cfg_hw.epsilon_inner = gripper_cfg_hw.epsilon_outer = 0.1
gripper_cfg_hw.speed = 0.1
gripper_cfg_hw.force = 30
gripper = hw.FrankaHand(ROBOT_IP, gripper_cfg_hw)
gripper = hw.FrankaHand(gripper_cfg_hw)
input("the robot is going to move, press enter whenever you are ready")

# move to home position and open gripper
Expand Down
2 changes: 1 addition & 1 deletion examples/fr3/fr3_readme.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@

# gripper
gripper = sim.SimGripper(simulation, gripper_cfg)
env = GripperWrapper(env, gripper, binary=True)
env = GripperWrapper(env, gripper)

env = RobotSimWrapper(env)
env = GripperWrapperSim(env)
Expand Down
3 changes: 1 addition & 2 deletions examples/ur5e/ur5e_env_cartesian_control.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,12 +20,11 @@

def main():
if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
robot_cfg = UR5eConfig()
robot_cfg = UR5eConfig(ip=ROBOT_IP)
robot_cfg.async_control = False
env_rel = RCSUR5eEnvCreator()(
robot_cfg=robot_cfg,
control_mode=ControlMode.CARTESIAN_TQuat,
ip=ROBOT_IP,
camera_set=None,
max_relative_movement=0.2,
relative_to=RelativeTo.LAST_STEP,
Expand Down
3 changes: 1 addition & 2 deletions examples/ur5e/ur5e_env_joint_control.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,10 @@
def main():

if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
robot_cfg = UR5eConfig()
robot_cfg = UR5eConfig(ip=ROBOT_IP)
env_rel = RCSUR5eEnvCreator()(
control_mode=ControlMode.JOINTS,
robot_cfg=robot_cfg,
ip=ROBOT_IP,
camera_set=None,
max_relative_movement=np.deg2rad(5),
relative_to=RelativeTo.LAST_STEP,
Expand Down
24 changes: 13 additions & 11 deletions examples/xarm7/xarm7_env_cartesian_control.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
from rcs.envs.base import ControlMode, RelativeTo
from rcs.envs.creators import SimEnvCreator
from rcs_xarm7.creators import RCSXArm7EnvCreator
from rcs_xarm7.hw import XArm7Config

import rcs
from rcs import sim
Expand All @@ -30,15 +31,16 @@
def main():

if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
robot_cfg = XArm7Config(ip=ROBOT_IP)
env_rel = RCSXArm7EnvCreator()(
robot_cfg=robot_cfg,
control_mode=ControlMode.CARTESIAN_TQuat,
ip=ROBOT_IP,
relative_to=RelativeTo.LAST_STEP,
max_relative_movement=0.5,
)
else:
robot_cfg = sim.SimRobotConfig()
robot_cfg.actuators = [
robot_sim_cfg = sim.SimRobotConfig()
robot_sim_cfg.actuators = [
"act1",
"act2",
"act3",
Expand All @@ -47,7 +49,7 @@ def main():
"act6",
"act7",
]
robot_cfg.joints = [
robot_sim_cfg.joints = [
"joint1",
"joint2",
"joint3",
Expand All @@ -56,15 +58,15 @@ def main():
"joint6",
"joint7",
]
robot_cfg.base = "base"
robot_cfg.robot_type = rcs.common.RobotType.XArm7
robot_cfg.attachment_site = "attachment_site"
robot_cfg.arm_collision_geoms = []
robot_sim_cfg.base = "base"
robot_sim_cfg.robot_type = rcs.common.RobotType.XArm7
robot_sim_cfg.attachment_site = "attachment_site"
robot_sim_cfg.arm_collision_geoms = []
scene = rcs.scenes["xarm7_empty_world"]
robot_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene
robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot
robot_sim_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene
robot_sim_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot
env_rel = SimEnvCreator()(
robot_cfg=robot_cfg,
robot_cfg=robot_sim_cfg,
control_mode=ControlMode.CARTESIAN_TQuat,
gripper_cfg=None,
# cameras=default_mujoco_cameraset_cfg(),
Expand Down
24 changes: 13 additions & 11 deletions examples/xarm7/xarm7_env_joint_control.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
from rcs.envs.base import ControlMode, RelativeTo
from rcs.envs.creators import SimEnvCreator
from rcs_xarm7.creators import RCSXArm7EnvCreator
from rcs_xarm7.hw import XArm7Config

import rcs
from rcs import sim
Expand All @@ -30,15 +31,16 @@

def main():
if ROBOT_INSTANCE == RobotPlatform.HARDWARE:
robot_cfg = XArm7Config(ip=ROBOT_IP)
env_rel = RCSXArm7EnvCreator()(
robot_cfg=robot_cfg,
control_mode=ControlMode.JOINTS,
ip=ROBOT_IP,
relative_to=RelativeTo.LAST_STEP,
max_relative_movement=np.deg2rad(5),
)
else:
robot_cfg = sim.SimRobotConfig()
robot_cfg.actuators = [
robot_sim_cfg = sim.SimRobotConfig()
robot_sim_cfg.actuators = [
"act1",
"act2",
"act3",
Expand All @@ -47,7 +49,7 @@ def main():
"act6",
"act7",
]
robot_cfg.joints = [
robot_sim_cfg.joints = [
"joint1",
"joint2",
"joint3",
Expand All @@ -56,15 +58,15 @@ def main():
"joint6",
"joint7",
]
robot_cfg.base = "base"
robot_cfg.robot_type = rcs.common.RobotType.XArm7
robot_cfg.attachment_site = "attachment_site"
robot_cfg.arm_collision_geoms = []
robot_sim_cfg.base = "base"
robot_sim_cfg.robot_type = rcs.common.RobotType.XArm7
robot_sim_cfg.attachment_site = "attachment_site"
robot_sim_cfg.arm_collision_geoms = []
scene = rcs.scenes["xarm7_empty_world"]
robot_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene
robot_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot
robot_sim_cfg.mjcf_scene_path = scene.mjb or scene.mjcf_scene
robot_sim_cfg.kinematic_model_path = rcs.scenes["xarm7_empty_world"].mjcf_robot
env_rel = SimEnvCreator()(
robot_cfg=robot_cfg,
robot_cfg=robot_sim_cfg,
control_mode=ControlMode.JOINTS,
gripper_cfg=None,
# cameras=default_mujoco_cameraset_cfg(),
Expand Down
53 changes: 26 additions & 27 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 @@ -19,17 +19,15 @@

namespace rcs {
namespace hw {
Franka::Franka(const std::string& ip,
std::optional<std::shared_ptr<common::Kinematics>> ik,
const std::optional<FrankaConfig>& cfg)
: robot(ip), m_ik(ik) {
Franka::Franka(const FrankaConfig& cfg,
std::optional<std::shared_ptr<common::Kinematics>> ik)
: m_cfg(cfg),
m_ik(ik),
robot(cfg.ip, cfg.ignore_realtime ? franka::RealtimeConfig::kIgnore
: franka::RealtimeConfig::kEnforce) {
// set collision behavior and impedance
this->set_default_robot_behavior();
this->set_guiding_mode(true, true, true, true, true, true, true);

if (cfg.has_value()) {
this->cfg = cfg.value();
} // else default constructor
}

Franka::~Franka() {
Expand All @@ -46,11 +44,11 @@ Franka::~Franka() {
* otherwise the call will fail
*/
bool Franka::set_config(const FrankaConfig& cfg) {
this->cfg = cfg;
this->cfg.speed_factor = std::min(std::max(cfg.speed_factor, 0.0), 1.0);
this->m_cfg = cfg;
this->m_cfg.speed_factor = std::min(std::max(cfg.speed_factor, 0.0), 1.0);

if (this->cfg.load_parameters.has_value()) {
auto load_value = &(this->cfg.load_parameters.value());
if (this->m_cfg.load_parameters.has_value()) {
auto load_value = &(this->m_cfg.load_parameters.value());
if (!load_value->f_x_cload.has_value()) {
load_value->f_x_cload = Eigen::Vector3d::Zero();
}
Expand All @@ -69,7 +67,7 @@ bool Franka::set_config(const FrankaConfig& cfg) {
FrankaConfig* Franka::get_config() {
// copy config to heap
FrankaConfig* cfg = new FrankaConfig();
*cfg = this->cfg;
*cfg = this->m_cfg;
return cfg;
}

Expand Down Expand Up @@ -110,19 +108,19 @@ common::Pose Franka::get_cartesian_position() {
x = common::Pose(this->curr_state.O_T_EE);
this->interpolator_mutex.unlock();
}
if (!this->cfg.tcp_offset_configured_in_desk) {
return x * cfg.tcp_offset;
if (!this->m_cfg.tcp_offset_configured_in_desk) {
return x * this->m_cfg.tcp_offset;
}
return x;
}

void Franka::set_joint_position(const common::VectorXd& q) {
if (this->cfg.async_control) {
if (this->m_cfg.async_control) {
this->controller_set_joint_position(q);
return;
}
// sync control
FrankaMotionGenerator motion_generator(this->cfg.speed_factor, q);
FrankaMotionGenerator motion_generator(this->m_cfg.speed_factor, q);
this->robot.control(motion_generator);
}

Expand Down Expand Up @@ -635,8 +633,8 @@ void Franka::move_home() {
// sync
this->stop_control_thread();
FrankaMotionGenerator motion_generator(
this->cfg.speed_factor,
common::robots_meta_config.at(this->cfg.robot_type).q_home);
this->m_cfg.speed_factor,
common::robots_meta_config.at(this->m_cfg.robot_type).q_home);
this->robot.control(motion_generator);
}

Expand Down Expand Up @@ -713,28 +711,28 @@ 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
if (this->cfg.async_control) {
if (this->m_cfg.async_control) {
this->osc_set_cartesian_position(x);
return;
}
// TODO: this should handled with tcp offset config
common::Pose nominal_end_effector_frame_value;
if (this->cfg.nominal_end_effector_frame.has_value()) {
if (this->m_cfg.nominal_end_effector_frame.has_value()) {
nominal_end_effector_frame_value =
this->cfg.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->cfg.ik_solver == IKSolver::franka_ik) {
if (this->m_cfg.ik_solver == IKSolver::franka_ik) {
// 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);

} else if (this->cfg.ik_solver == IKSolver::rcs_ik) {
} else if (this->m_cfg.ik_solver == IKSolver::rcs_ik) {
this->set_cartesian_position_ik(x);
}
}
Expand All @@ -746,7 +744,7 @@ void Franka::set_cartesian_position_ik(const common::Pose& pose) {
"position.");
}
auto joints = this->m_ik.value()->inverse(pose, this->get_joint_position(),
this->cfg.tcp_offset);
this->m_cfg.tcp_offset);

if (joints.has_value()) {
this->set_joint_position(joints.value());
Expand All @@ -758,8 +756,9 @@ void Franka::set_cartesian_position_ik(const common::Pose& pose) {
}

common::Pose Franka::get_base_pose_in_world_coordinates() {
return this->cfg.world_to_robot.has_value() ? this->cfg.world_to_robot.value()
: common::Pose();
return this->m_cfg.world_to_robot.has_value()
? this->m_cfg.world_to_robot.value()
: common::Pose();
}

void Franka::set_cartesian_position_internal(const common::Pose& pose,
Expand Down
12 changes: 5 additions & 7 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 @@ -32,9 +32,7 @@ enum IKSolver { franka_ik = 0, rcs_ik };
// control
enum Controller { none = 0, jsc, osc, ztc };
struct FrankaConfig : common::RobotConfig {
// TODO: max force and elbow?
// TODO: we can either write specific bindings for each, or we use python
// dictionaries with these objects
std::string ip;
common::RobotType robot_type = common::RobotType::FR3;
common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE;
IKSolver ik_solver = IKSolver::rcs_ik;
Expand All @@ -44,6 +42,7 @@ struct FrankaConfig : common::RobotConfig {
std::optional<common::Pose> world_to_robot = std::nullopt;
bool async_control = false;
bool tcp_offset_configured_in_desk = true;
bool ignore_realtime = false;
};

struct FR3Config : FrankaConfig {};
Expand All @@ -58,7 +57,7 @@ struct FrankaState : common::RobotState {
class Franka : public common::Robot {
private:
franka::Robot robot;
FrankaConfig cfg;
FrankaConfig m_cfg;
std::optional<std::shared_ptr<common::Kinematics>> m_ik;
std::optional<std::thread> control_thread = std::nullopt;
common::LinearPoseTrajInterpolator traj_interpolator;
Expand All @@ -75,9 +74,8 @@ class Franka : public common::Robot {
void check_for_background_errors();

public:
Franka(const std::string& ip,
std::optional<std::shared_ptr<common::Kinematics>> ik = std::nullopt,
const std::optional<FrankaConfig>& cfg = std::nullopt);
Franka(const FrankaConfig& cfg,
std::optional<std::shared_ptr<common::Kinematics>> ik = std::nullopt);
~Franka() override;

bool set_config(const FrankaConfig& cfg);
Expand Down
Loading
Loading