public void updateContactPointsForUpcomingFootstep(Footstep nextFootstep) { RobotSide robotSide = nextFootstep.getRobotSide(); List<Point2d> predictedContactPoints = nextFootstep.getPredictedContactPoints(); if ((predictedContactPoints != null) && (!predictedContactPoints.isEmpty())) { setFootPlaneContactPoints(robotSide, predictedContactPoints); } else { resetFootPlaneContactPoint(robotSide); } }
public void update(Footstep footstep) { footstep.getSolePose(footstepPose); yoFootstepPose.setAndMatchFrame(footstepPose); List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); List<Point2d> contactPointsToVisualize; if (predictedContactPoints == null || predictedContactPoints.isEmpty()) contactPointsToVisualize = defaultContactPointsInSoleFrame; else contactPointsToVisualize = predictedContactPoints; foothold.setAndUpdate(contactPointsToVisualize, contactPointsToVisualize.size()); yoFoothold.setConvexPolygon2d(foothold); poseViz.update(); footholdViz.update(); }
public AdjustFootstepMessage(Footstep footstep) { uniqueId = VALID_MESSAGE_DEFAULT_ID; origin = FootstepOrigin.AT_ANKLE_FRAME; robotSide = footstep.getRobotSide(); location = new Point3d(); orientation = new Quat4d(); footstep.getPositionInWorldFrame(location); footstep.getOrientationInWorldFrame(orientation); List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints(); if (footstepContactPoints != null) { if (predictedContactPoints == null) { predictedContactPoints = new ArrayList<>(); } else { predictedContactPoints.clear(); } for (Point2d contactPoint : footstepContactPoints) { predictedContactPoints.add((Point2d) contactPoint.clone()); } } else { predictedContactPoints = null; } }
private void computeEntryCMPForFootstep(FramePoint entryCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { ReferenceFrame soleFrame = footstep.getSoleReferenceFrame(); List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); RobotSide robotSide = footstep.getRobotSide(); if (predictedContactPoints != null) tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints); else tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide)); computeEntryCMP(entryCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfPreviousSupportFoot, previousExitCMP); }
private void addPredictedContactPointsToPolygon(Footstep footstep, FrameConvexPolygon2d convexPolygonToExtend) { List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); if (predictedContactPoints != null && !predictedContactPoints.isEmpty()) { int numberOfContactPoints = predictedContactPoints.size(); for (int i = 0; i < numberOfContactPoints; i++) { tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), predictedContactPoints.get(i)); convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint); } } else { ConvexPolygon2d defaultPolygon = defaultFootPolygons.get(footstep.getRobotSide()); for (int i = 0; i < defaultPolygon.getNumberOfVertices(); i++) { tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), defaultPolygon.getVertex(i)); convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint); } } }
public static AdjustFootstepMessage createAdjustFootstepMessage(Footstep footstep) { AdjustFootstepMessage message = new AdjustFootstepMessage(); message.setRobotSide(footstep.getRobotSide().toByte()); FramePoint3D location = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(location, orientation); footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getLocation().set(location); message.getOrientation().set(orientation); MessageTools.copyData(footstep.getPredictedContactPoints().stream().map(Point3D::new).collect(Collectors.toList()), message.getPredictedContactPoints2d()); return message; }
for (int i = 0; i < footstepToAdjust.getPredictedContactPoints().size(); i++) contactPoints.add(footstepToAdjust.getPredictedContactPoints().get(i)); footstepToAdjust.setPredictedContactPointsFromPoint2ds(contactPoints);
footstep.getOrientationInWorldFrame(orientation); List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints(); if (footstepContactPoints != null)
public void visualizeFootstep(Footstep footstep, ContactablePlaneBody bipedFoot) List<Point2D> predictedContactPoints = footstep.getPredictedContactPoints();
public void visualizeFootstep(Footstep footstep, ContactablePlaneBody bipedFoot) List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
private void computeExitCMPForFootstep(FramePoint exitCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfUpcomingSupportFoot, boolean isUpcomingFootstepLast) { if (useTwoCMPsPerSupport) { ReferenceFrame soleFrame = footstep.getSoleReferenceFrame(); List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); RobotSide robotSide = footstep.getRobotSide(); if (predictedContactPoints != null) tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints); else tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide)); computeExitCMP(exitCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfUpcomingSupportFoot, isUpcomingFootstepLast); } else { exitCMPToPack.setToNaN(worldFrame); } }
partialFootholdPolygon.addVertices(footstep.getPredictedContactPoints(), footstep.getPredictedContactPoints().size()); partialFootholdPolygon.update();
public static FramePoint getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter) { Point2d footstepCenter; List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); if (predictedContactPoints != null) { ConvexPolygon2d contactPolygon = new ConvexPolygon2d(predictedContactPoints); footstepCenter = contactPolygon.getCentroid(); } else { footstepCenter = new Point2d(); } footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter); footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter)); FramePoint footstepCenter3d = new FramePoint(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0); footstepCenter3d.changeFrame(worldFrame); return footstepCenter3d; }
public static FootstepDataMessage createFootstepDataMessage(Footstep footstep) { FootstepDataMessage message = new FootstepDataMessage(); message.setRobotSide(footstep.getRobotSide().toByte()); FramePoint3D location = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(location, orientation); footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getLocation().set(location); message.getOrientation().set(orientation); packPredictedContactPoints(footstep.getPredictedContactPoints(), message); message.setTrajectoryType(footstep.getTrajectoryType().toByte()); message.setSwingHeight(footstep.getSwingHeight()); message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration()); if (footstep.getCustomPositionWaypoints().size() != 0) { for (int i = 0; i < footstep.getCustomPositionWaypoints().size(); i++) { FramePoint3D framePoint = footstep.getCustomPositionWaypoints().get(i); framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getCustomPositionWaypoints().add().set(framePoint); } } return message; }
public static FramePoint3D getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter) { Point2D footstepCenter; List<Point2D> predictedContactPoints = footstep.getPredictedContactPoints(); if (predictedContactPoints != null) { ConvexPolygon2D contactPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(predictedContactPoints)); footstepCenter = new Point2D(contactPolygon.getCentroid()); } else { footstepCenter = new Point2D(); } footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter); footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter)); FramePoint3D footstepCenter3d = new FramePoint3D(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0); footstepCenter3d.changeFrame(worldFrame); return footstepCenter3d; }
partialFootholdPolygon.addVertices(Vertex2DSupplier.asVertex2DSupplier(footstep.getPredictedContactPoints())); partialFootholdPolygon.update();
assertTrue((generatedSnappedFootstep.getPredictedContactPoints() == null) || (generatedSnappedFootstep.getPredictedContactPoints().size() > 2)); boolean pointsBelowPlane = pointsBelowPlane(footstepSnapper.getPointList(), solePlane, tolerance); assertTrue("queryPoint = " + queryPoint + " yaw = " + soleYaw.getDoubleValue() + " planeNormal = " + planeNormal + ", pointsBelowPlane = "
List<Point2d> predictedContactPoints = nextFootstep.getPredictedContactPoints(); if (predictedContactPoints != null && !predictedContactPoints.isEmpty())