public double getDesired(T controlSpace) { return desiredQs.get(controlSpace).getValue(); }
public double getDesiredVelocity(T controlSpace) { return desiredQds.get(controlSpace).getValue(); }
private void setTrajectories() { for (int i = 0; i < controlledFingerJoints.size(); i++) { T key = controlledFingerJoints.get(i); trajectoryGenerators.get(key).clear(); trajectoryGenerators.get(key).appendWaypoint(0.0, desiredQs.get(key).getValue(), 0.0); TDoubleArrayList wayPointPositions = this.wayPointPositions.get(key); TDoubleArrayList wayPointTimes = this.wayPointTimes.get(key); for (int j = 0; j < wayPointPositions.size(); j++) { trajectoryGenerators.get(key).appendWaypoint(wayPointTimes.get(j), wayPointPositions.get(j), 0.0); } } }
private void computeSwingTimeRemaining() { if (!currentTimeWithSwingSpeedUp.isNaN()) { double swingTimeRemaining = (swingTimeProvider.getValue() - currentTimeWithSwingSpeedUp.getDoubleValue()) / swingTimeSpeedUpFactor.getDoubleValue(); this.swingTimeRemaining.set(swingTimeRemaining); } else { this.swingTimeRemaining.set(swingTimeProvider.getValue() - getTimeInCurrentState()); } }
@Override public void doTransitionIntoAction() { super.doTransitionIntoAction(); maxSwingTimeSpeedUpFactor.set(Math.max(swingTimeProvider.getValue() / minSwingTimeForDisturbanceRecovery.getDoubleValue(), 1.0)); swingTimeSpeedUpFactor.set(1.0); currentTimeWithSwingSpeedUp.set(Double.NaN); }
private void updatePrivilegedConfiguration() { if (currentTime.getDoubleValue() > (percentOfSwingToStraightenLeg.getDoubleValue() * swingTimeProvider.getValue()) && attemptToStraightenLegs && !hasSwitchedToStraightLegs.getBooleanValue()) hasSwitchedToStraightLegs.set(true); }
protected void reinitializeTrajectory() { orientationTrajectoryGenerator.setTrajectoryTime(swingTimeProvider.getValue()); if (useNewSwingTrajectoyOptimization) { TrajectoryType trajectoryType = trajectoryParametersProvider.getTrajectoryParameters().getTrajectoryType(); finalConfigurationProvider.getPosition(finalPosition); touchdownVelocityProvider.get(finalVelocity); swingTrajectoryGeneratorNew.setFinalConditions(finalPosition, finalVelocity); swingTrajectoryGeneratorNew.setStepTime(swingTimeProvider.getValue()); swingTrajectoryGeneratorNew.setTrajectoryType(trajectoryType, swingWaypointsForSole); } positionTrajectoryGenerator.initialize(); orientationTrajectoryGenerator.initialize(); trajectoryWasReplanned = false; replanTrajectory.set(false); }
protected void initializeTrajectory() { if (!hasInitialAngularConfigurationBeenProvided.getBooleanValue()) { currentAngularVelocityProvider.get(initialAngularVelocity); initialOrientation.setToZero(controlFrame); orientationTrajectoryGenerator.setInitialConditions(initialOrientation, initialAngularVelocity); } orientationTrajectoryGenerator.setTrajectoryTime(swingTimeProvider.getValue()); if (useNewSwingTrajectoyOptimization) { TrajectoryType trajectoryType = trajectoryParametersProvider.getTrajectoryParameters().getTrajectoryType(); double swingHeight = trajectoryParametersProvider.getTrajectoryParameters().getSwingHeight(); initialConfigurationProvider.getPosition(initialPosition); initialVelocityProvider.get(initialVelocity); finalConfigurationProvider.getPosition(finalPosition); touchdownVelocityProvider.get(finalVelocity); stanceConfigurationProvider.getPosition(stanceFootPosition); swingTrajectoryGeneratorNew.setInitialConditions(initialPosition, initialVelocity); swingTrajectoryGeneratorNew.setFinalConditions(finalPosition, finalVelocity); swingTrajectoryGeneratorNew.setStepTime(swingTimeProvider.getValue()); swingTrajectoryGeneratorNew.setTrajectoryType(trajectoryType, swingWaypointsForSole); swingTrajectoryGeneratorNew.setSwingHeight(swingHeight); swingTrajectoryGeneratorNew.setStanceFootPosition(stanceFootPosition); } positionTrajectoryGenerator.initialize(); orientationTrajectoryGenerator.initialize(); trajectoryWasReplanned = false; replanTrajectory.set(false); }
minSwingTimeForDisturbanceRecovery.set(walkingControllerParameters.getMinimumSwingTimeForDisturbanceRecovery()); maxSwingTimeSpeedUpFactor = new DoubleYoVariable(namePrefix + "MaxSwingTimeSpeedUpFactor", registry); maxSwingTimeSpeedUpFactor.set(Math.max(swingTimeProvider.getValue() / minSwingTimeForDisturbanceRecovery.getDoubleValue(), 1.0)); currentTime = new DoubleYoVariable(namePrefix + "CurrentTime", registry); currentTimeWithSwingSpeedUp = new DoubleYoVariable(namePrefix + "CurrentTimeWithSwingSpeedUp", registry);
assertEquals(stateMachine.getCurrentState(), stateStart); while (timeProvider.getValue() < 10.0)
time.setValue(time.getValue() + 0.01); assertEquals(time.getValue(), listener.time, 1e-7); time.setValue(time.getValue() + 0.01); assertEquals(stateOne, listenerTwo.oldState); assertEquals(stateTwo, listenerTwo.newState); assertEquals(time.getValue(), listener.time, 1e-7); time.setValue(time.getValue() + 0.01); time.setValue(time.getValue() + 0.01); assertEquals(time.getValue(), listener.time, 1e-7); time.setValue(time.getValue() + 0.01); time.setValue(time.getValue() + 0.01);