Randomiser.hh

Go to the documentation of this file.

#ifndef _RANDOMISER_HH_
#define _RANDOMISER_HH_

// Random distributions
#include <random>
// Required fields workaround
#include <limits>

// Open YAML config files
#include "yaml-cpp/yaml.h"

// Gazebo
#include <gazebo/gazebo_client.hh>

// Custom messages
#include "dr_request.pb.h"
// Domain randomization plugin interface
#include "DRInterface.hh"

// Debug streams
#include "debug.hh"

typedef gap::msgs::DRRequest DRRequest;
typedef gap::msgs::ModelCmd ModelCmdMsg;

// Random samplers

class IRandomSampler
{
    public: virtual ~IRandomSampler() {};

    public: virtual double sample(std::mt19937 & gen);
};

class GaussianSampler : public IRandomSampler
{
    public: std::normal_distribution<double> dist;

    private: double mean;
    private: double std;

    public: GaussianSampler(
        double mean,
        double std);

    public: ~GaussianSampler() override {};

    public: double sample(std::mt19937 & gen) override;
};

class UniformSampler : public IRandomSampler
{
    public: std::uniform_real_distribution<double> dist;

    private: double a;
    private: double b;
    private: bool log_uniform {false};

    public: UniformSampler(
        double a,
        double b,
        bool log_uniform=false);

    public: ~UniformSampler() override {};

    public: double sample(std::mt19937 & gen) override;
};

// Random properties

class RandomProperty
{
    public: IRandomSampler *sampler;
    public: bool additive;

    public: static const char CFG_TARGET_OBJ[];

    public: RandomProperty(
        IRandomSampler *sampler_,
        bool additive_);

    public: virtual ~RandomProperty();

    public: virtual void fillMsg(DRRequest & msg,
        DRInterface & api,
        std::mt19937 & gen,
        std::string & target);
};

class ModelScale : public RandomProperty
{
    public: std::vector<std::string> models;

    public: ModelScale(
        IRandomSampler *sampler_,
        bool additive_,
        std::vector<std::string> & models_);

    public: ~ModelScale() override {};

    public: void fillMsg(DRRequest & msg,
        DRInterface & api,
        std::mt19937 & gen,
        std::string & target) override;
};

class LinkMass : public RandomProperty
{
    public: std::vector<std::string> models;
    public: std::vector<std::string> links;
    public: std::vector<double> masses;

    public: LinkMass(
        IRandomSampler *sampler_,
        bool additive_,
        std::vector<std::string> & models_,
        std::vector<std::string> & links_,
        std::vector<double> & masses_);

    public: ~LinkMass() override {};

    public: void fillMsg(DRRequest & msg,
        DRInterface & api,
        std::mt19937 & gen,
        std::string & target) override;
};

class FrictionCoefficient : public RandomProperty
{
    public: std::vector<std::string> models;
    public: std::vector<std::string> links;
    public: std::vector<double> mu1;
    public: std::vector<double> mu2;
    public: std::vector<double> kp;
    public: std::vector<double> kd;

    public: FrictionCoefficient(
        IRandomSampler *sampler_,
        bool additive_,
        std::vector<std::string> & models_,
        std::vector<std::string> & links_,
        std::vector<double> & mu1_,
        std::vector<double> & mu2_,
        std::vector<double> & kp_,
        std::vector<double> & kd_);

    public: ~FrictionCoefficient() override {};

    public: void fillMsg(DRRequest & msg,
        DRInterface & api,
        std::mt19937 & gen,
        std::string & target) override;
};

class JointDampingCoefficient : public RandomProperty
{
    public: std::vector<std::string> models;
    public: std::vector<std::string> joints;
    public: std::vector<double> damping;

    public: JointDampingCoefficient(
        IRandomSampler *sampler_,
        bool additive_,
        std::vector<std::string> & models_,
        std::vector<std::string> & joints_,
        std::vector<double> & damping_);

    public: ~JointDampingCoefficient() override {};

    public: void fillMsg(DRRequest & msg,
        DRInterface & api,
        std::mt19937 & gen,
        std::string & target) override;
};

class PGain : public RandomProperty
{
    public: std::vector<std::string> models;
    public: std::vector<std::string> joints;
    public: std::vector<int> types;
    public: std::vector<double> p_gains;

    public: PGain(
        IRandomSampler *sampler_,
        bool additive_,
        std::vector<std::string> & models_,
        std::vector<std::string> & joints_,
        std::vector<int> & types_,
        std::vector<double> & p_gains_);

    public: ~PGain() override {};

    public: void fillMsg(DRRequest & msg,
        DRInterface & api,
        std::mt19937 & gen,
        std::string & target) override;
};

class JointLimit : public RandomProperty
{
    public: std::vector<std::string> models;
    public: std::vector<std::string> joints;
    public: std::vector<double> lower;
    public: std::vector<double> upper;

    public: JointLimit(
        IRandomSampler *sampler_,
        bool additive_,
        std::vector<std::string> & models_,
        std::vector<std::string> & joints_,
        std::vector<double> & lower_,
        std::vector<double> & upper_);

    public: ~JointLimit() override {};

    public: void fillMsg(DRRequest & msg,
        DRInterface & api,
        std::mt19937 & gen,
        std::string & target) override;
};

class Gravity : public RandomProperty
{
    public: std::vector<double> gravity;

    public: Gravity(
        IRandomSampler *sampler_,
        bool additive_,
        std::vector<double> & gravity_);

    public: ~Gravity() override {};

    public: void fillMsg(DRRequest & msg,
        DRInterface & api,
        std::mt19937 & gen,
        std::string & target) override;
};


// Randomiser class

class Randomiser
{
    // Private attributes

    private: DRInterface api;
    private: std::vector<RandomProperty*> properties;
    private: std::mt19937 m_mt;
    private: std::string target {"TARGET_OBJECT"};

    public: Randomiser(const std::string & config);

    public: ~Randomiser();

    public: void randomise(bool blocking=true);

    public: void setTargetName(const std::string & target);

    // Public constants for yml config file
    // TODO - move to file?

    public: static const char CFG_PROPERTIES[];
    public: static const char CFG_MODEL_SCALE[];
    public: static const char CFG_LINK_MASS[];
    public: static const char CFG_FRICTION[];
    public: static const char CFG_JOINT_DAMPING[];
    public: static const char CFG_P_GAIN[];
    public: static const char CFG_JOINT_LIMIT[];
    public: static const char CFG_GRAVITY[];

    public: static const char CFG_DIST[];
    public: static const char CFG_UNIFORM[];
    public: static const char CFG_LOG_UNIFORM[];
    public: static const char CFG_UNIFORM_A[];
    public: static const char CFG_UNIFORM_B[];
    public: static const char CFG_GAUSSIAN[];
    public: static const char CFG_GAUSSIAN_MEAN[];
    public: static const char CFG_GAUSSIAN_STD[];

    public: static const char CFG_ADDITIVE[];
    public: static const char CFG_VECTOR[];
    public: static const char CFG_TARGET[];

    public: static const char CFG_MODEL[];
    public: static const char CFG_LINK[];
    public: static const char CFG_JOINT[];
    public: static const char CFG_MASS[];
    public: static const char CFG_DAMPING[];
    public: static const char CFG_LOWER[];
    public: static const char CFG_UPPER[];
    public: static const char CFG_MU1[];
    public: static const char CFG_MU2[];
    public: static const char CFG_KP[];
    public: static const char CFG_KD[];
    public: static const char CFG_TYPE[];
    public: static const char CFG_P[];
    public: static const char CFG_TYPE_POS[];
};

#endif