Abstract variable bounds functions into variable, fix memetic initial…

…ization bug

include/pick_ik/robot.hpp

@@ -13,17 +13,27 @@ namespace pick_ik {
 
 struct Robot {
     struct Variable {
-        /// @brief Min and max position values of the variable.
-        double min, max;
+        /// @brief Min, max, and middle position values of the variable.
+        double min, max, mid;
 
         /// @brief Whether the variable's position is bounded.
         bool bounded;
 
-        /// @brief The span (min - max) of the variable, or a default value if unbounded.
-        double span;
+        /// @brief The half-span (min - max) / 2.0 of the variable, or a default value if unbounded.
+        double half_span;
 
         double max_velocity_rcp;
         double minimal_displacement_factor;
+
+        /// @brief Generates a valid variable value given an optional initial value (for unbounded
+        /// joints).
+        auto generate_valid_value(double init_val = 0.0) const -> double;
+
+        /// @brief Returns true if a value is valid given the variable bounds.
+        auto is_valid(double val) const -> bool;
+
+        /// @brief Clamps a configuration to joint limits.
+        auto clamp_to_limits(double val) const -> double;
     };
     std::vector<Variable> variables;
 
@@ -32,7 +42,10 @@ struct Robot {
                      moveit::core::JointModelGroup const* jmg,
                      std::vector<size_t> tip_link_indices) -> Robot;
 
-    /** @brief Sets a variable vector to a random valid configuration. */
+    /**
+     * @brief Sets a variable vector to a random configuration.
+     * @details Here, "valid" denotes that the joint values are with their specified limits.
+     */
     auto set_random_valid_configuration(std::vector<double>& config) const -> void;
 
     /** @brief Check is a configuration is valid. */

src/goal.cpp

@@ -119,10 +119,10 @@ auto make_avoid_joint_limits_cost_fn(Robot robot) -> CostFn {
 
             auto const position = active_positions[i];
             auto const weight = variable.minimal_displacement_factor;
-            auto const mid = (variable.min + variable.max) * 0.5;
-            auto const span = variable.span;
-            sum +=
-                std::pow(std::fmax(0.0, std::fabs(position - mid) * 2.0 - span * 0.5) * weight, 2);
+            sum += std::pow(
+                std::fmax(0.0, std::fabs(position - variable.mid) * 2.0 - variable.half_span) *
+                    weight,
+                2);
         }
         return sum;
     };

src/ik_gradient.cpp

@@ -77,13 +77,7 @@ auto step(GradientIk& self, Robot const& robot, CostFn const& cost_fn, double st
     for (size_t i = 0; i < count; ++i) {
         auto const& var = robot.variables[i];
         auto updated_value = self.local[i] - self.gradient[i] * joint_diff;
-        if (var.bounded) {
-            self.working[i] = std::clamp(updated_value, var.min, var.max);
-        } else {
-            self.working[i] = std::clamp(updated_value,
-                                         self.local[i] - var.span / 2.0,
-                                         self.local[i] + var.span / 2.0);
-        }
+        self.working[i] = var.clamp_to_limits(updated_value);
     }
 
     // Always accept the solution and continue

src/ik_memetic.cpp

@@ -108,6 +108,10 @@ void MemeticIk::initPopulation(Robot const& robot,
         population_[i] = Individual{initial_guess, 0.0, 1.0, zero_grad};
     }
 
+    // Initialize fitnesses and extinctions
+    for (auto& individual : population_) {
+        individual.fitness = cost_fn(individual.genes);
+    }
     computeExtinctions();
     previous_fitness_.reset();
 }
@@ -152,17 +156,11 @@ void MemeticIk::reproduce(Robot const& robot, CostFn const& cost_fn) {
 
                 // Mutate
                 if (rsl::uniform_real(0.0, 1.0) < mutation_prob) {
-                    gene += extinction * joint.span * rsl::uniform_real(-0.5, 0.5);
+                    gene += extinction * joint.half_span * rsl::uniform_real(-1.0, 1.0);
                 }
 
                 // Clamp to valid joint values
-                if (joint.bounded) {
-                    gene = std::clamp(gene, joint.min, joint.max);
-                } else {
-                    gene = std::clamp(gene,
-                                      original_gene - joint.span / 2.0,
-                                      original_gene + joint.span / 2.0);
-                }
+                gene = robot.variables[j_idx].clamp_to_limits(gene);
 
                 // Approximate gradient
                 population_[i].gradient[j_idx] = gene - original_gene;

src/robot.cpp

@@ -14,12 +14,33 @@
 #include <moveit/robot_state/robot_state.h>
 
 namespace {
-constexpr double kUnboundedVariableSpan = 2.0 * M_PI;
+constexpr double kUnboundedVariableHalfSpan = M_PI;
 constexpr double kUnboundedJointSampleSpread = M_PI;
 }  // namespace
 
 namespace pick_ik {
 
+auto Robot::Variable::generate_valid_value(double init_val /* = 0.0*/) const -> double {
+    if (bounded) {
+        return rsl::uniform_real(min, max);
+    } else {
+        return rsl::uniform_real(init_val - kUnboundedJointSampleSpread,
+                                 init_val + kUnboundedJointSampleSpread);
+    }
+}
+
+auto Robot::Variable::is_valid(double val) const -> bool {
+    return (!bounded) || (val <= max && val >= min);
+}
+
+auto Robot::Variable::clamp_to_limits(double val) const -> double {
+    if (bounded) {
+        return std::clamp(val, min, max);
+    } else {
+        return std::clamp(val, val - half_span, val + half_span);
+    }
+}
+
 auto Robot::from(std::shared_ptr<moveit::core::RobotModel const> const& model,
                  moveit::core::JointModelGroup const* jmg,
                  std::vector<size_t> tip_link_indices) -> Robot {
@@ -41,8 +62,8 @@ auto Robot::from(std::shared_ptr<moveit::core::RobotModel const> const& model,
         var.bounded = bounds.position_bounded_;
         var.min = bounds.min_position_;
         var.max = bounds.max_position_;
-
-        var.span = var.bounded ? var.max - var.min : kUnboundedVariableSpan;
+        var.mid = 0.5 * (var.min + var.max);
+        var.half_span = var.bounded ? (var.max - var.min) / 2.0 : kUnboundedVariableHalfSpan;
 
         auto const max_velocity = bounds.max_velocity_;
         var.max_velocity_rcp = max_velocity > 0.0 ? 1.0 / max_velocity : 0.0;
@@ -69,24 +90,14 @@ auto Robot::set_random_valid_configuration(std::vector<double>& config) const ->
         config.resize(num_vars);
     }
     for (size_t idx = 0; idx < num_vars; ++idx) {
-        auto const& var = variables[idx];
-        if (var.bounded) {
-            config[idx] = rsl::uniform_real(var.min, var.max);
-        } else {
-            config[idx] = rsl::uniform_real(config[idx] - kUnboundedJointSampleSpread,
-                                            config[idx] + kUnboundedJointSampleSpread);
-        }
+        config[idx] = variables[idx].generate_valid_value(config[idx]);
     }
 }
 
 auto Robot::is_valid_configuration(std::vector<double> const& config) const -> bool {
     auto const num_vars = variables.size();
     for (size_t idx = 0; idx < num_vars; ++idx) {
-        auto const var = variables[idx];
-        if (!var.bounded) {
-            continue;
-        }
-        if (config[idx] > var.max || config[idx] < var.min) {
+        if (!variables[idx].is_valid(config[idx])) {
             return false;
         }
     }