Revert and cleanup

Author: evan-palmer

.dockerignore

@@ -3,15 +3,14 @@
 *
 
 # Except the following
-
+!auv_control_demos
+!auv_control_msgs
 !auv_controllers
-!velocity_controllers
+!controller_common
+!ik_solvers
 !thruster_allocation_matrix_controller
 !thruster_controllers
-!auv_control_msgs
-!auv_control_demos
-!whole_body_controllers
-!ik_solvers
-!controller_common
 !topic_sensors
+!velocity_controllers
+!whole_body_controllers
 !ros2.repos

auv_controllers/package.xml

@@ -16,14 +16,14 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <exec_depend>thruster_allocation_matrix_controller</exec_depend>
-  <exec_depend>thruster_controllers</exec_depend>
-  <exec_depend>velocity_controllers</exec_depend>
   <exec_depend>auv_control_msgs</exec_depend>
   <exec_depend>controller_common</exec_depend>
+  <exec_depend>ik_solvers</exec_depend>
+  <exec_depend>thruster_allocation_matrix_controller</exec_depend>
+  <exec_depend>thruster_controllers</exec_depend>
   <exec_depend>topic_sensors</exec_depend>
+  <exec_depend>velocity_controllers</exec_depend>
   <exec_depend>whole_body_controllers</exec_depend>
-  <exec_depend>ik_solvers</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

ik_solvers/COLCON_IGNORE

@@ -47,7 +47,7 @@ class Constraint
   : primal_(primal),
     constraint_(constraint),
     priority_(priority),
-    gain_(gain){};
+    gain_(gain) {};
 
   /// Destructor.
   virtual ~Constraint() = default;

ik_solvers/include/ik_solvers/task_priority_solver.hpp

@@ -172,7 +172,6 @@ namespace
 /// Compute the nullspace of the Jacobian matrix using the pseudoinverse.
 auto compute_nullspace(const Eigen::MatrixXd & augmented_jacobian) -> Eigen::MatrixXd
 {
-  // TODO(evan-palmer): do we need the damping here?
   const Eigen::MatrixXd eye = Eigen::MatrixXd::Identity(augmented_jacobian.cols(), augmented_jacobian.cols());
   return eye - (augmented_jacobian.completeOrthogonalDecomposition().pseudoInverse() * augmented_jacobian);
 }
@@ -189,7 +188,7 @@ auto construct_augmented_jacobian(const std::vector<Eigen::MatrixXd> & jacobians
     jacobians.begin(), jacobians.end(), 0, [](int sum, const Eigen::MatrixXd & jac) { return sum + jac.rows(); });
 
   Eigen::MatrixXd augmented_jacobian(n_rows, n_cols);
-  int current_row = 0;
+  int current_row = 0;  // NOLINT(misc-const-correctness)
 
   for (const auto & jac : jacobians) {
     augmented_jacobian.block(current_row, 0, jac.rows(), jac.cols()) = jac;
@@ -228,6 +227,21 @@ auto tpik(const hierarchy::ConstraintSet & tasks, size_t nv, double damping)
   return vel;
 }
 
+/// Check if the solution is feasible with respect to the set constraints.
+auto is_feasible(const hierarchy::ConstraintSet & constraints, const Eigen::VectorXd & solution) -> bool
+{
+  for (const auto & constraint : constraints) {
+    auto set_task = std::dynamic_pointer_cast<hierarchy::SetConstraint>(constraint);
+    const double pred = (constraint->jacobian() * solution).value();
+
+    if (!((set_task->primal() > set_task->lower_threshold() && pred < 0) ||
+          (set_task->primal() < set_task->upper_threshold() && pred > 0))) {
+      return false;
+    }
+  }
+  return true;
+}
+
 /// Search for the feasible solutions to the task hierarchy and return the one with the smallest norm.
 auto search_solutions(
   const hierarchy::ConstraintSet & set_tasks,
@@ -256,23 +270,9 @@ auto search_solutions(
       if (!out.has_value()) {
         continue;
       }
-      const Eigen::VectorXd & current_solution = out.value();
 
-      // Check if the solution violates any set constraints
-      bool valid = true;
-      for (const auto & task : set_tasks) {
-        auto set_task = std::dynamic_pointer_cast<hierarchy::SetConstraint>(task);
-        const double pred = (task->jacobian() * current_solution).value();
-
-        valid = (set_task->primal() > set_task->lower_threshold() && pred < 0) ||
-                (set_task->primal() < set_task->upper_threshold() && pred > 0);
-
-        if (!valid) {
-          break;
-        }
-      }
-
-      if (valid) {
+      const Eigen::VectorXd & current_solution = out.value();
+      if (is_feasible(set_tasks, current_solution)) {
         solutions.push_back(current_solution);
       }
     }
@@ -309,6 +309,7 @@ auto TaskPriorityIKSolver::initialize(
   params_ = param_listener_->get_params();
 }
 
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
 auto TaskPriorityIKSolver::update_parameters() -> void
 {
   if (!param_listener_->is_old(params_)) {

ik_solvers/src/task_priority_solver.cpp

@@ -9,23 +9,21 @@ include(GNUInstallDirs)
 
 # dependencies
 set(THIS_PACKAGE_INCLUDE_DEPENDS
-    controller_common
     controller_interface
-    control_msgs
-    Eigen3
-    eigen3_cmake_module
-    geometry_msgs
     hardware_interface
-    message_transforms
-    nav_msgs
-    pinocchio
     rclcpp
     rclcpp_lifecycle
     realtime_tools
+    nav_msgs
     std_msgs
+    control_msgs
+    pinocchio
+    geometry_msgs
     tf2_ros
     tf2_eigen
-    trajectory_msgs
+    message_transforms
+    controller_common
+    ik_solvers
 )
 
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
@@ -40,45 +38,6 @@ generate_parameter_library(ik_controller_parameters
   src/ik_controller_parameters.yaml
 )
 
-generate_parameter_library(task_priority_solver_parameters
-  src/ik_solvers/task_priority_solver_parameters.yaml
-)
-
-# ik base solver library
-# this needs to be independent of the plugin library
-add_library(solver_base SHARED)
-target_sources(
-    solver_base
-    PRIVATE src/ik_solvers/solver.cpp
-    PUBLIC
-        FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/whole_body_controllers/ik_solvers/solver.hpp
-)
-ament_target_dependencies(solver_base PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_compile_features(solver_base PUBLIC cxx_std_23)
-
-# ik solver plugin library
-add_library(ik_solvers SHARED)
-target_sources(
-    ik_solvers
-    PRIVATE src/ik_solvers/task_priority_solver.cpp
-    PUBLIC
-        FILE_SET HEADERS
-        BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
-        FILES
-            ${CMAKE_CURRENT_SOURCE_DIR}/include/whole_body_controllers/ik_solvers/task_priority_solver.hpp
-)
-ament_target_dependencies(ik_solvers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
-target_compile_features(ik_solvers PUBLIC cxx_std_23)
-target_link_libraries(
-    ik_solvers
-    PUBLIC solver_base task_priority_solver_parameters
-)
-
-# the first argument is the name of the package, NOT the target name
-pluginlib_export_plugin_description_file(ik_solvers solvers.xml)
-
 add_library(whole_body_controllers SHARED)
 target_sources(
     whole_body_controllers
@@ -91,11 +50,11 @@ target_sources(
 )
 ament_target_dependencies(whole_body_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 target_compile_features(whole_body_controllers PUBLIC cxx_std_23)
-target_link_libraries(whole_body_controllers PUBLIC ik_controller_parameters ik_solvers)
+target_link_libraries(whole_body_controllers PUBLIC ik_controller_parameters)
 pluginlib_export_plugin_description_file(controller_interface whole_body_controllers.xml)
 
 install(
-    TARGETS whole_body_controllers ik_controller_parameters solver_base ik_solvers task_priority_solver_parameters
+    TARGETS whole_body_controllers ik_controller_parameters
     EXPORT export_whole_body_controllers
     RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
     LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}