Finish feature/end-to-end-planner-tests

Author: stephenmcc

Atlas/test/us/ihmc/atlas/roughTerrainWalking/AtlasBipedalFootstepPlannerEndToEndTest.java

@@ -0,0 +1,23 @@
+package us.ihmc.atlas.roughTerrainWalking;
+
+import us.ihmc.atlas.AtlasRobotModel;
+import us.ihmc.atlas.AtlasRobotVersion;
+import us.ihmc.avatar.drcRobot.DRCRobotModel;
+import us.ihmc.avatar.drcRobot.RobotTarget;
+import us.ihmc.avatar.roughTerrainWalking.AvatarBipedalFootstepPlannerEndToEndTest;
+import us.ihmc.simulationConstructionSetTools.bambooTools.BambooTools;
+
+public class AtlasBipedalFootstepPlannerEndToEndTest extends AvatarBipedalFootstepPlannerEndToEndTest
+{
+   @Override
+   public DRCRobotModel getRobotModel()
+   {
+      return new AtlasRobotModel(AtlasRobotVersion.ATLAS_UNPLUGGED_V5_NO_HANDS, RobotTarget.SCS, false);
+   }
+
+   @Override
+   public String getSimpleRobotName()
+   {
+      return BambooTools.getSimpleRobotNameFor(BambooTools.SimpleRobotNameKeys.ATLAS);
+   }
+}

CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/trajectories/SwingOverPlanarRegionsTrajectoryExpander.java

@@ -16,6 +16,7 @@
 import us.ihmc.euclid.tuple3D.Point3D;
 import us.ihmc.euclid.tuple3D.Vector3D;
 import us.ihmc.graphicsDescription.yoGraphics.YoGraphicsListRegistry;
+import us.ihmc.robotics.MathTools;
 import us.ihmc.robotics.geometry.FrameConvexPolygon2d;
 import us.ihmc.robotics.geometry.FramePose;
 import us.ihmc.robotics.geometry.PlanarRegion;
@@ -54,7 +55,7 @@ public class SwingOverPlanarRegionsTrajectoryExpander
    private final RecyclingArrayList<FramePoint3D> adjustedWaypoints;
    private final double minimumSwingHeight;
    private final double maximumSwingHeight;
-   private final double soleToToeLength;
+   private double collisionSphereRadius;
 
    private final SphereWithConvexPolygonIntersector sphereWithConvexPolygonIntersector;
    private final Map<SwingOverPlanarRegionsTrajectoryCollisionType, FramePoint3D> closestPolygonPointMap;
@@ -106,8 +107,7 @@ public SwingOverPlanarRegionsTrajectoryExpander(WalkingControllerParameters walk
             walkingControllerParameters.getSteppingParameters().getMaxSwingHeightFromStanceFoot(), parentRegistry, graphicsListRegistry);
       minimumSwingHeight = walkingControllerParameters.getSteppingParameters().getMinSwingHeightFromStanceFoot();
       maximumSwingHeight = walkingControllerParameters.getSteppingParameters().getMaxSwingHeightFromStanceFoot();
-      soleToToeLength = walkingControllerParameters.getSteppingParameters().getActualFootLength() / 2.0;
-      System.out.println("soltotoelength: " + soleToToeLength);
+      collisionSphereRadius = walkingControllerParameters.getSteppingParameters().getActualFootLength() / 2.0;
 
       numberOfCheckpoints = new YoInteger(namePrefix + "NumberOfCheckpoints", parentRegistry);
       numberOfTriesCounter = new YoCounter(namePrefix + "NumberOfTriesCounter", parentRegistry);
@@ -201,12 +201,12 @@ public double expandTrajectoryOverPlanarRegions(FramePose stanceFootPose, FrameP
 
       tempPlaneNormal.sub(swingEndPosition.getPoint(), swingStartPosition.getPoint());
       tempPlaneNormal.normalize();
-      tempPointOnPlane.scaleAdd(soleToToeLength, tempPlaneNormal, swingStartPosition.getPoint());
+      tempPointOnPlane.scaleAdd(collisionSphereRadius, tempPlaneNormal, swingStartPosition.getPoint());
       swingStartToeFacingSwingEndPlane.set(tempPointOnPlane, tempPlaneNormal);
 
       tempPlaneNormal.sub(swingStartPosition.getPoint(), swingEndPosition.getPoint());
       tempPlaneNormal.normalize();
-      tempPointOnPlane.scaleAdd(soleToToeLength, tempPlaneNormal, swingEndPosition.getPoint());
+      tempPointOnPlane.scaleAdd(collisionSphereRadius, tempPlaneNormal, swingEndPosition.getPoint());
       swingEndHeelFacingSwingStartPlane.set(tempPointOnPlane, tempPlaneNormal);
 
       status.set(SwingOverPlanarRegionsTrajectoryExpansionStatus.SEARCHING_FOR_SOLUTION);
@@ -244,7 +244,7 @@ private SwingOverPlanarRegionsTrajectoryExpansionStatus tryATrajectory(PlanarReg
          solePoseReferenceFrame.setPositionAndUpdate(trajectoryPosition.getFrameTuple());
 
          footCollisionSphere.setToZero(WORLD);
-         footCollisionSphere.setRadius(soleToToeLength);
+         footCollisionSphere.setRadius(collisionSphereRadius);
          footCollisionSphere.getSphere3d().setPosition(solePoseReferenceFrame.getPosition());
 
          footCollisionSphere.changeFrame(WORLD);
@@ -315,6 +315,11 @@ private SwingOverPlanarRegionsTrajectoryExpansionStatus tryATrajectory(PlanarReg
       return SwingOverPlanarRegionsTrajectoryExpansionStatus.SOLUTION_FOUND;
    }
 
+   public void setCollisionSphereRadius(double collisionSphereRadius)
+   {
+      this.collisionSphereRadius = Math.max(0.0, collisionSphereRadius);
+   }
+
    private void updateClosestAndMostSevereIntersectionPoint(SwingOverPlanarRegionsTrajectoryCollisionType collisionType)
    {
       if (collisionType.ordinal() > this.mostSevereCollisionType.getEnumValue().ordinal())

IHMCAvatarInterfaces/src/us/ihmc/avatar/networkProcessor/footstepPlanningToolboxModule/FootstepDataListWithSwingOverTrajectoriesAssembler.java

@@ -178,4 +178,9 @@ public FootstepDataListMessage assemble(List<Footstep> footstepList, double swin
       footstepDataListMessage.setExecutionMode(executionMode);
       return footstepDataListMessage;
    }
+
+   public void setCollisionSphereRadius(double collisionSphereRadius)
+   {
+      this.swingOverPlanarRegionsTrajectoryExpander.setCollisionSphereRadius(collisionSphereRadius);
+   }
 }

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

@@ -63,6 +63,7 @@
 public class FootstepPlanningToolboxController extends ToolboxController
 {
    private final boolean visualize = true;
+   private HumanoidRobotDataReceiver robotDataReceiver;
 
    private enum Planners
    {
@@ -88,7 +89,8 @@ private enum Planners
    private final WalkingControllerParameters walkingControllerParameters;
    private final FootstepDataListWithSwingOverTrajectoriesAssembler footstepDataListWithSwingOverTrajectoriesAssembler;
    private final FootstepNodeExpansion expansion;
-   
+
+   private final double collisionSphereRadius = 0.2;
    private final PacketCommunicator packetCommunicator;
    private long plannerCount = 0;
    private double dt;
@@ -115,6 +117,7 @@ public FootstepPlanningToolboxController(DRCRobotModel drcRobotModel, FullHumano
 
       humanoidReferenceFrames = createHumanoidReferenceFrames(fullHumanoidRobotModel);
       footstepDataListWithSwingOverTrajectoriesAssembler = new FootstepDataListWithSwingOverTrajectoriesAssembler(humanoidReferenceFrames, walkingControllerParameters, parentRegistry, new YoGraphicsListRegistry());
+      footstepDataListWithSwingOverTrajectoriesAssembler.setCollisionSphereRadius(collisionSphereRadius);
 
       plannerMap.put(Planners.PLANAR_REGION_BIPEDAL, createPlanarRegionBipedalPlanner(contactPointsInSoleFrame, fullHumanoidRobotModel));
       plannerMap.put(Planners.PLAN_THEN_SNAP, new PlanThenSnapPlanner(new TurnWalkTurnPlanner(), contactPointsInSoleFrame));
@@ -134,7 +137,7 @@ private PlanarRegionBipedalFootstepPlanner createPlanarRegionBipedalPlanner(Side
       footstepPlanner.setFeetPolygons(footPolygonsInSoleFrame, footPolygonsInSoleFrame);
       footstepPlanner.setMaximumNumberOfNodesToExpand(Integer.MAX_VALUE);
       footstepPlanner.setExitAfterInitialSolution(false);
-      footstepPlanner.setTimeout(5.0);
+      footstepPlanner.setTimeout(20.0);
 
       if (visualize)
       {
@@ -149,8 +152,9 @@ private PlanarRegionBipedalFootstepPlanner createPlanarRegionBipedalPlanner(Side
    @Override
    protected void updateInternal()
    {
+      robotDataReceiver.updateRobotModel();
       toolboxTime.add(dt);
-      if (toolboxTime.getDoubleValue() > 10.0)
+      if (toolboxTime.getDoubleValue() > 20.0)
       {
          reportMessage(packResult(null, FootstepPlanningResult.TIMED_OUT_BEFORE_SOLUTION));
          isDone.set(true);
@@ -272,7 +276,7 @@ public void receivedPacket(PlanarRegionsListMessage packet)
    public HumanoidReferenceFrames createHumanoidReferenceFrames(FullHumanoidRobotModel fullHumanoidRobotModel)
    {
       ForceSensorDataHolder forceSensorDataHolder = new ForceSensorDataHolder(Arrays.asList(fullHumanoidRobotModel.getForceSensorDefinitions()));
-      HumanoidRobotDataReceiver robotDataReceiver = new HumanoidRobotDataReceiver(fullHumanoidRobotModel, forceSensorDataHolder);
+      robotDataReceiver = new HumanoidRobotDataReceiver(fullHumanoidRobotModel, forceSensorDataHolder);
 
       packetCommunicator.attachListener(RobotConfigurationData.class, robotDataReceiver);
       return robotDataReceiver.getReferenceFrames();
@@ -301,6 +305,7 @@ private FootstepPlanningToolboxOutputStatus packResult(FootstepPlan footstepPlan
       }
       else
       {
+         System.out.println("Found a plan!");
          if (planarRegionsList.isPresent())
          {
             PrintTools.debug(this, "Planar regions present. Assembling footstep data list message");