simple_optimization_task.h

Go to the documentation of this file.

#ifndef EXAMPLES_SIMPLE_OPTIMIZATION_TASK_H_
#define EXAMPLES_SIMPLE_OPTIMIZATION_TASK_H_

#include <stomp_core/task.h>

namespace stomp_core_examples
{


class SimpleOptimizationTask: public stomp_core::Task
{
public:
  SimpleOptimizationTask(const Eigen::MatrixXd& parameters_bias,
            const std::vector<double>& bias_thresholds,
            const std::vector<double>& std_dev):
              parameters_bias_(parameters_bias),
              bias_thresholds_(bias_thresholds),
              std_dev_(std_dev)
  {

    // generate smoothing matrix
    int num_timesteps = parameters_bias.cols();
    stomp_core::generateSmoothingMatrix(num_timesteps,1.0,smoothing_M_);
    srand(time(0));

  }


  bool generateNoisyParameters(const Eigen::MatrixXd& parameters,
                               std::size_t start_timestep,
                               std::size_t num_timesteps,
                               int iteration_number,
                               int rollout_number,
                               Eigen::MatrixXd& parameters_noise,
                               Eigen::MatrixXd& noise) override
  {
    double rand_noise;
    for(std::size_t d = 0; d < parameters.rows(); d++)
    {
      for(std::size_t t = 0; t < parameters.cols(); t++)
      {
        rand_noise = static_cast<double>(rand()%RAND_MAX)/static_cast<double>(RAND_MAX - 1); // 0 to 1
        rand_noise = 2*(0.5 - rand_noise);
        noise(d,t) =  rand_noise*std_dev_[d];
      }
    }

    parameters_noise = parameters + noise;

    return true;
  }

  bool computeCosts(const Eigen::MatrixXd& parameters,
                    std::size_t start_timestep,
                    std::size_t num_timesteps,
                    int iteration_number,
                    Eigen::VectorXd& costs,
                    bool& validity) override
  {
    return computeNoisyCosts(parameters,start_timestep,num_timesteps,iteration_number,-1,costs,validity);
  }

  bool computeNoisyCosts(const Eigen::MatrixXd& parameters,
                         std::size_t start_timestep,
                         std::size_t num_timesteps,
                         int iteration_number,
                         int rollout_number,
                         Eigen::VectorXd& costs,
                         bool& validity) override
  {
    costs.setZero(num_timesteps);
    double diff;
    double cost = 0.0;
    validity = true;

    for(std::size_t t = 0u; t < num_timesteps; t++)
    {
      cost = 0;
      for(std::size_t d = 0u; d < parameters.rows() ; d++)
      {

        diff = std::abs(parameters(d,t) - parameters_bias_(d,t));
        if( diff > std::abs(bias_thresholds_[d]))
        {
          cost += diff;
          validity = false;
        }
      }

      costs(t) = cost;
    }

    return true;
  }

  bool filterParameterUpdates(std::size_t start_timestep,
                              std::size_t num_timesteps,
                              int iteration_number,
                              const Eigen::MatrixXd& parameters,
                              Eigen::MatrixXd& updates) override
  {
    return smoothParameterUpdates(start_timestep,num_timesteps,iteration_number,updates);
  }


protected:

  bool smoothParameterUpdates(std::size_t start_timestep,
                              std::size_t num_timesteps,
                              int iteration_number,
                              Eigen::MatrixXd& updates)
  {

    for(auto d = 0u; d < updates.rows(); d++)
    {
      updates.row(d).transpose() = smoothing_M_*(updates.row(d).transpose());
    }

    return true;
  }

protected:

  Eigen::MatrixXd parameters_bias_;          
  std::vector<double> bias_thresholds_; 
  std::vector<double> std_dev_;         
  Eigen::MatrixXd smoothing_M_;         
};

}


#endif /* EXAMPLES_SIMPLE_OPTIMIZATION_TASK_H_ */