public double distance(FramePoint2d framePoint2d) { return getFrameTuple2d().distance(framePoint2d); } }
public double getICPErrorMagnitude() { momentumBasedController.getCapturePoint(capturePoint2d); return capturePoint2d.distance(yoDesiredCapturePoint.getFrameTuple2d()); }
public double getPositionDistance(FramePose2d framePose) { checkReferenceFrameMatch(framePose); framePose.getPosition(otherPosition); return position.distance(otherPosition); }
private FramePoint2d closestPoint(FramePoint2d point, FramePoint2d candidate1, FramePoint2d candidate2) { if (candidate1.containsNaN() && candidate2.containsNaN()) return null; if (candidate1.containsNaN()) return candidate2; if (candidate2.containsNaN()) return candidate1; if (point.distance(candidate1) <= point.distance(candidate2)) return candidate1; return candidate2; }
/** * Compute the capture point position at a given time. * x<sup>ICP<sub>des</sub></sup> = (e<sup>ω0 t</sup>) x<sup>ICP<sub>0</sub></sup> + (1-e<sup>ω0 t</sup>)x<sup>CMP<sub>0</sub></sup> * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointToPack */ public static void computeCapturePointPosition(double omega0, double time, FramePoint2d initialCapturePoint, FramePoint2d initialCMP, FramePoint2d desiredCapturePointToPack) { desiredCapturePointToPack.setToZero(initialCapturePoint.getReferenceFrame()); if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointToPack.interpolate(initialCMP, initialCapturePoint, Math.exp(omega0 * time)); else desiredCapturePointToPack.set(initialCapturePoint); } }
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); }
public void updateForSingleSupport(FramePoint2d desiredCapturePoint2d, FramePoint2d capturePoint2d, double omega0) { if (!isEnabled()) return; this.omega0 = omega0; this.capturePoint2d.setIncludingFrame(capturePoint2d); this.desiredCapturePoint2d.setIncludingFrame(desiredCapturePoint2d); isICPErrorTooLarge.set(this.desiredCapturePoint2d.distance(this.capturePoint2d) > icpErrorThreshold.getDoubleValue()); }
public StraightLineOverheadPath(FramePose2d startPose, FramePoint2d endPosition) { startPose.checkReferenceFrameMatch(endPosition); this.startPoint = new FramePoint2d(); startPose.getPosition(startPoint); this.endPoint = new FramePoint2d(endPosition); this.orientation = new FrameOrientation2d(); startPose.getOrientation(orientation); this.distance = endPoint.distance(startPoint); }
double distance = pointToMove.distance(intersection); if (distance < minDistance) double distanceBetweenIntersections = intersections[0].distance(intersections[1]); boolean constraintFeasible = distanceBetweenIntersections > 2.0 * distanceToBeInside; double distance = intersections[i].distance(pointToMove); if (distance < distanceToBeInside)
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(); }
public void checkIfRobotIsFalling(FramePoint2d capturePoint2d, FramePoint2d desiredCapturePoint2d) { updateCombinedPolygon(); if (isUsingNextFootstep.getBooleanValue()) icpDistanceFromFootPolygon.set(combinedFootPolygonWithNextFootstep.distance(capturePoint2d)); else icpDistanceFromFootPolygon.set(combinedFootPolygon.distance(capturePoint2d)); // TODO need to investigate this method, seems to be buggy // boolean isCapturePointCloseToFootPolygon = combinedFootPolygon.isPointInside(capturePoint, icpDistanceFromFootPolygonThreshold.getDoubleValue()); boolean isCapturePointCloseToFootPolygon = icpDistanceFromFootPolygon.getDoubleValue() < icpDistanceFromFootPolygonThreshold.getDoubleValue(); boolean isCapturePointCloseToDesiredCapturePoint = desiredCapturePoint2d.distance(capturePoint2d) < icpDistanceFromFootPolygonThreshold.getDoubleValue(); isRobotFalling.set(!isCapturePointCloseToFootPolygon && !isCapturePointCloseToDesiredCapturePoint); if (isRobotFalling.getBooleanValue()) { tempFallingDirection.set(capturePoint2d); FramePoint2d footCenter = combinedFootPolygon.getCentroid(); tempFallingDirection.changeFrame(ReferenceFrame.getWorldFrame()); footCenter.changeFrame(ReferenceFrame.getWorldFrame()); tempFallingDirection.sub(footCenter); fallingDirection.set(tempFallingDirection); } }
this.desiredCapturePoint2d.changeFrame(midFeetZUp); isICPErrorTooLarge.set(this.desiredCapturePoint2d.distance(this.capturePoint2d) > icpErrorThreshold.getDoubleValue()); projectedCapturePoint2d.setByProjectionOntoXYPlaneIncludingFrame(projectedCapturePoint); distanceICPToFeet.get(robotSide).set(projectedCapturePoint2d.distance(footPolygon.getCentroid())); isRobotBackToSafeState.set(false);
double distanceToTravel = robotPosition.distance(goalPoint); addStraightWalk(footstepList, robotPosition, distanceToTravel);