private void initializeSubTrajectory(int waypointIndex) { initialPositionProvider.setValue(waypoints.get(waypointIndex).getPosition()); initialVelocityProvider.setValue(waypoints.get(waypointIndex).getVelocity()); finalPositionProvider.setValue(waypoints.get(waypointIndex + 1).getPosition()); finalVelocityProvider.setValue(waypoints.get(waypointIndex + 1).getVelocity()); if (waypointIndex > 0) initialAccelerationProvider.setValue(subTrajectory.getAcceleration()); finalAccelerationProvider.setValue(0.0); double subTrajectoryTime = waypoints.get(waypointIndex + 1).getTime() - waypoints.get(waypointIndex).getTime(); trajectoryTimeProvider.setValue(subTrajectoryTime); subTrajectory.initialize(); }
public double getDesired(T controlSpace) { return desiredQs.get(controlSpace).getValue(); }
SettableDoubleProvider time = new SettableDoubleProvider(); StateMachine<ExampleStateName> stateMachine = new StateMachine<ExampleStateName>("stateMachine", "switchTime", ExampleStateName.class, time, registry); if (time.getValue() < 1.01) else if (time.getValue() < 2.01) else if (time.getValue() < 3.01) assertEquals(ExampleStateName.StateTwo, currentStateEnum); else time.setValue(time.getValue() + 0.01); while (time.getValue() < 10.0);
public void testSimpleStateMachine() SettableDoubleProvider timeProvider = new SettableDoubleProvider(); YoVariableRegistry registry = new YoVariableRegistry("SimpleStateMachine"); assertEquals(stateMachine.getCurrentState(), stateStart); while (timeProvider.getValue() < 10.0) timeProvider.add(0.01);
SettableDoubleProvider timeProvider = new SettableDoubleProvider(); double updateDT = 1.0e-8; timeProvider.add(updateDT); allFrames.forEach(ReferenceFrame::update);
desiredQs.put(key, new SettableDoubleProvider(initialValue)); desiredQds.put(key, new SettableDoubleProvider(0.0));
SettableDoubleProvider time = new SettableDoubleProvider(); StateMachine<ExampleStateName> stateMachine = new StateMachine<ExampleStateName>("stateMachine", "switchTime", ExampleStateName.class, time, registry); 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);
@Before public void setUp() { referenceFrame = ReferenceFrame.constructARootFrame("rootFrameTEST"); orientation = new FrameQuaternion(referenceFrame); trajectoryTimeProvider = new SettableDoubleProvider(trajectoryTime); initialOrientationProvider = new ConstantOrientationProvider(orientation); finalOrientationProvider = new ConstantOrientationProvider(orientation); parentRegistry = new YoVariableRegistry("registry"); }
private void initializeSubTrajectory(int waypointIndex) { initialPositionProvider.setValue(waypoints.get(waypointIndex).getPosition()); initialVelocityProvider.setValue(waypoints.get(waypointIndex).getVelocity()); finalPositionProvider.setValue(waypoints.get(waypointIndex + 1).getPosition()); finalVelocityProvider.setValue(waypoints.get(waypointIndex + 1).getVelocity()); if (waypointIndex > 0) initialAccelerationProvider.setValue(subTrajectory.getAcceleration()); finalAccelerationProvider.setValue(0.0); double subTrajectoryTime = waypoints.get(waypointIndex + 1).getTime() - waypoints.get(waypointIndex).getTime(); trajectoryTimeProvider.setValue(subTrajectoryTime); subTrajectory.initialize(); }
public double getDesiredVelocity(T controlSpace) { return desiredQds.get(controlSpace).getValue(); }
SettableDoubleProvider time = new SettableDoubleProvider(); StateMachine<ExampleStateName> stateMachine = new StateMachine<ExampleStateName>("stateMachine", "switchTime", ExampleStateName.class, time, registry);
private void initializeSubTrajectory(int waypointIndex) { initialPositionProvider.setValue(waypoints.get(waypointIndex).getPosition()); initialVelocityProvider.setValue(waypoints.get(waypointIndex).getVelocity()); finalPositionProvider.setValue(waypoints.get(waypointIndex + 1).getPosition()); finalVelocityProvider.setValue(waypoints.get(waypointIndex + 1).getVelocity()); double subTrajectoryTime = waypoints.get(waypointIndex + 1).getTime() - waypoints.get(waypointIndex).getTime(); trajectoryTimeProvider.setValue(subTrajectoryTime); subTrajectory.initialize(); }
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 initializeSubTrajectory(int waypointIndex) { initialPositionProvider.setValue(waypoints.get(waypointIndex).getPosition()); initialVelocityProvider.setValue(waypoints.get(waypointIndex).getVelocity()); finalPositionProvider.setValue(waypoints.get(waypointIndex + 1).getPosition()); finalVelocityProvider.setValue(waypoints.get(waypointIndex + 1).getVelocity()); double subTrajectoryTime = waypoints.get(waypointIndex + 1).getTime() - waypoints.get(waypointIndex).getTime(); trajectoryTimeProvider.setValue(subTrajectoryTime); subTrajectory.initialize(); }
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 initialize() { if (isEmpty()) { throw new RuntimeException("Trajectory has no waypoints."); } currentWaypointIndex.set(0); if (numberOfWaypoints.getIntegerValue() == 1) { YoOneDoFTrajectoryPoint firstWaypoint = waypoints.get(0); initialPositionProvider.setValue(firstWaypoint.getPosition()); initialVelocityProvider.setValue(firstWaypoint.getVelocity()); finalPositionProvider.setValue(firstWaypoint.getPosition()); finalVelocityProvider.setValue(firstWaypoint.getVelocity()); trajectoryTimeProvider.setValue(0.0); subTrajectory.initialize(); } else { initializeSubTrajectory(0); } }
@Override public void doTransitionIntoAction() { super.doTransitionIntoAction(); maxSwingTimeSpeedUpFactor.set(Math.max(swingTimeProvider.getValue() / minSwingTimeForDisturbanceRecovery.getDoubleValue(), 1.0)); swingTimeSpeedUpFactor.set(1.0); currentTimeWithSwingSpeedUp.set(Double.NaN); }
@Override public void initialize() { if (isEmpty()) { throw new RuntimeException("Trajectory has no waypoints."); } currentWaypointIndex.set(0); if (numberOfWaypoints.getIntegerValue() == 1) { YoOneDoFTrajectoryPoint firstWaypoint = waypoints.get(0); finalPositionProvider.setValue(firstWaypoint.getPosition()); finalVelocityProvider.setValue(firstWaypoint.getVelocity()); trajectoryTimeProvider.setValue(0.0); subTrajectory.initialize(); } else initializeSubTrajectory(0); }
private void updatePrivilegedConfiguration() { if (currentTime.getDoubleValue() > (percentOfSwingToStraightenLeg.getDoubleValue() * swingTimeProvider.getValue()) && attemptToStraightenLegs && !hasSwitchedToStraightLegs.getBooleanValue()) hasSwitchedToStraightLegs.set(true); }
@Override public void initialize() { if (isEmpty()) { throw new RuntimeException("Trajectory has no waypoints."); } currentWaypointIndex.set(0); if (numberOfWaypoints.getIntegerValue() == 1) { YoOneDoFTrajectoryPoint firstWaypoint = waypoints.get(0); finalPositionProvider.setValue(firstWaypoint.getPosition()); finalVelocityProvider.setValue(firstWaypoint.getVelocity()); trajectoryTimeProvider.setValue(0.0); subTrajectory.initialize(); } else initializeSubTrajectory(0); }