for (int i = 0; i < swingWaypoints.size(); i++) this.swingWaypointsForSole.add().setIncludingFrame(worldFrame, swingWaypoints.get(i)); trajectoryParametersProvider.set(new TrajectoryParameters(trajectoryType)); return; trajectoryParametersProvider.set(new TrajectoryParameters(TrajectoryType.OBSTACLE_CLEARANCE, footstep.getSwingHeight())); trajectoryParametersProvider.set(new TrajectoryParameters(trajectoryType, footstep.getSwingHeight()));
private List<FramePoint> getWaypointsFromTrajectoryParameters(TrajectoryParameters trajectoryParameters) { double swingHeight = trajectoryParameters.getSwingHeight(); swingHeight = Math.max(swingHeight, TwoWaypointTrajectoryGeneratorParameters.getDefaultGroundClearance()); double initialHeight = allPositions[0].getZ(); if (stancePositionSource != null) { double stanceHeight = stancePosition.getZ(); double maxWorldHeightForSwing = stanceHeight + maxSwingHeightFromStanceFoot; if (initialHeight + swingHeight > maxWorldHeightForSwing) { swingHeight = maxWorldHeightForSwing - initialHeight; } } switch (trajectoryParameters.getTrajectoryType()) { case OBSTACLE_CLEARANCE: return getWaypointsForObstacleClearance(swingHeight); case DEFAULT: default: return getWaypointsAtGroundClearance(TwoWaypointTrajectoryGeneratorParameters.getDefaultGroundClearance()); } }
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); }
VectorProvider finalVelocityProvider = new ConstantVectorProvider(new FrameVector3D(worldFrame, new double[] {0.1, 0.01, -0.02})); TrajectoryParameters trajectoryParameters = new TrajectoryParameters(); TrajectoryParametersProvider trajectoryParametersProvider = new TrajectoryParametersProvider(trajectoryParameters);
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); }
VectorProvider finalVelocityProvider = new ConstantVectorProvider(new FrameVector3D(worldFrame, new double[] {0, 0, -0.02})); TrajectoryParameters trajectoryParameters = new TrajectoryParameters(); TrajectoryParametersProvider trajectoryParametersProvider = new TrajectoryParametersProvider(trajectoryParameters);