public void orthogonalProjection(FramePoint2d point) { putYoValuesIntoFrameLine(); frameLine.orthogonalProjection(point); }
/** * Given a capture point trajectory line and the actual * capture point position, project the current capture point * onto the capture point trajectory line. * * @param actualICP * @param capturePointTrajectoryLine * @param projectedCapturePoint */ public static void computeCapturePointOnTrajectoryAndClosestToActualCapturePoint(FramePoint actualICP, FrameLine2d capturePointTrajectoryLine, FramePoint2d projectedCapturePoint) { projectedCapturePoint.set(actualICP.getX(), actualICP.getY()); capturePointTrajectoryLine.orthogonalProjection(projectedCapturePoint); }
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(); }