Program Listing for File ik_memetic.hpp
↰ Return to documentation for file (/tmp/ws/src/pick_ik/include/pick_ik/ik_memetic.hpp
)
#pragma once
#include <pick_ik/goal.hpp>
#include <pick_ik/ik_gradient.hpp>
#include <pick_ik/robot.hpp>
#include <rsl/random.hpp>
#include <atomic>
#include <chrono>
#include <memory>
#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_model/robot_model.h>
#include <optional>
#include <vector>
namespace pick_ik {
struct Individual {
std::vector<double> genes; // Joint angles
double fitness;
double extinction;
std::vector<double> gradient;
};
struct MemeticIkParams {
// Evolutionary algorithm parameters
size_t elite_size = 4; // Number of "keep alive" population members.
size_t population_size = 16; // Number of total population members.
double wipeout_fitness_tol = 0.00001; // Min fitness must improve by at least this much or the
// population is reinitialized.
int max_generations = 100; // Maximum iterations for evolutionary algorithm.
double max_time = 1.0; // Maximum time for evolutionary algorithm.
size_t num_threads = 1; // Number of species to solve in parallel.
// If false, keeps running after finding a solution to further optimize the solution until a
// time or iteration limit is reached. If true, stop thread on finding a valid solution.
bool stop_optimization_on_valid_solution = true;
// If true, returns first solution and terminates other threads.
// If false, waits for all threads to join and returns best solution.
bool stop_on_first_soln = true;
// Gradient descent parameters for memetic exploitation.
GradientIkParams gd_params;
};
class MemeticIk {
private:
// Evolutionary algorithm values
std::vector<Individual> population_;
std::vector<Individual*> mating_pool_;
Individual best_; // Best solution overall.
Individual best_curr_; // Best solution so far.
std::optional<double> previous_fitness_;
// Solver parameters
MemeticIkParams params_;
// Scaling coefficients (cached since they do not change).
std::vector<double> extinction_grading_;
double inverse_gene_size_;
public:
MemeticIk(std::vector<double> const& initial_guess, double cost, MemeticIkParams const& params);
static MemeticIk from(std::vector<double> const& initial_guess,
CostFn const& cost_fn,
MemeticIkParams const& params);
Individual best() const { return best_; };
Individual bestCurrent() const { return best_curr_; };
size_t eliteCount() const { return params_.elite_size; };
bool checkWipeout();
void computeExtinctions();
void gradientDescent(size_t const i,
Robot const& robot,
CostFn const& cost_fn,
GradientIkParams const& gd_params);
void initPopulation(Robot const& robot,
CostFn const& cost_fn,
std::vector<double> const& initial_guess);
void reproduce(Robot const& robot, CostFn const& cost_fn);
size_t populationCount() const { return params_.population_size; };
void printPopulation() const;
void sortPopulation();
};
// Implementation of memetic IK solve.
auto ik_memetic_impl(std::vector<double> const& initial_guess,
Robot const& robot,
CostFn const& cost_fn,
SolutionTestFn const& solution_fn,
MemeticIkParams const& params,
std::atomic<bool>& terminate,
bool approx_solution = false,
bool print_debug = false) -> std::optional<Individual>;
// Top-level IK solution implementation that handles single vs. multithreading.
auto ik_memetic(std::vector<double> const& initial_guess,
Robot const& robot,
CostFn const& cost_fn,
SolutionTestFn const& solution_fn,
MemeticIkParams const& params,
bool approx_solution = false,
bool print_debug = false) -> std::optional<std::vector<double>>;
} // namespace pick_ik