public double timeInCurrentState() { return time.getValue() - switchTimeYoVariable.getDoubleValue(); }
private void setStepTime() { double stepTime = stepTimeProvider.getValue(); if (stepTime <= 0) { throw new RuntimeException("stepTimeProvider must provide a positive time value."); } this.stepTime.set(stepTime); timeIntoStep.set(0.0); }
@Override public void compute(double time) { super.compute(time); if (isDone()) { currentValue.set(finalPositionProvider.getValue()); currentVelocity.set(finalVelocityProvider.getValue()); currentAcceleration.set(0.0); } else if (time <= 0.0) { currentValue.set(initialPositionProvider.getValue()); currentVelocity.set(initialVelocityProvider.getValue()); currentAcceleration.set(0.0); } } }
public void setInitialTimePositionsAndVelocities() { t0 = startTimeProvider.getValue(); startTime.set(startTimeProvider.getValue()); timeIntoTouchdown.set(0.0); initialPositionSource.getPosition(p0); p0.changeFrame(referenceFrame); velocitySource.get(pd0); pd0.changeFrame(referenceFrame); accelerationSource.get(pdd0); pdd0.changeFrame(referenceFrame); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); setPolynomial(); compute(0.0); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); initialPositionProvider.getPosition(tempFramePoint); tempFramePoint.changeFrame(referenceFrame); double y = tempFramePoint.getY(); double x = tempFramePoint.getX(); z.set(tempFramePoint.getZ()); radius.set(Math.sqrt(x * x + y * y)); double initialAngle = Math.atan2(y, x); double finalAngle = initialAngle + desiredRotationAngleProvider.getValue(); anglePolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialAngle, 0.0, 0.0, finalAngle, 0.0, 0.0); }
public GenericStateMachine(String stateYoVariableName, String switchTimeName, Class<E> enumType, E initialState, DoubleProvider timeProvider, YoVariableRegistry registry) { stateYoVariable = new EnumYoVariable<E>(stateYoVariableName, "State machine variable to keep track of the state.", registry, enumType, false); previousStateYoVariable = new EnumYoVariable<E>(stateYoVariableName + "PreviousState", "State machine variable to keep track of the previous state.", registry, enumType, true); enumsToStates = new EnumMap<>(enumType); if (initialState != null) { stateYoVariable.set(initialState); } previousStateYoVariable.set(null); switchTimeYoVariable = new DoubleYoVariable(switchTimeName, registry); this.time = timeProvider; switchTimeYoVariable.set(time.getValue()); }
@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(); }
public void initialize(FramePoint initialPosition, FrameVector initialVelocity, FrameVector initialAcceleration, FramePoint finalDesiredPosition, FrameVector finalDesiredVelocity) { timeIntoStep.set(0.0); this.stepTime.set(stepTimeProvider.getValue()); if (stepTime.getDoubleValue() < 1e-10) { stepTime.set(1e-10); } minimumJerkTrajectory.setParams(0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, stepTime.getDoubleValue()); double middleOfTrajectoryParameter = 0.5; parabolicTrajectoryGenerator.initialize(initialPosition, finalDesiredPosition, groundClearance.getDoubleValue(), middleOfTrajectoryParameter); }
/** * Desired joint angles and velocities come from reading the joints, this method can override them those position and velocity values. * @param currentDesiredPosition Sets the desired joint position. * @param currentDesiredVelocity Sets the desired joint velocity. */ @Override public void initialize(double initialPosition, double initialVelocity) { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); this.polynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialPosition, initialVelocity, 0.0, finalPosition.getDoubleValue(), 0.0, 0.0); currentPosition.set(initialPosition); currentVelocity.set(initialVelocity); currentAcceleration.set(0.0); }
public void initialize() { double trajectoryTime = trajectoryTimeProvider.getValue(); MathTools.checkIfInRange(trajectoryTime, 0.0, Double.POSITIVE_INFINITY); this.trajectoryTime.set(trajectoryTime); currentTime.set(0.0); parameterPolynomial.setQuintic(0.0, trajectoryTime, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0); updateInitialOrientation(); updateFinalOrientation(); desiredOrientation.set(initialOrientation); desiredAngularVelocity.setToZero(); desiredAngularAcceleration.setToZero(); }
public double getValue() { FramePoint initialPosition = new FramePoint(); initialPositionProvider.getPosition(initialPosition); initialPosition.changeFrame(ReferenceFrame.getWorldFrame()); FramePoint finalPosition = new FramePoint(); finalPositionProvider.getPosition(finalPosition); finalPosition.changeFrame(ReferenceFrame.getWorldFrame()); double distance = initialPosition.distance(finalPosition); double time = distance / averageVelocityProvider.getValue(); time = MathTools.clipToMinMax(time, minimumTime, Double.POSITIVE_INFINITY); return time; } }
public void initialize() { timeIntoStep.set(0.0); this.stepTime.set(stepTimeProvider.getValue()); if (stepTime.getDoubleValue() < 1e-10) { stepTime.set(1e-10); } minimumJerkTrajectory.setParams(0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, stepTime.getDoubleValue()); double middleOfTrajectoryParameter = 0.5; FramePoint initialPosition = new FramePoint(ReferenceFrame.getWorldFrame()); initialPositionProvider.getPosition(initialPosition); FramePoint finalPosition = new FramePoint(ReferenceFrame.getWorldFrame()); finalPositionProvider.getPosition(finalPosition); initialPosition.changeFrame(parabolicTrajectoryGenerator.getReferenceFrame()); finalPosition.changeFrame(parabolicTrajectoryGenerator.getReferenceFrame()); double maxAnkleHeight = Math.max(initialPosition.getZ(), finalPosition.getZ()); double apexHeight = maxAnkleHeight + groundClearance.getDoubleValue(); parabolicTrajectoryGenerator.initialize(initialPosition, finalPosition, apexHeight, middleOfTrajectoryParameter); }