Skip to content

Commit 220ba22

Browse files
PotatoPeeler3000PotatoPeeler3000
committed
Cleaned up the ParameterBasedStepExpansion and tunes some parameters, updated a couple tests as well
1 parent f38284f commit 220ba22

File tree

3 files changed

+102
-64
lines changed

3 files changed

+102
-64
lines changed

‎ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/parameters/DefaultFootstepPlannerParameters.java‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -77,8 +77,10 @@ public class DefaultFootstepPlannerParameters extends StoredPropertySet implemen
7777
public static final DoubleStoredPropertyKey scaledFootPolygonPercentage = keys.addDoubleKey("Scaled foot polygon percentage");
7878
public static final DoubleStoredPropertyKey cliffHeightThreshold = keys.addDoubleKey("Cliff height threshold");
7979

80-
81-
public DefaultFootstepPlannerParameters() // for tests and stuff that's probably not gonna save
80+
/**
81+
* These parameters are used for tests, please don't use on an actual robot, and please don't change these values without checking that all the tests pass
82+
*/
83+
public DefaultFootstepPlannerParameters()
8284
{
8385
this(null);
8486
}

‎ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedStepExpansion.java‎

Lines changed: 30 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,6 @@
22

33
import gnu.trove.list.array.TDoubleArrayList;
44
import gnu.trove.list.array.TIntArrayList;
5-
import us.ihmc.commons.InterpolationTools;
65
import us.ihmc.commons.MathTools;
76
import us.ihmc.euclid.axisAngle.AxisAngle;
87
import us.ihmc.euclid.geometry.ConvexPolygon2D;
@@ -20,6 +19,8 @@
2019

2120
public class ParameterBasedStepExpansion implements FootstepExpansion
2221
{
22+
private static final boolean SORT_FULL_EXPANSION = false;
23+
2324
private final List<FootstepGraphNode> fullExpansion = new ArrayList<>();
2425
private final DefaultFootstepPlannerParametersReadOnly parameters;
2526
private final IdealStepCalculatorInterface idealStepCalculator;
@@ -38,7 +39,9 @@ public class ParameterBasedStepExpansion implements FootstepExpansion
3839
private final TIntArrayList xyExpansionMask = new TIntArrayList();
3940
private final TIntArrayList yawExpansionMask = new TIntArrayList();
4041

41-
public ParameterBasedStepExpansion(DefaultFootstepPlannerParametersReadOnly parameters, IdealStepCalculatorInterface idealStepCalculator, SideDependentList<ConvexPolygon2D> footPolygons)
42+
public ParameterBasedStepExpansion(DefaultFootstepPlannerParametersReadOnly parameters,
43+
IdealStepCalculatorInterface idealStepCalculator,
44+
SideDependentList<ConvexPolygon2D> footPolygons)
4245
{
4346
this.parameters = parameters;
4447
this.idealStepCalculator = idealStepCalculator;
@@ -76,12 +79,8 @@ public void initialize()
7679
if (reachSquared > maxReachSquared)
7780
continue;
7881

79-
double reachFraction = EuclidCoreTools.fastSquareRoot(reachSquared) / parameters.getMaxStepReach();
80-
double minYawAtFullExtension = parameters.getMinStepYaw();
81-
double maxYawAtFullExtension = parameters.getMaxStepYaw();
82-
83-
double minYaw = InterpolationTools.linearInterpolate(parameters.getMinStepYaw(), minYawAtFullExtension, reachFraction);
84-
double maxYaw = InterpolationTools.linearInterpolate(parameters.getMaxStepYaw(), maxYawAtFullExtension, reachFraction);
82+
double minYaw = parameters.getMinStepYaw();
83+
double maxYaw = parameters.getMaxStepYaw();
8584

8685
for (double yaw = minYaw; yaw <= maxYaw; yaw += LatticePoint.gridSizeYaw)
8786
{
@@ -148,13 +147,16 @@ public void doFullExpansion(FootstepGraphNode nodeToExpand, List<FootstepGraphNo
148147
applyMask(fullExpansionToPack, nodeToExpand);
149148
}
150149

151-
// sorting is primarily a debug tool for checking proximity to ideal step - skip by default
150+
// Sorting is primarily a debug tool for checking proximity to ideal step
151+
if (SORT_FULL_EXPANSION)
152+
{
153+
if (idealStepCalculator != null)
154+
{
155+
idealStepProximityComparator.update(nodeToExpand, idealStepCalculator);
156+
fullExpansionToPack.sort(idealStepProximityComparator);
157+
}
158+
}
152159

153-
// if (idealStepCalculator != null)
154-
// {
155-
// idealStepProximityComparator.update(nodeToExpand, idealStepCalculator);
156-
// fullExpansionToPack.sort(idealStepProximityComparator);
157-
// }
158160
}
159161

160162
private void applyMask(List<FootstepGraphNode> listToFilter, FootstepGraphNode stanceNode)
@@ -177,18 +179,6 @@ private void applyMask(List<FootstepGraphNode> listToFilter, FootstepGraphNode s
177179
});
178180
}
179181

180-
private static int computeMinYawDistance(List<FootstepGraphNode> listToFilter, DiscreteFootstep idealStep)
181-
{
182-
int minYawDistance = Integer.MAX_VALUE;
183-
for (int i = 0; i < listToFilter.size(); i++)
184-
{
185-
int yawDistance = idealStep.computeYawIndexDistance(listToFilter.get(i).getSecondStep());
186-
if (yawDistance < minYawDistance)
187-
minYawDistance = yawDistance;
188-
}
189-
return minYawDistance;
190-
}
191-
192182
private static int computeMinXYManhattanDistance(List<FootstepGraphNode> listToFilter, DiscreteFootstep idealStep)
193183
{
194184
int minXYManhattanDistance = Integer.MAX_VALUE;
@@ -201,6 +191,18 @@ private static int computeMinXYManhattanDistance(List<FootstepGraphNode> listToF
201191
return minXYManhattanDistance;
202192
}
203193

194+
private static int computeMinYawDistance(List<FootstepGraphNode> listToFilter, DiscreteFootstep idealStep)
195+
{
196+
int minYawDistance = Integer.MAX_VALUE;
197+
for (int i = 0; i < listToFilter.size(); i++)
198+
{
199+
int yawDistance = idealStep.computeYawIndexDistance(listToFilter.get(i).getSecondStep());
200+
if (yawDistance < minYawDistance)
201+
minYawDistance = yawDistance;
202+
}
203+
return minYawDistance;
204+
}
205+
204206
static class IdealStepProximityComparator implements Comparator<FootstepGraphNode>
205207
{
206208
private DiscreteFootstep idealStep = null;
@@ -239,8 +241,8 @@ private DiscreteFootstep constructNodeInPreviousNodeFrame(double stepLength, dou
239241
footstepRotation.transform(footstepTranslation);
240242

241243
return new DiscreteFootstep(step.getX() + footstepTranslation.getX(),
242-
step.getY() + footstepTranslation.getY(),
243-
stepYaw + step.getYaw(),
244+
step.getY() + footstepTranslation.getY(),
245+
stepYaw + step.getYaw(),
244246
step.getRobotSide().getOppositeSide());
245247
}
246248
}

‎ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/graphSearch/stepExpansion/ParameterBasedNodeExpansionTest.java‎

Lines changed: 68 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,8 @@
11
package us.ihmc.footstepPlanning.graphSearch.stepExpansion;
22

33
import org.junit.jupiter.api.Assertions;
4+
import org.junit.jupiter.api.BeforeEach;
5+
import org.junit.jupiter.api.Disabled;
46
import org.junit.jupiter.api.Test;
57
import us.ihmc.commons.InterpolationTools;
68
import us.ihmc.euclid.geometry.ConvexPolygon2D;
@@ -19,81 +21,105 @@
1921
import java.util.*;
2022
import java.util.function.ToDoubleFunction;
2123

22-
import static us.ihmc.robotics.Assert.assertTrue;
24+
import static org.junit.jupiter.api.Assertions.*;
2325

2426
public class ParameterBasedNodeExpansionTest
2527
{
2628
private static final double epsilon = 1e-6;
29+
private DefaultFootstepPlannerParameters parameters;
30+
31+
@BeforeEach
32+
public void setupParameters()
33+
{
34+
// We create default parameters for the tests
35+
parameters = new DefaultFootstepPlannerParameters();
36+
}
2737

2838
@Test
29-
public void testExpansionAlongBoundsFromOriginDefaultParametersWithRight()
39+
public void testExpansionAlongBoundsFromOriginWithRight()
3040
{
31-
DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters();
3241
ParameterBasedStepExpansion expansion = new ParameterBasedStepExpansion(parameters, null, PlannerTools.createDefaultFootPolygons());
3342
expansion.initialize();
3443

35-
double maxYaw = parameters.getMaxStepYaw();
36-
double minYaw = parameters.getMinStepYaw();
37-
38-
List<FootstepGraphNode> childNodes = new ArrayList<>();
39-
44+
// Set up the feet to test moving a right foot
4045
DiscreteFootstep stanceStep = new DiscreteFootstep(0.0, 0.0, 0.0, RobotSide.LEFT);
4146
DiscreteFootstep startOfSwingStep = new DiscreteFootstep(0.0, 0.3, 0.0, RobotSide.RIGHT);
4247

48+
// Do full expansion of the steps
49+
List<FootstepGraphNode> childNodes = new ArrayList<>();
4350
expansion.doFullExpansion(new FootstepGraphNode(startOfSwingStep, stanceStep), childNodes);
51+
52+
// Check the edges of where steps can reach in the x and y directions
4453
DiscreteFootstep mostForward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getX()));
45-
DiscreteFootstep furthestReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth())));
4654
DiscreteFootstep mostBackward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getX()));
4755
DiscreteFootstep mostInward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getY()));
4856
DiscreteFootstep mostOutward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getY()));
57+
58+
assertTrue(mostForward.getX() <= parameters.getMaxStepReach());
59+
assertTrue(mostBackward.getX() >= parameters.getMinStepLength());
60+
assertTrue(mostInward.getY() <= -parameters.getMinStepWidth());
61+
assertTrue(mostOutward.getY() >= -parameters.getMaxStepWidth());
62+
63+
// Check the min and max a step can yaw
4964
DiscreteFootstep mostOutwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -snapToCircle(node.getYaw())));
5065
DiscreteFootstep mostInwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> snapToCircle(node.getYaw())));
5166

52-
assertTrue(mostForward.getX() < parameters.getMaxStepReach() + epsilon);
53-
assertTrue(mostBackward.getX() > parameters.getMinStepLength() - epsilon);
54-
assertTrue(mostInward.getY() < -parameters.getMinStepWidth() + epsilon);
55-
assertTrue(mostOutward.getY() > -parameters.getMaxStepWidth() - epsilon);
67+
// The yaw is a little tricky because we don't have any notion of things being negative.
68+
// So if the yaw is clockwise (which should be negative) it will be ~5.7 or something just less then 2 PI radians
69+
// To account for this, we need to subtract the yaw we got from 2 PI.
70+
double outwardYawFromZero = Math.PI * 2 - mostOutwardYawed.getYaw();
71+
assertTrue(outwardYawFromZero <= parameters.getMaxStepYaw());
72+
assertTrue(mostInwardYawed.getYaw() >= parameters.getMinStepYaw());
5673

57-
assertTrue(getReachAtNode(furthestReach, parameters.getIdealFootstepWidth()) < parameters.getMaxStepReach());
74+
// Get the footstep closest to the ideal step and ensure that it's less than the max reach
75+
DiscreteFootstep idealReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth())));
76+
assertTrue(getReachAtNode(idealReach, parameters.getIdealFootstepWidth()) < parameters.getMaxStepReach());
5877
}
5978

6079
@Test
61-
public void testExpansionAlongBoundsFromOriginDefaultParametersWithLeft()
80+
public void testExpansionAlongBoundsFromOriginWithLeft()
6281
{
63-
DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters();
6482
ParameterBasedStepExpansion expansion = new ParameterBasedStepExpansion(parameters, null, PlannerTools.createDefaultFootPolygons());
6583
expansion.initialize();
6684

67-
double maxYaw = parameters.getMaxStepYaw();
68-
double minYaw = parameters.getMinStepYaw();
69-
85+
// Set up the feet to test moving a right foot
7086
DiscreteFootstep stanceStep = new DiscreteFootstep(0.0, 0.0, 0.0, RobotSide.RIGHT);
7187
DiscreteFootstep startOfSwingStep = new DiscreteFootstep(0.0, -0.3, 0.0, RobotSide.LEFT);
7288

89+
// Do full expansion of the steps
7390
List<FootstepGraphNode> childNodes = new ArrayList<>();
7491
expansion.doFullExpansion(new FootstepGraphNode(startOfSwingStep, stanceStep), childNodes);
92+
93+
// Check the edges of where steps can reach in the x and y directions
7594
DiscreteFootstep mostForward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getX()));
76-
DiscreteFootstep furthestReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth())));
7795
DiscreteFootstep mostBackward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getX()));
7896
DiscreteFootstep mostInward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getY()));
7997
DiscreteFootstep mostOutward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getY()));
80-
DiscreteFootstep mostOutwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> snapToCircle(node.getYaw())));
81-
DiscreteFootstep mostInwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -snapToCircle(node.getYaw())));
8298

83-
assertTrue(mostForward.getX() < parameters.getMaxStepReach() + epsilon);
84-
assertTrue(mostBackward.getX() > parameters.getMinStepLength() - epsilon);
85-
assertTrue(mostInward.getY() > parameters.getMinStepWidth() - epsilon);
86-
assertTrue(mostOutward.getY() < parameters.getMaxStepWidth() + epsilon);
99+
assertTrue(mostForward.getX() <= parameters.getMaxStepReach());
100+
assertTrue(mostBackward.getX() >= parameters.getMinStepLength());
101+
assertTrue(mostInward.getY() >= parameters.getMinStepWidth());
102+
assertTrue(mostOutward.getY() <= parameters.getMaxStepWidth());
87103

88-
double mostOutwardYawedReach = getReachAtNode(mostOutwardYawed, parameters.getIdealFootstepWidth());
89-
double mostInwardYawedReach = getReachAtNode(mostInwardYawed, parameters.getIdealFootstepWidth());
90-
assertTrue(getReachAtNode(furthestReach, parameters.getIdealFootstepWidth()) < parameters.getMaxStepReach());
104+
// Check the min and max a step can yaw
105+
DiscreteFootstep mostOutwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -snapToCircle(node.getYaw())));
106+
DiscreteFootstep mostInwardYawed = getExtremumNode(childNodes, Comparator.comparingDouble(node -> snapToCircle(node.getYaw())));
107+
108+
// The yaw is a little tricky because we don't have any notion of things being negative.
109+
// So if the yaw is clockwise (which should be negative) it will be ~5.7 or something just less then 2 PI radians
110+
// To account for this, we need to subtract the yaw we got from 2 PI.
111+
double outwardYawFromZero = Math.PI * 2 - mostOutwardYawed.getYaw();
112+
assertTrue(outwardYawFromZero <= parameters.getMaxStepYaw());
113+
assertTrue(mostInwardYawed.getYaw() >= parameters.getMinStepYaw());
114+
115+
// Get the footstep closest to the ideal step and ensure that it's less than the max reach
116+
DiscreteFootstep idealReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth())));
117+
assertTrue(getReachAtNode(idealReach, parameters.getIdealFootstepWidth()) < parameters.getMaxStepReach());
91118
}
92119

93120
@Test
94121
public void testExpansionAlongBoundsFromOrigin()
95122
{
96-
DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters();
97123
ParameterBasedStepExpansion expansion = new ParameterBasedStepExpansion(parameters, null, PlannerTools.createDefaultFootPolygons());
98124
expansion.initialize();
99125

@@ -112,7 +138,8 @@ public void testExpansionAlongBoundsFromOrigin()
112138
List<FootstepGraphNode> childNodes = new ArrayList<>();
113139
expansion.doFullExpansion(new FootstepGraphNode(startOfSwingStep, stanceStep), childNodes);
114140
DiscreteFootstep mostForward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getX()));
115-
DiscreteFootstep furthestReach = getExtremumNode(childNodes, Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth())));
141+
DiscreteFootstep furthestReach = getExtremumNode(childNodes,
142+
Comparator.comparingDouble(node -> getReachAtNode(node, parameters.getIdealFootstepWidth())));
116143
DiscreteFootstep mostBackward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getX()));
117144
DiscreteFootstep mostInward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> node.getY()));
118145
DiscreteFootstep mostOutward = getExtremumNode(childNodes, Comparator.comparingDouble(node -> -node.getY()));
@@ -171,11 +198,11 @@ else if (comparator.compare(node.getSecondStep(), extremumNode) == 1)
171198
@Test
172199
public void testPartialExpansionSize()
173200
{
174-
DefaultFootstepPlannerParameters parameters = new DefaultFootstepPlannerParameters();
175201
int branchFactor = 100;
176202
parameters.setMaxBranchFactor(branchFactor);
177203

178-
IdealStepCalculatorInterface idealStepCalculator = (stance, startOfSwing) -> new DiscreteFootstep(stance.getLatticePoint(), stance.getRobotSide().getOppositeSide());
204+
IdealStepCalculatorInterface idealStepCalculator = (stance, startOfSwing) -> new DiscreteFootstep(stance.getLatticePoint(),
205+
stance.getRobotSide().getOppositeSide());
179206
ParameterBasedStepExpansion expansion = new ParameterBasedStepExpansion(parameters, idealStepCalculator, PlannerTools.createDefaultFootPolygons());
180207

181208
expansion.initialize();
@@ -202,7 +229,13 @@ public void testPartialExpansionSize()
202229
Assertions.assertTrue(expansionList.isEmpty());
203230
}
204231

232+
/**
233+
* This test is meant to check if the full expansion returns a sorted list or not.
234+
* We don't always sort the full expansion, so by default, this test is
235+
* disabled.
236+
*/
205237
@Test
238+
@Disabled
206239
public void testFullExpansionReturnsSortedOrder()
207240
{
208241
Random random = new Random(329032);
@@ -230,7 +263,8 @@ public void testFullExpansionReturnsSortedOrder()
230263
expansion.doFullExpansion(node, fullExpansion);
231264
List<FootstepGraphNode> fullExpansionSorted = new ArrayList<>(fullExpansion);
232265

233-
ToDoubleFunction<FootstepGraphNode> stepDistance = step -> ParameterBasedStepExpansion.IdealStepProximityComparator.calculateStepProximity(step.getSecondStep(), idealStep);
266+
ToDoubleFunction<FootstepGraphNode> stepDistance = step -> ParameterBasedStepExpansion.IdealStepProximityComparator.calculateStepProximity(step.getSecondStep(),
267+
idealStep);
234268
Comparator<FootstepGraphNode> sorter = Comparator.comparingDouble(stepDistance);
235269
fullExpansionSorted.sort(sorter);
236270

0 commit comments

Comments
 (0)