public void setIncludingFrame(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { this.referenceFrame = firstEndpoint.getReferenceFrame(); set(firstEndpoint, secondEndpoint); }
private void putYoValuesIntoFrameLineSegment() { frameLineSegment.set(referenceFrame, firstEndpointX.getDoubleValue(), firstEndpointY.getDoubleValue(), secondEndpointX.getDoubleValue(), secondEndpointY.getDoubleValue()); }
public void set(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { referenceFrame.checkReferenceFrameMatch(firstEndpoint.getReferenceFrame()); referenceFrame.checkReferenceFrameMatch(secondEndpoint.getReferenceFrame()); frameLineSegment.set(firstEndpoint, secondEndpoint); getYoValuesFromFrameLineSegment(); }
public void set(FramePoint2d firstEndpoint, FrameVector2d vectorToSecondEndpoint) { referenceFrame.checkReferenceFrameMatch(firstEndpoint.getReferenceFrame()); referenceFrame.checkReferenceFrameMatch(vectorToSecondEndpoint.getReferenceFrame()); frameLineSegment.set(firstEndpoint, vectorToSecondEndpoint); getYoValuesFromFrameLineSegment(); }
public void setFrameLineSegment2d(FrameLineSegment2d lineSegment) { if (lineSegment == null) { firstEndpointX.set(Double.NaN); firstEndpointY.set(Double.NaN); secondEndpointX.set(Double.NaN); secondEndpointY.set(Double.NaN); return; } lineSegment.checkReferenceFrameMatch(referenceFrame); this.frameLineSegment.set(lineSegment); getYoValuesFromFrameLineSegment(); }
footCenterCoPLineSegment.set(footFrame, 0.0, 0.0, tempCoP2d.getX(), tempCoP2d.getY());
private double estimateDeltaTimeBetweenDesiredICPAndActualICP(double time, FramePoint2d actualCapturePointPosition) { computeDesiredCapturePoint(computeAndReturnTimeInCurrentState(time)); computeDesiredCentroidalMomentumPivot(); desiredCapturePointPosition.getFrameTuple2dIncludingFrame(desiredICP2d); singleSupportFinalICP.getFrameTuple2dIncludingFrame(finalICP2d); if (desiredICP2d.distance(finalICP2d) < 1.0e-10) return Double.NaN; desiredICPToFinalICPLineSegment.set(desiredICP2d, finalICP2d); actualICP2d.setIncludingFrame(actualCapturePointPosition); double percentAlongLineSegmentICP = desiredICPToFinalICPLineSegment.percentageAlongLineSegment(actualICP2d); if (percentAlongLineSegmentICP < 0.0) { desiredICPToFinalICPLine.set(desiredICP2d, finalICP2d); desiredICPToFinalICPLine.orthogonalProjection(actualICP2d); } else { desiredICPToFinalICPLineSegment.orthogonalProjection(actualICP2d); } double actualDistanceDueToDisturbance = desiredCentroidalMomentumPivotPosition.getXYPlaneDistance(actualICP2d); double expectedDistanceAccordingToPlan = desiredCentroidalMomentumPivotPosition.getXYPlaneDistance(desiredCapturePointPosition); computeTimeInCurrentState(time); double distanceRatio = actualDistanceDueToDisturbance / expectedDistanceAccordingToPlan; if (distanceRatio < 1.0e-3) return 0.0; else return Math.log(distanceRatio) / omega0.getDoubleValue(); }