public YoFramePoseUsingYawPitchRoll(String prefix, String suffix, ReferenceFrame frame, YoVariableRegistry registry) { position = new YoFramePoint3D(prefix, suffix, frame, registry); orientation = new YoFrameYawPitchRoll(prefix, suffix, frame, registry); }
public void setYawPitchRoll(double yaw, double pitch, double roll) { setYawPitchRoll(yaw, pitch, roll, true); }
public void interpolate(YoFrameYawPitchRoll orientationOne, YoFrameYawPitchRoll orientationTwo, double alpha) { orientationOne.putYoValuesIntoFrameOrientation(); orientationTwo.putYoValuesIntoFrameOrientation(); tempFrameOrientation.setToZero(getReferenceFrame()); tempFrameOrientation.interpolate(orientationOne.tempFrameOrientation, orientationTwo.tempFrameOrientation, alpha); this.set(tempFrameOrientation); }
public void set(RotationMatrixReadOnly rotation) { tempFrameOrientation.setIncludingFrame(getReferenceFrame(), rotation); set(tempFrameOrientation); }
public void set(FrameQuaternionReadOnly orientation) { set(orientation, true); }
YoFramePoint3D yoFramePolygonPosition = new YoFramePoint3D("yoPolygonPosition", "", worldFrame, registry); yoFramePolygonPosition.set(2.0, 1.0, 0.3); final YoFrameYawPitchRoll yoFramePolygonOrientation = new YoFrameYawPitchRoll("yoPolygonOrientation", "", worldFrame, registry); yoFramePolygonOrientation.setYawPitchRoll(1.2, 0.1, 0.4); final YoGraphicPolygon yoGraphicYoFramePolygon = new YoGraphicPolygon("YoFramePolygon", yoFramePolygon, yoFramePolygonPosition, yoFramePolygonOrientation, 1.0, YoAppearance.DarkBlue()); double boxSize = 0.3; boxPosition.set(boxSize / 2.0, boxSize / 2.0, boxSize / 2.0); YoFrameYawPitchRoll boxOrientation = new YoFrameYawPitchRoll("boxOrientation", worldFrame, registry); YoGraphicShape yoGraphicBoxGhost = new YoGraphicShape("boxGhost", boxGhostGraphics, boxPosition, boxOrientation, boxSize);
/** * 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 YoFrameYawPitchRoll duplicate(YoVariableRegistry newRegistry) { YoDouble yaw = (YoDouble) newRegistry.getVariable(this.yaw.getFullNameWithNameSpace()); YoDouble pitch = (YoDouble) newRegistry.getVariable(this.pitch.getFullNameWithNameSpace()); YoDouble roll = (YoDouble) newRegistry.getVariable(this.roll.getFullNameWithNameSpace()); return new YoFrameYawPitchRoll(yaw, pitch, roll, getReferenceFrame()); }
lidarRange.set(30.0); final YoFrameYawPitchRoll lidarYawPitchRoll = new YoFrameYawPitchRoll("lidar", null, registry); lidarYawPitchRoll.attachVariableChangedListener(new VariableChangedListener()
private void generateRandomSlipParamters() { double randomSlipTranslateX = pseudoRandomRealNumberWithinRange(minTranslationToSlipNextStep.getX(), maxTranslationToSlipNextStep.getX()); double randomSlipTranslateY = pseudoRandomRealNumberWithinRange(minTranslationToSlipNextStep.getY(), maxTranslationToSlipNextStep.getY()); double randomSlipTranslateZ = pseudoRandomRealNumberWithinRange(minTranslationToSlipNextStep.getZ(), maxTranslationToSlipNextStep.getZ()); nextTranslationToSlip.set(randomSlipTranslateX, randomSlipTranslateY, randomSlipTranslateZ); nextRotationToSlip.setYawPitchRoll(pseudoRandomRealNumberWithinRange(minRotationToSlipNextStep.getYawPitchRoll(), maxRotationToSlipNextStep.getYawPitchRoll())); double randomSlipAfterTimeDelta = pseudoRandomPositiveNumberWithinRange(minSlipAfterTimeDelta.getDoubleValue(), maxSlipAfterTimeDelta.getDoubleValue()); double randomPercentToSlipPerTick = pseudoRandomPositiveNumberWithinRange(minSlipPercentSlipPerTick.getDoubleValue(), maxSlipPercentSlipPerTick.getDoubleValue()); nextSlipAfterTimeDelta.set(randomSlipAfterTimeDelta); nextSlipPercentSlipPerTick.set(randomPercentToSlipPerTick); }
public YoDouble getYoYaw() { return getOrientation().getYaw(); }
private void applyRotationalSlip(double percentOfDelta) { FrameQuaternion identity = new FrameQuaternion(ReferenceFrame.getWorldFrame()); FrameQuaternion desired = slipRotation.getFrameOrientationCopy(); FrameQuaternion delta = new FrameQuaternion(); delta.interpolate(identity, desired, percentOfDelta); desired.interpolate(identity, desired, 1.0-percentOfDelta); slipRotation.set(desired); Point3D touchdownCoM = computeTouchdownCoM(); RotationMatrix deltaRotation = new RotationMatrix(delta); Point3D touchdownLocation = new Point3D(); for (int i = 0; i < groundContactPointsToSlip.size(); i++) { GroundContactPoint groundContactPointToSlip = groundContactPointsToSlip.get(i); boolean touchedDown = (groundContactPointToSlip.isInContact()); if (touchedDown) { groundContactPointToSlip.getTouchdownLocation(touchdownLocation); touchdownLocation.sub(touchdownCoM); deltaRotation.transform(touchdownLocation); touchdownLocation.add(touchdownCoM); groundContactPointToSlip.setTouchdownLocation(touchdownLocation); } } }
public YoDouble getYoRoll() { return getOrientation().getRoll(); }
public YoDouble getYoPitch() { return getOrientation().getPitch(); }
public boolean isDoneSlipping() { boolean translationalSlipDone = slipAmount.lengthSquared() < 0.0001 * 0.0001; Vector3D eulerAngles = new Vector3D(); slipRotation.getEulerAngles(eulerAngles); boolean rotationalSlipDone = eulerAngles.lengthSquared() < 0.001 * 0.001; return translationalSlipDone & rotationalSlipDone; }
@Override public void run() { int i = 0; while (i++ < 20) { quickPause(); ConvexPolygon2D newPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(secondPointList)); yoPolygon.set(newPolygon); ConvexPolygon2D newYoPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(pointList)); yoFramePolygon.set(newYoPolygon); yoGraphicYoFramePolygon.update(); Vector3D eulerAngles = new Vector3D(); yoFramePolygonOrientation.getEulerAngles(eulerAngles); eulerAngles.setY(eulerAngles.getY() + 0.1); yoFramePolygonOrientation.setEulerAngles(eulerAngles); yoGraphicText.setText("Hello"); yoGraphicText.update(); scs.tickAndUpdate(); quickPause(); newPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(pointList)); yoPolygon.set(newPolygon); yoGraphicYoFramePolygon.update(); newYoPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(secondPointList)); yoFramePolygon.set(newYoPolygon); yoGraphicText.setText("GoodBye"); yoGraphicText.update(); scs.tickAndUpdate(); } }
public void setOrientation(QuaternionReadOnly quaternion) { orientation.set(quaternion); }
public void set(QuaternionReadOnly quaternion, boolean notifyListeners) { tempFrameOrientation.setIncludingFrame(getReferenceFrame(), quaternion); set(tempFrameOrientation, notifyListeners); }
public double getYaw() { return getOrientation().getYaw().getDoubleValue(); }
public double getRoll() { return getOrientation().getRoll().getDoubleValue(); }