Remove unused code

Author: destogl

admittance_controller/include/admittance_controller/admittance_rule_impl.hpp

@@ -403,34 +403,6 @@ void AdmittanceRule::process_wrench_measurements(
   measured_wrench_.wrench = measured_wrench;
   filter_chain_->update(measured_wrench_, measured_wrench_filtered_);
 
-  // // get current states, and transform those into controller frame
-  // measured_wrench_.wrench = measured_wrench;
-  // try {
-  //   auto transform_to_world = tf_buffer_->lookupTransform(fixed_world_frame_,  measured_wrench_.header.frame_id, tf2::TimePointZero);
-  //   auto transform_to_sensor = tf_buffer_->lookupTransform(measured_wrench_.header.frame_id, fixed_world_frame_, tf2::TimePointZero);
-
-  //   geometry_msgs::msg::WrenchStamped measured_wrench_transformed;
-  //   tf2::doTransform(measured_wrench_, measured_wrench_transformed, transform_to_world);
-
-  //   geometry_msgs::msg::Vector3Stamped cog_transformed;
-  //   for (const auto & params : gravity_compensation_params_) {
-  //     auto transform_cog = tf_buffer_->lookupTransform(fixed_world_frame_,  params.cog_.header.frame_id, tf2::TimePointZero);
-  //     tf2::doTransform(params.cog_, cog_transformed, transform_cog);
-
-  //     measured_wrench_transformed.wrench.force.z += params.force_;
-  //     measured_wrench_transformed.wrench.torque.x += (params.force_ * cog_transformed.vector.y);
-  //     measured_wrench_transformed.wrench.torque.y -= (params.force_ * cog_transformed.vector.x);
-  //   }
-
-  //   tf2::doTransform(measured_wrench_transformed, measured_wrench_filtered_, transform_to_sensor);
-
-  // } catch (const tf2::TransformException & e) {
-  //   // TODO(destogl): Use RCLCPP_ERROR_THROTTLE
-  //   RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + fixed_world_frame_ + "' and '" + measured_wrench_.header.frame_id + "' or '<a cog frame>'.");
-  //   // If transform error just use measured force
-  //   measured_wrench_filtered_ = measured_wrench_;
-  // }
-
   // TODO(andyz): This is not flexible to work in other control frames besides ik_base
   transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_ik_base_frame_);
   convert_message_to_array(measured_wrench_ik_base_frame_, measured_wrench_ik_base_frame_arr_);

admittance_controller/src/admittance_controller.cpp

@@ -95,21 +95,6 @@ controller_interface::return_type AdmittanceController::init(const std::string &
     get_node()->declare_parameter<double>("admittance.stiffness.ry", std::numeric_limits<double>::quiet_NaN());
     get_node()->declare_parameter<double>("admittance.stiffness.rz", std::numeric_limits<double>::quiet_NaN());
 
-    // // TODO(destogl): use filters here to make it more flexible
-    // // declare and read Gravity compensation parameters
-    // for (const auto & name : std::vector<std::string>{"wrist", "tool"}) {
-    //   get_node()->declare_parameter<double>("gravity_compensation." + name + ".CoG_x",
-    //                                   std::numeric_limits<double>::quiet_NaN());
-    //   get_node()->declare_parameter<double>("gravity_compensation." + name + ".CoG_y",
-    //                                   std::numeric_limits<double>::quiet_NaN());
-    //   get_node()->declare_parameter<double>("gravity_compensation." + name + ".CoG_z",
-    //                                   std::numeric_limits<double>::quiet_NaN());
-    //   get_node()->declare_parameter<double>("gravity_compensation." + name + ".force",
-    //                                   std::numeric_limits<double>::quiet_NaN());
-    //   get_node()->declare_parameter<std::string>("gravity_compensation." + name + ".frame_id", "");
-    // }
-
-//     get_node()->declare_parameter<std::vector<double>>("")
   } catch (const std::exception & e) {
     fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
     return controller_interface::return_type::ERROR;