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); }
RigidBody rigidBody = contactableFoot.getRigidBody(); stanceConfigurationProvider = new CurrentConfigurationProvider(momentumBasedController.getReferenceFrames().getFootFrame(robotSide.getOppositeSide())); initialConfigurationProvider = new CurrentConfigurationProvider(soleFrame); initialVelocityProvider = new CurrentLinearVelocityProvider(soleFrame, rigidBody, twistCalculator);
initialConfigurationProvider.getPosition(oldFootstepPosition);