Commit 63116ce7 authored by Eivinas Butkus's avatar Eivinas Butkus

init commit

parents
#ifndef ROBOT_DART_DESCRIPTOR_HEXA_HPP
#define ROBOT_DART_DESCRIPTOR_HEXA_HPP
// for size_t
#include <cstddef>
namespace robot_dart {
class RobotDARTSimu;
class Robot;
namespace descriptor {
struct HexaDescriptor:public BaseDescriptor{
public:
HexaDescriptor(RobotDARTSimu& simu, size_t desc_dump = 1):BaseDescriptor(simu,desc_dump) // changed desc_dump from 100 to 1
{}
std::vector<Eigen::VectorXf> traj;
std::vector<float> final_pos;
virtual void operator()()
{
auto pos=_simu.robots().back()->skeleton()->getPositions().head(6).tail(3).cast <float> ();
traj.push_back(pos.head(3)); // 3 dimensions for keeping body low task
}
};
} // namespace descriptor
} // namespace robot_dart
#endif
#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/beta_vae/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);
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();
}
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; // changed from 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 decaying 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;
};
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 = 500;
};
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;
pop_t pop_advers;
// pop_t& get_pop_advers() { return this->pop_advers; }
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(32);
tbb::task_scheduler_init init(16);
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::Reconstruction<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();
}
This diff is collapsed.
This diff is collapsed.
#ifndef __PREPROCESSOR__HPP__
#define __PREPROCESSOR__HPP__
#include <iostream>
class Rescale_feature{
public:
Rescale_feature():no_prep(true){}
typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor > Mat;
void init()
{
no_prep=true;
}
void init(const Mat & data)
{
std::cout << "preprocessing init" << std::endl;
no_prep=false;
// _max = data.maxCoeff();
// _min = data.minCoeff();
_min = 0.0;
_max = 1.0;
a = 0.3;
b = 0.7;
std::cout << "preprocessing init done" << std::endl;
}
void apply(const Mat & data, Mat& res)const
{
// std::cout << "preprocessing apply" << std::endl;
if(no_prep)
{
res=data;
}
else
{
res = a + (data.array() - _min)*(b - a)/(_max - _min);
// std::cout << res.row(0) << std::endl;
}
// std::cout << "preprocessing apply done" << std::endl;
}
void deapply(const Mat& data, Mat& res) const {
// std::cout << "preprocessing deapply" << std::endl;
if(no_prep)
{
res=data;
}
else
{
res = _min + ((data.array() - a)*(_max - _min))/(b - a);
}
// std::cout << "preprocessing deapply done" << std::endl;
}
private:
float _min;
float _max;
float a;
float b;
bool no_prep;
};
#endif
#ifndef STAT_CURRENT_GEN_HPP_
#define STAT_CURRENT_GEN_HPP_
#include <sferes/stat/stat.hpp>
namespace sferes {
namespace stat {
SFERES_STAT(CurrentGen, Stat) {
public:
template<typename E>
void refresh(const E& ea) {
std::string fname = ea.res_dir() + "/current_gen";
std::ofstream ofs(fname.c_str());
ofs << "Current generation: " << ea.gen() << std::endl;
ofs << "Current generation size: " << ea.pop().size() << std::endl;
ofs.close();
}
};
}
}
#endif
#ifndef STAT_PROJECTION_HPP_
#define STAT_PROJECTION_HPP_
#include <numeric>
#include <sferes/stat/stat.hpp>
typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor > Mat;
//#define MAP_WRITE_PARENTS
namespace sferes
{
namespace stat
{
SFERES_STAT(Projection, Stat)
{
public:
template<typename E>
void refresh(const E& ea)
{
if (ea.gen() % 10 == 0)
_write_container(std::string("proj_"), ea);
}
template<typename EA>
void _write_container(const std::string& prefix,
const EA& ea) const
{
std::cout << "writing..." << prefix << ea.gen() << std::endl;
std::string fname = ea.res_dir() + "/"
+ prefix
+ boost::lexical_cast<
std::string>(ea.gen())
+ std::string(".dat");
std::ofstream ofs(fname.c_str());
size_t offset = 0;
ofs.precision(17);
for(auto it = ea.pop().begin(); it != ea.pop().end(); ++it)
{
ofs << offset << " "<<(*it)->fit().entropy()<<" ";
for(size_t dim = 0; dim < (*it)->fit().desc().size(); ++dim)
ofs << (*it)->fit().desc()[dim] << " ";
//ofs << " " << array(idx)->fit().value() << std::endl;
ofs<<" ";
for(size_t dim = 0; dim < (*it)->fit().gt().size(); ++dim)
ofs << (*it)->fit().gt()[dim] << " ";
ofs<<std::endl;
++offset;
}
}
};
}
}
#endif
#ifndef STAT_RECONSTRUCTION_HPP_
#define STAT_RECONSTRUCTION_HPP_
#include <numeric>
#include <sferes/stat/stat.hpp>
//#define MAP_WRITE_PARENTS
namespace sferes
{
namespace stat
{
SFERES_STAT(Reconstruction, Stat)
{
public:
template<typename E>