3 #include <angles/angles.h> 10 : StepCostEstimatorPlugin(
"boundary_step_cost_estimator")
16 if (!StepCostEstimatorPlugin::loadParams(params))
19 params.getParam(
"boundary_step_cost_estimator/max_diff_z",
max_diff_z);
20 params.getParam(
"boundary_step_cost_estimator/long_step_dist",
long_step_dist);
23 params.getParam(
"boundary_step_cost_estimator/cost_roll_abs",
cost_roll_abs);
24 params.getParam(
"boundary_step_cost_estimator/cost_pitch_abs",
cost_pitch_abs);
25 params.getParam(
"boundary_step_cost_estimator/cost_yaw_rel",
cost_yaw_rel);
31 bool BoundaryStepCostEstimator::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 if (swing_foot == left_foot || swing_foot == right_foot)
41 const State& stand_foot = swing_foot.getLeg() == LEFT ? right_foot : left_foot;
42 const State& swing_foot_before = swing_foot.getLeg() == LEFT ? left_foot : right_foot;
44 double diff_z = std::abs(swing_foot.getZ()-stand_foot.getZ());
51 tf::Transform step = stand_foot.getPose().inverse() * swing_foot.getPose();
58 double stance_yaw_diff = angles::shortest_angular_distance(swing_foot_before.getYaw(), stand_foot.getYaw());
59 double swing_foot_yaw_diff = angles::shortest_angular_distance(stand_foot.getYaw(), swing_foot.getYaw());
66 double turn_rate_diff = swing_foot_yaw_diff - stance_yaw_diff;
68 if (turn_rate_diff * swing_foot_yaw_diff <= 0.0)
83 #include <pluginlib/class_list_macros.h>