Refactoring examples.

franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h

@@ -31,12 +31,16 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn
   void update(const ros::Time&, const ros::Duration& period) override;
 
  private:
-  std::string arm_id_;
-  ros::NodeHandle node_handle_;
+  // Saturation
+  Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+      const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
+      const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
+      const Eigen::Matrix<double, 7, 1>& gravity);
+
   std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
   std::vector<hardware_interface::JointHandle> joint_handles_;
-  std::vector<std::string> joint_names_;
+
   double filter_params_{0.005};
   double nullspace_stiffness_{20.0};
   double nullspace_stiffness_target_{20.0};
@@ -51,12 +55,6 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn
   Eigen::Vector3d position_d_target_;
   Eigen::Quaterniond orientation_d_target_;
 
-  // Saturation
-  Eigen::Matrix<double, 7, 1> saturateTorqueRate(
-      const Eigen::Matrix<double, 7, 1>& tau_d_calculated,
-      const Eigen::Matrix<double, 7, 1>& tau_J_d,  // NOLINT (readability-identifier-naming)
-      const Eigen::Matrix<double, 7, 1>& gravity);
-
   // Dynamic reconfigure
   std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
       dynamic_server_compliance_param_;

franka_example_controllers/include/franka_example_controllers/cartesian_pose_example_controller.h

@@ -27,7 +27,6 @@ class CartesianPoseExampleController
   void update(const ros::Time&, const ros::Duration& period) override;
 
  private:
-  std::string arm_id_;
   franka_hw::FrankaPoseCartesianInterface* cartesian_pose_interface_;
   std::unique_ptr<franka_hw::FrankaCartesianPoseHandle> cartesian_pose_handle_;
   ros::Duration elapsed_time_;

franka_example_controllers/include/franka_example_controllers/cartesian_velocity_example_controller.h

@@ -26,7 +26,6 @@ class CartesianVelocityExampleController : public controller_interface::MultiInt
   void stopping(const ros::Time&) override;
 
  private:
-  std::string arm_id_;
   franka_hw::FrankaVelocityCartesianInterface* velocity_cartesian_interface_;
   std::unique_ptr<franka_hw::FrankaCartesianVelocityHandle> velocity_cartesian_handle_;
   ros::Duration elapsed_time_;

franka_example_controllers/include/franka_example_controllers/joint_impedance_example_controller.h

@@ -30,8 +30,8 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf
   void update(const ros::Time&, const ros::Duration& period) override;
 
  private:
+  // Saturation
   std::array<double, 7> saturateTorqueRate(
-      const double delta_tau_max,
       const std::array<double, 7>& tau_d_calculated,
       const std::array<double, 7>& tau_J_d,  // NOLINT (readability-identifier-naming)
       const std::array<double, 7>& gravity);
@@ -40,6 +40,7 @@ class JointImpedanceExampleController : public controller_interface::MultiInterf
   std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
   std::vector<hardware_interface::JointHandle> joint_handles_;
 
+  const double delta_tau_max_{1.0};
   double radius_{0.1};
   double acceleration_time_{2.0};
   double vel_max_{0.05};

franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h

@@ -25,7 +25,6 @@ class JointPositionExampleController : public controller_interface::MultiInterfa
   void update(const ros::Time&, const ros::Duration& period) override;
 
  private:
-  std::vector<std::string> joint_names_;
   hardware_interface::PositionJointInterface* position_joint_interface_;
   std::vector<hardware_interface::JointHandle> position_joint_handles_;
   ros::Duration elapsed_time_;

franka_example_controllers/include/franka_example_controllers/joint_velocity_example_controller.h

@@ -26,7 +26,6 @@ class JointVelocityExampleController : public controller_interface::MultiInterfa
   void stopping(const ros::Time&) override;
 
  private:
-  std::vector<std::string> joint_names_;
   hardware_interface::VelocityJointInterface* velocity_joint_interface_;
   std::vector<hardware_interface::JointHandle> velocity_joint_handles_;
   ros::Duration elapsed_time_;

franka_example_controllers/src/cartesian_impedance_example_controller.cpp

@@ -17,17 +17,18 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
                                                ros::NodeHandle& node_handle) {
   std::vector<double> cartesian_stiffness_vector;
   std::vector<double> cartesian_damping_vector;
-  node_handle_ = node_handle;
+  std::string arm_id;
+  std::vector<std::string> joint_names;
 
-  sub_equilibrium_pose_ = node_handle_.subscribe(
+  sub_equilibrium_pose_ = node_handle.subscribe(
       "/equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
       ros::TransportHints().reliable().tcpNoDelay());
 
-  if (!node_handle_.getParam("arm_id", arm_id_)) {
+  if (!node_handle.getParam("arm_id", arm_id)) {
     ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
     return false;
   }
-  if (!node_handle_.getParam("joint_names", joint_names_) || joint_names_.size() != 7) {
+  if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
     ROS_ERROR(
         "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
         "aborting "
@@ -44,7 +45,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
   }
   try {
     model_handle_.reset(
-        new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id_ + "_model")));
+        new franka_hw::FrankaModelHandle(model_interface->getHandle(arm_id + "_model")));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "CartesianImpedanceExampleController: Exception getting model handle from interface: "
@@ -61,7 +62,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
   }
   try {
     state_handle_.reset(
-        new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id_ + "_robot")));
+        new franka_hw::FrankaStateHandle(state_interface->getHandle(arm_id + "_robot")));
   } catch (hardware_interface::HardwareInterfaceException& ex) {
     ROS_ERROR_STREAM(
         "CartesianImpedanceExampleController: Exception getting state handle from interface: "
@@ -78,7 +79,7 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
   }
   for (size_t i = 0; i < 7; ++i) {
     try {
-      joint_handles_.push_back(effort_joint_interface->getHandle(joint_names_[i]));
+      joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
     } catch (const hardware_interface::HardwareInterfaceException& ex) {
       ROS_ERROR_STREAM(
           "CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what());

franka_example_controllers/src/cartesian_pose_example_controller.cpp

@@ -18,6 +18,7 @@ namespace franka_example_controllers {
 bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware,
                                           ros::NodeHandle& root_node_handle,
                                           ros::NodeHandle& /* controller_node_handle */) {
+  std::string arm_id;
   cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
   if (cartesian_pose_interface_ == nullptr) {
     ROS_ERROR(
@@ -26,14 +27,14 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
     return false;
   }
 
-  if (!root_node_handle.getParam("arm_id", arm_id_)) {
+  if (!root_node_handle.getParam("arm_id", arm_id)) {
     ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
     return false;
   }
 
   try {
     cartesian_pose_handle_.reset(new franka_hw::FrankaCartesianPoseHandle(
-        cartesian_pose_interface_->getHandle(arm_id_ + "_robot")));
+        cartesian_pose_interface_->getHandle(arm_id + "_robot")));
   } catch (const hardware_interface::HardwareInterfaceException& e) {
     ROS_ERROR_STREAM(
         "CartesianPoseExampleController: Exception getting Cartesian handle: " << e.what());
@@ -47,7 +48,7 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
   }
 
   try {
-    auto state_handle = state_interface->getHandle(arm_id_ + "_robot");
+    auto state_handle = state_interface->getHandle(arm_id + "_robot");
 
     std::array<double, 7> q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
     for (size_t i = 0; i < q_start.size(); i++) {

franka_example_controllers/src/cartesian_velocity_example_controller.cpp

@@ -18,7 +18,8 @@ namespace franka_example_controllers {
 bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware,
                                               ros::NodeHandle& root_node_handle,
                                               ros::NodeHandle& /* controller_node_handle */) {
-  if (!root_node_handle.getParam("arm_id", arm_id_)) {
+  std::string arm_id;
+  if (!root_node_handle.getParam("arm_id", arm_id)) {
     ROS_ERROR("CartesianVelocityExampleController: Could not get parameter arm_id");
     return false;
   }
@@ -33,7 +34,7 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot
   }
   try {
     velocity_cartesian_handle_.reset(new franka_hw::FrankaCartesianVelocityHandle(
-        velocity_cartesian_interface_->getHandle(arm_id_ + "_robot")));
+        velocity_cartesian_interface_->getHandle(arm_id + "_robot")));
   } catch (const hardware_interface::HardwareInterfaceException& e) {
     ROS_ERROR_STREAM(
         "CartesianVelocityExampleController: Exception getting Cartesian handle: " << e.what());
@@ -47,7 +48,7 @@ bool CartesianVelocityExampleController::init(hardware_interface::RobotHW* robot
   }
 
   try {
-    auto state_handle = state_interface->getHandle(arm_id_ + "_robot");
+    auto state_handle = state_interface->getHandle(arm_id + "_robot");
 
     std::array<double, 7> q_start = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
     for (size_t i = 0; i < q_start.size(); i++) {

franka_example_controllers/src/joint_impedance_example_controller.cpp

@@ -174,9 +174,8 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
 
   // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is
   // 1000 * (1 / sampling_time).
-  const double delta_tau_max = 1.0;  // NOLINT (readability-identifier-naming)
   std::array<double, 7> tau_d_saturated =
-      saturateTorqueRate(delta_tau_max, tau_d_calculated, robot_state.tau_J_d, gravity);
+      saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d, gravity);
 
   for (size_t i = 0; i < 7; ++i) {
     joint_handles_[i].setCommand(tau_d_saturated[i]);
@@ -205,7 +204,6 @@ void JointImpedanceExampleController::update(const ros::Time& /*time*/,
 }
 
 std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate(
-    const double delta_tau_max,  // NOLINT (readability-identifier-naming)
     const std::array<double, 7>& tau_d_calculated,
     const std::array<double, 7>& tau_J_d,  // NOLINT (readability-identifier-naming)
     const std::array<double, 7>& gravity) {
@@ -214,7 +212,7 @@ std::array<double, 7> JointImpedanceExampleController::saturateTorqueRate(
     // TODO(sga): After gravity is removed from tau_J_d, do not subtract it any more.
     double difference = tau_d_calculated[i] - (tau_J_d[i] - gravity[i]);
     tau_d_saturated[i] =
-        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max), -delta_tau_max);
+        (tau_J_d[i] - gravity[i]) + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
   }
   return tau_d_saturated;
 }

franka_example_controllers/src/joint_position_example_controller.cpp

@@ -32,11 +32,11 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har
     return false;
   }
   position_joint_handles_.resize(7);
-  joint_names_.resize(7);
+  std::array<std::string, 7> joint_names;
   for (size_t i = 0; i < 7; ++i) {
-    joint_names_[i] = static_cast<std::string>(parameters[i]);
+    joint_names[i] = static_cast<std::string>(parameters[i]);
     try {
-      position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names_[i]);
+      position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names[i]);
     } catch (const hardware_interface::HardwareInterfaceException& e) {
       ROS_ERROR_STREAM(
           "JointPositionExampleController: Exception getting joint handles: " << e.what());

franka_example_controllers/src/joint_velocity_example_controller.cpp

@@ -32,11 +32,11 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
     return false;
   }
   velocity_joint_handles_.resize(7);
-  joint_names_.resize(7);
+  std::array<std::string, 7> joint_names;
   for (size_t i = 0; i < 7; ++i) {
-    joint_names_[i] = static_cast<std::string>(parameters[i]);
+    joint_names[i] = static_cast<std::string>(parameters[i]);
     try {
-      velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names_[i]);
+      velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names[i]);
     } catch (const hardware_interface::HardwareInterfaceException& ex) {
       ROS_ERROR_STREAM(
           "JointVelocityExampleController: Exception getting joint handles: " << ex.what());

franka_example_controllers/src/model_example_controller.cpp

@@ -32,7 +32,6 @@ namespace franka_example_controllers {
 bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
                                   ros::NodeHandle& node_handle) {
   std::string arm_id;
-
   franka_state_interface_ = robot_hw->get<franka_hw::FrankaStateInterface>();
   if (franka_state_interface_ == nullptr) {
     ROS_ERROR("ModelExampleController: Could not get Franka state interface from hardware");