Skip to content

Commit ffd4f30

Browse files
author
Shlok Agarwal
committed
Merge pull request ihmcrobotics#250 in ROB/ihmc-open-robotics-software from feature/NRI-396-footstep-actionset to develop
Reachable Footstep Expansion specific to Robot. Refactoring FootstepPlanningParameters to QuadTreeFootstepPlanningParameters to avoid confusion between PlanarRegionFootstepPlanningParameters and QuadTreeFootstepPlanningParameters. * commit '41e93c281d98fa4b36c807aca495c0e64efbc811': changes to update reference frames source. renamed footstep planning files for quad-tree and planar region parameters. added V3Exo model in bamboo test tool. renamed footstep planner parameters to planar region parameters to avoid name conflict. Custom reachable footstep based for Atlas and Valkyrie robot. Changed the DRCRobotModel Interface to get robot specific footstep parameters. Added footstep parameters in the Atlas and Valkyrie projects. Changed the create flat and rough terrain constructors to take argument of expansion type. the footstep planning toolbox now can set type of expansion of footstep neighbors based on the robot. added class ReachableFootstepsBasedExpansion which adds neighbors for expansion. the neighbors of atlas are the same as in SimpleSideBasedExpansion. Neighbors for Valkyrie robot have been added.
2 parents 4469a6b + 41e93c2 commit ffd4f30

File tree

20 files changed

+397
-24
lines changed

20 files changed

+397
-24
lines changed

‎Atlas/src/us/ihmc/atlas/AtlasRobotModel.java‎

Lines changed: 18 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,10 @@
77
import us.ihmc.atlas.initialSetup.AtlasSimInitialSetup;
88
import us.ihmc.atlas.parameters.AtlasContactPointParameters;
99
import us.ihmc.atlas.parameters.AtlasContinuousCMPPlannerParameters;
10+
import us.ihmc.atlas.parameters.AtlasFootstepPlannerParameters;
1011
import us.ihmc.atlas.parameters.AtlasFootstepPlanningParameters;
1112
import us.ihmc.atlas.parameters.AtlasPhysicalProperties;
13+
import us.ihmc.atlas.parameters.AtlasReachableFootstepExpansion;
1214
import us.ihmc.atlas.parameters.AtlasSensorInformation;
1315
import us.ihmc.atlas.parameters.AtlasSmoothCMPPlannerParameters;
1416
import us.ihmc.atlas.parameters.AtlasStateEstimatorParameters;
@@ -31,8 +33,9 @@
3133
import us.ihmc.commons.Conversions;
3234
import us.ihmc.euclid.matrix.Matrix3D;
3335
import us.ihmc.euclid.transform.RigidBodyTransform;
36+
import us.ihmc.footstepPlanning.PlanarRegionFootstepPlanningParameters;
3437
import us.ihmc.humanoidRobotics.communication.streamingData.HumanoidGlobalDataProducer;
35-
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.FootstepPlanningParameters;
38+
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.QuadTreeFootstepPlanningParameters;
3639
import us.ihmc.ihmcPerception.depthData.CollisionBoxProvider;
3740
import us.ihmc.modelFileLoaders.ModelFileLoaderConversionsHelper;
3841
import us.ihmc.modelFileLoaders.SdfLoader.DRCRobotSDFLoader;
@@ -99,6 +102,7 @@ public class AtlasRobotModel implements DRCRobotModel, SDFDescriptionMutator
99102
private final ICPWithTimeFreezingPlannerParameters capturePointPlannerParameters;
100103
private final AtlasWalkingControllerParameters walkingControllerParameters;
101104
private final AtlasStateEstimatorParameters stateEstimatorParameters;
105+
private final PlanarRegionFootstepPlanningParameters planarRegionFootstepPlannerParameters;
102106

103107
private final RobotDescription robotDescription;
104108

@@ -153,7 +157,9 @@ public AtlasRobotModel(AtlasRobotVersion atlasVersion, RobotTarget target, boole
153157
capturePointPlannerParameters = new AtlasSmoothCMPPlannerParameters(atlasPhysicalProperties);
154158
else
155159
capturePointPlannerParameters = new AtlasContinuousCMPPlannerParameters(atlasPhysicalProperties);
156-
160+
161+
planarRegionFootstepPlannerParameters = new AtlasFootstepPlannerParameters();
162+
157163
walkingControllerParameters = new AtlasWalkingControllerParameters(target, jointMap, contactPointParameters);
158164
stateEstimatorParameters = new AtlasStateEstimatorParameters(jointMap, sensorInformation, runningOnRealRobot, getEstimatorDT());
159165

@@ -370,7 +376,7 @@ public MultiThreadedRobotControlElement createSimulatedHandController(FloatingRo
370376
}
371377

372378
@Override
373-
public FootstepPlanningParameters getFootstepPlanningParameters()
379+
public QuadTreeFootstepPlanningParameters getQuadTreeFootstepPlanningParameters()
374380
{
375381
return new AtlasFootstepPlanningParameters();
376382
}
@@ -781,4 +787,13 @@ private void modifyLinkInertia(SDFLinkHolder linkHolder, Matrix3D inertia)
781787
{
782788
linkHolder.setInertia(inertia);
783789
}
790+
791+
/**
792+
* Adds robot specific footstep parameters
793+
*/
794+
@Override
795+
public PlanarRegionFootstepPlanningParameters getPlanarRegionFootstepPlannerParameters()
796+
{
797+
return planarRegionFootstepPlannerParameters;
798+
}
784799
}

‎Atlas/src/us/ihmc/atlas/parameters/AtlasFootstepPlannerParameters.java‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
package us.ihmc.atlas.parameters;
2+
3+
import us.ihmc.footstepPlanning.PlanarRegionFootstepPlanningParameters;
4+
import us.ihmc.footstepPlanning.aStar.implementations.ReachableFootstepsBasedExpansion;
5+
6+
public class AtlasFootstepPlannerParameters implements PlanarRegionFootstepPlanningParameters
7+
{
8+
private final double timeout = Double.POSITIVE_INFINITY;
9+
10+
@Override
11+
public ReachableFootstepsBasedExpansion getReachableFootstepExpansion()
12+
{
13+
return new AtlasReachableFootstepExpansion();
14+
}
15+
16+
@Override
17+
public double getTimeout()
18+
{
19+
return timeout;
20+
}
21+
}

‎Atlas/src/us/ihmc/atlas/parameters/AtlasFootstepPlanningParameters.java‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,9 @@
55
import java.util.List;
66

77
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.FootstepPlanState;
8-
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.FootstepPlanningParameters;
8+
import us.ihmc.humanoidRobotics.footstep.footstepGenerator.QuadTreeFootstepPlanningParameters;
99

10-
public class AtlasFootstepPlanningParameters extends FootstepPlanningParameters
10+
public class AtlasFootstepPlanningParameters extends QuadTreeFootstepPlanningParameters
1111
{
1212
public double maxStepUp = 0.20;
1313
public double minStepDown = -0.17;

‎Atlas/src/us/ihmc/atlas/parameters/AtlasReachableFootstepExpansion.java‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
package us.ihmc.atlas.parameters;
2+
3+
import us.ihmc.footstepPlanning.aStar.implementations.ReachableFootstepsBasedExpansion;
4+
/**
5+
* This class adds footstep neighbors specific to Atlas Robot
6+
* @author Shlok Agarwal
7+
*
8+
*/
9+
public class AtlasReachableFootstepExpansion extends ReachableFootstepsBasedExpansion
10+
{
11+
/**
12+
* Adds footstep offsets for Atlas robot
13+
* Note: These are exactly the parameters used in SimpleSideBasedExpansion
14+
*/
15+
@Override
16+
public void addNeighbors()
17+
{
18+
19+
/** Side Steps*/
20+
/** x offset*/ /** y offset*/ /** yaw offset*/
21+
addOffset(0.0 , 0.25 , 0.0);
22+
addOffset(0.0 , 0.25 , 0.174);
23+
addOffset(0.0 , 0.25 , 0.392);
24+
25+
addOffset(0.0 , 0.15 , 0.0);
26+
addOffset(0.0 , 0.20 , 0.0);
27+
addOffset(0.0 , 0.30 , 0.0);
28+
29+
/** Forward Steps*/
30+
/** x offset*/ /** y offset*/ /** yaw offset*/
31+
32+
addOffset(0.05 , 0.25 , 0.0);
33+
addOffset(0.05 , 0.25 , 0.174);
34+
addOffset(0.05 , 0.25 , 0.392);
35+
36+
addOffset(0.1 , 0.25 , 0.0);
37+
addOffset(0.1 , 0.25 , 0.174);
38+
addOffset(0.1 , 0.25 , 0.392);
39+
40+
addOffset(0.2 , 0.25 , 0.0);
41+
addOffset(0.2 , 0.25 , 0.174);
42+
addOffset(0.2 , 0.25 , 0.392);
43+
44+
addOffset(0.3 , 0.25 , 0.0);
45+
addOffset(0.3 , 0.25 , 0.174);
46+
addOffset(0.3 , 0.25 , 0.392);
47+
48+
addOffset(0.4 , 0.25 , 0.0);
49+
addOffset(0.4 , 0.25 , 0.174);
50+
addOffset(0.4 , 0.25 , 0.392);
51+
52+
/** Backward Steps*/
53+
/** x offset*/ /** y offset*/ /** yaw offset*/
54+
55+
addOffset(-0.05 , 0.25 , 0.0);
56+
addOffset(-0.05 , 0.25 , 0.174);
57+
addOffset(-0.05 , 0.25 , 0.392);
58+
59+
addOffset(-0.1 , 0.25 , 0.0);
60+
addOffset(-0.1 , 0.25 , 0.174);
61+
addOffset(-0.1 , 0.25 , 0.392);
62+
63+
addOffset(-0.2 , 0.25 , 0.0);
64+
addOffset(-0.2 , 0.25 , 0.174);
65+
addOffset(-0.2 , 0.25 , 0.392);
66+
67+
addOffset(-0.3 , 0.25 , 0.0);
68+
addOffset(-0.3 , 0.25 , 0.174);
69+
addOffset(-0.3 , 0.25 , 0.392);
70+
71+
addOffset(-0.4 , 0.25 , 0.0);
72+
addOffset(-0.4 , 0.25 , 0.174);
73+
addOffset(-0.4 , 0.25 , 0.392);
74+
75+
/** Turn Step*/
76+
addOffset(-0.047 , 0.25 , 0.392);
77+
78+
}
79+
}

‎IHMCAvatarInterfaces/src/us/ihmc/avatar/drcRobot/DRCRobotModel.java‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
import us.ihmc.avatar.ros.DRCROSPPSTimestampOffsetProvider;
88
import us.ihmc.avatar.sensors.DRCSensorSuiteManager;
99
import us.ihmc.commonWalkingControlModules.configurations.SliderBoardParameters;
10+
import us.ihmc.footstepPlanning.PlanarRegionFootstepPlanningParameters;
1011
import us.ihmc.humanoidRobotics.communication.streamingData.HumanoidGlobalDataProducer;
1112
import us.ihmc.ihmcPerception.depthData.CollisionBoxProvider;
1213
import us.ihmc.multicastLogDataProtocol.modelLoaders.LogModelProvider;
@@ -81,4 +82,9 @@ public default UIParameters getUIParameters()
8182
{
8283
return null;
8384
}
85+
86+
public default PlanarRegionFootstepPlanningParameters getPlanarRegionFootstepPlannerParameters()
87+
{
88+
return null;
89+
}
8490
}

‎IHMCAvatarInterfaces/src/us/ihmc/avatar/networkProcessor/footstepPlanningToolboxModule/FootstepPlanningToolboxController.java‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131
import us.ihmc.footstepPlanning.FootstepPlannerUtils;
3232
import us.ihmc.footstepPlanning.FootstepPlanningResult;
3333
import us.ihmc.footstepPlanning.aStar.AStarFootstepPlanner;
34+
import us.ihmc.footstepPlanning.aStar.FootstepNodeExpansion;
3435
import us.ihmc.footstepPlanning.graphSearch.BipedalFootstepPlannerParameters;
3536
import us.ihmc.footstepPlanning.graphSearch.PlanarRegionBipedalFootstepPlanner;
3637
import us.ihmc.footstepPlanning.graphSearch.PlanarRegionBipedalFootstepPlannerVisualizer;
@@ -86,7 +87,8 @@ private enum Planners
8687
private final RobotContactPointParameters contactPointParameters;
8788
private final WalkingControllerParameters walkingControllerParameters;
8889
private final FootstepDataListWithSwingOverTrajectoriesAssembler footstepDataListWithSwingOverTrajectoriesAssembler;
89-
90+
private final FootstepNodeExpansion expansion;
91+
9092
private final PacketCommunicator packetCommunicator;
9193
private long plannerCount = 0;
9294
private double dt;
@@ -102,14 +104,21 @@ public FootstepPlanningToolboxController(DRCRobotModel drcRobotModel, FullHumano
102104
this.dt = dt;
103105
packetCommunicator.attachListener(PlanarRegionsListMessage.class, createPlanarRegionsConsumer());
104106

107+
/**
108+
* A robot specific node expansion can be achieved with this.
109+
* Currently only supported in A-star planner for Atlas and Valkyrie.
110+
* Use SimpleSideBasedExpansion ( defaults to Atlas) if using other robots or add custom footstep expansion class.
111+
* */
112+
this.expansion = drcRobotModel.getPlanarRegionFootstepPlannerParameters().getReachableFootstepExpansion();
113+
105114
SideDependentList<ConvexPolygon2D> contactPointsInSoleFrame = createFootPolygonsFromContactPoints(contactPointParameters);
106115

107116
humanoidReferenceFrames = createHumanoidReferenceFrames(fullHumanoidRobotModel);
108117
footstepDataListWithSwingOverTrajectoriesAssembler = new FootstepDataListWithSwingOverTrajectoriesAssembler(humanoidReferenceFrames, walkingControllerParameters, parentRegistry, new YoGraphicsListRegistry());
109118

110119
plannerMap.put(Planners.PLANAR_REGION_BIPEDAL, createPlanarRegionBipedalPlanner(contactPointsInSoleFrame, fullHumanoidRobotModel));
111120
plannerMap.put(Planners.PLAN_THEN_SNAP, new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), contactPointsInSoleFrame));
112-
plannerMap.put(Planners.A_STAR, AStarFootstepPlanner.createRoughTerrainPlanner(null, contactPointsInSoleFrame, registry));
121+
plannerMap.put(Planners.A_STAR, AStarFootstepPlanner.createRoughTerrainPlanner(null, contactPointsInSoleFrame, expansion, registry));
113122
activePlanner.set(Planners.PLANAR_REGION_BIPEDAL);
114123

115124
usePlanarRegions.set(true);

‎IHMCFootstepPlanning/src/us/ihmc/footstepPlanning/PlanarRegionFootstepPlanningParameters.java‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
package us.ihmc.footstepPlanning;
2+
3+
import us.ihmc.footstepPlanning.aStar.implementations.ReachableFootstepsBasedExpansion;
4+
5+
public interface PlanarRegionFootstepPlanningParameters
6+
{
7+
public ReachableFootstepsBasedExpansion getReachableFootstepExpansion();
8+
9+
public double getTimeout();
10+
}

‎IHMCFootstepPlanning/src/us/ihmc/footstepPlanning/aStar/AStarFootstepPlanner.java‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import us.ihmc.footstepPlanning.FootstepPlannerGoalType;
1515
import us.ihmc.footstepPlanning.FootstepPlanningResult;
1616
import us.ihmc.footstepPlanning.aStar.implementations.*;
17+
import us.ihmc.footstepPlanning.polygonSnapping.PlanarRegionsListPolygonSnapper;
1718
import us.ihmc.robotics.geometry.FramePose;
1819
import us.ihmc.robotics.geometry.PlanarRegionsList;
1920
import us.ihmc.robotics.referenceFrames.PoseReferenceFrame;
@@ -194,7 +195,7 @@ private void planInternal()
194195
}
195196

196197
RobotSide nodeSide = nodeToExpand.getRobotSide();
197-
if (nodeToExpand.equals(goalNodes.get(nodeSide)))
198+
if (nodeToExpand.equals(goalNodes.get(nodeSide))) // ?
198199
{
199200
goalNode = goalNodes.get(nodeSide.getOppositeSide());
200201
graph.checkAndSetEdge(nodeToExpand, goalNode, 0.0);
@@ -204,6 +205,7 @@ private void planInternal()
204205
HashSet<FootstepNode> neighbors = nodeExpansion.expandNode(nodeToExpand);
205206
for (FootstepNode neighbor : neighbors)
206207
{
208+
/** Checks if the footstep (center of the foot) is on a planar region*/
207209
if (!nodeChecker.isNodeValid(neighbor, nodeToExpand))
208210
continue;
209211

@@ -245,12 +247,12 @@ private void checkGoalType(FootstepPlannerGoal goal)
245247
throw new RuntimeException("Planner does not support goals other then " + supportedGoalType);
246248
}
247249

248-
public static AStarFootstepPlanner createFlatGroundPlanner(GraphVisualization viz, SideDependentList<ConvexPolygon2D> footPolygons, YoVariableRegistry registry)
250+
public static AStarFootstepPlanner createFlatGroundPlanner(GraphVisualization viz, SideDependentList<ConvexPolygon2D> footPolygons, FootstepNodeExpansion expansion, YoVariableRegistry registry)
249251
{
250252
double yawWeight = 0.1;
251253

252254
AlwaysValidNodeChecker nodeChecker = new AlwaysValidNodeChecker();
253-
SimpleSideBasedExpansion expansion = new SimpleSideBasedExpansion();
255+
//SimpleSideBasedExpansion expansion = new SimpleSideBasedExpansion();
254256
FlatGroundFootstepNodeSnapper snapper = new FlatGroundFootstepNodeSnapper(footPolygons);
255257

256258
DistanceAndYawBasedHeuristics heuristics = new DistanceAndYawBasedHeuristics(yawWeight, registry);
@@ -260,13 +262,13 @@ public static AStarFootstepPlanner createFlatGroundPlanner(GraphVisualization vi
260262
}
261263

262264
public static AStarFootstepPlanner createRoughTerrainPlanner(GraphVisualization viz, SideDependentList<ConvexPolygon2D> footPolygons,
263-
YoVariableRegistry registry)
265+
FootstepNodeExpansion expansion, YoVariableRegistry registry)
264266
{
265267
double yawWeight = 0.1;
266268

267269
SimplePlanarRegionFootstepNodeSnapper snapper = new SimplePlanarRegionFootstepNodeSnapper(footPolygons);
268270
SnapBasedNodeChecker nodeChecker = new SnapBasedNodeChecker(footPolygons, snapper, registry);
269-
SimpleSideBasedExpansion expansion = new SimpleSideBasedExpansion();
271+
//SimpleSideBasedExpansion expansion = new SimpleSideBasedExpansion();
270272

271273
DistanceAndYawBasedHeuristics heuristics = new DistanceAndYawBasedHeuristics(yawWeight, registry);
272274
DistanceAndYawBasedCost stepCostCalculator = new DistanceAndYawBasedCost(yawWeight, registry);

‎IHMCFootstepPlanning/src/us/ihmc/footstepPlanning/aStar/implementations/ReachableFootstepsBasedExpansion.java‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
package us.ihmc.footstepPlanning.aStar.implementations;
2+
3+
import java.util.HashSet;
4+
5+
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
6+
import us.ihmc.footstepPlanning.aStar.FootstepNode;
7+
import us.ihmc.footstepPlanning.aStar.FootstepNodeExpansion;
8+
import us.ihmc.robotics.geometry.FramePose;
9+
import us.ihmc.robotics.referenceFrames.PoseReferenceFrame;
10+
import us.ihmc.robotics.robotSide.RobotSide;
11+
/**
12+
* This class expands the node based on a fixed number of defined configurations.
13+
* The list of neighbours generated is robot-specific.
14+
* @author Shlok Agarwal
15+
*
16+
*/
17+
public abstract class ReachableFootstepsBasedExpansion implements FootstepNodeExpansion
18+
{
19+
20+
private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
21+
22+
private ReferenceFrame stanceFrame;
23+
private RobotSide stepSide;
24+
private double yawStanceFoot;
25+
private HashSet<FootstepNode> neighbors;
26+
27+
@Override
28+
public HashSet<FootstepNode> expandNode(FootstepNode node)
29+
{
30+
neighbors = new HashSet<>();
31+
32+
/** Get reference frame of stance leg */
33+
FramePose stanceFootPose = new FramePose(worldFrame);
34+
stanceFootPose.setYawPitchRoll(node.getYaw(), 0.0, 0.0);
35+
stanceFootPose.setX(node.getX());
36+
stanceFootPose.setY(node.getY());
37+
yawStanceFoot = node.getYaw();
38+
stanceFrame = new PoseReferenceFrame("stanceFrame", stanceFootPose);
39+
40+
/** Get swing leg side */
41+
stepSide = node.getRobotSide().getOppositeSide();
42+
43+
addNeighbors();
44+
45+
return neighbors;
46+
}
47+
48+
/**
49+
* Adds offsets to the current swing foot position to find potential neighbors
50+
* @param xOffset
51+
* @param yOffset
52+
* @param yawOffset
53+
*/
54+
public void addOffset(double xOffset, double yOffset, double yawOffset)
55+
{
56+
/** Based on stance foot side, step width sign would change*/
57+
double ySign = stepSide.negateIfRightSide(1.0);
58+
59+
FramePose step = new FramePose(stanceFrame);
60+
step.setX(xOffset);
61+
step.setY(ySign * yOffset);
62+
step.changeFrame(worldFrame);
63+
64+
neighbors.add(new FootstepNode(step.getX(), step.getY(), yawStanceFoot + ySign * yawOffset, stepSide));
65+
}
66+
67+
/**
68+
* This function adds footstep neighbors specific to the robot.
69+
* You can find implementation in AtlasReachableFootstepExpansion & ValkyrieReachableFootstepExpansion
70+
*/
71+
public abstract void addNeighbors();
72+
73+
}
74+

‎IHMCFootstepPlanning/src/us/ihmc/footstepPlanning/aStar/implementations/SimpleGridResolutionBasedExpansion.java‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,11 @@
44

55
import us.ihmc.footstepPlanning.aStar.FootstepNode;
66
import us.ihmc.footstepPlanning.aStar.FootstepNodeExpansion;
7-
7+
/**
8+
* This class expands nodes in a 8 connected way.
9+
* It does not account for turn footsteps. Yaw angle of potential footstep neighbour by default would be zero.
10+
* Very small 5cm steps.
11+
*/
812
public class SimpleGridResolutionBasedExpansion implements FootstepNodeExpansion
913
{
1014
@Override

0 commit comments

Comments
 (0)