@Override public double getProportionalGain() { return proportionalGain.getValue(); }
@Override public double getPositionDeadband() { return deadband.getValue(); }
@Override public double getMaximumFeedback() { return maxOutput.getValue(); }
@Override public double getIntegralGain() { return integralGain.getValue(); }
@Override public double getTime() { return timeProvider.getValue(); }
@Override public double getTimeInCurrentState() { return timeProvider.getValue() - timeOfLastStateChange; }
@Override public double getTime() { return timeProvider.getValue(); }
@Override public double getPositionDeadband() { return positionDeadband.getValue(); } }
@Override public double getIntegralLeakRatio() { return integralLeakRatio.getValue(); }
@Override public void getQMatrix(DenseMatrix64F matrixToPack) { matrixToPack.reshape(size, size); CommonOps.fill(matrixToPack, 0.0); matrixToPack.set(angularAccelerationStart + 0, angularAccelerationStart + 0, angularAccelerationVariance.getValue() * sqrtHz); matrixToPack.set(angularAccelerationStart + 1, angularAccelerationStart + 1, angularAccelerationVariance.getValue() * sqrtHz); matrixToPack.set(angularAccelerationStart + 2, angularAccelerationStart + 2, angularAccelerationVariance.getValue() * sqrtHz); matrixToPack.set(linearAccelerationStart + 0, linearAccelerationStart + 0, linearAccelerationVariance.getValue() * sqrtHz); matrixToPack.set(linearAccelerationStart + 1, linearAccelerationStart + 1, linearAccelerationVariance.getValue() * sqrtHz); matrixToPack.set(linearAccelerationStart + 2, linearAccelerationStart + 2, linearAccelerationVariance.getValue() * sqrtHz); }
@Override public double getTimeInCurrentState() { double t = timeProvider.getValue() - timeOfLastStateChange.getDoubleValue(); timeInCurrentState.set(t); return t; }
private void updateUsingDifference(double difference) { double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; double alpha = alphaVariable.getValue(); this.set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); }
private void updateForwardVelocity(double alpha) { double minMaxVelocity = maxStepLength / stepTime.getValue(); forwardVelocityProperty.set(minMaxVelocity * MathTools.clamp(alpha, 1.0)); }
private void updateLateralVelocity(double alpha) { double minMaxVelocity = maxStepWidth / stepTime.getValue(); lateralVelocityProperty.set(minMaxVelocity * MathTools.clamp(alpha, 1.0)); }
private void updateUsingDifference(double difference) { double previousFilteredDerivative = getDoubleValue(); double currentRawDerivative = difference / dt; double alpha = alphaVariable == null ? alphaDouble : alphaVariable.getValue(); set(alpha * previousFilteredDerivative + (1.0 - alpha) * currentRawDerivative); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testOrientation() { trajectoryGenerator.setControlHandAngleAboutAxis(true); // v = omega x r checkVEqualsOmegaCrossR(worldFrame, trajectoryGenerator, random); checkOrientationAtVariousPoints(trajectoryGenerator, initialOrientationProvider, trajectoryTimeProvider.getValue(), worldFrame); }
@Override public void getRMatrix(DenseMatrix64F matrixToPack) { matrixToPack.reshape(getMeasurementSize(), getMeasurementSize()); CommonOps.setIdentity(matrixToPack); CommonOps.scale(variance.getValue() * sqrtHz, matrixToPack); }
@Override public void initialize() { currentTime.set(0.0); trajectoryTime.set(trajectoryTimeProvider.getValue()); parameterPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); updateInitialPosition(); updateFinalPosition(); }
private void updateActionMode() { timeInCurrentMode.add(controlDt); updateWindowSize(); StictionActionMode estimatedCurrentMode = estimateCurrentActionMode(); if (timeInCurrentMode.getDoubleValue() > minTimeInMode.getValue() && estimatedCurrentMode != stictionActionMode.getEnumValue()) { stictionActionMode.set(estimatedCurrentMode); timeInCurrentMode.set(0.0); } }