@Override
public double calculateCost(FramePose stanceFoot, FramePose swingStartFoot, FramePose idealFootstep, FramePose candidateFootstep, double percentageOfFoothold)
{
double cost = footstepBaseCost.getDoubleValue();
setXYVectorFromPoseToPoseNormalize(forwardCostVector, swingStartFoot, idealFootstep);
setXYVectorFromPoseToPoseNormalize(backwardCostVector, idealFootstep, swingStartFoot);
inwardCostVector.set(forwardCostVector.getY(), -forwardCostVector.getX());
outwardCostVector.set(-forwardCostVector.getY(), forwardCostVector.getX());
upwardCostVector.set(0.0, 0.0, 1.0);
downwardVector.set(0.0, 0.0, -1.0);
setVectorFromPoseToPose(idealToCandidateVector, idealFootstep, candidateFootstep);
setOrientationFromPoseToPose(idealToCandidateOrientation, idealFootstep, candidateFootstep);
double downwardPenalizationWeightConsideringStancePitch = downwardCostScalar.getDoubleValue();
if (stanceFoot.getPitch() < 0)
{
downwardPenalizationWeightConsideringStancePitch += -stanceFoot.getPitch() * stancePitchDownwardCostScalar.getDoubleValue();
}
cost += penalizeCandidateFootstep(forwardCostVector, forwardCostScalar.getDoubleValue());
cost += penalizeCandidateFootstep(backwardCostVector, backwardCostScalar.getDoubleValue());
cost += penalizeCandidateFootstep(inwardCostVector, inwardCostScalar.getDoubleValue());
cost += penalizeCandidateFootstep(outwardCostVector, outwardCostScalar.getDoubleValue());
cost += penalizeCandidateFootstep(upwardCostVector, upwardCostScalar.getDoubleValue());
cost += penalizeCandidateFootstep(downwardVector, downwardPenalizationWeightConsideringStancePitch);
cost += angularCostScalar.getDoubleValue() * Math.abs(idealToCandidateOrientation.getYaw().getDoubleValue());
cost += angularCostScalar.getDoubleValue() * Math.abs(idealToCandidateOrientation.getPitch().getDoubleValue());
cost += angularCostScalar.getDoubleValue() * Math.abs(idealToCandidateOrientation.getRoll().getDoubleValue());
cost += (1.0 - percentageOfFoothold) * negativeFootholdLinearCostScalar.getDoubleValue();
return cost;
}