style: format

Author: juelg

extensions/rcs_fr3/src/hw/Franka.cpp

@@ -146,8 +146,8 @@ void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,
   S_inv.setZero();
   double damping = damping_factor * damping_factor;
   for (int i = 0; i < singular_vals.size(); i++) {
-    S_inv(i, i) = singular_vals(i) /
-                  (singular_vals(i) * singular_vals(i) + damping);
+    S_inv(i, i) =
+        singular_vals(i) / (singular_vals(i) * singular_vals(i) + damping);
   }
   M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose());
 }
@@ -193,15 +193,14 @@ void Franka::controller_set_joint_position(const common::Vector7d &desired_q) {
       throw std::runtime_error(
           "Desired joint configuration is too far from current configuration. "
           "Distance: " +
-          std::to_string(joint_dist) +
-          " rad, Limit: " + std::to_string(this->cfg.max_joint_dist.value()) +
-          " rad.");
+          std::to_string(joint_dist) + " rad, Limit: " +
+          std::to_string(this->cfg.max_joint_dist.value()) + " rad.");
     }
   }
 
-  this->joint_interpolator.reset(
-      this->controller_time, current_q, desired_q, policy_rate, traj_rate,
-      traj_interpolation_time_fraction);
+  this->joint_interpolator.reset(this->controller_time, current_q, desired_q,
+                                 policy_rate, traj_rate,
+                                 traj_interpolation_time_fraction);
 
   // if not thread is running, then start
   if (this->running_controller == Controller::none) {
@@ -237,18 +236,18 @@ void Franka::osc_set_cartesian_position(
 
   // Safety check for distance
   if (this->cfg.max_cartesian_pos_dist.has_value()) {
-    double dist = (curr_pose.translation() -
-                   desired_pose_EE_in_base_frame.translation())
-                      .norm();
+    double dist =
+        (curr_pose.translation() - desired_pose_EE_in_base_frame.translation())
+            .norm();
     if (dist > this->cfg.max_cartesian_pos_dist.value()) {
       if (this->running_controller == Controller::osc) {
         this->interpolator_mutex.unlock();
       }
       throw std::runtime_error(
-          "Desired cartesian position is too far from current position (dist: " +
-          std::to_string(dist) +
-          "m, max: " + std::to_string(cfg.max_cartesian_pos_dist.value()) +
-          "m)");
+          "Desired cartesian position is too far from current position "
+          "(dist: " +
+          std::to_string(dist) + "m, max: " +
+          std::to_string(cfg.max_cartesian_pos_dist.value()) + "m)");
     }
   }
   // Safety check for orientation
@@ -263,9 +262,8 @@ void Franka::osc_set_cartesian_position(
       throw std::runtime_error(
           "Desired cartesian orientation is too far from current orientation "
           "(dist: " +
-          std::to_string(angle) +
-          "rad, max: " + std::to_string(cfg.max_cartesian_ori_dist.value()) +
-          "rad)");
+          std::to_string(angle) + "rad, max: " +
+          std::to_string(cfg.max_cartesian_ori_dist.value()) + "rad)");
     }
   }
 

extensions/rcs_fr3/src/hw/Franka.h

@@ -46,7 +46,8 @@ struct FrankaConfig : common::RobotConfig {
   // Safety limits for rejecting distant targets
   std::optional<double> max_cartesian_pos_dist = 0.3;   // in meters
   std::optional<double> max_cartesian_ori_dist = 1.57;  // in radians (90 deg)
-  std::optional<double> max_joint_dist = 1.5;  // norm of joint vector in radians
+  std::optional<double> max_joint_dist =
+      1.5;  // norm of joint vector in radians
 };
 
 struct FR3Config : FrankaConfig {};

extensions/rcs_fr3/src/pybind/rcs.cpp

@@ -67,8 +67,10 @@ PYBIND11_MODULE(_core, m) {
                      &rcs::hw::FrankaConfig::nominal_end_effector_frame)
       .def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot)
       .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control)
-      .def_readwrite("max_cartesian_pos_dist", &rcs::hw::FrankaConfig::max_cartesian_pos_dist)
-      .def_readwrite("max_cartesian_ori_dist", &rcs::hw::FrankaConfig::max_cartesian_ori_dist)
+      .def_readwrite("max_cartesian_pos_dist",
+                     &rcs::hw::FrankaConfig::max_cartesian_pos_dist)
+      .def_readwrite("max_cartesian_ori_dist",
+                     &rcs::hw::FrankaConfig::max_cartesian_ori_dist)
       .def_readwrite("max_joint_dist", &rcs::hw::FrankaConfig::max_joint_dist);
 
   py::class_<rcs::hw::FR3Config, rcs::hw::FrankaConfig>(hw, "FR3Config")