Go to the documentation of this file.00001 #include <vigir_footstep_planning_default_plugins/step_cost_estimators/euclidean_step_cost_estimator.h>
00002
00003
00004
00005 namespace vigir_footstep_planning
00006 {
00007 EuclideanStepCostEstimator::EuclideanStepCostEstimator()
00008 : StepCostEstimatorPlugin("euclidean_step_cost_estimator")
00009 {
00010 }
00011
00012 bool EuclideanStepCostEstimator::getCost(const State& left_foot, const State& right_foot, const State& swing_foot, double& cost, double& cost_multiplier, double& risk, double& risk_multiplier) const
00013 {
00014 cost = 0.0;
00015 cost_multiplier = 1.0;
00016 risk = 0.0;
00017 risk_multiplier = 1.0;
00018
00019 if (swing_foot == left_foot || swing_foot == right_foot)
00020 return true;
00021
00022 const State& swing_foot_before = swing_foot.getLeg() == LEFT ? left_foot : right_foot;
00023 cost = 0.5*euclidean_distance(swing_foot_before.getX(), swing_foot_before.getY(), swing_foot.getX(), swing_foot.getY());
00024
00025 return true;
00026 }
00027 }
00028
00029 #include <pluginlib/class_list_macros.h>
00030 PLUGINLIB_EXPORT_CLASS(vigir_footstep_planning::EuclideanStepCostEstimator, vigir_footstep_planning::StepCostEstimatorPlugin)