00001 #include <vigir_footstep_planning_default_plugins/step_cost_estimators/ground_contact_step_cost_estimator.h> 00002 00003 #include <angles/angles.h> 00004 00005 00006 00007 namespace vigir_footstep_planning 00008 { 00009 GroundContactStepCostEstimator::GroundContactStepCostEstimator() 00010 : StepCostEstimatorPlugin("ground_contact_step_cost_estimator") 00011 { 00012 } 00013 00014 bool GroundContactStepCostEstimator::loadParams(const vigir_generic_params::ParameterSet& params) 00015 { 00016 if (!StepCostEstimatorPlugin::loadParams(params)) 00017 return false; 00018 00019 params.getParam("foot_contact_support/minimal_support", min_contact_support); 00020 return true; 00021 } 00022 00023 bool GroundContactStepCostEstimator::getCost(const State& /*left_foot*/, const State& /*right_foot*/, const State& swing_foot, double& cost, double& cost_multiplier, double& risk, double& risk_multiplier) const 00024 { 00025 cost = 0.0; 00026 cost_multiplier = 1.0; 00027 risk = 0.0; 00028 risk_multiplier = 1.0; 00029 00030 if (swing_foot.getGroundContactSupport() < 1.0) 00031 { 00032 if (swing_foot.getGroundContactSupport() > min_contact_support) 00033 cost_multiplier = risk_multiplier = 1.0/swing_foot.getGroundContactSupport(); 00034 else 00035 return false; 00036 } 00037 00038 return true; 00039 } 00040 } 00041 00042 #include <pluginlib/class_list_macros.h> 00043 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::GroundContactStepCostEstimator, vigir_footstep_planning::StepCostEstimatorPlugin)