/** * Creates a new {@code YoFrameLineSegment2D}. * * @param namePrefix a unique name string to use as the prefix for child variable names. * @param nameSuffix a string to use as the suffix for child variable names. * @param referenceFrame the reference frame for this line segment. * @param registry the registry to register child variables to. */ public YoFrameLineSegment2D(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { firstEndpoint = new YoFramePoint2D(namePrefix + "FirstEndpoint", nameSuffix, referenceFrame, registry); secondEndpoint = new YoFramePoint2D(namePrefix + "SecondEndpoint", nameSuffix, referenceFrame, registry); }
public void setPosition(Point2DReadOnly point2D) { point.set(point2D); }
/** * Creates a new {@code YoFrameLine2D} using the given {@code YoVariable}s and sets its reference * frame to {@code referenceFrame}. * * @param firstEndpoint the {@code YoFramePoint2D} to use internally for the first endpoint. * @param secondEndpoint the {@code YoFramePoint2D} to use internally for the second endpoint. * @throws ReferenceFrameMismatchException if {@code firstEndpoint} and {@code secondEndpoint} * are not expressed in the same reference frame. */ public YoFrameLineSegment2D(YoFramePoint2D firstEndpoint, YoFramePoint2D secondEndpoint) { this(firstEndpoint.getYoX(), firstEndpoint.getYoY(), secondEndpoint.getYoX(), secondEndpoint.getYoY(), firstEndpoint.getReferenceFrame()); firstEndpoint.checkReferenceFrameMatch(secondEndpoint); }
private static void swap(List<YoFramePoint2D> vertexBuffer, int i, int j) { if (i == j) return; YoFramePoint2D iVertex = vertexBuffer.get(i); double x_i = iVertex.getX(); double y_i = iVertex.getY(); YoFramePoint2D jVertex = vertexBuffer.get(j); double x_j = jVertex.getX(); double y_j = jVertex.getY(); iVertex.set(x_j, y_j); jVertex.set(x_i, y_i); }
YoFramePoint2D center = new YoFramePoint2D("center", ReferenceFrame.getWorldFrame(), registry); YoFrameVector2D radii = new YoFrameVector2D("radii", ReferenceFrame.getWorldFrame(), registry); center.set(-1.0, -1.0); radii.set(1.0, 0.7); YoFramePoint2D point1 = new YoFramePoint2D("point1", ReferenceFrame.getWorldFrame(), registry); YoFramePoint2D polyPoint1 = point1; polyPoint1.set(1.5, 2.0); polygon2.set(FrameVertex2DSupplier.asFrameVertex2DSupplier(new FramePoint2DReadOnly[] {polyPoint1})); YoFramePoint2D polyPoint2 = new YoFramePoint2D("point2", ReferenceFrame.getWorldFrame(), registry); polyPoint2.set(2.0, 2.5); YoFramePoint2D polyPoint3 = new YoFramePoint2D("point3", ReferenceFrame.getWorldFrame(), registry); polyPoint3.set(1.0, 3.0); polygon3.set(FrameVertex2DSupplier.asFrameVertex2DSupplier(new FramePoint2DReadOnly[] {polyPoint1, polyPoint2, polyPoint3})); YoFramePoint2D pointZ = new YoFramePoint2D("pointZ", ReferenceFrame.getWorldFrame(), registry); pointZ.set(-2.2, 3.0); YoFramePoint2D point4 = new YoFramePoint2D("point4", ReferenceFrame.getWorldFrame(), registry); point4.set(-2.0, 3.0); YoFramePoint2D point5 = new YoFramePoint2D("point5", ReferenceFrame.getWorldFrame(), registry); point5.set(-1.8, 3.0); YoFramePoint2D point6 = new YoFramePoint2D("point6", ReferenceFrame.getWorldFrame(), registry); point6.set(-1.6, 3.0); YoFramePoint2D point7 = new YoFramePoint2D("point7", ReferenceFrame.getWorldFrame(), registry); point7.set(-1.4, 3.0); YoFramePoint2D point8 = new YoFramePoint2D("point8", ReferenceFrame.getWorldFrame(), registry); point8.set(-1.2, 3.0); YoFramePoint2D point9 = new YoFramePoint2D("point9", ReferenceFrame.getWorldFrame(), registry);
/** * Creates a copy of {@code this} by finding the duplicated {@code YoVariable}s in the given * {@link YoVariableRegistry}. * <p> * This method does not duplicate {@code YoVariable}s. Assuming the given registry is a duplicate * of the registry that was used to create {@code this}, this method searches for the duplicated * {@code YoVariable}s and use them to duplicate {@code this}. * </p> * * @param newRegistry YoVariableRegistry to duplicate {@code this} to. * @return the duplicate of {@code this}. */ public YoFramePoint2D duplicate(YoVariableRegistry newRegistry) { YoDouble x = (YoDouble) newRegistry.getVariable(getYoX().getFullNameWithNameSpace()); YoDouble y = (YoDouble) newRegistry.getVariable(getYoY().getFullNameWithNameSpace()); return new YoFramePoint2D(x, y, getReferenceFrame()); } }
final YoEnum<RobotSide> yoSupportSide = new YoEnum<>("supportSide", registry, RobotSide.class); final YoDouble swingTimeRemaining = new YoDouble("swingTimeRemaining", registry); final YoFramePoint2D yoICP = new YoFramePoint2D("ICP", worldFrame, registry); yoGraphicsListRegistry.registerArtifact("ICP", new YoGraphicPosition("ICP", yoICP, 0.02, YoAppearance.Blue(), GraphicType.CROSS).createArtifact()); final double omega0 = 3.4; yoICP.attachVariableChangedListener(variableChangedListener); yoSupportSide.addVariableChangedListener(variableChangedListener); yoICP.set(0.1, 0.2); variableChangedListener.notifyOfVariableChange(null);
YoFramePoint2D unclippedFootstepSolution = new YoFramePoint2D("unclippedFootstepSolution", ReferenceFrame.getWorldFrame(), registry); FramePose3D footstepPose = new FramePose3D(); FramePoint2D footstepXYSolution = new FramePoint2D(); assertTrue(unclippedFootstepSolution.epsilonEquals(expectedUnclippedSolution, 1e-3)); assertEquals(wasAdjusted, solutionHandler.wasFootstepAdjusted()); assertTrue(TupleTools.epsilonEquals(clippedAdjustment, solutionHandler.getClippedFootstepAdjustment(), 1e-3));
public YoGraphicPosition(String name, YoFramePoint2D framePoint, double scale, AppearanceDefinition appearance, GraphicType type) { super(name); framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); x = framePoint.getYoX(); y = framePoint.getYoY(); z = null; this.scale = scale; this.type = type; this.appearance = appearance; }
public static BetaFilteredYoFramePoint2d createBetaFilteredYoFramePoint2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, int beta, YoFramePoint2D unfilteredPoint) { // beta is a int BetaFilteredYoVariable x = new BetaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoX()); BetaFilteredYoVariable y = new BetaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, beta, unfilteredPoint.getYoY()); BetaFilteredYoFramePoint2d ret = new BetaFilteredYoFramePoint2d(x, y, unfilteredPoint.getReferenceFrame()); return ret; }
YoFramePoint2D vertex = new YoFramePoint2D(name + "Vertex" + i, worldFrame, registry); vertex.setToNaN(); vertexBuffer.add(vertex);
private void updateCapturePointError() { tempFramePoint2d.set(yoDesiredCapturePoint.getX(), yoDesiredCapturePoint.getY()); double error = Math.abs(yoCapturePoint.distance(tempFramePoint2d)); // if (error > maxObservedCapturePointError) // { // maxObservedCapturePointError = error; //// System.out.println("TurnValveBehavior: Max Capture Point Error : " + maxObservedCapturePointError); // } icpError.set(error); } }
/** * Gets the internal reference to the y-coordinate used for the first endpoint of this line * segment. * * @return the first endpoint y-coordinate as {@code YoVariable}. */ public YoDouble getYoFirstEndpointY() { return firstEndpoint.getYoY(); }
/** * Gets the internal reference to the x-coordinate used for the second endpoint of this line * segment. * * @return the second endpoint x-coordinate as {@code YoVariable}. */ public YoDouble getYoSecondEndpointX() { return secondEndpoint.getYoX(); }
/** * Creates a copy of {@code this} by finding the duplicated {@code YoVariable}s in the given * {@link YoVariableRegistry}. * <p> * This method does not duplicate {@code YoVariable}s. Assuming the given registry is a duplicate * of the registry that was used to create {@code this}, this method searches for the duplicated * {@code YoVariable}s and use them to duplicate {@code this}. * </p> * * @param newRegistry YoVariableRegistry to duplicate {@code this} to. * @return the duplicate of {@code this}. */ public YoFrameLineSegment2D duplicate(YoVariableRegistry newRegistry) { return new YoFrameLineSegment2D(firstEndpoint.duplicate(newRegistry), secondEndpoint.duplicate(newRegistry)); }
/** {@inheritDoc} */ @Override public ReferenceFrame getReferenceFrame() { return firstEndpoint.getReferenceFrame(); }
YoFramePoint2D yoCapturePoint = new YoFramePoint2D("CapturePoint", worldFrame, registry); YoArtifactPosition capturePointViz = new YoArtifactPosition("Capture Point", yoCapturePoint, GraphicType.BALL_WITH_ROTATED_CROSS, Blue().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), capturePointViz); yoCapturePoint.set(capturePoint); YoFramePoint2D yoDesiredCMP = new YoFramePoint2D("DesiredCMP", worldFrame, registry); YoArtifactPosition desiredCMPViz = new YoArtifactPosition("Desired CMP Position", yoDesiredCMP, GraphicType.SOLID_BALL, DarkRed().getAwtColor(), 0.008); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), desiredCMPViz); yoDesiredCMP.set(desiredCMP); YoFramePoint2D yoProjectedCMP = new YoFramePoint2D("ProjectedCMP", worldFrame, registry); YoArtifactPosition projectedCMPViz = new YoArtifactPosition("Projected CMP Position", yoProjectedCMP, GraphicType.BALL_WITH_ROTATED_CROSS, DarkRed().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), projectedCMPViz); yoProjectedCMP.set(projectedCMP); YoFramePoint2D yoExpectedCMP = new YoFramePoint2D("ExpectedProjection", worldFrame, registry); YoArtifactPosition expectedCMPViz = new YoArtifactPosition("Expected Projection", yoExpectedCMP, GraphicType.BALL, LawnGreen().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), expectedCMPViz); yoExpectedCMP.set(expectedProjection); YoFramePoint2D yoFinalCapturePoint = new YoFramePoint2D("FinalCapturePoint", worldFrame, registry); YoArtifactPosition finalCapturePointViz = new YoArtifactPosition("Final Capture Point", yoFinalCapturePoint, GraphicType.BALL_WITH_ROTATED_CROSS, Beige().getAwtColor(), 0.01); graphicsListRegistry.registerArtifact(getClass().getSimpleName(), finalCapturePointViz); yoFinalCapturePoint.set(finalCapturePoint);
/** * Creates a new {@code YoFrameLine2D} using the given {@code YoVariable}s and sets its reference * frame to {@code referenceFrame}. * * @param point the {@code YoFramePoint2D} to use internally for this line point. * @param direction the {@code YoFrameVector2D} to use internally for this line direction. * @throws ReferenceFrameMismatchException if {@code point} and {@code direction} are not * expressed in the same reference frame. */ public YoFrameLine2D(YoFramePoint2D point, YoFrameVector2D direction) { this(point.getYoX(), point.getYoY(), direction.getYoX(), direction.getYoY(), point.getReferenceFrame()); point.checkReferenceFrameMatch(direction); }