private void assertAllStepsLevelAndZeroHeight(String message, ArrayList<Footstep> footSteps, double zToAnkle) { double eps = 1e-15; for (Footstep footstep : footSteps) { assertEquals(message + " foot should be level and zero height.", 0.0, footstep.getZ(), eps); } }
@Override public Footstep generateSnappedFootstep(double soleX, double soleY, double yaw, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { FramePose2d footPose2d = new FramePose2d(ReferenceFrame.getWorldFrame(), new Point2d(soleX, soleY), yaw); Footstep footstep = generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap); double z = footstep.getZ(); if (Double.isInfinite(z) || Double.isNaN(z)) { System.out.println("Houston, we have a problem in the SimpleFootstepSnapper"); } return footstep; }
@Override public Footstep generateSnappedFootstep(double soleX, double soleY, double yaw, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, HeightMapWithPoints heightMap) throws InsufficientDataException { FramePose2D footPose2d = new FramePose2D(ReferenceFrame.getWorldFrame(), new Point2D(soleX, soleY), yaw); Footstep footstep = generateFootstepUsingHeightMap(footPose2d, foot, soleFrame, robotSide, heightMap); double z = footstep.getZ(); if (Double.isInfinite(z) || Double.isNaN(z)) { System.out.println("Houston, we have a problem in the SimpleFootstepSnapper"); } return footstep; }
public void set(ArrayList<Footstep> footsteps, double swingTime, double transferTime) { FootstepDataListMessage footstepDataList = new FootstepDataListMessage(swingTime,transferTime); for (int i = 0; i < footsteps.size(); i++) { Footstep footstep = footsteps.get(i); Point3d location = new Point3d(footstep.getX(), footstep.getY(), footstep.getZ()); Quat4d orientation = new Quat4d(); footstep.getOrientation(orientation); RobotSide footstepSide = footstep.getRobotSide(); FootstepDataMessage footstepData = new FootstepDataMessage(footstepSide, location, orientation); footstepDataList.add(footstepData); } set(footstepDataList); }
transferToFootstep.setZ(transferToFootstep.getZ() + RandomNumbers.nextDouble(random, 0.0, maxZChange)); previousFootstep = transferToFootstep;