step_cost_estimator.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_plugins/plugin_aggregators/step_cost_estimator.h>
00002 
00003 
00004 
00005 namespace vigir_footstep_planning
00006 {
00007 StepCostEstimator::StepCostEstimator()
00008   : ExtendedPluginAggregator<StepCostEstimator, StepCostEstimatorPlugin>("StepCostEstimator")
00009 {
00010 }
00011 
00012 bool StepCostEstimator::loadParams(const vigir_generic_params::ParameterSet& params)
00013 {
00014   if (!ExtendedPluginAggregator<StepCostEstimator, StepCostEstimatorPlugin>::loadParams(params))
00015     return false;
00016 
00017   params.getParam("max_risk", max_risk_, 1.0);
00018 
00019   return true;
00020 }
00021 
00022 bool StepCostEstimator::getCost(const State& left_foot, const State& right_foot, const State& swing_foot, double& cost, double& risk) const
00023 {
00024   cost = 0.0;
00025   double cost_multiplier = 1.0;
00026   risk = 0.0;
00027   double risk_multiplier = 1.0;
00028 
00029   //ROS_INFO("-----------------------------------");
00030   for (StepCostEstimatorPlugin::Ptr plugin : getPlugins())
00031   {
00032     if (!plugin)
00033       continue;
00034 
00035     double c, c_m, r, r_m;
00036     if (plugin->getCost(left_foot, right_foot, swing_foot, c, c_m, r, r_m))
00037     {
00038       cost += c;
00039       cost_multiplier *= c_m;
00040       risk += r;
00041       risk_multiplier *= r_m;
00042       //ROS_INFO("[%s]: %.3f %.3f %.3f %.3f", step_cost_estimator->getName().c_str(), c, r, c_m, r_m);
00043     }
00044     else
00045       return false;
00046   }
00047 
00048   cost *= cost_multiplier;
00049   risk *= risk_multiplier;
00050 
00051   return risk < max_risk_;
00052 }
00053 
00054 bool StepCostEstimator::getCost(const State& left_foot, const State& right_foot, const State& swing_foot, float& cost, float& risk) const
00055 {
00056   double cost_d, risk_d;
00057   bool result = getCost(left_foot, right_foot, swing_foot, cost_d, risk_d);
00058   cost = static_cast<float>(cost_d);
00059   risk = static_cast<float>(risk_d);
00060   return result;
00061 }
00062 }


vigir_footstep_planning_plugins
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:48:02