Commit 217c5372 authored by Eivinas Butkus's avatar Eivinas Butkus

init commit

parents
#ifndef ___FIT_HEXA_HPP__
#define ___FIT_HEXA_HPP__
#include <robot_dart/robot_dart_simu.hpp>
#include <robot_dart/control/hexa_control.hpp>
#ifdef GRAPHIC
#include <robot_dart/graphics/camera_osr.hpp>
#include <robot_dart/graphics/graphics.hpp>
#endif
#include <dart/collision/bullet/BulletCollisionDetector.hpp>
#include <dart/constraint/ConstraintSolver.hpp>
#include <ctime>
// amounts to use of each RGB channel when converting to grayscale
float const R = 0.2126;
float const G = 0.7152;
float const B = 0.0722;
namespace global{
std::shared_ptr<robot_dart::Robot> global_robot;
}
typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> Mat;
void load_and_init_robot()
{
std::cout<<"INIT Robot"<<std::endl;
global::global_robot = std::make_shared<robot_dart::Robot>("exp/umap/resources/hexapod_v2.urdf");
global::global_robot->set_position_enforced(true);
global::global_robot->set_actuator_types(dart::dynamics::Joint::SERVO);
global::global_robot->skeleton()->enableSelfCollisionCheck();
std::cout<<"End init Robot"<<std::endl;
}
FIT_QD(Fit_hexa)
{
public:
Fit_hexa() : _entropy(-1) {}
typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor > Mat;
float& entropy() {return _entropy;}
const std::vector<float>& observations() const{
return _image;
}
template<typename block_t>
void get_flat_observations(block_t& data)const
{
// std::cout << _image.size() << std::endl;
for (size_t i = 0; i < _image.size(); i++) {
data(0, i) = _image[i];
}
}
size_t get_flat_obs_size()const{
assert(observations().size());
return observations().size();
}
template<typename Indiv>
void eval(Indiv& ind)
{
_ctrl.resize(36);
for(size_t i=0; i<_ctrl.size(); i++)
{
_ctrl[i] = round( ind.data(i) * 1000.0 ) / 1000.0;// limite numerical issues
}
simulate(_ctrl);
this->_value = -1; // FITNESS: constant because we're interested in exploration
// std::vector<double> desc;
// std::cout << "GTs: " << _gt[0] << " " << _gt[1] << std::endl;
// desc.push_back(_gt[0]);
// desc.push_back(_gt[1]);
// desc.push_back(_traj.back()[1]/2);// this is divided by two, to be sure that the DB is between 0 and 1
//
// this->set_desc(desc);
// if(desc[0]<0 || desc[0]>1 ||desc[1]<0 || desc[1]>1)
// this->_dead=true; //if something is wrong, we kill this solution.
}
void simulate(const std::vector<double> ctrl)
{
auto g_robot=global::global_robot->clone();
g_robot->skeleton()->setPosition(5, 0.15);
float simulation_length = 3;
double ctrl_dt = 0.015;
g_robot->add_controller(std::make_shared<robot_dart::control::HexaControl>(ctrl_dt, ctrl));
std::static_pointer_cast<robot_dart::control::HexaControl>(g_robot->controllers()[0])->set_h_params(std::vector<double>(1, ctrl_dt));
robot_dart::RobotDARTSimu simu(0.03); // changed from 0.001
#ifdef GRAPHIC
auto cam1 = std::make_shared<robot_dart::graphics::CameraOSR>
(simu.world(), Params::image_width*Params::times_downsample,
Params::image_height * Params::times_downsample, false);
simu.add_camera(cam1);
cam1->set_enable(false);
cam1->set_recording(false);
cam1->look_at({0.0, 0.0, 3.5}, {0.0, 0.0, 0.5});
// simu.set_graphics(std::make_shared<robot_dart::graphics::Graphics>(simu.world()));
#endif
simu.world()->getConstraintSolver()->setCollisionDetector(dart::collision::BulletCollisionDetector::create());
simu.add_floor();
simu.add_robot(g_robot);
// don't need this because I am going to take a picture
// simu.add_descriptor(std::make_shared<robot_dart::descriptor::HexaDescriptor>(robot_dart::descriptor::HexaDescriptor(simu)));
simu.run(simulation_length);
// take picture here and copy to private member variable _image
// cam1->set_recording(true); // to be deleted
// cam1->set_filename("exp/camera_experiment/saved_images/test_" + std::to_string(rand() % 10) + ".png");
// cam1->set_enable(true);
cam1->take_single_shot();
// waiting for the thread that's taking the shot
while(!cam1->shot_done())
usleep(100);
auto image_cam1 = cam1->image();
_image = convert_to_vector(image_cam1);
auto final_pos = simu.robots().back()->skeleton()->getPositions().head(6).tail(3).cast <float> ().head(2);
_gt.push_back(final_pos[0]);
_gt.push_back(final_pos[1]);
// getting the rotation angle
Eigen::Matrix3d rot = simu.robots().back()->body_rot("base_link");
float z_angle = atan2(rot(1,0), rot(0,0));
z_angle = abs(z_angle);
z_angle /= 10*M_PI;
// z_angle is the one we need
_gt.push_back(z_angle);
g_robot.reset();
cam1.reset();
// _traj=std::static_pointer_cast<robot_dart::descriptor::HexaDescriptor>(simu.descriptor(0))->traj;
}
void downsample(std::vector<float>& img, int k) const {
// std::cout << "downsample: img size " << img.size() << "k: " << k << std::endl;
int image_height = Params::image_height * k;
int image_width = Params::image_width * k;
Mat image;
image.resize(image_height, image_width);
// std::cout << "image in matrix form resized: " << image.rows() << "x" << image.cols() << std::endl;
size_t count = 0;
for (int i = 0; i < image_height; i++) {
for (int j = 0; j < image_width; j++) {
image(i,j) = img[count++];
}
}
img.clear();
for (int i = 0; i < image_height; i += 2) {
for (int j = 0; j < image_width; j += 2) {
img.push_back((image(i,j) + image(i+1,j) + image(i,j+1) + image(i+1,j+1))/4);
}
}
}
// converts RGB OSG Image to grayscale vector of floats (0 to 1)
std::vector<float> convert_to_vector(const osg::ref_ptr<osg::Image> image) const {
std::vector<u_char> img_uchar;
std::vector<float> img_float;
for (osg::Image::DataIterator itr(image.get()); itr.valid(); ++itr) {
img_uchar.insert(img_uchar.end(), itr.data(), itr.data() + itr.size());
}
float pixel;
for (size_t i = 0; i < img_uchar.size(); i += 3) {
pixel = R*(float)img_uchar[i] + G*(float)img_uchar[i+1] + B*(float)img_uchar[i+2];
img_float.push_back(pixel/255.0);
}
for (int k = Params::times_downsample; k > 1; k /= 2) {
downsample(img_float, k);
}
// std::cout << img_float.size() << std::endl;
return img_float;
}
std::vector<float>& gt() {return _gt;}
private:
std::vector<double> _ctrl;
// std::vector<Eigen::VectorXf> _traj;
float _entropy;
std::vector<float> _image;
std::vector<float> _gt;
};
#endif
//| This file is a part of the sferes2 framework.
//| Copyright 2016, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software. You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.
#include <iostream>
#include <sferes/eval/parallel.hpp>
#include <sferes/gen/evo_float.hpp>
#include <sferes/modif/dummy.hpp>
#include <sferes/phen/parameters.hpp>
#include <sferes/run.hpp>
#include <sferes/stat/best_fit.hpp>
#include <sferes/stat/qd_container.hpp>
#include <sferes/stat/qd_selection.hpp>
#include <sferes/stat/qd_progress.hpp>
#include <sferes/fit/fit_qd.hpp>
#include <sferes/qd/container/archive.hpp>
#include <sferes/qd/container/kdtree_storage.hpp>
#include <sferes/qd/quality_diversity.hpp>
#include <sferes/qd/selector/value_selector.hpp>
#include <sferes/qd/selector/score_proportionate.hpp>
// replacing physics with fit_hexa
// #include "minimal_physics.hpp"
#include "fit_hexa.hpp"
// #include "network_loader.hpp"
#include "preprocessor.hpp"
#include "modifier_dim_red.hpp"
#include "stat_projection.hpp"
// #include "stat_reconstruction.hpp"
#include "stat_current_gen.hpp"
typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> Mat;
using namespace sferes::gen::evo_float;
struct Params {
SFERES_CONST size_t update_period = 50;
SFERES_CONST size_t image_width = 32;
SFERES_CONST size_t image_height = 32;
SFERES_CONST size_t resolution = 10000; // influences l
SFERES_CONST size_t update_frequency = 50; // -1 means exponentially decreasing update frequency
SFERES_CONST size_t times_downsample = 2; // for taking the image
struct nov {
static double l;
SFERES_CONST double k = 15;
SFERES_CONST double eps = 0.1;
};
// TODO: move to a qd::
struct pop {
SFERES_CONST size_t init_size = 8000; // doesn't seem to work
// size of a batch
SFERES_CONST size_t size = 200;
SFERES_CONST size_t nb_gen = 2501;
SFERES_CONST size_t dump_period = 50;
};
struct parameters {
// used to avoid really degenerated experiments with trajectories not doing anything.
SFERES_CONST float min = 0.0;
SFERES_CONST float max = 1.0;
};
struct evo_float {
SFERES_CONST float cross_rate = 0.0f;
SFERES_CONST float mutation_rate = 0.1f;
SFERES_CONST float eta_m = 10.0f;
SFERES_CONST float eta_c = 10.0f;
SFERES_CONST mutation_t mutation_type = polynomial;
SFERES_CONST cross_over_t cross_over_type = sbx;
};
struct qd {
SFERES_CONST size_t dim = 2;
SFERES_CONST size_t behav_dim = 2;
};
};
double Params::nov::l;
// quick hack to have "write" access to the container, this need to be added to the main API later.
template<typename Phen, typename Eval, typename Stat, typename FitModifier, typename Select, typename Container , typename Params, typename Exact = stc::Itself>
class QualityDiversity_2 : public sferes::qd::QualityDiversity < Phen, Eval, Stat, FitModifier, Select, Container, Params, typename stc::FindExact<QualityDiversity_2<Phen, Eval, Stat, FitModifier, Select, Container, Params, Exact>, Exact>::ret>
{
public:
typedef Phen phen_t;
typedef boost::shared_ptr<Phen> indiv_t;
typedef typename std::vector<indiv_t> pop_t;
typedef typename pop_t::iterator it_t;
Container& container() { return this->_container; }
void add(pop_t& pop_off, std::vector<bool>& added, pop_t& pop_parents){
this->_add(pop_off, added, pop_parents);
}
// Same function, but without the need of parent.
void add(pop_t& pop_off, std::vector<bool>& added ){
std::cout<<"adding with l: "<<Params::nov::l<<std::endl;
this->_add(pop_off, added);
}
};
int main(int argc, char **argv)
{
srand(time(0));
load_and_init_robot();
robot_dart::graphics::PbufferManager::createPbufferManager(128);
tbb::task_scheduler_init init(64);
Params::nov::l = 0;
using namespace sferes;
typedef Fit_hexa<Params> fit_t;
typedef gen::EvoFloat<36, Params> gen_t;
typedef phen::Parameters<gen_t, fit_t, Params> phen_t;
typedef modif::ModifDimRed<phen_t,Params> modifier_t;
// For the Archive, you can chose one of the following storage:
// kD_tree storage, recommended for small behavioral descriptors (behav_dim<10)
typedef qd::container::KdtreeStorage< boost::shared_ptr<phen_t>, Params::qd::behav_dim > storage_t;
// Sort_based storage, recommended for larger behavioral descriptors.
//typedef qd::container::SortBasedStorage< boost::shared_ptr<phen_t> > storage_t;
typedef qd::container::Archive<phen_t, storage_t, Params> container_t;
typedef eval::Parallel<Params> eval_t;
// typedef eval::Eval<Params> eval_t;
typedef boost::fusion::vector<
stat::CurrentGen<phen_t, Params>,
stat::QdContainer<phen_t, Params>,
stat::QdProgress<phen_t, Params>,
stat::Projection<phen_t, Params>
> stat_t;
typedef qd::selector::ScoreProportionate<phen_t,qd::selector::getCuriosity, Params> select_t;
typedef QualityDiversity_2<phen_t, eval_t, stat_t, modifier_t, select_t, container_t, Params> ea_t;
ea_t ea;
run_ea(argc, argv, ea);
// ea.run();
}
#ifndef __DIM_RED_MODIFIER_HPP__
#define __DIM_RED_MODIFIER_HPP__
#include <sferes/stc.hpp>
#include "preprocessor.hpp"
#include "umap.hpp"
namespace sferes {
namespace modif {
template<typename Phen, typename Params>
class ModifDimRed {
public:
typedef Phen phen_t;
typedef boost::shared_ptr<Phen> indiv_t;
typedef typename std::vector<indiv_t> pop_t;
ModifDimRed():last_update(0),update_id(0){
_prep.init();
umap_model = std::unique_ptr<UMAP>(new UMAP());
}
typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor > Mat;
// defining new matrix for better precision when calculating the new minimum distance l
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor > Mat_dist;
template<typename Ea>
void apply(Ea& ea)
{
// std::cout << "mod dim apply" << std::endl;
if (Params::update_frequency == -1) {
if (Params::update_period >0 && (ea.gen() == 1 || ea.gen() == last_update + Params::update_period * std::pow(2,update_id-1)))
{
update_id++;
last_update=ea.gen();
update_descriptors(ea);
}
} else {
if (ea.gen() > 0 && ea.gen() % Params::update_frequency == 0 || ea.gen() == 1) update_descriptors(ea);
}
if(!ea.offspring().size())
return;
assign_desc(ea.offspring());
// std::cout << "mod dim apply done" << std::endl;
}
// template<typename Ea>
// double get_avg_recon_loss(Ea& ea){
// Mat data;
// this->collect_dataset(data, ea);
// float avg_recon = network_eval_data(data);
// return avg_recon;
// }
void assign_desc(pop_t& pop)
{
// std::cout << "assign_desc" << std::endl;
pop_t filtered_pop;
for(auto ind:pop)
if(!ind->fit().dead())
filtered_pop.push_back(ind);
else
{
std::vector<double>dd={-1,-1}; // CHANGED from float to double
ind->fit().set_desc(dd);
}
Mat data;
this->get_data(filtered_pop, data);
Mat res;//will be resized
get_descriptor(data,res);
for(size_t i=0; i<filtered_pop.size();i++){
std::vector<double>dd={(double)res(i,0), (double)res(i,1)}; // CHANGED from float to double
filtered_pop[i]->fit().set_desc(dd);
}
// std::cout << "assign_desc done" << std::endl;
}
void get_descriptor(const Mat & data, Mat& res)const
{
desc_ae(data, res);
}
template<typename EA>
void update_descriptors(EA& ea)
{
// std::cout << "update_descriptors" << std::endl;
Mat data;
collect_dataset(data, ea); // gather the data from the indiv in the archive into a dataset
train_umap(data);
update_container(ea); // clear the archive and re-fill it using the new network
// std::cout << "update_descriptors done" << std::endl;
}
template <typename EA>
void collect_dataset(Mat& data, EA& ea)
{
std::vector<typename EA::indiv_t> content;
ea.container().get_full_content(content);
size_t pop_size=content.size();
get_data(content, data);
std::cout<<"training set is composed of "<<data.rows()<<" samples ("<<ea.gen()<<" archive size : "<<pop_size<<")"<<std::endl;
}
void desc_ae(const Mat & data, Mat& res) const
{
// std::cout << "desc ae" << std::endl;
Mat scaled_data;
_prep.apply(data,scaled_data);
Mat desc;
umap_model->get_desc(scaled_data, desc) ;
res=Mat(desc.rows(),desc.cols());
res<<desc;
// std::cout << "desc ae done" << std::endl;
}
void train_umap(const Mat & data)
{
// we change the data normalisation each time we train/refine network, could cause small changes in loss between two trainings.
_prep.init(data);
Mat scaled_data;
_prep.apply(data,scaled_data);
umap_model->training(scaled_data); // true means full training without early stopping
}
// void get_reconstruction(const Mat & data, Mat & res)const
// {
// Mat scaled_data, scaled_res;
// _prep.apply(data,scaled_data);
// network->get_reconstruction(scaled_data, scaled_res);
// _prep.deapply(scaled_res, res);
// }
// double network_eval_data(const Mat & data, std::ostream& oo = std::cout) const
// {
// Mat scaled_data;
// _prep.apply(data,scaled_data);
// return network->get_avg_recon_loss(scaled_data);
// }
void get_bd(const pop_t& pop, Mat& data)const
{
data=Mat(pop.size(), Params::qd::dim );
for(size_t i=0; i<pop.size();i++)
{
auto desc = pop[i]->fit().desc();
for(size_t id=0;id<Params::qd::dim;id++)
data(i,id)=desc[id];
}
}
void get_data(const pop_t& pop, Mat& data)const
{
// std::cout << "get_data" << std::endl;
if(pop[0]->fit().dead())
std::cout << std::endl; // if no flush, then EIGEN (auto row=data.row(i)) gives an error
data=Mat(pop.size(), pop[0]->fit().get_flat_obs_size() );
for(size_t i=0; i<pop.size();i++)
{
// std::cout << data.rows() << std::endl;
auto row=data.row(i);
// std::cout << "here" << std::endl;
pop[i]->fit().get_flat_observations(row);
}
// std::cout << "get_data done" << std::endl;
}
void distance(const Mat& X, Mat_dist& dist)const{
// Compute norms
Mat_dist X_double = X.cast<double>();
Mat_dist XX = X_double.array().square().rowwise().sum();
Mat_dist XY = (2*X_double)*X_double.transpose();
// Compute final expression
dist = XX * Eigen::MatrixXd::Ones(1,XX.rows());
dist = dist + Eigen::MatrixXd::Ones(XX.rows(),1) * (XX.transpose());
dist = dist - XY;
}
float get_new_l(const pop_t& pop)const{
Mat data;