public static boolean isGoalNextToLastStep(Point3D desiredPosition, FootstepPlan footstepPlan, double epsilon) { Point3D goalPosition = getEndPosition(footstepPlan); if (goalPosition.epsilonEquals(desiredPosition, epsilon)) return true; else return false; }
private String assertPlanIsValid(String datasetName, FootstepPlanningResult result, FootstepPlan plan, Point3D goal) { String errorMessage = ""; if(!result.validForExecution()) { errorMessage = "Planning result for " + datasetName + " is invalid, result was " + result; } else if(!PlannerTools.isGoalNextToLastStep(goal, plan)) { errorMessage = datasetName + " did not reach goal. Made it to " + PlannerTools.getEndPosition(plan) + ", trying to get to " + goal; } if((VISUALIZE || DEBUG) && !errorMessage.isEmpty()) LogTools.error(errorMessage); return errorMessage; }
protected String assertPlanIsValid(String datasetName, FootstepPlanningResult result, FootstepPlan plan, Point3D goal) { String errorMessage = ""; if(!result.validForExecution()) { errorMessage = "Planning result for " + datasetName + " is invalid, result was " + result; } else if(!PlannerTools.isGoalNextToLastStep(goal, plan)) { errorMessage = datasetName + " did not reach goal. Made it to " + PlannerTools.getEndPosition(plan) + ", trying to get to " + goal; } if((VISUALIZE || DEBUG) && !errorMessage.isEmpty()) LogTools.error(errorMessage); return errorMessage; }
protected String assertPlanIsValid(String datasetName, FootstepPlanningResult result, FootstepPlan plan, Point3D goal) { String errorMessage = ""; errorMessage += assertTrue(datasetName, "Planning result for " + datasetName + " is invalid, result was " + result, result.validForExecution()); if (result.validForExecution()) { errorMessage += assertTrue(datasetName, datasetName + " did not reach goal. Made it to " + PlannerTools.getEndPosition(plan) + ", trying to get to " + goal, PlannerTools.isGoalNextToLastStep(goal, plan)); } return errorMessage; }