Removed duplicated code from the FootstepCostCalculator that lives in…

… the HeightMapCliffAvoider
ihmcrobotics · Nov 19, 2024 · dcebecf · dcebecf
1 parent 857e59b
commit dcebecf
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 81 deletions.
diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/AStarFootstepPlanner.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/AStarFootstepPlanner.java
@@ -89,10 +89,10 @@ public AStarFootstepPlanner(DefaultFootstepPlannerParametersBasics footstepPlann
       this.footstepPlannerParameters = footstepPlannerParameters;
       this.bodyPathPlanHolder = bodyPathPlanHolder;
       this.footPolygons = footPolygons;
-      this.plannerEnvironmentHandler = new FootstepPlannerEnvironmentHandler();
-      this.snapper = new FootstepSnapAndWiggler(footPolygons, footstepPlannerParameters, plannerEnvironmentHandler);
       this.stopwatch = stopwatch;
       this.statusCallbacks = statusCallbacks;
+      this.plannerEnvironmentHandler = new FootstepPlannerEnvironmentHandler();
+      this.snapper = new FootstepSnapAndWiggler(footPolygons, footstepPlannerParameters, plannerEnvironmentHandler);
 
       this.heightMapFootstepChecker = new HeightMapFootstepChecker(footstepPlannerParameters,
                                                                    footPolygons,
@@ -105,9 +105,9 @@ public AStarFootstepPlanner(DefaultFootstepPlannerParametersBasics footstepPlann
                                                          bodyPathPlanHolder,
                                                          plannerEnvironmentHandler,
                                                          registry);
-      this.stepCostCalculator = new FootstepCostCalculator(footstepPlannerParameters, snapper, idealStepCalculator, footPolygons, registry);
 
       this.nominalExpansion = new ParameterBasedStepExpansion(footstepPlannerParameters, idealStepCalculator, footPolygons);
+      this.stepCostCalculator = new FootstepCostCalculator(footstepPlannerParameters, snapper, idealStepCalculator, registry);
       this.iterationConductor = new AStarFootstepPlannerIterationConductor(nominalExpansion, heightMapFootstepChecker, stepCostCalculator, idealStepCalculator);
 
       this.completionChecker = new FootstepPlannerCompletionChecker(footstepPlannerParameters, iterationConductor, idealStepCalculator, snapper);
@@ -165,9 +165,7 @@ public void handleRequest(FootstepPlannerRequest request, FootstepPlannerOutput
       snapper.clearSnapData();
       plannerEnvironmentHandler.setHeightMap(heightMapData);
       plannerEnvironmentHandler.setTerrainMapData(terrainMapData);
-
       heightMapFootstepChecker.setHeightMapData(heightMapData);
-      stepCostCalculator.setHeightMapData(heightMapData);
 
       double pathLength = bodyPathPlanHolder.computePathLength(0.0);
       boolean imposeHorizonLength =

diff --git a/...g/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepCost/FootstepCostCalculator.java b/...g/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepCost/FootstepCostCalculator.java
@@ -1,13 +1,7 @@
 package us.ihmc.footstepPlanning.graphSearch.stepCost;
 
-import us.ihmc.euclid.Axis3D;
-import us.ihmc.euclid.geometry.ConvexPolygon2D;
-import us.ihmc.euclid.geometry.Plane3D;
-import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly;
 import us.ihmc.euclid.tools.EuclidCoreTools;
 import us.ihmc.euclid.transform.RigidBodyTransform;
-import us.ihmc.euclid.transform.interfaces.RigidBodyTransformReadOnly;
-import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
 import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapDataReadOnly;
 import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapperReadOnly;
 import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep;
@@ -16,26 +10,18 @@
 import us.ihmc.footstepPlanning.graphSearch.stepExpansion.IdealStepCalculatorInterface;
 import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersReadOnly;
 import us.ihmc.robotics.geometry.AngleTools;
-import us.ihmc.robotics.robotSide.RobotSide;
-import us.ihmc.robotics.robotSide.SideDependentList;
-import us.ihmc.sensorProcessing.heightMap.HeightMapData;
 import us.ihmc.yoVariables.registry.YoRegistry;
-import us.ihmc.yoVariables.variable.YoBoolean;
 import us.ihmc.yoVariables.variable.YoDouble;
 
 import java.util.function.ToDoubleFunction;
 
 public class FootstepCostCalculator implements FootstepCostCalculatorInterface
 {
-   private static final boolean enableCliffCost = false;
-   private static final double cliffCost = 0.3;
-
    private final YoRegistry registry = new YoRegistry(getClass().getSimpleName());
    private final DefaultFootstepPlannerParametersReadOnly parameters;
    private final FootstepSnapperReadOnly snapper;
    private final IdealStepCalculatorInterface idealStepCalculator;
    private final ToDoubleFunction<FootstepGraphNode> heuristics;
-   private final SideDependentList<ConvexPolygon2DReadOnly> scaledFootPolygons = new SideDependentList<>();
 
    private final RigidBodyTransform stanceStepTransform = new RigidBodyTransform();
    private final RigidBodyTransform idealStepTransform = new RigidBodyTransform();
@@ -52,32 +38,16 @@ public class FootstepCostCalculator implements FootstepCostCalculatorInterface
    private final YoDouble pitchOffset = new YoDouble("pitchOffset", registry);
    private final YoDouble rollOffset = new YoDouble("rollOffset", registry);
 
-   private HeightMapData heightMapData;
-   private final YoBoolean cliffDetected = new YoBoolean("cliffDetected", registry);
-   private final ConvexPolygon2D scaledFootPolygon = new ConvexPolygon2D();
-   private final Plane3D bestFitPlane = new Plane3D();
-
    public FootstepCostCalculator(DefaultFootstepPlannerParametersReadOnly parameters,
                                  FootstepSnapperReadOnly snapper,
                                  IdealStepCalculatorInterface idealStepCalculator,
-                                 SideDependentList<? extends ConvexPolygon2DReadOnly> footPolygons,
                                  YoRegistry parentRegistry)
    {
       this.parameters = parameters;
       this.snapper = snapper;
       this.idealStepCalculator = idealStepCalculator;
       this.heuristics = idealStepCalculator.getFootstepPlannerHeuristicCalculator()::compute;
 
-      /* Scale's by a factor of the foot length/width */
-      double polygonScaleFactor = 0.65;
-
-      for (RobotSide robotSide : RobotSide.values())
-      {
-         ConvexPolygon2D scaledFootPolygon = new ConvexPolygon2D(footPolygons.get(robotSide));
-         scaledFootPolygon.scale(1.0 + polygonScaleFactor);
-         scaledFootPolygons.put(robotSide, scaledFootPolygon);
-      }
-
       parentRegistry.addChild(registry);
    }
 
@@ -120,11 +90,6 @@ public double computeCost(DiscreteFootstep candidateStep, DiscreteFootstep stanc
          edgeCost.add(rmsAlpha * parameters.getRMSErrorCost());
       }
 
-      if (heightMapData != null && enableCliffCost)
-      {
-         edgeCost.add(computeHeightMapCliffCost(candidateStep));
-      }
-
       edgeCost.add(computeAreaCost(candidateStep));
       edgeCost.add(parameters.getCostPerStep());
 
@@ -147,11 +112,6 @@ public double computeCost(DiscreteFootstep candidateStep, DiscreteFootstep stanc
       return edgeCost.getValue();
    }
 
-   public void setHeightMapData(HeightMapData heightMapData)
-   {
-      this.heightMapData = heightMapData;
-   }
-
    private double computeAreaCost(DiscreteFootstep footstep)
    {
       FootstepSnapDataReadOnly snapData = snapper.snapFootstep(footstep);
@@ -171,41 +131,6 @@ private double computeAreaCost(DiscreteFootstep footstep)
       }
    }
 
-   private double computeHeightMapCliffCost(DiscreteFootstep footstep)
-   {
-      /* Transform to step location */
-      FootstepSnapDataReadOnly snapData = snapper.snapFootstep(footstep);
-      DiscreteFootstepTools.getFootPolygon(footstep, scaledFootPolygons.get(footstep.getRobotSide()), scaledFootPolygon);
-      RigidBodyTransformReadOnly snapTransform = snapData.getSnapTransform();
-      scaledFootPolygon.applyTransform(snapTransform, false);
-
-      /* Compute best-fit plane */
-      RigidBodyTransformReadOnly snappedStepTransform = snapData.getSnappedStepTransform(footstep);
-      bestFitPlane.getPoint().set(snappedStepTransform.getTranslation());
-      bestFitPlane.getNormal().set(Axis3D.Z);
-      snappedStepTransform.getRotation().transform(bestFitPlane.getNormal());
-
-      /* Compute cliff cost */
-      double cliffThreshold = 0.07;
-
-      for (int pointIdx = 0; pointIdx < scaledFootPolygon.getNumberOfVertices(); pointIdx++)
-      {
-         Point2DReadOnly polygonVertex = scaledFootPolygon.getVertex(pointIdx);
-         double zBestFitPlane = bestFitPlane.getZOnPlane(polygonVertex.getX(), polygonVertex.getY());
-         double zHeightMap = heightMapData.getHeightAt(polygonVertex.getX(), polygonVertex.getY());
-         double distanceFromBestFitPlane = Math.abs(zBestFitPlane - zHeightMap);
-
-         if (distanceFromBestFitPlane > cliffThreshold)
-         {
-            cliffDetected.set(true);
-            return cliffCost;
-         }
-      }
-
-      cliffDetected.set(false);
-      return 0.0;
-   }
-
    public void resetLoggedVariables()
    {
       edgeCost.setToNaN();

diff --git a/...anning/src/test/java/us/ihmc/footstepPlanning/graphSearch/FootstepCostCalculatorTest.java b/...anning/src/test/java/us/ihmc/footstepPlanning/graphSearch/FootstepCostCalculatorTest.java
@@ -48,7 +48,6 @@ public void testFootstepCostCalculation()
       FootstepCostCalculator stepCostCalculator = new FootstepCostCalculator(footstepPlannerParameters,
                                                                              snapper,
                                                                              idealStepCalculator,
-                                                                             defaultFootPolygons,
                                                                              registry);
       int numberOfTests = 1000;