8 : StepCostEstimatorPlugin(
"travel_time_cost_estimator")
14 if (!StepCostEstimatorPlugin::loadParams(params))
17 params.getParam(
"travel_time_cost_estimator/sway/parabol_a",
a_sway_inv, 0.0);
19 params.getParam(
"travel_time_cost_estimator/sway/parabol_b",
b_sway_inv, 0.0);
21 params.getParam(
"travel_time_cost_estimator/sway/const_time",
const_sway_time, 0.0);
23 params.getParam(
"travel_time_cost_estimator/swing/parabol_a",
a_swing_inv, 0.0);
25 params.getParam(
"travel_time_cost_estimator/swing/parabol_b",
b_swing_inv, 0.0);
27 params.getParam(
"travel_time_cost_estimator/swing/const_time",
const_swing_time, 0.0);
32 bool TravelTimeStepCostEstimator::getCost(
const State& left_foot,
const State& right_foot,
const State& swing_foot,
double& cost,
double& cost_multiplier,
double& risk,
double& risk_multiplier)
const 34 cost_multiplier = 1.0;
36 risk_multiplier = 1.0;
38 const State& stand_foot = swing_foot.getLeg() == LEFT ? right_foot : left_foot;
39 const State& swing_foot_before = swing_foot.getLeg() == LEFT ? left_foot : right_foot;
41 tf::Transform sway = swing_foot_before.getPose().inverse() * stand_foot.getPose();
42 tf::Transform swing = swing_foot_before.getPose().inverse() * swing_foot.getPose();
47 cost = sway_duration + step_duration;
53 #include <pluginlib/class_list_macros.h>