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(); }
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(); }
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 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(); }
@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 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); }
@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); }
@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); }
public void requestTouchdownForDisturbanceRecovery() { if (isPerformingTouchdown.getBooleanValue()) return; positionTrajectoryGenerator.getPosition(desiredPosition); currentDesiredFootPosition.set(desiredPosition); touchdownInitialTimeProvider.setValue(getTimeInCurrentState()); positionTrajectoryForDisturbanceRecovery.initialize(); isPerformingTouchdown.set(true); }
public void setFootstep(Footstep footstep, double swingTime) swingTimeProvider.setValue(swingTime); footstep.getSolePose(footstepSolePose);
time.setValue(time.getValue() + 0.01); assertEquals(time.getValue(), listener.time, 1e-7); 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); assertEquals(time.getValue(), listener.time, 1e-7); time.setValue(time.getValue() + 0.01); time.setValue(time.getValue() + 0.01);
time.setValue(time.getValue() + 0.01);