public YoGraphicLineSegment(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, AppearanceDefinition appearance, YoVariableRegistry registry) { this(namePrefix, new YoFramePoint3D(namePrefix, nameSuffix + "Start", referenceFrame, registry), new YoFramePoint3D(namePrefix, nameSuffix + "End", referenceFrame, registry), appearance); }
public void set(FramePoint3D framePoint) { this.framePoint.set(framePoint); } }
private void setupSlidersForSupportBaseControl() { int sliderChannel = 1; for (int i = 0; i < baseControlPoints.length; i++) { YoFramePoint3D baseControlPoint = baseControlPoints[i]; sliderBoard.setSlider(sliderChannel++, baseControlPoint.getYoX(), baseControlPoint.getX() - 2, baseControlPoint.getX() + 2); sliderBoard.setSlider(sliderChannel++, baseControlPoint.getYoY(), baseControlPoint.getY() - 2, baseControlPoint.getY() + 2); } }
public YoGraphicPosition(String name, YoFramePoint3D framePoint, double scale, AppearanceDefinition appearance, GraphicType type) { super(name); framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); x = framePoint.getYoX(); y = framePoint.getYoY(); z = framePoint.getYoZ(); this.scale = scale; this.type = type; this.appearance = appearance; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeDesiredCapturePointVelocity() { YoFramePoint3D initialCapturePointPosition = new YoFramePoint3D("", ReferenceFrame.getWorldFrame(), registry); YoFramePoint3D computedCapturePoint1 = new YoFramePoint3D("1", ReferenceFrame.getWorldFrame(), registry); YoFramePoint3D computedCapturePoint2 = new YoFramePoint3D("2", ReferenceFrame.getWorldFrame(), registry); YoFramePoint3D initialCenterOfPressure = new YoFramePoint3D("3", ReferenceFrame.getWorldFrame(), registry); YoFrameVector3D differentiatedCapturePointPosition = new YoFrameVector3D("4", ReferenceFrame.getWorldFrame(), registry); YoFrameVector3D computedCapturePointVelocity = new YoFrameVector3D("5", ReferenceFrame.getWorldFrame(), registry); for (int i = 0; i < nTests; i++) { initialCapturePointPosition.set(random.nextDouble(), random.nextDouble(), 0); initialCenterOfPressure.set(initialCapturePointPosition.getX() + 0.02, initialCapturePointPosition.getY() + 0.01, 0); double deltaT = 0.001; double time = random.nextDouble() * 0.1 + 0.05; double omega0 = 0.5; CapturePointTools.computeDesiredCapturePointPosition(omega0, time, initialCapturePointPosition, initialCenterOfPressure, computedCapturePoint1); CapturePointTools.computeDesiredCapturePointPosition(omega0, time + deltaT, initialCapturePointPosition, initialCenterOfPressure, computedCapturePoint2); differentiatedCapturePointPosition.set(computedCapturePoint2); differentiatedCapturePointPosition.sub(computedCapturePoint1); differentiatedCapturePointPosition.scale(1 / deltaT); CapturePointTools.computeDesiredCapturePointVelocity(omega0, time + deltaT, initialCapturePointPosition, initialCenterOfPressure, computedCapturePointVelocity); EuclidCoreTestTools.assertTuple3DEquals("", computedCapturePointVelocity, differentiatedCapturePointPosition, 1e-3); } }
public void set(List<? extends Point3DReadOnly> points) { if (points.size() > this.ccwOrderedYoFramePoints.length) throw new RuntimeException("Cannot plot more vertices than the maximum number"); numberOfPoints.set(points.size()); for (int i = 0; i < numberOfPoints.getValue(); i++) ccwOrderedYoFramePoints[i].set(points.get(i)); for (int i = numberOfPoints.getValue(); i < ccwOrderedYoFramePoints.length; i++) ccwOrderedYoFramePoints[i].setToNaN(); }
@Before public void setUp() { referenceFrame = ReferenceFrame.constructARootFrame("rootNameTEST"); registry = new YoVariableRegistry("parentRegistryTEST"); yoFramePoint = new YoFramePoint3D(namePrefix, referenceFrame, registry); yoFramePoint.set(xValue, yValue, zValue); }
private void setupVisualization(String prefix, YoGraphicsListRegistry graphicsListRegistry, YoVariableRegistry registry) { YoGraphicsList yoGraphicsList = new YoGraphicsList(prefix + "VisGraph"); for (int i = 0; i < bodyPathPointsForVisualization; i++) { YoFramePoint3D point = new YoFramePoint3D(prefix + "BodyPathPoint" + i, ReferenceFrame.getWorldFrame(), registry); point.setToNaN(); bodyPathPoints.add(point); YoGraphicPosition pointVisualization = new YoGraphicPosition(prefix + "BodyPathPoint" + i, point, 0.02, YoAppearance.Yellow()); yoGraphicsList.add(pointVisualization); } graphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
@Override public void initialize() { currentTime.set(0.0); double tIntermediate = trajectoryTime.getDoubleValue() / 2.0; xPolynomial.setQuadraticWithFinalVelocityConstraint(0.0,trajectoryTime.getDoubleValue(), initialPosition.getX(), finalPosition.getX(), finalVelocity.getX()); yPolynomial.setQuadraticWithFinalVelocityConstraint(0.0,trajectoryTime.getDoubleValue(), initialPosition.getY(), finalPosition.getY(), finalVelocity.getY()); zPolynomial.setCubicWithIntermediatePositionAndFinalVelocityConstraint(0.0, tIntermediate, trajectoryTime.getDoubleValue(), initialPosition.getZ(), intermediateZPosition.getDoubleValue(), finalPosition.getZ(), finalVelocity.getZ()); currentPosition.set(initialPosition); currentAcceleration.setToZero(); }
public void handleClick(Point3D intersectionPosition, Point3D cameraPosition) { LaunchedBall nextBall = poolOfBalls.get(nextBallIndex); final double ballVelocityMagnitude = directedPerturbance.getBallVelocityMagnitude(); tempPoint.set(ballTarget.getX(), ballTarget.getY(), ballTarget.getZ()); Point3D initialPosition = computeInitialPosition(intersectionPosition, ballTarget.getZ()); final Point3D finalPosition = computeFinalPosition(initialPosition, ballVelocityMagnitude); nextBall.launch(initialPosition, finalPosition, directedPerturbance.getBallMass(), ballVelocityMagnitude); nextBallIndex++; if (nextBallIndex >= poolOfBalls.size()) { nextBallIndex = 0; } }
public YoGraphicLineSegment(String name, YoFramePoint3D startPoint, YoFramePoint3D endPoint, double scale, AppearanceDefinition appearance, boolean drawArrowhead) { this(name, startPoint.getYoX(), startPoint.getYoY(), startPoint.getYoZ(), endPoint.getYoX(), endPoint.getYoY(), endPoint.getYoZ(), scale, appearance, drawArrowhead); if (!startPoint.getReferenceFrame().isWorldFrame() || !endPoint.getReferenceFrame().isWorldFrame()) { System.err.println("Warning: Should be in a World Frame to create a YoGraphicLineSegment. startPoint = " + startPoint + ", endPoint = " + endPoint); } }
public void setConstantPose(FramePose3D constantPose) { position.checkReferenceFrameMatch(constantPose); position.set(constantPose.getX(), constantPose.getY(), constantPose.getZ()); orientation.setYawPitchRoll(constantPose.getYaw(), constantPose.getPitch(), constantPose.getRoll()); }
@Test// timeout=300000 public void testGetYoPosition() { YoFramePoint3D yoPosition = kinematicPoint.getYoPosition(); String frameName = yoPosition.getReferenceFrame().getName(); assertEquals("( 0.000, 0.000, 0.000 )-" + frameName, yoPosition.toString()); yoPosition.set(new Point3D(5.0, 5.1, 5.2)); assertEquals("( 5.000, 5.100, 5.200 )-" + frameName, yoPosition.toString()); }
@Override public void setPositionToNaN() { position.setToNaN(); }
@Override protected boolean containsNaN() { if (pointOne.containsNaN()) return true; if (pointTwo.containsNaN()) return true; if (pointThree.containsNaN()) return true; return false; }
public double getX() { return getPosition().getX(); }
public double getY() { return getPosition().getY(); }