stomp_optimization_task.cpp
Go to the documentation of this file.
1 
26 #include <stdexcept>
28 
29 using PluginConfigs = std::vector< std::pair<std::string,XmlRpc::XmlRpcValue> >;
30 
31 static const std::string COST_FUNCTIONS_FIELD = "cost_functions";
32 static const std::string NOISY_FILTERS_FIELD = "noisy_filters";
33 static const std::string UPDATE_FILTERS_FIELD = "update_filters";
34 static const std::string NOISE_GENERATOR_FIELD = "noise_generator";
35 
44  std::string param_name,
45  PluginConfigs& plugins)
46 {
47  if(config.hasMember(param_name) && (config[param_name].getType() == XmlRpc::XmlRpcValue::TypeArray))
48  {
49  XmlRpc::XmlRpcValue& plugin_list = config[param_name];
50  std::string class_name;
51 
52  // look for the 'class' entry
53  for(auto i = 0u ; i < plugin_list.size(); i++)
54  {
55  XmlRpc::XmlRpcValue& plugin_config = plugin_list[i];
56  if(plugin_config.hasMember("class") && (plugin_config["class"].getType() == XmlRpc::XmlRpcValue::TypeString))
57  {
58  class_name = static_cast<std::string>(plugin_config["class"]);
59  plugins.push_back(std::make_pair(class_name,plugin_config));
60  }
61  else
62  {
63  ROS_ERROR("Element in the '%s' array parameter did not contain a 'class' entry",param_name.c_str());
64  return false;
65  }
66  }
67  }
68  else
69  {
70  ROS_WARN("Plugin under entry '%s' was not found in ros parameter.",param_name.c_str());
71  ROS_DEBUG("Failed to find plugin under entry '%s' in ros parameter %s",param_name.c_str(),config.toXml().c_str());
72  return false;
73  }
74 
75  return !plugins.empty();
76 }
77 
89 struct PluginData
90 {
91  XmlRpc::XmlRpcValue config;
92  std::string param_key;
93  bool critical;
94  bool single_instance;
95  std::string plugin_desc;
96  moveit::core::RobotModelConstPtr robot_model;
97  std::string group_name;
98 };
99 
100 template <typename PluginPtr,typename ClassLoaderPtr>
101 bool loadPlugins(const PluginData plugin_data,
102  ClassLoaderPtr class_loader ,std::vector<PluginPtr>& plugin_array)
103 {
104  PluginConfigs plugins;
105 
106 
107  if(parsePluginConfigs(plugin_data.config,plugin_data.param_key,plugins))
108  {
109  for(auto& entry: plugins)
110  {
111  // instantiating
112  PluginPtr plugin;
113  try
114  {
115  plugin.reset(class_loader->createUnmanagedInstance(entry.first));
116  }
118  {
119  if(plugin_data.critical)
120  {
121  ROS_ERROR("%s plugin '%s' could not be created",plugin_data.plugin_desc.c_str() ,entry.first.c_str());
122  return false;
123  }
124  else
125  {
126  ROS_WARN("%s plugin '%s' could not be created",plugin_data.plugin_desc.c_str() ,entry.first.c_str());
127  continue;
128  }
129  }
130 
131  // initializing
132  if(plugin->initialize(plugin_data.robot_model,plugin_data.group_name,entry.second))
133  {
134  plugin_array.push_back(plugin);
135  ROS_INFO_STREAM("Stomp Optimization Task loaded "<< plugin_data.plugin_desc <<" '"<<plugin->getName()<<"' plugin");
136  }
137  else
138  {
139  if(plugin_data.critical)
140  {
141  ROS_ERROR("%s plugin '%s' failed to initialize",plugin_data.plugin_desc.c_str(), entry.first.c_str());
142  return false;
143  }
144  else
145  {
146  ROS_WARN("%s plugin '%s' failed to initialize",plugin_data.plugin_desc.c_str(), entry.first.c_str());
147  continue;
148  }
149  }
150 
151  if(plugin_data.single_instance)
152  {
153  break;
154  }
155  }
156 
157  std::stringstream ss;
158  ss<<"[";
159  auto arrayToString = [&ss](PluginConfigs::value_type& p)
160  {
161  ss<<p.first<<" ";
162  };
163 
164  std::for_each(plugins.begin(),plugins.end(),arrayToString);
165  ss<<"]";
166 
167  ROS_DEBUG("Loaded %s plugins: %s", plugin_data.plugin_desc.c_str(), ss.str().c_str());
168  }
169  else
170  {
171  return false;
172  }
173 
174  return true;
175 }
176 
177 namespace stomp_moveit
178 {
179 
180 StompOptimizationTask::StompOptimizationTask(
181  moveit::core::RobotModelConstPtr robot_model_ptr,
182  std::string group_name,
183  const XmlRpc::XmlRpcValue& config):
184  robot_model_ptr_(robot_model_ptr),
185  group_name_(group_name)
186 {
187  // initializing plugin loaders
188  cost_function_loader_.reset(new CostFunctionLoader("stomp_moveit", "stomp_moveit::cost_functions::StompCostFunction"));
189  noise_generator_loader_.reset(new NoiseGeneratorLoader("stomp_moveit","stomp_moveit::noise_generators::StompNoiseGenerator"));
190  noisy_filter_loader_.reset(new NoisyFilterLoader("stomp_moveit", "stomp_moveit::noisy_filters::StompNoisyFilter"));
191  update_filter_loader_.reset(new UpdateFilterLoader("stomp_moveit", "stomp_moveit::update_filters::StompUpdateFilter"));
192 
193  // preparing plugin init data
194  PluginData plugin_data;
195  plugin_data.config = config;
196  plugin_data.group_name = group_name_;
197  plugin_data.robot_model = robot_model_ptr_;
198 
199  // loading cost function plugins
200  plugin_data.param_key = COST_FUNCTIONS_FIELD;
201  plugin_data.plugin_desc = "CostFunction";
202  plugin_data.critical = true;
203  plugin_data.single_instance = false;
204  if(!loadPlugins(plugin_data,cost_function_loader_,cost_functions_))
205  {
206  ROS_ERROR("StompOptimizationTask/%s failed to load '%s' plugins from yaml",group_name.c_str(),COST_FUNCTIONS_FIELD.c_str());
207  throw std::logic_error("plugin not found");
208  }
209 
210  // loading noise generators
211  plugin_data.param_key = NOISE_GENERATOR_FIELD;
212  plugin_data.plugin_desc = "NoiseGenerator";
213  plugin_data.critical = true;
214  plugin_data.single_instance = true;
215  if(!loadPlugins(plugin_data, noise_generator_loader_,noise_generators_))
216  {
217  ROS_ERROR("StompOptimizationTask/%s failed to load '%s' plugins from yaml",group_name.c_str(),
218  NOISE_GENERATOR_FIELD.c_str());
219  throw std::logic_error("plugin not found");
220  }
221 
222  // loading noisy filter plugins
223  plugin_data.param_key = NOISY_FILTERS_FIELD;
224  plugin_data.plugin_desc = "NoisyFilter";
225  plugin_data.critical = false;
226  plugin_data.single_instance = false;
227  if(!loadPlugins(plugin_data,noisy_filter_loader_,noisy_filters_))
228  {
229  ROS_WARN("StompOptimizationTask/%s failed to load '%s' plugins from yaml",group_name.c_str(),NOISY_FILTERS_FIELD.c_str());
230  }
231 
232  // loading filter plugins
233  plugin_data.param_key = UPDATE_FILTERS_FIELD;
234  plugin_data.plugin_desc = "UpdateFilter";
235  plugin_data.critical = false;
236  plugin_data.single_instance = false;
237  if(!loadPlugins(plugin_data,update_filter_loader_,update_filters_))
238  {
239  ROS_WARN("StompOptimizationTask/%s failed to load '%s' plugins from yaml",group_name.c_str(),UPDATE_FILTERS_FIELD.c_str());
240  }
241 }
242 
243 StompOptimizationTask::~StompOptimizationTask()
244 {
245  // TODO Auto-generated destructor stub
246 }
247 
248 bool StompOptimizationTask::generateNoisyParameters(const Eigen::MatrixXd& parameters,
249  std::size_t start_timestep,
250  std::size_t num_timesteps,
251  int iteration_number,
252  int rollout_number,
253  Eigen::MatrixXd& parameters_noise,
254  Eigen::MatrixXd& noise)
255 {
256  return noise_generators_.back()->generateNoise(parameters,start_timestep,num_timesteps,iteration_number,rollout_number,
257  parameters_noise,noise);
258 }
259 
260 bool StompOptimizationTask::computeNoisyCosts(const Eigen::MatrixXd& parameters,
261  std::size_t start_timestep,
262  std::size_t num_timesteps,
263  int iteration_number,
264  int rollout_number,
265  Eigen::VectorXd& costs,
266  bool& validity)
267 {
268  Eigen::MatrixXd cost_matrix = Eigen::MatrixXd::Zero(num_timesteps,cost_functions_.size());
269  Eigen::VectorXd state_costs = Eigen::VectorXd::Zero(num_timesteps);
270  validity = true;
271  for(auto i = 0u; i < cost_functions_.size(); i++ )
272  {
273  bool valid;
274  auto cf = cost_functions_[i];
275 
276  if(!cf->computeCosts(parameters,start_timestep,num_timesteps,iteration_number,rollout_number,state_costs,valid))
277  {
278  return false;
279  }
280 
281  validity &= valid;
282 
283  cost_matrix.col(i) = state_costs * cf->getWeight();
284  }
285  costs = cost_matrix.rowwise().sum();
286  return true;
287 }
288 
289 bool StompOptimizationTask::computeCosts(const Eigen::MatrixXd& parameters,
290  std::size_t start_timestep,
291  std::size_t num_timesteps,
292  int iteration_number,
293  Eigen::VectorXd& costs,
294  bool& validity)
295 {
296  Eigen::MatrixXd cost_matrix = Eigen::MatrixXd::Zero(num_timesteps,cost_functions_.size());
297  Eigen::VectorXd state_costs = Eigen::VectorXd::Zero(num_timesteps);
298  validity = true;
299  for(auto i = 0u; i < cost_functions_.size(); i++ )
300  {
301  bool valid;
302  auto cf = cost_functions_[i];
303 
304  if(!cf->computeCosts(parameters,start_timestep,num_timesteps,iteration_number,cf->getOptimizedIndex(),state_costs,valid))
305  {
306  return false;
307  }
308 
309  validity &= valid;
310 
311  cost_matrix.col(i) = state_costs * cf->getWeight();
312  }
313  costs = cost_matrix.rowwise().sum();
314  return true;
315 }
316 
317 bool StompOptimizationTask::setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
318  const moveit_msgs::MotionPlanRequest &req,
319  const stomp_core::StompConfiguration &config,
320  moveit_msgs::MoveItErrorCodes& error_code)
321 {
322  for(auto p: noise_generators_)
323  {
324  if(!p->setMotionPlanRequest(planning_scene,req,config,error_code))
325  {
326  ROS_ERROR("Failed to set Plan Request on noise generator %s",p->getName().c_str());
327  return false;
328  }
329  }
330 
331  for(auto p : cost_functions_)
332  {
333  if(!p->setMotionPlanRequest(planning_scene,req,config,error_code))
334  {
335  ROS_ERROR("Failed to set Plan Request on cost function %s",p->getName().c_str());
336  return false;
337  }
338  }
339 
340  for(auto p: noisy_filters_)
341  {
342  if(!p->setMotionPlanRequest(planning_scene,req,config,error_code))
343  {
344  ROS_ERROR("Failed to set Plan Request on noisy filter %s",p->getName().c_str());
345  return false;
346  }
347  }
348 
349  for(auto p: update_filters_)
350  {
351  if(!p->setMotionPlanRequest(planning_scene,req,config,error_code))
352  {
353  ROS_ERROR("Failed to set Plan Request on update filter %s",p->getName().c_str());
354  return false;
355  }
356  }
357 
358  return true;
359 }
360 
361 bool StompOptimizationTask::filterNoisyParameters(std::size_t start_timestep,
362  std::size_t num_timesteps,
363  int iteration_number,
364  int rollout_number,
365  Eigen::MatrixXd& parameters,bool& filtered)
366 {
367  filtered = false;
368  bool temp;
369  for(auto& f: noisy_filters_)
370  {
371  if(f->filter(start_timestep,num_timesteps,iteration_number,rollout_number,parameters,temp))
372  {
373  filtered |= temp;
374  }
375  else
376  {
377  return false;
378  }
379  }
380  return true;
381 }
382 
383 bool StompOptimizationTask::filterParameterUpdates(std::size_t start_timestep,
384  std::size_t num_timesteps,
385  int iteration_number,
386  const Eigen::MatrixXd& parameters,
387  Eigen::MatrixXd& updates)
388 {
389  bool filtered = false;
390  bool temp;
391  for(auto& f: update_filters_)
392  {
393  if(f->filter(start_timestep,num_timesteps,iteration_number,parameters,updates,temp))
394  {
395  filtered |= temp;
396  }
397  else
398  {
399  return false;
400  }
401  }
402  return true;
403 }
404 
405 void StompOptimizationTask::postIteration(std::size_t start_timestep,
406  std::size_t num_timesteps,int iteration_number,double cost,const Eigen::MatrixXd& parameters)
407 {
408  for(auto p : noise_generators_)
409  {
410  p->postIteration(start_timestep,num_timesteps,iteration_number,cost,parameters);
411  }
412 
413  for(auto p : cost_functions_)
414  {
415  p->postIteration(start_timestep,num_timesteps,iteration_number,cost,parameters);
416  }
417 
418  for(auto p: noisy_filters_)
419  {
420  p->postIteration(start_timestep,num_timesteps,iteration_number,cost,parameters);
421  }
422 
423  for(auto p: update_filters_)
424  {
425  p->postIteration(start_timestep,num_timesteps,iteration_number,cost,parameters);
426  }
427 }
428 
429 void StompOptimizationTask::done(bool success,int total_iterations,double final_cost,const Eigen::MatrixXd& parameters)
430 {
431  for(auto p : noise_generators_)
432  {
433  p->done(success,total_iterations,final_cost,parameters);
434  }
435 
436 
437  for(auto p : cost_functions_)
438  {
439  p->done(success,total_iterations,final_cost,parameters);
440  }
441 
442  for(auto p: noisy_filters_)
443  {
444  p->done(success,total_iterations,final_cost,parameters);
445  }
446 
447  for(auto p: update_filters_)
448  {
449  p->done(success,total_iterations,final_cost,parameters);
450  }
451 }
452 
453 } /* namespace stomp_moveit */
f
virtual bool setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::MotionPlanRequest &req, const stomp_core::StompConfiguration &config, moveit_msgs::MoveItErrorCodes &error_code)
Passes the planning details down to each loaded plugin.
int size() const
Type const & getType() const
bool parsePluginConfigs(XmlRpc::XmlRpcValue config, std::string param_name, PluginConfigs &plugins)
Convenience method to load an array of STOMP plugins.
#define ROS_WARN(...)
virtual bool filterNoisyParameters(std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::MatrixXd &parameters, bool &filtered) override
Filters the given noisy parameters which is applied after noisy trajectory generation. It could be used for clipping of joint limits or projecting into the null space of the Jacobian. It accomplishes this by calling the loaded Noisy Filter plugins.
virtual 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 optimized parameters for each time step. It does this by calling the loaded Cost Function plugins
Packs plugin information into a single struct.
virtual 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 noisy parameters for each time step. It does this by calling the loaded Cost Function plugins
std::string toXml() const
virtual 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
Generates a noisy trajectory from the parameters by calling the active Noise Generator plugin...
virtual void done(bool success, int total_iterations, double final_cost, const Eigen::MatrixXd &parameters) override
Called by Stomp at the end of the optimization process.
virtual 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 calculating the update. It could be used for clip...
virtual void postIteration(std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, double cost, const Eigen::MatrixXd &parameters)
Called by STOMP at the end of each iteration.
bool hasMember(const std::string &name) const
#define ROS_INFO_STREAM(args)
This defines stomp&#39;s optimization task.
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


stomp_moveit
Author(s): Jorge Nicho
autogenerated on Fri May 8 2020 03:35:47