public YoFrameLineSegment2d(DoubleYoVariable firstEndpointX, DoubleYoVariable firstEndpointY, DoubleYoVariable secondEndpointX, DoubleYoVariable secondEndpointY, ReferenceFrame frame) { this.firstEndpointX = firstEndpointX; this.firstEndpointY = firstEndpointY; this.secondEndpointX = secondEndpointX; this.secondEndpointY = secondEndpointY; this.referenceFrame = frame; frameLineSegment = new FrameLineSegment2d(referenceFrame); }
public void setIncludingFrame(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { this.referenceFrame = firstEndpoint.getReferenceFrame(); set(firstEndpoint, secondEndpoint); }
public void orthogonalProjection(FramePoint2d point) { putYoValuesIntoFrameLineSegment(); frameLineSegment.orthogonalProjection(point); }
@Override public FrameLineSegment2d applyTransformAndProjectToXYPlaneCopy(RigidBodyTransform transform) { FrameLineSegment2d copy = new FrameLineSegment2d(this); copy.applyTransformAndProjectToXYPlane(transform); return copy; }
@Override public FrameLineSegment2d applyTransformCopy(RigidBodyTransform transform) { FrameLineSegment2d copy = new FrameLineSegment2d(this); copy.applyTransform(transform); return copy; }
@Override public FrameLineSegment2d changeFrameAndProjectToXYPlaneCopy(ReferenceFrame desiredFrame) { FrameLineSegment2d copy = new FrameLineSegment2d(this); copy.changeFrameAndProjectToXYPlane(desiredFrame); return copy; }
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(); }
FrameLineSegment2d guideLineSegment = new FrameLineSegment2d(intersections); FrameVector2d frameVector2d = new FrameVector2d(); guideLineSegment.getFrameVector(frameVector2d); if (intersectionToUse == intersections[1]) frameVector2d.scale(-1.0);
public double dotProduct(FrameLineSegment2d frameLineSegment2d) { checkReferenceFrameMatch(frameLineSegment2d); return lineSegment.dotProduct(frameLineSegment2d.lineSegment); }
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(); }
public void setIncludingFrame(FramePoint2d[] endpoints) { setIncludingFrame(endpoints[0], endpoints[1]); }
closestEdgeToCoP.getFrameVector(edgeVector2d); edgeVector.setXYIncludingFrame(edgeVector2d); edgeVector.normalize(); double rotationOnEdge = edgeVector.dot(desiredRotationVector); if (closestEdgeToCoP.isPointOnLeftSideOfLineSegment(footPolygon.getCentroid()))
@Override public FramePoint2d intersectionWith(FrameLineSegment2d secondLineSegment) { checkReferenceFrameMatch(secondLineSegment); Point2d intersection = this.lineSegment.intersectionWith(secondLineSegment.lineSegment); if (intersection == null) { return null; } return new FramePoint2d(secondLineSegment.getReferenceFrame(), intersection); }
@Override public void changeFrameAndProjectToXYPlane(ReferenceFrame desiredFrame) { // this is in the correct frame already if (desiredFrame == referenceFrame) return; if (temporaryTransformToDesiredFrame == null) temporaryTransformToDesiredFrame = new RigidBodyTransform(); referenceFrame.getTransformToDesiredFrame(temporaryTransformToDesiredFrame, desiredFrame); applyTransformAndProjectToXYPlane(temporaryTransformToDesiredFrame); referenceFrame = desiredFrame; }
@Override public double distance(FrameLine2d line) { checkReferenceFrameMatch(line); return this.lineSegment.distance(line.line); }
lineSegmentOfRotation.setIncludingFrame(frameConvexPolygonWithLineIntersector2d.getIntersectionPointOne(), frameConvexPolygonWithLineIntersector2d.getIntersectionPointTwo()); yoLineOfRotation.setFrameLineSegment2d(lineSegmentOfRotation);
public FrameLineSegment2d getFrameLineSegment2d() { putYoValuesIntoFrameLineSegment(); return new FrameLineSegment2d(frameLineSegment); }
private void putYoValuesIntoFrameLineSegment() { frameLineSegment.set(referenceFrame, firstEndpointX.getDoubleValue(), firstEndpointY.getDoubleValue(), secondEndpointX.getDoubleValue(), secondEndpointY.getDoubleValue()); }
@Override public double distance(FrameLineSegment2d secondLineSegment) { checkReferenceFrameMatch(secondLineSegment); return this.lineSegment.distance(secondLineSegment.lineSegment); }
lineSegmentOfRotation.setIncludingFrame(frameConvexPolygonWithLineIntersector2d.getIntersectionPointOne(), frameConvexPolygonWithLineIntersector2d.getIntersectionPointTwo()); yoLineOfRotation.setFrameLineSegment2d(lineSegmentOfRotation);