✨ ROS parameters (TAMS-Group#8)

Signed-off-by: Tyler Weaver <tylerjw@gmail.com>

CMakeLists.txt

@@ -42,12 +42,14 @@ include(cmake/StaticAnalyzers.cmake)
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
+find_package(backward_ros REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(fmt REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(range-v3 REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(backward_ros REQUIRED)
 
 # TODO(tylerjw): re-enable these
 # find_package(OpenMP)
@@ -79,11 +81,12 @@ find_package(backward_ros REQUIRED)
 
 set(SOURCES
     src/goal_types.cpp
-    src/problem.cpp
-    src/ik_test.cpp
-    src/ik_gradient.cpp
     src/ik_evolution_1.cpp
     src/ik_evolution_2.cpp
+    src/ik_gradient.cpp
+    src/ik_test.cpp
+    src/parameters.cpp
+    src/problem.cpp
 )
 
 # TODO(tylerjw): re-enable this
@@ -107,12 +110,13 @@ target_link_libraries(
     project_options
     project_warnings
   PUBLIC
-    rclcpp::rclcpp
-    moveit_core::moveit_robot_model
-    moveit_core::moveit_kinematics_base
+    Eigen3::Eigen
     moveit_core::moveit_collision_detection_fcl
+    moveit_core::moveit_kinematics_base
+    moveit_core::moveit_robot_model
     moveit_ros_planning::moveit_rdf_loader
-    Eigen3::Eigen
+    range-v3::range-v3
+    rclcpp::rclcpp
     # TODO(tylerjw): what does this change?
     -static-libgcc
     -static-libstdc++

include/bio_ik/ik_base.hpp

@@ -138,7 +138,7 @@ struct IKSolver : Random {
   virtual void setParams([[maybe_unused]] const IKParams&) {}
 
   IKSolver(const IKParams& params)
-      : Random(static_cast<long unsigned int>(params.random_seed)),
+      : Random(static_cast<long unsigned int>(params.ros_params.random_seed)),
         model_(params.robot_model),
         modelInfo_(params.robot_model),
         params_(params) {

include/bio_ik/ik_evolution_1.hpp

@@ -28,6 +28,8 @@
 
 #include <memory>
 #include <optional>
+#include <set>
+#include <string>
 
 #include "bio_ik/ik_base.hpp"  // for IKSolver
 
@@ -36,4 +38,6 @@ namespace bio_ik {
 std::optional<std::unique_ptr<IKSolver>> makeEvolution1Solver(
     const IKParams& params);
 
+std::set<std::string> getEvolution1ModeSet();
+
 }  // namespace bio_ik

include/bio_ik/ik_evolution_2.hpp

@@ -28,6 +28,8 @@
 
 #include <memory>
 #include <optional>
+#include <set>
+#include <string>
 
 #include "bio_ik/ik_base.hpp"  // for IKSolver
 
@@ -36,4 +38,6 @@ namespace bio_ik {
 std::optional<std::unique_ptr<IKSolver>> makeEvolution2Solver(
     const IKParams& params);
 
+std::set<std::string> getEvolution2ModeSet();
+
 }  // namespace bio_ik

include/bio_ik/ik_gradient.hpp

@@ -36,6 +36,8 @@
 #include <kdl/frames.hpp>      // for Twist, Vector
 #include <memory>
 #include <optional>
+#include <set>
+#include <string>
 #include <vector>  // for vector, allocator
 
 #include "bio_ik/frame.hpp"       // for Frame, frameTwist
@@ -104,5 +106,6 @@ struct IKJacobian : IKJacobianBase<IKSolver> {
 
 std::optional<std::unique_ptr<IKSolver>> makeGradientDecentSolver(
     const IKParams& params);
+std::set<std::string> getGradientDecentModeSet();
 
 }  // namespace bio_ik

include/bio_ik/ik_test.hpp

@@ -28,11 +28,14 @@
 
 #include <memory>
 #include <optional>
+#include <set>
+#include <string>
 
 #include "bio_ik/ik_base.hpp"  // for IKSolver
 
 namespace bio_ik {
 
 std::optional<std::unique_ptr<IKSolver>> makeTestSolver(const IKParams& params);
+std::set<std::string> getTestModeSet();
 
 }  // namespace bio_ik

include/bio_ik/parameters.hpp

@@ -0,0 +1,83 @@
+// Copyright (c) 2022, Tyler Weaver
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Universität Hamburg nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <cfloat>
+#include <limits>
+#include <optional>
+#include <random>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+
+namespace bio_ik {
+
+/**
+ * @brief      Parameters settable via ros.
+ */
+struct RosParameters {
+  // Plugin parameters
+  bool enable_profiler = false;
+
+  // IKParallel parameters
+  std::string mode = "bio2_memetic";
+  bool enable_counter = false;
+  int64_t random_seed = static_cast<int64_t>(std::random_device()());
+
+  // Problem parameters
+  double dpos = DBL_MAX;
+  double drot = DBL_MAX;
+  double dtwist = 1e-5;
+
+  // ik_evolution_1 parameters
+  bool skip_wipeout = false;
+  size_t population_size = 8;
+  size_t elite_count = 4;
+  bool enable_linear_fitness = false;
+};
+
+/**
+ * @brief      Validates a ros_params struct
+ *
+ * @param[in]  ros_params  The ros parameters struct
+ *
+ * @return     error string if invalid, nullopt if valid
+ */
+std::optional<std::string> validate(const RosParameters& ros_params);
+
+/**
+ * @brief      Gets the ros parameters
+ *
+ * @param[in]  node  The ros node
+ *
+ * @return     The ros parameters on success, nullopt otherwise
+ */
+std::optional<RosParameters> get_ros_parameters(
+    const rclcpp::Node::SharedPtr& node);
+
+}  // namespace bio_ik

include/bio_ik/utils.hpp

@@ -41,36 +41,14 @@
 #include <unordered_map>
 #include <unordered_set>
 
-// #include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
-
-// #include <XmlRpcException.h>
-
-//#include <link.h>
-
-//#include <boost/align/aligned_allocator.hpp>
-//#include <Eigen/Eigen>
+#include "bio_ik/parameters.hpp"
 
 namespace bio_ik {
 
 struct IKParams {
   moveit::core::RobotModelConstPtr robot_model;
   const moveit::core::JointModelGroup* joint_model_group;
-
-  // IKParallel parameters
-  std::string solver_class_name;
-  bool enable_counter;
-  int random_seed;
-
-  // Problem parameters
-  double dpos;
-  double drot;
-  double dtwist;
-
-  // ik_evolution_1 parameters
-  bool opt_no_wipeout;
-  int population_size;
-  int elite_count;
-  bool linear_fitness;
+  RosParameters ros_params;
 };
 
 // Uncomment to enable logging

package.xml

@@ -15,13 +15,15 @@
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
 
+  <depend>backward_ros</depend>
   <depend>eigen</depend>
+  <depend>fmt</depend>
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning</depend>
   <depend>pluginlib</depend>
+  <depend>range-v3</depend>
   <depend>rclcpp</depend>
   <depend>tf2_kdl</depend>
-  <depend>backward_ros</depend>
 
   <!-- <depend>tf2</depend> -->
   <!-- <depend>tf2_eigen</depend> -->

src/ik_evolution_1.cpp

@@ -143,15 +143,15 @@ struct IKEvolution1 : IKSolver {
   std::vector<Individual> tempOffspring_;
   std::vector<double> initialGuess_;
 
-  bool opt_no_wipeout_;
+  bool skip_wipeout_;
 
-  bool linear_fitness_;
+  bool enable_linear_fitness_;
 
   void setParams(const IKParams& params) {
-    opt_no_wipeout_ = params.opt_no_wipeout;
-    populationSize_ = static_cast<size_t>(params.population_size);
-    eliteCount_ = static_cast<size_t>(params.elite_count);
-    linear_fitness_ = params.linear_fitness;
+    skip_wipeout_ = params.ros_params.skip_wipeout;
+    populationSize_ = static_cast<size_t>(params.ros_params.population_size);
+    eliteCount_ = static_cast<size_t>(params.ros_params.elite_count);
+    enable_linear_fitness_ = params.ros_params.enable_linear_fitness;
   }
 
   bool in_final_adjustment_loop_;
@@ -268,7 +268,7 @@ struct IKEvolution1 : IKSolver {
   }
 
   double computeFitness(const std::vector<double>& genes, bool balanced) {
-    if (linear_fitness_) {
+    if (enable_linear_fitness_) {
       model_.applyConfiguration(genes);
       double fitness_sum = 0.0;
       for (size_t goal_index = 0; goal_index < problem_.goals.size();
@@ -548,7 +548,7 @@ struct IKEvolution1 : IKSolver {
     computeExtinctions();
 
     if (tryUpdateSolution()) return true;
-    if (opt_no_wipeout_) return false;
+    if (skip_wipeout_) return false;
     if (!checkWipeout()) return false;
 
     init();
@@ -566,11 +566,13 @@ struct IKEvolution1 : IKSolver {
 
 std::optional<std::unique_ptr<IKSolver>> makeEvolution1Solver(
     const IKParams& params) {
-  const auto& name = params.solver_class_name;
+  const auto& name = params.ros_params.mode;
   if (name == "bio1")
     return std::make_unique<IKEvolution1>(params);
   else
     return std::nullopt;
 }
 
+std::set<std::string> getEvolution1ModeSet() { return {"bio1"}; }
+
 }  // namespace bio_ik

src/ik_evolution_2.cpp

@@ -39,8 +39,10 @@
 #include <bio_ik/problem.hpp>             // for Problem
 #include <bio_ik/utils.hpp>               // for aligned_vector, BLOCKPRO...
 #include <memory>                         // for allocator, __shared_ptr_...
-#include <utility>                        // for swap
-#include <vector>                         // for vector, vector::size_type
+#include <set>
+#include <string>
+#include <utility>  // for swap
+#include <vector>   // for vector, vector::size_type
 
 #include "bio_ik/frame.hpp"       // for Frame, normalizeFast
 #include "bio_ik/robot_info.hpp"  // for RobotInfo
@@ -649,7 +651,7 @@ struct IKEvolution2 : IKSolver {
 
 std::optional<std::unique_ptr<IKSolver>> makeEvolution2Solver(
     const IKParams& params) {
-  const auto& name = params.solver_class_name;
+  const auto& name = params.ros_params.mode;
   if (name == "bio2")
     return std::make_unique<IKEvolution2<0>>(params);
   else if (name == "bio2_memetic")
@@ -660,4 +662,8 @@ std::optional<std::unique_ptr<IKSolver>> makeEvolution2Solver(
     return std::nullopt;
 }
 
+std::set<std::string> getEvolution2ModeSet() {
+  return {"bio2", "bio2_memetic", "bio2_memetic_l"};
+}
+
 }  // namespace bio_ik

src/ik_gradient.cpp

@@ -217,7 +217,7 @@ void IKJacobian<N_THREADS>::initialize(const Problem& problem) {
 
 std::optional<std::unique_ptr<IKSolver>> makeGradientDecentSolver(
     const IKParams& params) {
-  const auto& name = params.solver_class_name;
+  const auto& name = params.ros_params.mode;
   if (name == "gd")
     return std::make_unique<IKGradientDescent<' ', 1>>(params);
   else if (name == "gd_2")
@@ -253,4 +253,13 @@ std::optional<std::unique_ptr<IKSolver>> makeGradientDecentSolver(
   else
     return std::nullopt;
 }
+
+std::set<std::string> getGradientDecentModeSet() {
+  return {
+      "gd",     "gd_2",   "gd_4",  "gd_8",   "gd_r",   "gd_r_2",
+      "gd_r_4", "gd_r_8", "gd_c",  "gd_c_2", "gd_c_4", "gd_c_8",
+      "jac",    "jac_2",  "jac_4", "jac_8",
+  };
+}
+
 }  // namespace bio_ik

src/ik_parallel.hpp

@@ -114,13 +114,13 @@ struct IKParallel {
   double best_fitness_;
 
   IKParallel(const IKParams& params)
-      : params_(params), enable_counter_(params.enable_counter) {
+      : params_(params), enable_counter_(params.ros_params.enable_counter) {
     // create solvers_
     auto create_solver = [&params]() {
       auto result = makeSolver(params);
       if (!result) {
         throw std::runtime_error("Invalid Solver Name: " +
-                                 params.solver_class_name);
+                                 params.ros_params.mode);
       }
       return std::move(*result);
     };