public void projectOntoShrunkenPolygon(FramePoint2d pointToProject) { shrunkFootPolygon.orthogonalProjection(pointToProject); }
public static void projectOntoPolygonAndCheckDistance(FramePoint2d point, FrameConvexPolygon2d polygon, double epsilon) { ReferenceFrame originalReferenceFrame = point.getReferenceFrame(); point.changeFrame(polygon.getReferenceFrame()); FramePoint2d originalPoint = new FramePoint2d(point); polygon.orthogonalProjection(point); double distance = originalPoint.distance(point); if (distance > epsilon) throw new RuntimeException("point outside polygon by " + distance); point.changeFrame(originalReferenceFrame); }
private void putExitCMPOnToes(FrameConvexPolygon2d footSupportPolygon, FramePoint2d exitCMPToPack) { // Set x to have the CMP slightly inside the support polygon exitCMPToPack.setToZero(footSupportPolygon.getReferenceFrame()); exitCMPToPack.setX(footSupportPolygon.getMaxX() - 1.6e-2); exitCMPToPack.setY(footSupportPolygon.getCentroid().getY()); // Then constrain the computed CMP to be inside a safe support region tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon); convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdgesWhenSteppingDown.getDoubleValue(), footSupportPolygon); footSupportPolygon.orthogonalProjection(exitCMPToPack); }
public void requestICPPlannerToHoldCurrentCoM() { centerOfMassPosition.setToZero(centerOfMassFrame); FrameConvexPolygon2d supportPolygonInMidFeetZUp = bipedSupportPolygons.getSupportPolygonInMidFeetZUp(); convexPolygonShrinker.shrinkConstantDistanceInto(supportPolygonInMidFeetZUp, distanceToShrinkSupportPolygonWhenHoldingCurrent.getDoubleValue(), shrunkSupportPolygon); centerOfMassPosition.changeFrame(shrunkSupportPolygon.getReferenceFrame()); centerOfMassPosition2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassPosition); shrunkSupportPolygon.orthogonalProjection(centerOfMassPosition2d); centerOfMassPosition.setXY(centerOfMassPosition2d); centerOfMassPosition.changeFrame(worldFrame); icpPlanner.holdCurrentICP(yoTime.getDoubleValue(), centerOfMassPosition); }
private void constrainCMPAccordingToSupportPolygonAndUserOffsets(FramePoint2d cmpToPack, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidOfFootstepToConsider, YoFrameVector2d cmpOffset, double minForwardCMPOffset, double maxForwardCMPOffset) { // First constrain the computed CMP to the given min/max along the x-axis. FramePoint2d footSupportCentroid = footSupportPolygon.getCentroid(); double cmpXOffsetFromCentroid = stepLengthToCMPOffsetFactor.getDoubleValue() * (centroidOfFootstepToConsider.getX() - footSupportCentroid.getX()) + cmpOffset.getX(); cmpXOffsetFromCentroid = MathTools.clipToMinMax(cmpXOffsetFromCentroid, minForwardCMPOffset, maxForwardCMPOffset); cmpToPack.setIncludingFrame(footSupportCentroid); cmpToPack.add(cmpXOffsetFromCentroid, cmpOffset.getY()); // Then constrain the computed CMP to be inside a safe support region tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon); convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdges.getDoubleValue(), footSupportPolygon); footSupportPolygon.orthogonalProjection(cmpToPack); }
/** * This function projects the footstep midpoint in the capture region. * Might be a bit conservative it should be sufficient to slightly overlap the capture region * and the touch-down polygon. */ private void projectFootstepInCaptureRegion(Footstep footstep, FramePoint2d projectionPoint, FrameConvexPolygon2d captureRegion) { projection.setIncludingFrame(projectionPoint); projection.changeFrame(footstep.getParentFrame()); // move the position of the footstep to the capture region centroid nextStep2d.setIncludingFrame(captureRegion.getCentroid()); nextStep2d.changeFrame(footstep.getParentFrame()); // move the position as far away from the projectionPoint as possible direction.setIncludingFrame(nextStep2d); direction.sub(projection); direction.normalize(); direction.scale(10.0); nextStep2d.add(direction); nextStep2d.changeFrame(captureRegion.getReferenceFrame()); captureRegion.orthogonalProjection(nextStep2d); nextStep2d.changeFrame(footstep.getParentFrame()); footstep.setPositionChangeOnlyXY(nextStep2d); calculateTouchdownFootPolygon(footstep, captureRegion.getReferenceFrame(), touchdownFootPolygon); }
supportPolygon.orthogonalProjection(desiredCMP);
safeSupportPolygonToConstrainICPOffset.orthogonalProjection(desiredICPToModify);
projectionArea.orthogonalProjection(projectedCMP); activeProjection.set(ProjectionMethod.ORTHOGONAL_PROJECTION);