/** * Changes frame of this FramePoint2d to the given ReferenceFrame, projects into xy plane, and returns a copy. * * @param desiredFrame ReferenceFrame to change the FramePoint2d into. * @return Copied FramePoint2d in the new reference frame. */ public FramePoint2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FramePoint2d ret = new FramePoint2d(this); ret.changeFrameAndProjectToXYPlane(desiredFrame); return ret; }
/** * Add a vertex to this polygon after doing {@code newVertex.changeFrameAndProjectToXYPlane(this.referenceFrame)}. * Note that this method recycles memory. * @param newVertex {@code FramePoint2d} the new vertex (it is not modified). */ public void addVertexChangeFrameAndProjectToXYPlane(FramePoint2d newVertex) { tempPoint2d.setIncludingFrame(newVertex); tempPoint2d.changeFrameAndProjectToXYPlane(referenceFrame); convexPolygon.addVertex(tempPoint2d.getPoint()); }
public void getAdjustedDesiredCapturePoint(FramePoint2d desiredCapturePoint, FramePoint2d adjustedDesiredCapturePoint) { filteredYoAngularMomentum.getFrameTuple(angularMomentum); ReferenceFrame comFrame = angularMomentum.getReferenceFrame(); localDesiredCapturePoint.setIncludingFrame(desiredCapturePoint); localDesiredCapturePoint.changeFrameAndProjectToXYPlane(comFrame); double scaleFactor = momentumGain.getDoubleValue() * omega0.getDoubleValue() / (totalMass.getDoubleValue() * gravity); adjustedDesiredCapturePoint.setIncludingFrame(comFrame, -angularMomentum.getY(), angularMomentum.getX()); adjustedDesiredCapturePoint.scale(scaleFactor); adjustedDesiredCapturePoint.add(localDesiredCapturePoint); adjustedDesiredCapturePoint.changeFrameAndProjectToXYPlane(desiredCapturePoint.getReferenceFrame()); }
this.desiredCMP.changeFrameAndProjectToXYPlane(projectionArea.getReferenceFrame()); this.capturePoint.setIncludingFrame(capturePoint); this.capturePoint.changeFrameAndProjectToXYPlane(projectionArea.getReferenceFrame()); if (finalCapturePoint != null) this.finalCapturePoint.setIncludingFrame(finalCapturePoint); else this.finalCapturePoint.setToNaN(); this.finalCapturePoint.changeFrameAndProjectToXYPlane(projectionArea.getReferenceFrame()); this.desiredCMP.changeFrameAndProjectToXYPlane(worldFrame); projectedCMP.changeFrameAndProjectToXYPlane(worldFrame); if (cmpWasProjected.getBooleanValue())
public void changeFrame(PixelsReferenceFrame pixelsReferenceFrame) { if (getReferenceFrame() instanceof MetersReferenceFrame) { changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); scale(pixelsReferenceFrame.getConversionToPixels().getX(), pixelsReferenceFrame.getConversionToPixels().getY()); } super.changeFrameAndProjectToXYPlane(pixelsReferenceFrame); } }
public void changeFrame(PixelsReferenceFrame pixelsReferenceFrame) { if (getReferenceFrame() instanceof MetersReferenceFrame) { changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); scale(pixelsReferenceFrame.getConversionToPixels().getX(), pixelsReferenceFrame.getConversionToPixels().getY()); } super.changeFrameAndProjectToXYPlane(pixelsReferenceFrame); } }
public void changeFrame(MetersReferenceFrame metersReferenceFrame) { if (getReferenceFrame() instanceof PixelsReferenceFrame) { changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); scale(metersReferenceFrame.getConversionToMeters().getX(), metersReferenceFrame.getConversionToMeters().getY()); } super.changeFrameAndProjectToXYPlane(metersReferenceFrame); }
public void changeFrame(MetersReferenceFrame metersReferenceFrame) { if (getReferenceFrame() instanceof PixelsReferenceFrame) { changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); scale(metersReferenceFrame.getConversionToMeters().getX(), metersReferenceFrame.getConversionToMeters().getY()); } super.changeFrameAndProjectToXYPlane(metersReferenceFrame); }
private void computeEntryCMP(FramePoint entryCMPToPack, RobotSide robotSide, ReferenceFrame soleFrame, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { if (useTwoCMPsPerSupport) { if (centroidInSoleFrameOfPreviousSupportFoot != null) centroidOfFootstepToConsider.setIncludingFrame(centroidInSoleFrameOfPreviousSupportFoot); else centroidOfFootstepToConsider.setToZero(soleFrame); centroidOfFootstepToConsider.changeFrameAndProjectToXYPlane(soleFrame); if (previousExitCMP != null) { previousExitCMP.getFrameTuple2dIncludingFrame(previousExitCMP2d); previousExitCMP2d.changeFrameAndProjectToXYPlane(soleFrame); // Choose the laziest option if (Math.abs(previousExitCMP2d.getX()) < Math.abs(centroidOfFootstepToConsider.getX())) centroidOfFootstepToConsider.set(previousExitCMP2d); } constrainCMPAccordingToSupportPolygonAndUserOffsets(cmp2d, footSupportPolygon, centroidOfFootstepToConsider, entryCMPUserOffsets.get(robotSide), minForwardEntryCMPOffset.getDoubleValue(), maxForwardEntryCMPOffset.getDoubleValue()); } else { cmp2d.setIncludingFrame(footSupportPolygon.getCentroid()); YoFrameVector2d offset = entryCMPUserOffsets.get(robotSide); cmp2d.add(offset.getX(), offset.getY()); } entryCMPToPack.setXYIncludingFrame(cmp2d); entryCMPToPack.changeFrame(worldFrame); }
private void checkIfUseUpperBodyMomentumDoubleSupport() { safeArea.setIncludingFrameAndUpdate(supportPolygon); safeCMPArea.setIncludingFrameAndUpdate(safeArea); // hysteresis: // shrink the safe area if we are already using upper body momentum if (usingUpperBodyMomentum.getBooleanValue()) { polygonShrinker.shrinkConstantDistanceInto(safeArea, distanceToShrinkSafeAreaIfRecoveringDS.getDoubleValue(), tempPolygon1); safeArea.setIncludingFrameAndUpdate(tempPolygon1); } else { polygonShrinker.shrinkConstantDistanceInto(safeArea, distanceToShrinkSafeAreaDS.getDoubleValue(), tempPolygon1); safeArea.setIncludingFrameAndUpdate(tempPolygon1); } capturePoint2d.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame()); boolean icpInSafeArea = safeArea.isPointInside(capturePoint2d); if (!icpInSafeArea) { usingUpperBodyMomentum.set(true); } else { usingUpperBodyMomentum.set(false); } }
public void compute(double time, boolean footholdWasUpdated) { if (!footholdExplorationActive.getBooleanValue() || partialFootholdControlModule == null) { reset(); return; } if (timeExploring.isNaN()) startTime.set(time); timeExploring.set(time - startTime.getDoubleValue()); computeDesiredCenterOfPressure(time, footholdWasUpdated); desiredCopInWorld.setIncludingFrame(desiredCenterOfPressure); desiredCopInWorld.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame()); if (yoDesiredCop != null) yoDesiredCop.set(desiredCopInWorld); centerOfPressureCommand.setDesiredCoP(desiredCenterOfPressure.getPoint()); commandWeight.set(copCommandWeight.getDoubleValue(), copCommandWeight.getDoubleValue()); centerOfPressureCommand.setWeight(commandWeight); }
initialCoMXY.changeFrameAndProjectToXYPlane(referenceFrames.getMidFeetZUpFrame()); desiredCenterOfMassXYReference.set(initialCoMXY);
initialCoMXY.changeFrameAndProjectToXYPlane(referenceFrames.getMidFeetZUpFrame()); desiredCenterOfMassXYReference.set(initialCoMXY);
else centroidOfFootstepToConsider.setToZero(soleFrame); centroidOfFootstepToConsider.changeFrameAndProjectToXYPlane(soleFrame);
capturePoint2d.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame()); boolean icpInSafeArea = safeArea.isPointInside(capturePoint2d);