Merge pull request #276 from RobotControlStack/juelg/refactor-configs

refactor!: device ids are now passed through the config

Author: juelg

Makefile

@@ -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:

examples/fr3/fr3_direct_control.py

@@ -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

examples/fr3/fr3_readme.py

@@ -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)

examples/ur5e/ur5e_env_cartesian_control.py

@@ -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,

examples/ur5e/ur5e_env_joint_control.py

@@ -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,

examples/xarm7/xarm7_env_cartesian_control.py

@@ -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
@@ -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",
@@ -47,7 +49,7 @@ def main():
             "act6",
             "act7",
         ]
-        robot_cfg.joints = [
+        robot_sim_cfg.joints = [
             "joint1",
             "joint2",
             "joint3",
@@ -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(),

examples/xarm7/xarm7_env_joint_control.py

@@ -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
@@ -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",
@@ -47,7 +49,7 @@ def main():
             "act6",
             "act7",
         ]
-        robot_cfg.joints = [
+        robot_sim_cfg.joints = [
             "joint1",
             "joint2",
             "joint3",
@@ -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(),

extensions/rcs_fr3/src/hw/Franka.cpp

@@ -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() {
@@ -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();
     }
@@ -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;
 }
 
@@ -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);
 }
 
@@ -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);
 }
 
@@ -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);
   }
 }
@@ -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());
@@ -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,

extensions/rcs_fr3/src/hw/Franka.h

@@ -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;
@@ -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 {};
@@ -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;
@@ -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);