private SimpleFootstepSnapper createSimpleFootstepSnapper() { QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters(); BasicFootstepMask footstepMask = new BasicFootstepMask(snappingParameters.getCollisionPolygon(), 0.0); SimpleFootstepSnapper footstepSnapper = new SimpleFootstepSnapper(); boolean useMask = true; double kernelMaskSafetyBuffer = 0.15; double boundingBoxDimension = 0.15; footstepSnapper.setUseMask(useMask, kernelMaskSafetyBuffer, boundingBoxDimension); footstepSnapper.setMask(footstepMask); return footstepSnapper; }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3D planeNormal) { simpleSnapper.adjustFootstepWithoutHeightmap(footstep, height, planeNormal); }
@Override public Footstep generateFootstepWithoutHeightMap(FramePose2d footPose2d, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3d planeNormal) { return simpleSnapper.generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, height, planeNormal); }
double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3; footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension); footstepSnapper.snapFootstep(footstep, heightMap);
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){ FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); footstep.setFootstepType(type); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }
@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 testSimpleFootstepSnapperOnBumpyGround() throws InsufficientDataException { boolean assertPositionConditions = true; boolean assertPointConditions = false; boolean visualizeAndKeepUp = false; GroundProfile3D groundProfile = createBumpyGroundProfile(); SimpleFootstepSnapper footstepSnapper = createSimpleFootstepSnapper(); double maskSafetyBuffer = 0.01; double boundingBoxDimension = 0.3; footstepSnapper.setUseMask(true, maskSafetyBuffer, boundingBoxDimension); BoundingBox2D rangeOfPointsToTest = new BoundingBox2D(-1.0, -1.0, 1.0, 1.0); FootstepSnapperTestHelper helper = new FootstepSnapperTestHelper("Simple Bumpy Ground", footstepSnapper, null, visualizeAndKeepUp); double resolution = 0.02; double heightThreshold = 0.002; double quadTreeMaxMultiLevelZChangeToFilterNoise = 0.2; int maxSameHeightPointsPerNode = 20; double maxAllowableXYDistanceForAPointToBeConsideredClose = 0.2; int maxNodes = 1000000; helper.createHeightMap(groundProfile.getHeightMapIfAvailable(), rangeOfPointsToTest, resolution, heightThreshold, quadTreeMaxMultiLevelZChangeToFilterNoise, maxSameHeightPointsPerNode, maxAllowableXYDistanceForAPointToBeConsideredClose, maxNodes); int numberOfPointsToTest = 1000; helper.testRandomPoints(numberOfPointsToTest, rangeOfPointsToTest, assertPositionConditions, assertPointConditions); if (visualizeAndKeepUp) { ThreadTools.sleepForever(); } }
private SideDependentList<Footstep> updatedStartFeet(double startX, double startY, double startYaw) { double leftXFactor = initialDeltaFeetLocalX / 2 * Math.cos(startYaw) - initialDeltaFeetLocalY / 2 * Math.sin(startYaw); double leftYFactor = initialDeltaFeetLocalX / 2 * Math.sin(startYaw) + initialDeltaFeetLocalY / 2 * Math.cos(startYaw); QuadTreeFootstepSnapper footstepSnapper = new SimpleFootstepSnapper(); Point2D leftFootStartPoint = new Point2D(startX + leftXFactor, startY + leftYFactor); FramePose2D leftFootPose2d = new FramePose2D(WORLD_FRAME, leftFootStartPoint, startYaw + initialDeltaFeetYaw / 2); Point2D rightFootStartPoint = new Point2D(startX - leftXFactor, startY - leftYFactor); FramePose2D rightFootPose2d = new FramePose2D(WORLD_FRAME, rightFootStartPoint, startYaw - initialDeltaFeetYaw / 2); FramePoint3D leftPosition = new FramePoint3D(WORLD_FRAME, leftFootStartPoint.getX(), leftFootStartPoint.getY(), 0); FramePoint3D rightPosition = new FramePoint3D(WORLD_FRAME, rightFootStartPoint.getX(), rightFootStartPoint.getY(), 0); FrameQuaternion leftOrientation = new FrameQuaternion(WORLD_FRAME, leftFootPose2d.getYaw(), 0.0, 0.0); FrameQuaternion rightOrientation = new FrameQuaternion(WORLD_FRAME, rightFootPose2d.getYaw(), 0.0, 0.0); leftContactableFoot.setSoleFrame(leftPosition, leftOrientation); rightContactableFoot.setSoleFrame(rightPosition, rightOrientation); Footstep leftStart = footstepSnapper.generateFootstepWithoutHeightMap(leftFootPose2d, feet.get(RobotSide.LEFT), contactableFeet.get(RobotSide.LEFT).getSoleFrame(), RobotSide.LEFT, GROUND_HEIGHT, PLANE_NORMAL); Footstep rightStart = footstepSnapper.generateFootstepWithoutHeightMap(rightFootPose2d, feet.get(RobotSide.RIGHT), contactableFeet.get(RobotSide.RIGHT).getSoleFrame(), RobotSide.RIGHT, GROUND_HEIGHT, PLANE_NORMAL); SideDependentList<Footstep> startFeet = new SideDependentList<Footstep>(leftStart, rightStart); return startFeet; }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); footstep.setFootstepType(type); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }
@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; }
private SimpleFootstepSnapper createSimpleFootstepSnapper() { QuadTreeFootstepSnappingParameters snappingParameters = new GenericFootstepSnappingParameters(); BasicFootstepMask footstepMask = new BasicFootstepMask(snappingParameters.getCollisionPolygon(), 0.0); SimpleFootstepSnapper footstepSnapper = new SimpleFootstepSnapper(); boolean useMask = true; double kernelMaskSafetyBuffer = 0.15; double boundingBoxDimension = 0.15; footstepSnapper.setUseMask(useMask, kernelMaskSafetyBuffer, boundingBoxDimension); footstepSnapper.setMask(footstepMask); return footstepSnapper; }
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3D planeNormal) { simpleSnapper.adjustFootstepWithoutHeightmap(footstep, height, planeNormal); }
@Override public Footstep generateFootstepWithoutHeightMap(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3D planeNormal) { return simpleSnapper.generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, height, planeNormal); }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3d planeNormal) { simpleSnapper.adjustFootstepWithoutHeightmap(footstep, height, planeNormal); }
return generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, height, surfaceNormal);
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3d planeNormal) { simpleSnapper.adjustFootstepWithoutHeightmap(footstep, height, planeNormal); }
return generateFootstepWithoutHeightMap(footPose2d, foot, soleFrame, robotSide, height, surfaceNormal);
adjustFootstepWithoutHeightmap(footstep, height, surfaceNormal); return Footstep.FootstepType.FULL_FOOTSTEP;
adjustFootstepWithoutHeightmap(footstep, height, surfaceNormal); return Footstep.FootstepType.FULL_FOOTSTEP;