simple_optimization_task.h
Go to the documentation of this file.
1 
27 #ifndef EXAMPLES_SIMPLE_OPTIMIZATION_TASK_H_
28 #define EXAMPLES_SIMPLE_OPTIMIZATION_TASK_H_
29 
30 #include <stomp_core/task.h>
31 
32 namespace stomp_core_examples
33 {
34 
36 
38 {
39 public:
46  SimpleOptimizationTask(const Eigen::MatrixXd& parameters_bias,
47  const std::vector<double>& bias_thresholds,
48  const std::vector<double>& std_dev):
49  parameters_bias_(parameters_bias),
50  bias_thresholds_(bias_thresholds),
51  std_dev_(std_dev)
52  {
53 
54  // generate smoothing matrix
55  int num_timesteps = parameters_bias.cols();
56  stomp_core::generateSmoothingMatrix(num_timesteps,1.0,smoothing_M_);
57  srand(time(0));
58 
59  }
60 
62 
73  bool generateNoisyParameters(const Eigen::MatrixXd& parameters,
74  std::size_t start_timestep,
75  std::size_t num_timesteps,
76  int iteration_number,
77  int rollout_number,
78  Eigen::MatrixXd& parameters_noise,
79  Eigen::MatrixXd& noise) override
80  {
81  double rand_noise;
82  for(std::size_t d = 0; d < parameters.rows(); d++)
83  {
84  for(std::size_t t = 0; t < parameters.cols(); t++)
85  {
86  rand_noise = static_cast<double>(rand()%RAND_MAX)/static_cast<double>(RAND_MAX - 1); // 0 to 1
87  rand_noise = 2*(0.5 - rand_noise);
88  noise(d,t) = rand_noise*std_dev_[d];
89  }
90  }
91 
92  parameters_noise = parameters + noise;
93 
94  return true;
95  }
96 
107  bool computeCosts(const Eigen::MatrixXd& parameters,
108  std::size_t start_timestep,
109  std::size_t num_timesteps,
110  int iteration_number,
111  Eigen::VectorXd& costs,
112  bool& validity) override
113  {
114  return computeNoisyCosts(parameters,start_timestep,num_timesteps,iteration_number,-1,costs,validity);
115  }
116 
128  bool computeNoisyCosts(const Eigen::MatrixXd& parameters,
129  std::size_t start_timestep,
130  std::size_t num_timesteps,
131  int iteration_number,
132  int rollout_number,
133  Eigen::VectorXd& costs,
134  bool& validity) override
135  {
136  costs.setZero(num_timesteps);
137  double diff;
138  double cost = 0.0;
139  validity = true;
140 
141  for(std::size_t t = 0u; t < num_timesteps; t++)
142  {
143  cost = 0;
144  for(std::size_t d = 0u; d < parameters.rows() ; d++)
145  {
146 
147  diff = std::abs(parameters(d,t) - parameters_bias_(d,t));
148  if( diff > std::abs(bias_thresholds_[d]))
149  {
150  cost += diff;
151  validity = false;
152  }
153  }
154 
155  costs(t) = cost;
156  }
157 
158  return true;
159  }
160 
172  bool filterParameterUpdates(std::size_t start_timestep,
173  std::size_t num_timesteps,
174  int iteration_number,
175  const Eigen::MatrixXd& parameters,
176  Eigen::MatrixXd& updates) override
177  {
178  return smoothParameterUpdates(start_timestep,num_timesteps,iteration_number,updates);
179  }
180 
181 
182 protected:
183 
192  bool smoothParameterUpdates(std::size_t start_timestep,
193  std::size_t num_timesteps,
194  int iteration_number,
195  Eigen::MatrixXd& updates)
196  {
197 
198  for(auto d = 0u; d < updates.rows(); d++)
199  {
200  updates.row(d).transpose() = smoothing_M_*(updates.row(d).transpose());
201  }
202 
203  return true;
204  }
205 
206 protected:
207 
208  Eigen::MatrixXd parameters_bias_;
209  std::vector<double> bias_thresholds_;
210  std::vector<double> std_dev_;
211  Eigen::MatrixXd smoothing_M_;
212 };
213 
214 }
215 
216 
217 #endif /* EXAMPLES_SIMPLE_OPTIMIZATION_TASK_H_ */
d
bool smoothParameterUpdates(std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, Eigen::MatrixXd &updates)
Perform a smooth update given a noisy update.
SimpleOptimizationTask(const Eigen::MatrixXd &parameters_bias, const std::vector< double > &bias_thresholds, const std::vector< double > &std_dev)
A simple task for demonstrating how to use Stomp.
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
[SimpleOptimizationTask Inherit]
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
computes the state costs as a function of the distance from the bias parameters
bool filterParameterUpdates(std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, const Eigen::MatrixXd &parameters, Eigen::MatrixXd &updates) override
Filters the given parameters which is applied after the update. It could be used for clipping of join...
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
computes the state costs as a function of the distance from the bias parameters
This defines the stomp task.
Defines the STOMP improvement policy.
Definition: task.h:41


stomp_core
Author(s): Jorge Nicho
autogenerated on Fri May 8 2020 03:35:43