private double[] calculateLengthOfPath(ArrayList<FramePoint> pointsOnPath) { double[] distanceOnPath = new double[pointsOnPath.size()]; distanceOnPath[0] = 0.0; for (int i = 1; i < pointsOnPath.size(); i++) { distanceOnPath[i] = distanceOnPath[i - 1] + pointsOnPath.get(i).distance(pointsOnPath.get(i - 1)); } return distanceOnPath; }
public double distance(FramePoint framePoint) { return getFrameTuple().distance(framePoint); }
public double getAlpha(FramePoint pointToCheck) { double minimumDistance = Double.POSITIVE_INFINITY; double alphaToReturn = 1.0; double deltaAlpha = 0.01; // +++JEP 3/4/2007: Needed to add deltaAlpha/2.0 or it might never get to 1.0 for (double alpha = 0.0; alpha <= 1.0 + deltaAlpha / 2.0; alpha = alpha + deltaAlpha) { if (alpha > 1.0) alpha = 1.0; FramePoint testPoint = this.getPointOnPath(alpha); double distance = testPoint.distance(pointToCheck); if (distance < minimumDistance) { minimumDistance = distance; alphaToReturn = alpha; } } return alphaToReturn; }
public ListOfPointsTrajectory(final ArrayList<FramePoint> listOfPointsForSegment) { if ((listOfPointsForSegment == null) || (listOfPointsForSegment.size() < 2)) { throw new RuntimeException("listOfPoints must have at least 2 elements."); } this.listOfPoints = new ArrayList<FramePoint>(); // add the start and end point this.listOfPoints.add(new FramePoint(listOfPointsForSegment.get(0))); for (int i = 1; i < listOfPointsForSegment.size(); i++) { double distanceToPreviousPoint = listOfPointsForSegment.get(i).distance(listOfPointsForSegment.get(i - 1)); if (distanceToPreviousPoint > 0.0) this.listOfPoints.add(new FramePoint(listOfPointsForSegment.get(i))); } // set all the Z's to zero and add to list // for() // { // framePoint.setZ(0.0); // this.listOfPoints.add(new FramePoint(framePoint)); // } setupAlphaArrayAndLinearInterpolator(); }
/** * Compute the desired capture point velocity at a given time. * ICPv_d = w * * e^{w*t} * ICP0 - p0 * w * e^{w*t} * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointVelocityToPack */ public static void computeDesiredCapturePointVelocity(double omega0, double time, FramePoint initialCapturePoint, FramePoint initialCMP, YoFrameVector desiredCapturePointVelocityToPack) { if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointVelocityToPack.subAndScale(omega0 * Math.exp(omega0 * time), initialCapturePoint, initialCMP); else desiredCapturePointVelocityToPack.setToZero(); }
/** * Compute the desired 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 computeDesiredCapturePointPosition(double omega0, double time, FramePoint initialCapturePoint, FramePoint initialCMP, YoFramePoint desiredCapturePointToPack) { if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointToPack.interpolate(initialCMP, initialCapturePoint, Math.exp(omega0 * time)); else desiredCapturePointToPack.set(initialCapturePoint); }
/** * Compute the desired capture point velocity at a given time. * ICPv_d = w^2 * e^{w*t} * ICP0 - p0 * w^2 * e^{w*t} * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointAccelerationToPack */ public static void computeDesiredCapturePointAcceleration(double omega0, double time, FramePoint initialCapturePoint, FramePoint initialCMP, FrameVector desiredCapturePointAccelerationToPack) { if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointAccelerationToPack.subAndScale(omega0 * omega0 * Math.exp(omega0 * time), initialCapturePoint, initialCMP); else desiredCapturePointAccelerationToPack.setToZero(); }
/** * Compute the desired capture point velocity at a given time. * ICPv_d = w^2 * e^{w*t} * ICP0 - p0 * w^2 * e^{w*t} * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointAccelerationToPack */ public static void computeDesiredCapturePointAcceleration(double omega0, double time, FramePoint initialCapturePoint, FramePoint initialCMP, YoFrameVector desiredCapturePointAccelerationToPack) { if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointAccelerationToPack.subAndScale(omega0 * omega0 * Math.exp(omega0 * time), initialCapturePoint, initialCMP); else desiredCapturePointAccelerationToPack.setToZero(); }
/** * Compute the desired 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 computeDesiredCapturePointPosition(double omega0, double time, FramePoint initialCapturePoint, FramePoint initialCMP, FramePoint desiredCapturePointToPack) { desiredCapturePointToPack.setToZero(initialCapturePoint.getReferenceFrame()); if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointToPack.interpolate(initialCMP, initialCapturePoint, Math.exp(omega0 * time)); else desiredCapturePointToPack.set(initialCapturePoint); }
/** * Compute the desired capture point velocity at a given time. * ICPv_d = w * * e^{w*t} * ICP0 - p0 * w * e^{w*t} * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointVelocityToPack */ public static void computeDesiredCapturePointVelocity(double omega0, double time, FramePoint initialCapturePoint, FramePoint initialCMP, FrameVector desiredCapturePointVelocityToPack) { desiredCapturePointVelocityToPack.setToZero(initialCapturePoint.getReferenceFrame()); if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointVelocityToPack.subAndScale(omega0 * Math.exp(omega0 * time), initialCapturePoint, initialCMP); else desiredCapturePointVelocityToPack.setToZero(); }
public double getValue() { FramePoint initialPosition = new FramePoint(); initialPositionProvider.getPosition(initialPosition); initialPosition.changeFrame(ReferenceFrame.getWorldFrame()); FramePoint finalPosition = new FramePoint(); finalPositionProvider.getPosition(finalPosition); finalPosition.changeFrame(ReferenceFrame.getWorldFrame()); double distance = initialPosition.distance(finalPosition); double time = distance / averageVelocityProvider.getValue(); time = MathTools.clipToMinMax(time, minimumTime, Double.POSITIVE_INFINITY); return time; } }
return false; boolean isStepLongEnough = tempLeadingFootPosition.distance(tempTrailingFootPosition) > minStepLengthForToeOff.getDoubleValue(); boolean isStepLongEnoughAlongX = tempLeadingFootPosition.getX() > footLength; return isStepLongEnough && isStepLongEnoughAlongX;
private void setArcLengths() { pi.setToZero(referenceFrame); piPlusOne.setToZero(referenceFrame); arcLengths[0].set(0.0); double tiPlusOne = t0.getDoubleValue(); compute(t0.getDoubleValue()); getPosition(piPlusOne); double differentialDistance; for (int i = 0; i < arcLengthCalculatorDivisions; i++) { pi.set(piPlusOne); tiPlusOne = getTime(i + 1); compute(tiPlusOne); getPosition(piPlusOne); differentialDistance = pi.distance(piPlusOne); arcLengths[i + 1].set(arcLengths[i].getDoubleValue() + differentialDistance); } }
scanPoint.changeFrame(worldFrame); double distanceFromSensor = scanPoint.distance(lidarPosition); if (distanceFromSensor > maxRange || distanceFromSensor < minRange || scanPoint.getZ() > lidarPosition.getZ() + maxZ) continue;
|| midGroundPoint.distance(sphereWithConvexPolygonIntersector.getClosestPointOnPolygon()) < midGroundPoint.distance(solePoseReferenceFrame.getPositionUnsafe())) adjustedWaypoints.get(1).add(waypointAdjustmentVector); if (adjustedWaypoints.get(0).distance(originalWaypoints.get(0)) > maximumAdjustmentDistance.getDoubleValue() || adjustedWaypoints.get(1).distance(originalWaypoints.get(1)) > maximumAdjustmentDistance.getDoubleValue())